/*******************************************************************************
|
* 文件名称 : 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 <math.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"
|
#include "motion_mode.h"
|
#include "SBUS.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)
|
{
|
HIDO_UINT32 gps_timestamp = (gprmi.m_u32UTCTime != 0U) ? gprmi.m_u32UTCTime : (HIDO_UINT32)now;
|
if ((gprmi.m_u32UTCTime == 0U) || (gps_timestamp != 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 = gps_timestamp;
|
g_last_sensor_timestamp_ms = gps_timestamp;
|
}
|
}
|
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;
|
HIDO_BOOL sbus_valid = (SBUS_IsSignalValid(MOTION_SBUS_TIMEOUT_MS) == HIDO_TRUE);
|
HIDO_UINT16 ch8 = SBUS_GetChannel(MOTION_SBUS_AUTO_CHANNEL);
|
|
/* 检查 GPS 状态：必须初始化完成且连接到 GNSS */
|
HIDO_BOOL gps_ready = HIDO_FALSE;
|
if (gps_valid == HIDO_TRUE)
|
{
|
HIDO_UINT32 status = gprmi.m_u32StatusFlags;
|
HIDO_BOOL init_ok = ((status & IM23A_STATUS_READY) != 0U);
|
HIDO_BOOL gnss_connected = ((status & IM23A_STATUS_GNSS_CONNECT) != 0U);
|
gps_ready = (init_ok == HIDO_TRUE) && (gnss_connected == HIDO_TRUE);
|
}
|
|
/* 只有当满足所有条件时才执行自动控制：
|
* 1. SBUS 信号有效
|
* 2. CH8 > 阈值（自动模式开关）
|
* 3. GPS 初始化完成（FINIT_OK）
|
* 4. GPS 连接到 GNSS（GNSS_CONNECT）*/
|
if (sbus_valid == HIDO_TRUE && ch8 > MOTION_SBUS_AUTO_THRESHOLD_US && gps_ready == HIDO_TRUE)
|
{
|
MC_Compute(&g_motion_state, 1.0f / MC_CFG_CONTROL_HZ, &output);
|
}
|
else
|
{
|
/* 手动模式或条件不满足：重置输出并让出控制权 */
|
memset(&output, 0, sizeof(MC_Output));
|
output.active = HIDO_FALSE;
|
|
/* 定期打印状态提示 */
|
static HIDO_UINT32 s_status_log = 0U;
|
if ((s_status_log++ % 100U) == 0U)
|
{
|
if (sbus_valid == HIDO_FALSE)
|
{
|
HIDO_Debug2("[MC_CTRL] Waiting: SBUS signal lost\r\n");
|
}
|
else if (ch8 <= MOTION_SBUS_AUTO_THRESHOLD_US)
|
{
|
HIDO_Debug2("[MC_CTRL] Waiting: CH8=%u (manual mode)\r\n", ch8);
|
}
|
else if (gps_ready == HIDO_FALSE)
|
{
|
HIDO_UINT32 status = gps_valid ? gprmi.m_u32StatusFlags : 0U;
|
HIDO_Debug2("[MC_CTRL] Waiting: GPS not ready (status=0x%08X)\r\n", status);
|
}
|
}
|
}
|
|
static HIDO_UINT32 s_ctrl_log_idx = 0U;
|
if ((s_ctrl_log_idx++ % 10U) == 0U)
|
{
|
/* 使用整数表示法打印浮点数，避免栈损坏 */
|
int pos_x_int = (int)g_motion_state.pos[0];
|
int pos_x_frac = (int)(fabsf(g_motion_state.pos[0] - pos_x_int) * 100);
|
int pos_y_int = (int)g_motion_state.pos[1];
|
int pos_y_frac = (int)(fabsf(g_motion_state.pos[1] - pos_y_int) * 100);
|
int pos_z_int = (int)g_motion_state.pos[2];
|
int pos_z_frac = (int)(fabsf(g_motion_state.pos[2] - pos_z_int) * 100);
|
int hdg_int = (int)g_motion_state.heading_deg;
|
int hdg_frac = (int)(fabsf(g_motion_state.heading_deg - hdg_int) * 100);
|
int tgt_x_int = (int)g_motion_state.current_target_xy[0];
|
int tgt_x_frac = (int)(fabsf(g_motion_state.current_target_xy[0] - tgt_x_int) * 100);
|
int tgt_y_int = (int)g_motion_state.current_target_xy[1];
|
int tgt_y_frac = (int)(fabsf(g_motion_state.current_target_xy[1] - tgt_y_int) * 100);
|
int fwd_int = (int)output.forward_mps;
|
int fwd_frac = (int)(fabsf(output.forward_mps - fwd_int) * 100);
|
int turn_int = (int)output.turn_rate;
|
int turn_frac = (int)(fabsf(output.turn_rate - turn_int) * 100);
|
|
HIDO_Debug2("[MC_CTRL] stage=%d pos=(%d.%02d,%d.%02d,%d.%02d) hdg=%d.%02d tgt=(%d.%02d,%d.%02d) fwd=%d.%02d turn=%d.%02d path_idx=%u\n",
|
g_motion_state.stage,
|
pos_x_int, pos_x_frac,
|
pos_y_int, pos_y_frac,
|
pos_z_int, pos_z_frac,
|
hdg_int, hdg_frac,
|
tgt_x_int, tgt_x_frac,
|
tgt_y_int, tgt_y_frac,
|
fwd_int, fwd_frac,
|
turn_int, turn_frac,
|
g_motion_state.nearest_index);
|
}
|
|
/* --- 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
|
{
|
if(ch8 > MOTION_SBUS_AUTO_THRESHOLD_US)
|
{
|
MotionControl_StopOutputs();
|
applied_steering = MC_CFG_PWM_CENTER_US;
|
applied_throttle = MC_CFG_PWM_CENTER_US;
|
}
|
/* 显式回退到手动模式（遥控器直通在 SBUS 模块中处理，或者在此处完全不输出） */
|
}
|
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（使用校准模型反解）*/
|
static void MotionControl_ApplyOutput(const MC_Output *output,
|
HIDO_UINT16 *_pu16Steering,
|
HIDO_UINT16 *_pu16Throttle)
|
{
|
/* ========== 前向速度 → 油门PWM（使用校准模型反解）========== */
|
/* 校准模型：v = MC_CFG_FORWARD_K × (1500 - pwm) + MC_CFG_FORWARD_BIAS
|
* 反解：pwm = 1500 - (v - MC_CFG_FORWARD_BIAS) / MC_CFG_FORWARD_K
|
* 说明：1000=最大前进，1500=停止，2000=最大后退
|
*/
|
float target_velocity = output->forward_mps;
|
float throttle_f = 1500.0f - (target_velocity - MC_CFG_FORWARD_BIAS) / MC_CFG_FORWARD_K;
|
|
/* 限制PWM范围 */
|
if (throttle_f < 1000.0f)
|
{
|
throttle_f = 1000.0f;
|
}
|
else if (throttle_f > 2000.0f)
|
{
|
throttle_f = 2000.0f;
|
}
|
|
/* 死区处理：接近1500时直接设为1500 */
|
if (fabsf(throttle_f - 1500.0f) < (float)MC_CFG_FORWARD_DEADZONE_PWM)
|
{
|
throttle_f = 1500.0f;
|
}
|
|
/* ========== 转向角速度 → 转向PWM（使用校准模型反解）========== */
|
/* 校准模型：ω_left = MC_CFG_STEERING_K_LEFT × (1500 - pwm) 当 pwm < 1500 (左转)
|
* ω_right = -MC_CFG_STEERING_K_RIGHT × (pwm - 1500) 当 pwm > 1500 (右转)
|
* 反解：pwm_left = 1500 - ω / MC_CFG_STEERING_K_LEFT
|
* pwm_right = 1500 - ω / MC_CFG_STEERING_K_RIGHT
|
* 说明：1000=最大左转，1500=直行，2000=最大右转
|
*/
|
float target_yaw_rate = output->turn_rate; // rad/s，正值=左转，负值=右转
|
float steering_f = 1500.0f;
|
|
if (target_yaw_rate > 0.001f) // 左转（正角速度）
|
{
|
steering_f = 1500.0f - target_yaw_rate / MC_CFG_STEERING_K_LEFT;
|
if (steering_f < 1000.0f)
|
{
|
steering_f = 1000.0f;
|
}
|
}
|
else if (target_yaw_rate < -0.001f) // 右转（负角速度）
|
{
|
steering_f = 1500.0f - target_yaw_rate / MC_CFG_STEERING_K_RIGHT;
|
if (steering_f > 2000.0f)
|
{
|
steering_f = 2000.0f;
|
}
|
}
|
else // 直行：应用转向补偿修正机械偏差
|
{
|
steering_f = 1500.0f + (float)MC_CFG_STEERING_TRIM;
|
}
|
|
/* 转换为整数并输出 */
|
uint32_t throttle = (uint32_t)(throttle_f + 0.5f);
|
uint32_t steering = (uint32_t)(steering_f + 0.5f);
|
|
Set_Motor_Pulse(throttle);
|
Set_Steering_Pulse(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";
|
}
|
}
|
