/******************************************************************************* * 文件名称 : 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 #define ENABLE_MC_CTRL_LOG 1 #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); DBG_Printf("[MC_INIT] Path loaded: count=%u, first_point=(%.2f,%.2f)\r\n", g_motion_path_point_count, g_motion_path_xy[0], g_motion_path_xy[1]); 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; /* 检测pose_valid状态变化 */ if (g_last_pose_valid == HIDO_FALSE) { HIDO_Debug2("[MC_POSE]pose_valid: FALSE->TRUE (GPS recovered)\r\n"); } 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) { /* 检测pose_valid状态变化 */ if (g_last_pose_valid == HIDO_TRUE) { HIDO_Debug2("[MC_POSE]pose_valid: TRUE->FALSE (GPS timeout, age=%ums)\r\n", now - g_last_gps_ms); } 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_raw = SBUS_GetChannel(MOTION_SBUS_AUTO_CHANNEL); /* 过滤SBUS异常值： * 1024 = SBUS_CENTER_VALUE（信号丢失时的默认返回值） * <172 或 >1811 = SBUS有效范围之外（对应PWM 1000-2000） * 当检测到异常值时，保持之前的有效值不变 */ static HIDO_UINT16 ch8 = 1000; /* 默认手动模式 */ static HIDO_UINT32 s_ch8_failsafe_count = 0; static HIDO_UINT16 s_ch8_last_valid = 1000; /* 判断是否为有效SBUS值：172-1811范围内，且不是精确的1024 */ HIDO_BOOL is_valid = (ch8_raw >= 172 && ch8_raw <= 1811 && ch8_raw != 1024); if (is_valid) { ch8 = ch8_raw; /* 只更新有效值 */ s_ch8_last_valid = ch8_raw; if (s_ch8_failsafe_count > 0) { DBG_Printf("[MC_CTRL] CH8 recovered from failsafe, count=%u, new value=%u\r\n", s_ch8_failsafe_count, ch8); s_ch8_failsafe_count = 0; } } else { /* 异常值，保持之前的值，记录failsafe事件 */ s_ch8_failsafe_count++; if (s_ch8_failsafe_count == 1) { DBG_Printf("[MC_CTRL] CH8 failsafe detected (%u), keeping previous value=%u\r\n", ch8_raw, ch8); } } /* 检查 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; static HIDO_BOOL s_last_sbus_valid = HIDO_FALSE; static HIDO_BOOL s_last_gps_ready = HIDO_FALSE; static HIDO_UINT16 s_last_ch8 = 0; static HIDO_BOOL s_ch8_auto_state = HIDO_FALSE; /* CH8迟滞状态 */ static HIDO_BOOL s_last_ch8_auto_state = HIDO_FALSE; /* 上一次的CH8迟滞状态 */ /* CH8迟滞：防止在阈值附近抖动时反复触发 * 进入自动模式：CH8 > 1600 (高阈值) * 退出自动模式：CH8 < 1400 (低阈值) * 死区：1400-1600，在此区间保持之前的状态 * * 连续确认机制：需要连续3次检测到相同状态才切换 * 这是最稳定的防抖方案，防止瞬态干扰 */ #define CH8_THRESHOLD_ENTER 1600 #define CH8_THRESHOLD_EXIT 1400 #define CH8_CONFIRM_COUNT 3 static HIDO_UINT8 s_ch8_enter_count = 0; /* 连续检测到进入自动模式的次数 */ static HIDO_UINT8 s_ch8_exit_count = 0; /* 连续检测到退出自动模式的次数 */ if (s_ch8_auto_state == HIDO_FALSE) { /* 当前在手动模式，检查是否应该进入自动模式 */ if (ch8 > CH8_THRESHOLD_ENTER) { s_ch8_enter_count++; s_ch8_exit_count = 0; /* 重置退出计数 */ if (s_ch8_enter_count >= CH8_CONFIRM_COUNT) { /* 连续3次确认，切换到自动模式 */ s_ch8_auto_state = HIDO_TRUE; s_ch8_enter_count = 0; DBG_Printf("[MC_CTRL] CH8 state confirmed: MANUAL -> AUTO (ch8=%u)\r\n", ch8); } } else { s_ch8_enter_count = 0; /* 未满足条件，重置计数 */ } } else { /* 当前在自动模式，检查是否应该退出自动模式 */ if (ch8 < CH8_THRESHOLD_EXIT) { s_ch8_exit_count++; s_ch8_enter_count = 0; /* 重置进入计数 */ if (s_ch8_exit_count >= CH8_CONFIRM_COUNT) { /* 连续3次确认，切换到手动模式 */ s_ch8_auto_state = HIDO_FALSE; s_ch8_exit_count = 0; DBG_Printf("[MC_CTRL] CH8 state confirmed: AUTO -> MANUAL (ch8=%u)\r\n", ch8); } } else { s_ch8_exit_count = 0; /* 未满足条件，重置计数 */ } } HIDO_BOOL current_auto_condition = (sbus_valid == HIDO_TRUE && s_ch8_auto_state == HIDO_TRUE && gps_ready == HIDO_TRUE); /* 检测到从手动切换到自动模式的上升沿：重置状态机从头开始 * 增加稳定性要求：只有在IDLE或FINISHED状态时才允许重新初始化 * 如果正在执行任务（GOTO_START或FOLLOW_PATH），忽略模式切换抖动 */ if (s_last_auto_condition == HIDO_FALSE && current_auto_condition == HIDO_TRUE) { E_MCStage current_stage = g_motion_state.stage; if (current_stage == MC_STAGE_IDLE || current_stage == MC_STAGE_FINISHED) { DBG_Printf("[MC_CTRL] *** Auto mode triggered! Resetting to GOTO_START ***\r\n"); DBG_Printf("[MC_CTRL] SBUS: %d->%d, CH8: %u->%u (state=%d->%d), GPS_READY: %d->%d\r\n", s_last_sbus_valid, sbus_valid, s_last_ch8, ch8, s_last_ch8_auto_state, s_ch8_auto_state, s_last_gps_ready, gps_ready); MC_Init(&g_motion_state, &g_motion_config, g_motion_path_xy, g_motion_path_point_count); } else { /* 正在执行任务，忽略此次模式切换（可能是信号抖动） */ DBG_Printf("[MC_CTRL] WARNING: Auto mode re-triggered during %d, ignoring (SBUS interference)\r\n", current_stage); } } /* 检测条件变化并记录 */ if (s_last_sbus_valid != sbus_valid) { DBG_Printf("[MC_CTRL] SBUS valid changed: %d -> %d\r\n", s_last_sbus_valid, sbus_valid); } if (s_last_gps_ready != gps_ready) { DBG_Printf("[MC_CTRL] GPS ready changed: %d -> %d\r\n", s_last_gps_ready, gps_ready); } /* 记录状态变化（已在上面连续确认时记录，这里只更新last状态） */ s_last_ch8_auto_state = s_ch8_auto_state; s_last_auto_condition = current_auto_condition; s_last_sbus_valid = sbus_valid; s_last_gps_ready = gps_ready; s_last_ch8 = ch8; if (current_auto_condition == HIDO_TRUE) { /* 记录状态切换 */ static E_MCStage s_last_stage = MC_STAGE_IDLE; E_MCStage prev_stage = g_motion_state.stage; MC_Compute(&g_motion_state, 1.0f / MC_CFG_CONTROL_HZ, &output); if (prev_stage != output.stage) { const HIDO_CHAR *prev_label = MotionControl_StageLabel(prev_stage); const HIDO_CHAR *curr_label = MotionControl_StageLabel(output.stage); DBG_Printf("[MC_CTRL] *** Stage transition: %s -> %s ***\r\n", prev_label, curr_label); if (output.stage == MC_STAGE_GOTO_START && prev_stage == MC_STAGE_FOLLOW_PATH) { /* 异常：从follow_path回到goto_start！记录详细信息 */ DBG_Printf("[MC_CTRL] WARNING: Unexpected transition from FOLLOW_PATH to GOTO_START!\r\n"); DBG_Printf("[MC_CTRL] nearest_idx=%u, path_count=%u, pos=(%.2f,%.2f)\r\n", g_motion_state.nearest_index, g_motion_state.path_count, g_motion_state.pos[0], g_motion_state.pos[1]); } } s_last_stage = output.stage; } 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 ENABLE_MC_CTRL_LOG 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); } #endif } } static HIDO_UINT32 s_ctrl_log_idx = 0U; if ((s_ctrl_log_idx++ % 10U) == 0U) { #if ENABLE_MC_CTRL_LOG /* 使用整数表示法打印浮点数，避免栈损坏 */ 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 - (float)fwd_int) * 100.0f); /* 修复：保留turn的符号，使用带符号格式输出 */ float turn_abs = fabsf(output.turn_rate); int turn_int = (int)turn_abs; int turn_frac = (int)((turn_abs - (float)turn_int) * 100.0f); char turn_sign = (output.turn_rate < 0.0f) ? '-' : '+'; 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=%c%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_sign, turn_int, turn_frac, g_motion_state.nearest_index); #endif } /* --- 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; } else if (output.stage == MC_STAGE_FINISHED) { /* 已到达终点：停止输出 */ 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; /* 检测目标点跳变到(0,0)的情况 */ static HIDO_BOOL s_warn_zero_target = HIDO_FALSE; if (output.target_valid == HIDO_TRUE) { /* 检测目标点突然跳到接近(0,0)的情况 */ HIDO_BOOL is_near_zero = (fabsf(output.target_xy[0]) < 0.1f && fabsf(output.target_xy[1]) < 0.1f); HIDO_BOOL was_far_from_zero = (fabsf(g_last_target_xy[0]) > 1.0f || fabsf(g_last_target_xy[1]) > 1.0f); if (is_near_zero && was_far_from_zero && g_last_target_valid) { int old_x_int = (int)g_last_target_xy[0]; int old_x_frac = (int)(fabsf(g_last_target_xy[0] - old_x_int) * 100); int old_y_int = (int)g_last_target_xy[1]; int old_y_frac = (int)(fabsf(g_last_target_xy[1] - old_y_int) * 100); HIDO_Debug2("[MC_TGT]WARNING: Target jumped to (0,0)! Previous target=(%d.%02d,%d.%02d) stage=%d\r\n", old_x_int, old_x_frac, old_y_int, old_y_frac, output.stage); s_warn_zero_target = HIDO_TRUE; } else if (!is_near_zero && s_warn_zero_target) { int new_x_int = (int)output.target_xy[0]; int new_x_frac = (int)(fabsf(output.target_xy[0] - new_x_int) * 100); int new_y_int = (int)output.target_xy[1]; int new_y_frac = (int)(fabsf(output.target_xy[1] - new_y_int) * 100); HIDO_Debug2("[MC_TGT]Target recovered from (0,0) to (%d.%02d,%d.%02d)\r\n", new_x_int, new_x_frac, new_y_int, new_y_frac); s_warn_zero_target = HIDO_FALSE; } g_last_target_xy[0] = output.target_xy[0]; g_last_target_xy[1] = output.target_xy[1]; g_last_target_valid = HIDO_TRUE; } else { /* target_valid变为FALSE */ if (g_last_target_valid) { HIDO_Debug2("[MC_TGT]target_valid: TRUE->FALSE in task (will report 0,0)\r\n"); } g_last_target_valid = HIDO_FALSE; } 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; /* 调试LOG：打印转向映射过程（低频） */ 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 // 直行：应用转向补偿修正机械偏差（死区 ±0.001 rad/s） { 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"; } }