/*******************************************************************************
|
* 文件名称 : motion_control_task.c
|
* 文件说明 : 割草机运动控制 FreeRTOS 任务封装
|
* 创建日期 : 2025-11-22
|
*******************************************************************************/
|
|
#include "motion_control_task.h"
|
|
#include "FreeRTOS.h"
|
#include "task.h"
|
|
#include "stm32h7xx_hal.h"
|
#include <string.h>
|
|
#include "DBG.h"
|
#include "GPS.h"
|
#include "PythonLink.h"
|
#include "geo_utils.h"
|
#include "motion_config.h"
|
#include "motion_control.h"
|
#include "motion_path_data.h"
|
#include "pwm_ctrol.h"
|
|
#define RAD2DEG (57.29577951308232f)
|
|
#define MC_TASK_STACK_WORDS (2048U)
|
#define MC_TASK_PRIORITY (tskIDLE_PRIORITY + 4U)
|
#define MC_TASK_PERIOD_TICKS (pdMS_TO_TICKS((HIDO_UINT32)(1000.0f / MC_CFG_CONTROL_HZ)))
|
#define MC_CLAMP(v, lo, hi) ((v) < (lo) ? (lo) : (((v) > (hi)) ? (hi) : (v)))
|
|
extern TaskHandle_t g_app_task_handle;
|
|
static TaskHandle_t g_motion_task_handle = NULL;
|
static MC_State g_motion_state;
|
static MC_Config g_motion_config;
|
static ST_GeoOrigin g_motion_origin;
|
static HIDO_UINT32 g_last_gps_ms = 0U;
|
static HIDO_UINT32 g_last_imu_ms = 0U;
|
static HIDO_UINT32 g_last_gprmi_tow = 0U;
|
static HIDO_UINT32 g_last_control_report_ms = 0U;
|
static HIDO_UINT32 g_last_pose_report_ms = 0U;
|
static HIDO_UINT32 g_last_state_report_ms = 0U;
|
static HIDO_UINT32 g_last_stack_report_ms = 0U;
|
static HIDO_FLOAT g_control_freq_hz = 0.0f;
|
static HIDO_UINT32 g_freq_sample_count = 0U;
|
static HIDO_UINT32 g_freq_sample_start_ms = 0U;
|
static HIDO_FLOAT g_last_enu[3] = {0.0f, 0.0f, 0.0f};
|
static HIDO_FLOAT g_last_heading_deg = 0.0f;
|
static HIDO_FLOAT g_last_pitch_deg = 0.0f;
|
static HIDO_FLOAT g_last_roll_deg = 0.0f;
|
static HIDO_BOOL g_last_pose_valid = HIDO_FALSE;
|
static HIDO_FLOAT g_last_target_xy[2] = {0.0f, 0.0f};
|
static HIDO_BOOL g_last_target_valid = HIDO_FALSE;
|
static HIDO_UINT16 g_last_steering_pwm = MC_CFG_PWM_CENTER_US;
|
static HIDO_UINT16 g_last_throttle_pwm = MC_CFG_PWM_CENTER_US;
|
|
static HIDO_UINT32 g_last_sensor_timestamp_ms = 0U;
|
|
static void MotionControl_TaskEntry(void *argument);
|
static void MotionControl_ApplyOutput(const MC_Output *output, HIDO_UINT16 *_pu16Steering, HIDO_UINT16 *_pu16Throttle);
|
static void MotionControl_StopOutputs(void);
|
static const HIDO_CHAR *MotionControl_StageLabel(E_MCStage stage);
|
|
/* 初始化控制任务并创建 FreeRTOS 线程（只做一次） */
|
HIDO_VOID MotionControl_TaskInit(HIDO_VOID)
|
{
|
if (g_motion_task_handle != NULL)
|
{
|
return;
|
}
|
|
MC_DefaultConfig(&g_motion_config);
|
Geo_OriginInit(&g_motion_origin,
|
MC_CFG_ORIGIN_LAT_DEG,
|
MC_CFG_ORIGIN_LON_DEG,
|
MC_CFG_ORIGIN_ALT_M);
|
|
MC_Init(&g_motion_state,
|
&g_motion_config,
|
g_motion_path_xy,
|
g_motion_path_point_count);
|
|
BaseType_t ret = xTaskCreate(
|
MotionControl_TaskEntry,
|
"MotionCtrl",
|
MC_TASK_STACK_WORDS,
|
NULL,
|
MC_TASK_PRIORITY,
|
&g_motion_task_handle);
|
|
if (ret != pdPASS)
|
{
|
g_motion_task_handle = NULL;
|
DBG_Printf("[MotionCtrl] Task create failed\r\n");
|
}
|
}
|
|
/* 查询控制任务是否已经创建 */
|
HIDO_BOOL MotionControl_IsRunning(HIDO_VOID)
|
{
|
return (g_motion_task_handle != NULL) ? HIDO_TRUE : HIDO_FALSE;
|
}
|
|
/* 高优先级任务：采集 → 控制 → PWM 输出 → 反馈 */
|
static void MotionControl_TaskEntry(void *argument)
|
{
|
(void)argument;
|
const TickType_t period = (MC_TASK_PERIOD_TICKS == 0U) ? pdMS_TO_TICKS(13U) : MC_TASK_PERIOD_TICKS;
|
TickType_t last_wake = xTaskGetTickCount();
|
ST_GPRMI gprmi;
|
ST_GPIMU gpimu;
|
float enu[3];
|
|
DBG_Printf("[MotionCtrl] Task started (%.1f Hz)\r\n", MC_CFG_CONTROL_HZ);
|
|
for (;;)
|
{
|
vTaskDelayUntil(&last_wake, period);
|
|
/* --- 1) 采集最新 GPS/IMU 数据（轮询缓存，含超时保护） --- */
|
HIDO_BOOL gps_valid = (GPS_GetGPRMI(&gprmi) == HIDO_OK);
|
HIDO_BOOL imu_valid = (GPS_GetGPIMU(&gpimu) == HIDO_OK);
|
HIDO_UINT32 now = HAL_GetTick();
|
|
if (gps_valid == HIDO_TRUE && gprmi.m_u32TimeOfWeek != g_last_gprmi_tow)
|
{
|
/* 新的GPS数据到来（时间戳变化），才更新航向 */
|
Geo_GprmiToENU(&gprmi, &g_motion_origin, enu);
|
MC_UpdateGps(&g_motion_state, enu, &gprmi);
|
memcpy(g_last_enu, enu, sizeof(enu));
|
g_last_heading_deg = gprmi.m_fHeadingAngle;
|
g_last_pitch_deg = gprmi.m_fPitchAngle;
|
g_last_roll_deg = gprmi.m_fRollAngle;
|
g_last_pose_valid = HIDO_TRUE;
|
g_last_gps_ms = now;
|
g_last_gprmi_tow = gprmi.m_u32TimeOfWeek;
|
g_last_sensor_timestamp_ms = gprmi.m_u32TimeOfWeek;
|
}
|
else if (gps_valid == HIDO_FALSE && (now - g_last_gps_ms) > 200U)
|
{
|
g_motion_state.pose_valid = HIDO_FALSE;
|
g_last_pose_valid = HIDO_FALSE;
|
}
|
|
if (imu_valid == HIDO_TRUE)
|
{
|
MC_UpdateImu(&g_motion_state, &gpimu);
|
g_last_imu_ms = now;
|
if (gpimu.m_u32UTCTime != 0U)
|
{
|
g_last_sensor_timestamp_ms = gpimu.m_u32UTCTime;
|
}
|
}
|
else if ((now - g_last_imu_ms) > 200U)
|
{
|
g_motion_state.imu_valid = HIDO_FALSE;
|
}
|
|
/* --- 2) 调用运动控制器（固定 75 Hz dt） --- */
|
MC_Output output;
|
MC_Compute(&g_motion_state, 1.0f / MC_CFG_CONTROL_HZ, &output);
|
|
/* --- 3) 根据控制量更新 PWM，并把 forward/turn 回传给 Python --- */
|
HIDO_UINT16 applied_steering = MC_CFG_PWM_CENTER_US;
|
HIDO_UINT16 applied_throttle = MC_CFG_PWM_CENTER_US;
|
if (output.active == HIDO_TRUE)
|
{
|
MotionControl_ApplyOutput(&output, &applied_steering, &applied_throttle);
|
}
|
else
|
{
|
MotionControl_StopOutputs();
|
applied_steering = MC_CFG_PWM_CENTER_US;
|
applied_throttle = MC_CFG_PWM_CENTER_US;
|
}
|
g_last_steering_pwm = applied_steering;
|
g_last_throttle_pwm = applied_throttle;
|
|
memcpy(g_last_enu, output.pos_enu, sizeof(g_last_enu));
|
g_last_heading_deg = output.heading_deg;
|
g_last_pitch_deg = output.pitch_deg;
|
g_last_roll_deg = output.roll_deg;
|
g_last_pose_valid = g_motion_state.pose_valid;
|
g_last_target_valid = output.target_valid;
|
if (output.target_valid == HIDO_TRUE)
|
{
|
g_last_target_xy[0] = output.target_xy[0];
|
g_last_target_xy[1] = output.target_xy[1];
|
}
|
|
g_freq_sample_count++;
|
if (g_freq_sample_start_ms == 0U)
|
{
|
g_freq_sample_start_ms = now;
|
}
|
else
|
{
|
HIDO_UINT32 window = now - g_freq_sample_start_ms;
|
if (window >= 1000U)
|
{
|
if (window > 0U)
|
{
|
g_control_freq_hz = (HIDO_FLOAT)g_freq_sample_count * 1000.0f / (HIDO_FLOAT)window;
|
}
|
g_freq_sample_count = 0U;
|
g_freq_sample_start_ms = now;
|
}
|
}
|
|
const HIDO_CHAR *stage_label = MotionControl_StageLabel(output.stage);
|
const HIDO_FLOAT *target_ptr = (output.target_valid == HIDO_TRUE) ? output.target_xy : HIDO_NULL;
|
PythonLink_ReportControl(output.forward_mps,
|
output.turn_rate,
|
g_control_freq_hz,
|
g_last_steering_pwm,
|
g_last_throttle_pwm,
|
stage_label,
|
g_last_sensor_timestamp_ms,
|
output.pos_enu,
|
output.heading_deg,
|
output.target_heading_deg,
|
target_ptr);
|
if ((now - g_last_state_report_ms) >= 1000U)
|
{
|
g_last_state_report_ms = now;
|
PythonLink_ReportState(stage_label,
|
output.cross_track_error,
|
output.heading_error * RAD2DEG,
|
g_last_sensor_timestamp_ms);
|
}
|
|
if ((now - g_last_pose_report_ms) >= 1000U && g_last_pose_valid == HIDO_TRUE)
|
{
|
g_last_pose_report_ms = now;
|
PythonLink_ReportPose(g_last_enu,
|
g_last_heading_deg,
|
g_last_pitch_deg,
|
g_last_roll_deg,
|
g_last_target_valid ? g_last_target_xy : NULL,
|
g_last_sensor_timestamp_ms);
|
}
|
|
if ((now - g_last_stack_report_ms) >= 10000U)
|
{
|
g_last_stack_report_ms = now;
|
UBaseType_t motion_hw = uxTaskGetStackHighWaterMark(g_motion_task_handle);
|
UBaseType_t app_hw = (g_app_task_handle != NULL) ? uxTaskGetStackHighWaterMark(g_app_task_handle) : 0U;
|
UBaseType_t heap_free = xPortGetFreeHeapSize();
|
UBaseType_t heap_min = xPortGetMinimumEverFreeHeapSize();
|
PythonLink_ReportStack("Motion", (HIDO_UINT32)motion_hw, (HIDO_UINT32)heap_free, (HIDO_UINT32)heap_min);
|
if (g_app_task_handle != NULL)
|
{
|
PythonLink_ReportStack("App", (HIDO_UINT32)app_hw, (HIDO_UINT32)heap_free, (HIDO_UINT32)heap_min);
|
}
|
}
|
}
|
}
|
|
/* 将控制输出映射为 PWM，并发回 Python 端 */
|
static void MotionControl_ApplyOutput(const MC_Output *output,
|
HIDO_UINT16 *_pu16Steering,
|
HIDO_UINT16 *_pu16Throttle)
|
{
|
float forward_ratio = output->forward_mps / g_motion_config.max_forward_mps;
|
float turn_ratio = output->turn_rate / g_motion_config.max_turn_rate;
|
forward_ratio = MC_CLAMP(forward_ratio, -1.0f, 1.0f);
|
turn_ratio = MC_CLAMP(turn_ratio, -1.0f, 1.0f);
|
|
int32_t throttle = (int32_t)MC_CFG_PWM_CENTER_US + (int32_t)(forward_ratio * (float)MC_CFG_PWM_SPAN_US);
|
int32_t steering = (int32_t)MC_CFG_PWM_CENTER_US + (int32_t)(turn_ratio * (float)MC_CFG_PWM_SPAN_US);
|
|
if (throttle < 1000)
|
{
|
throttle = 1000;
|
}
|
else if (throttle > 2000)
|
{
|
throttle = 2000;
|
}
|
|
if (steering < 1000)
|
{
|
steering = 1000;
|
}
|
else if (steering > 2000)
|
{
|
steering = 2000;
|
}
|
|
Set_Motor_Pulse((uint32_t)throttle);
|
Set_Steering_Pulse((uint32_t)steering);
|
|
if (_pu16Steering != NULL)
|
{
|
*_pu16Steering = (HIDO_UINT16)steering;
|
}
|
if (_pu16Throttle != NULL)
|
{
|
*_pu16Throttle = (HIDO_UINT16)throttle;
|
}
|
}
|
|
/* 失效保护：回中 PWM 并上报零控制量 */
|
static void MotionControl_StopOutputs(void)
|
{
|
Set_Motor_Pulse(MC_CFG_PWM_CENTER_US);
|
Set_Steering_Pulse(MC_CFG_PWM_CENTER_US);
|
g_last_steering_pwm = MC_CFG_PWM_CENTER_US;
|
g_last_throttle_pwm = MC_CFG_PWM_CENTER_US;
|
}
|
|
static const HIDO_CHAR *MotionControl_StageLabel(E_MCStage stage)
|
{
|
switch (stage)
|
{
|
case MC_STAGE_GOTO_START:
|
return "goto_start";
|
case MC_STAGE_FOLLOW_PATH:
|
return "follow_path";
|
case MC_STAGE_FINISHED:
|
return "finished";
|
default:
|
return "idle";
|
}
|
}
|
