/******************************************************************************* * 文件名称 : motion_control_task.c * 文件说明 : 割草机运动控制 FreeRTOS 任务封装 * 创建日期 : 2025-11-22 *******************************************************************************/ #include "motion_control_task.h" #include "FreeRTOS.h" #include "task.h" #include "stm32h7xx_hal.h" #include #include #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" #include "AppConfig.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); } #if HITL_SIMULATION /* 硬件在环仿真模式： * 1. 强制 SBUS 信号有效 * 2. 模拟 CH8 通道：上电前 10 秒为 1000 (手动)，10 秒后变为 1800 (自动) * 这样可以测试从手动切换到自动的逻辑 (状态机复位) */ sbus_valid = HIDO_TRUE; /* 仿真模式下，只要收到过一次有效数据，就认为 GPS 就绪，避免因为标志位未置位导致不启动 */ if (gps_valid == HIDO_TRUE) { gps_ready = HIDO_TRUE; } static HIDO_UINT16 g_hitl_ch8 = 1000; static HIDO_UINT32 s_hitl_start_ms = 0; if (s_hitl_start_ms == 0) { s_hitl_start_ms = HAL_GetTick(); } if (HAL_GetTick() - s_hitl_start_ms > 1000U) { g_hitl_ch8 = 1800; // 10秒后自动切入自动模式 } ch8 = g_hitl_ch8; #endif /* 只有当满足所有条件时才执行自动控制： /* 只有当满足所有条件时才执行自动控制： * 1. SBUS 信号有效 * 2. CH8 > 阈值（自动模式开关） * 3. GPS 初始化完成（FINIT_OK） * 4. GPS 连接到 GNSS（GNSS_CONNECT）*/ static HIDO_BOOL s_last_auto_condition = HIDO_FALSE; HIDO_BOOL current_auto_condition = (sbus_valid == HIDO_TRUE && ch8 > MOTION_SBUS_AUTO_THRESHOLD_US && gps_ready == HIDO_TRUE); /* 检测到从手动切换到自动模式的上升沿：重置状态机从头开始 */ if (s_last_auto_condition == HIDO_FALSE && current_auto_condition == HIDO_TRUE) { DBG_Printf("[MC_CTRL] Auto mode triggered! Resetting to GOTO_START.\r\n"); MC_Init(&g_motion_state, &g_motion_config, g_motion_path_xy, g_motion_path_point_count); } s_last_auto_condition = current_auto_condition; if (current_auto_condition == 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; /* 移除这里的强制回中，改为在下方根据具体原因处理 */ // MotionControl_StopOutputs(); /* 定期打印状态提示 */ 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 { /* 非自动模式下，MotionControl_StopOutputs() 已在上面调用，这里不再重复设置 * 如果需要手动模式直通，应该在 SBUS 模块处理，或者在这里读取 SBUS 手动通道值并输出 * 目前逻辑是：非自动模式下，运动控制任务输出回中（安全停止），手动控制由 SBUS 回调或其他机制接管 * 如果系统设计是 MotionControl 任务在手动模式下也负责透传遥控器信号，则需要修改此处。 * 假设：手动模式下，SBUS 模块的中断回调会直接控制电机，或者有其他任务处理。 * 为防止冲突，这里仅保持记录。但上面已经调用 StopOutputs 设置了 PWM 为中值。 * 如果 SBUS 直接控制电机，这里的 StopOutputs 会与 SBUS 冲突吗？ * 通常做法：自动模式下本任务控制 PWM；手动模式下本任务不控制 PWM（或者输出无效值）。 * 但 StopOutputs 显式设置了 1500。如果 SBUS 也是设置 PWM，会有竞争。 * * 修正：如果 ch8 < 1500 (手动模式)，本任务不应该 Set_Motor_Pulse(1500)， * 否则会覆盖 SBUS 的手动控制信号。 * * 修改逻辑： * 1. 自动模式 (current_auto_condition == TRUE): ApplyOutput -> Set_Pulse * 2. 异常停止 (sbus_valid=FALSE 或 gps_ready=FALSE): StopOutputs -> Set_Pulse(1500) * 3. 手动模式 (sbus_valid=TRUE && ch8 < 1500): 不操作 PWM，让权给手动控制逻辑 */ if (sbus_valid == HIDO_FALSE) { /* 遥控器信号丢失：强制停车 */ MotionControl_StopOutputs(); applied_steering = MC_CFG_PWM_CENTER_US; applied_throttle = MC_CFG_PWM_CENTER_US; } else if (ch8 <= MOTION_SBUS_AUTO_THRESHOLD_US) { /* 手动模式：不输出 PWM，仅记录（假设 SBUS 模块或其他逻辑在控制电机） * 为了回显正确，这里赋值为 1500 或读取当前 PWM（如果能读到） */ applied_steering = MC_CFG_PWM_CENTER_US; applied_throttle = MC_CFG_PWM_CENTER_US; /* 注意：不要调用 MotionControl_StopOutputs()，否则会在此处把手动油门强行归零 */ } else if (gps_ready == HIDO_FALSE) { /* 自动模式开关打开，但 GPS 未就绪：安全停车 */ 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（使用校准模型反解）*/ 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"; } }