/******************************************************************************* * 文件名称 : motion_calibration_task.c * 文件说明 : 车辆运动模型自动校准任务实现 * 创建日期 : 2025-11-25 *******************************************************************************/ #include "motion_calibration_task.h" #include "FreeRTOS.h" #include "task.h" #include "stm32h7xx_hal.h" #include #include "DBG.h" #include "GPS.h" #include "SBUS.h" #include "pwm_ctrol.h" #include "motion_mode.h" /******************************************************************************* * 配置参数 * *******************************************************************************/ /* 任务配置 */ #define CAL_TASK_STACK_WORDS (1024U) #define CAL_TASK_PRIORITY (tskIDLE_PRIORITY + 3U) #define CAL_TASK_PERIOD_MS (50U) // 20Hz采样 /* PWM配置 - 双向控制（1000=最大前进/左转，1500=停止，2000=最大后退/右转）*/ #define CAL_PWM_CENTER (1500U) // 停止 /* 转向补偿值（用于修正直行偏差，正值=向右补偿，负值=向左补偿）*/ /* 车辆向左偏 → 设置正值（如+5到+15）来向右补偿 */ /* 车辆向右偏 → 设置负值（如-5到-15）来向左补偿 */ #define CAL_PWM_STEERING_TRIM (5) // 转向补偿：向右补偿10（修正左偏） /* 前进测试（1500→1000）：6个档位 */ #define CAL_PWM_FORWARD_VLOW (1400U) // 轻度前进 #define CAL_PWM_FORWARD_LOW (1300U) // 中低速前进 #define CAL_PWM_FORWARD_MID (1200U) // 中速前进 #define CAL_PWM_FORWARD_HIGH (1100U) // 高速前进 #define CAL_PWM_FORWARD_VHIGH (1050U) // 超高速前进 #define CAL_PWM_FORWARD_MAX (1000U) // 最大前进 /* 后退测试（1500→2000）：3个档位（可选） */ #define CAL_PWM_REVERSE_LOW (1600U) // 轻度后退 #define CAL_PWM_REVERSE_MID (1700U) // 中速后退 #define CAL_PWM_REVERSE_HIGH (1800U) // 高速后退 /* 左转测试（1500→1000）：5个档位 */ #define CAL_PWM_TURN_LEFT_VLOW (1400U) // 轻度左转 #define CAL_PWM_TURN_LEFT_LOW (1300U) // 中度左转 #define CAL_PWM_TURN_LEFT_MID (1200U) // 重度左转 #define CAL_PWM_TURN_LEFT_HIGH (1100U) // 超重左转 #define CAL_PWM_TURN_LEFT_MAX (1000U) // 最大左转 /* 右转测试（1500→2000）：5个档位 */ #define CAL_PWM_TURN_RIGHT_VLOW (1600U) // 轻度右转 #define CAL_PWM_TURN_RIGHT_LOW (1700U) // 中度右转 #define CAL_PWM_TURN_RIGHT_MID (1800U) // 重度右转 #define CAL_PWM_TURN_RIGHT_HIGH (1900U) // 超重右转 #define CAL_PWM_TURN_RIGHT_MAX (2000U) // 最大右转 /* 测试时长（毫秒） */ #define CAL_DURATION_WARMUP (2000U) // 预热 #define CAL_DURATION_ACCEL (3000U) // 加速 #define CAL_DURATION_CRUISE (5000U) // 巡航 #define CAL_DURATION_TURN (4000U) // 转向 #define CAL_DURATION_BRAKE (2000U) // 制动 #define CAL_DURATION_REST (1000U) // 间歇 /* 安全参数 */ #define CAL_SBUS_SAFETY_THRESHOLD (1500U) // CH8阈值 #define CAL_SBUS_TIMEOUT_MS (500U) // SBUS超时 /******************************************************************************* * 状态机定义 * *******************************************************************************/ typedef enum { CAL_STATE_IDLE = 0, // 空闲，等待启动 CAL_STATE_WARMUP, // 预热（原地静止，检查传感器） CAL_STATE_ACCEL_VLOW, // 超低速加速 CAL_STATE_CRUISE_VLOW, // 超低速巡航 CAL_STATE_REST_1, // 间歇1 CAL_STATE_ACCEL_LOW, // 低速加速 CAL_STATE_CRUISE_LOW, // 低速巡航 CAL_STATE_REST_2, // 间歇2 CAL_STATE_ACCEL_MLOW, // 中低速加速 CAL_STATE_CRUISE_MLOW, // 中低速巡航 CAL_STATE_REST_3, // 间歇3 CAL_STATE_ACCEL_MID, // 中速加速 CAL_STATE_CRUISE_MID, // 中速巡航 CAL_STATE_REST_4, // 间歇4 CAL_STATE_ACCEL_HIGH, // 高速加速 CAL_STATE_CRUISE_HIGH, // 高速巡航 CAL_STATE_REST_5, // 间歇5 CAL_STATE_ACCEL_VHIGH, // 超高速加速 CAL_STATE_CRUISE_VHIGH, // 超高速巡航 CAL_STATE_REST_6, // 间歇6 CAL_STATE_TURN_LEFT_VLIGHT, // 超轻左转 CAL_STATE_REST_7, // 间歇7 CAL_STATE_TURN_RIGHT_VLIGHT, // 超轻右转 CAL_STATE_REST_8, // 间歇8 CAL_STATE_TURN_LEFT_LIGHT, // 轻左转 CAL_STATE_REST_9, // 间歇9 CAL_STATE_TURN_RIGHT_LIGHT, // 轻右转 CAL_STATE_REST_10, // 间歇10 CAL_STATE_TURN_LEFT_MID, // 中左转 CAL_STATE_REST_11, // 间歇11 CAL_STATE_TURN_RIGHT_MID, // 中右转 CAL_STATE_REST_12, // 间歇12 CAL_STATE_TURN_LEFT_HEAVY, // 重左转 CAL_STATE_REST_13, // 间歇13 CAL_STATE_TURN_RIGHT_HEAVY, // 重右转 CAL_STATE_REST_14, // 间歇14 CAL_STATE_TURN_LEFT_VHEAVY, // 超重左转 CAL_STATE_REST_15, // 间歇15 CAL_STATE_TURN_RIGHT_VHEAVY, // 超重右转 CAL_STATE_BRAKE, // 制动 CAL_STATE_FINISHED, // 完成 CAL_STATE_ERROR // 错误（安全停止） } E_CalState; typedef struct { E_CalState state; HIDO_UINT32 state_start_ms; HIDO_UINT32 state_duration_ms; HIDO_UINT16 throttle_pwm; HIDO_UINT16 steering_pwm; const HIDO_CHAR *state_name; } ST_CalStateConfig; /******************************************************************************* * 全局变量 * *******************************************************************************/ static TaskHandle_t g_cal_task_handle = NULL; static E_CalState g_cal_state = CAL_STATE_IDLE; static HIDO_UINT32 g_state_start_time = 0U; static HIDO_BOOL g_cal_running = HIDO_FALSE; static HIDO_UINT32 g_sample_count = 0U; /* 状态机配置表 - 双向PWM校准 (1000=最大正向, 1500=停止, 2000=最大反向) */ static const ST_CalStateConfig g_state_configs[] = { {CAL_STATE_WARMUP, 0, CAL_DURATION_WARMUP, CAL_PWM_CENTER, CAL_PWM_CENTER, "WARMUP"}, /* 前进测试：6个档位 (1400→1000)，转向应用补偿值修正直行偏差 */ {CAL_STATE_ACCEL_VLOW, 0, CAL_DURATION_ACCEL, CAL_PWM_FORWARD_VLOW, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_VLOW"}, {CAL_STATE_CRUISE_VLOW, 0, CAL_DURATION_CRUISE, CAL_PWM_FORWARD_VLOW, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_VLOW_C"}, {CAL_STATE_REST_1, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_1"}, {CAL_STATE_ACCEL_LOW, 0, CAL_DURATION_ACCEL, CAL_PWM_FORWARD_LOW, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_LOW"}, {CAL_STATE_CRUISE_LOW, 0, CAL_DURATION_CRUISE, CAL_PWM_FORWARD_LOW, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_LOW_C"}, {CAL_STATE_REST_2, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_2"}, {CAL_STATE_ACCEL_MLOW, 0, CAL_DURATION_ACCEL, CAL_PWM_FORWARD_MID, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_MID"}, {CAL_STATE_CRUISE_MLOW, 0, CAL_DURATION_CRUISE, CAL_PWM_FORWARD_MID, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_MID_C"}, {CAL_STATE_REST_3, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_3"}, {CAL_STATE_ACCEL_MID, 0, CAL_DURATION_ACCEL, CAL_PWM_FORWARD_HIGH, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_HIGH"}, {CAL_STATE_CRUISE_MID, 0, CAL_DURATION_CRUISE, CAL_PWM_FORWARD_HIGH, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_HIGH_C"}, {CAL_STATE_REST_4, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_4"}, {CAL_STATE_ACCEL_HIGH, 0, CAL_DURATION_ACCEL, CAL_PWM_FORWARD_VHIGH, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_VHIGH"}, {CAL_STATE_CRUISE_HIGH, 0, CAL_DURATION_CRUISE, CAL_PWM_FORWARD_VHIGH, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_VHIGH_C"}, {CAL_STATE_REST_5, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_5"}, {CAL_STATE_ACCEL_VHIGH, 0, CAL_DURATION_ACCEL, CAL_PWM_FORWARD_MAX, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_MAX"}, {CAL_STATE_CRUISE_VHIGH, 0, CAL_DURATION_CRUISE, CAL_PWM_FORWARD_MAX, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_MAX_C"}, {CAL_STATE_REST_6, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_6"}, /* 左转测试：5个档位 (1400→1000, 原地转向) */ {CAL_STATE_TURN_LEFT_VLIGHT, 0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_LEFT_VLOW, "TURN_L_VLOW"}, {CAL_STATE_REST_7, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_7"}, {CAL_STATE_TURN_LEFT_LIGHT, 0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_LEFT_LOW, "TURN_L_LOW"}, {CAL_STATE_REST_8, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_8"}, {CAL_STATE_TURN_LEFT_MID, 0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_LEFT_MID, "TURN_L_MID"}, {CAL_STATE_REST_9, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_9"}, {CAL_STATE_TURN_LEFT_HEAVY, 0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_LEFT_HIGH, "TURN_L_HIGH"}, {CAL_STATE_REST_10, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_10"}, {CAL_STATE_TURN_LEFT_VHEAVY, 0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_LEFT_MAX, "TURN_L_MAX"}, {CAL_STATE_REST_11, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_11"}, /* 右转测试：5个档位 (1600→2000, 原地转向) */ {CAL_STATE_TURN_RIGHT_VLIGHT,0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_RIGHT_VLOW, "TURN_R_VLOW"}, {CAL_STATE_REST_12, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_12"}, {CAL_STATE_TURN_RIGHT_LIGHT, 0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_RIGHT_LOW, "TURN_R_LOW"}, {CAL_STATE_REST_13, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_13"}, {CAL_STATE_TURN_RIGHT_MID, 0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_RIGHT_MID, "TURN_R_MID"}, {CAL_STATE_REST_14, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_14"}, {CAL_STATE_TURN_RIGHT_HEAVY, 0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_RIGHT_HIGH, "TURN_R_HIGH"}, {CAL_STATE_REST_15, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_15"}, {CAL_STATE_TURN_RIGHT_VHEAVY,0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_RIGHT_MAX, "TURN_R_MAX"}, {CAL_STATE_BRAKE, 0, CAL_DURATION_BRAKE, CAL_PWM_CENTER, CAL_PWM_CENTER, "BRAKE"}, {CAL_STATE_FINISHED, 0, 0, CAL_PWM_CENTER, CAL_PWM_CENTER, "FINISHED"} }; /******************************************************************************* * 内部函数 * *******************************************************************************/ /* 安全检查：SBUS CH8 > 1500 且 GPS 就绪才允许运行 */ static HIDO_BOOL Cal_SafetyCheck(HIDO_VOID) { /* 检查SBUS信号有效性 */ if (SBUS_IsSignalValid(MOTION_SBUS_TIMEOUT_MS) == HIDO_FALSE) { return HIDO_FALSE; } /* 检查 CH8（索引7） > 1500 (自动模式阈值) */ HIDO_UINT16 ch8 = SBUS_GetChannel(MOTION_SBUS_AUTO_CHANNEL); if (ch8 <= MOTION_SBUS_AUTO_THRESHOLD_US) { /* CH8 <= 1500，处于手动模式，禁止校准任务运行 */ return HIDO_FALSE; } /* 检查 GPS 状态：必须初始化完成且连接到 GNSS */ ST_GPRMI gprmi; if (GPS_GetGPRMI(&gprmi) != HIDO_OK) { return HIDO_FALSE; } 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); if (init_ok == HIDO_FALSE || gnss_connected == HIDO_FALSE) { /* GPS 未就绪，禁止校准任务运行 */ return HIDO_FALSE; } return HIDO_TRUE; } /* 紧急停止 */ static HIDO_VOID Cal_EmergencyStop(HIDO_VOID) { Set_Motor_Pulse(CAL_PWM_CENTER); Set_Steering_Pulse(CAL_PWM_CENTER); g_cal_state = CAL_STATE_ERROR; g_cal_running = HIDO_FALSE; HIDO_Debug2("[CAL] EMERGENCY STOP - Safety check failed!\r\n"); } /* 输出PWM控制 */ static HIDO_VOID Cal_SetOutput(HIDO_UINT16 throttle, HIDO_UINT16 steering) { Set_Motor_Pulse(throttle); Set_Steering_Pulse(steering); } /* 获取状态配置 */ static const ST_CalStateConfig* Cal_GetStateConfig(E_CalState state) { for (HIDO_UINT32 i = 0; i < sizeof(g_state_configs) / sizeof(g_state_configs[0]); i++) { if (g_state_configs[i].state == state) { return &g_state_configs[i]; } } return NULL; } /* 采集并输出数据 */ static HIDO_VOID Cal_LogData(HIDO_UINT32 now_ms, HIDO_UINT16 throttle_pwm, HIDO_UINT16 steering_pwm, const HIDO_CHAR *state_name) { ST_GPRMI gprmi; ST_GPIMU gpimu; HIDO_BOOL gps_valid = (GPS_GetGPRMI(&gprmi) == HIDO_OK); HIDO_BOOL imu_valid = (GPS_GetGPIMU(&gpimu) == HIDO_OK); float enu[3] = {0.0f, 0.0f, 0.0f}; HIDO_BOOL enu_valid = (GPS_GetCurrentENU(enu) == HIDO_OK); /* 格式：$CAL,seq,time_ms,state,throttle_pwm,steering_pwm,enu_x,enu_y,enu_z,hdg,pitch,roll,gx,gy,gz,ax,ay,az */ if (gps_valid == HIDO_TRUE && imu_valid == HIDO_TRUE && enu_valid == HIDO_TRUE) { /* 直接使用浮点数格式，避免负小数的符号问题 */ int gx_int = (int)(gpimu.m_fGyroX * 1000.0f); int gy_int = (int)(gpimu.m_fGyroY * 1000.0f); int gz_int = (int)(gpimu.m_fGyroZ * 1000.0f); int ax_int = (int)(gpimu.m_fAccelX * 1000.0f); int ay_int = (int)(gpimu.m_fAccelY * 1000.0f); int az_int = (int)(gpimu.m_fAccelZ * 1000.0f); HIDO_Debug2("$CAL,%u,%u,%s,%u,%u,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%d,%d,%d,%d,%d,%d\r\n", g_sample_count, now_ms, state_name, throttle_pwm, steering_pwm, (double)enu[0], (double)enu[1], (double)enu[2], (double)gprmi.m_fHeadingAngle, (double)gprmi.m_fPitchAngle, (double)gprmi.m_fRollAngle, gx_int, gy_int, gz_int, ax_int, ay_int, az_int); g_sample_count++; } } /* 状态机更新 */ static HIDO_VOID Cal_UpdateStateMachine(HIDO_VOID) { HIDO_UINT32 now = HAL_GetTick(); /* 安全检查 */ if (Cal_SafetyCheck() == HIDO_FALSE) { Cal_EmergencyStop(); return; } /* 获取当前状态配置 */ const ST_CalStateConfig *config = Cal_GetStateConfig(g_cal_state); if (config == NULL) { Cal_EmergencyStop(); return; } /* 检查状态超时 */ HIDO_UINT32 elapsed = now - g_state_start_time; if (elapsed >= config->state_duration_ms && g_cal_state != CAL_STATE_FINISHED) { /* 进入下一个状态 */ g_cal_state = (E_CalState)(g_cal_state + 1); g_state_start_time = now; config = Cal_GetStateConfig(g_cal_state); if (config == NULL) { g_cal_state = CAL_STATE_FINISHED; g_cal_running = HIDO_FALSE; Cal_SetOutput(CAL_PWM_CENTER, CAL_PWM_CENTER); HIDO_Debug2("[CAL] Test sequence finished! Total samples: %u\r\n", g_sample_count); return; } HIDO_Debug2("[CAL] State change -> %s (duration: %u ms)\r\n", config->state_name, config->state_duration_ms); } /* 输出PWM */ Cal_SetOutput(config->throttle_pwm, config->steering_pwm); /* 采集数据 */ Cal_LogData(now, config->throttle_pwm, config->steering_pwm, config->state_name); } /* 校准任务主循环 */ static void MotionCalibration_TaskEntry(void *argument) { (void)argument; const TickType_t period = pdMS_TO_TICKS(CAL_TASK_PERIOD_MS); TickType_t last_wake = xTaskGetTickCount(); static HIDO_BOOL last_trigger = HIDO_FALSE; HIDO_Debug2("[CAL] Calibration task started\r\n"); HIDO_Debug2("[CAL] Trigger: CH8>1500 & GPS ready to start\r\n"); for (;;) { vTaskDelayUntil(&last_wake, period); /* 如果正在运行，执行状态机 */ if (g_cal_running == HIDO_TRUE) { Cal_UpdateStateMachine(); } else { /* 空闲时持续输出LOG，方便查看传感器数据 */ HIDO_UINT32 now = HAL_GetTick(); Cal_LogData(now, CAL_PWM_CENTER, CAL_PWM_CENTER, "IDLE"); /* 检测启动条件：CH8从≤1500变为>1500（上升沿触发） */ if (SBUS_IsSignalValid(MOTION_SBUS_TIMEOUT_MS) == HIDO_TRUE) { HIDO_UINT16 ch8 = SBUS_GetChannel(MOTION_SBUS_AUTO_CHANNEL); /* 触发条件：CH8 > 1500 */ HIDO_BOOL trigger = (ch8 > MOTION_SBUS_AUTO_THRESHOLD_US); /* 上升沿触发（避免重复启动） */ if (trigger == HIDO_TRUE && last_trigger == HIDO_FALSE) { MotionCalibration_Start(); } last_trigger = trigger; } } } } /******************************************************************************* * 公共接口 * *******************************************************************************/ HIDO_VOID MotionCalibration_TaskInit(HIDO_VOID) { if (g_cal_task_handle != NULL) { return; } BaseType_t ret = xTaskCreate( MotionCalibration_TaskEntry, "MotionCal", CAL_TASK_STACK_WORDS, NULL, CAL_TASK_PRIORITY, &g_cal_task_handle); if (ret != pdPASS) { g_cal_task_handle = NULL; DBG_Printf("[MotionCal] Task create failed\r\n"); } } HIDO_BOOL MotionCalibration_IsRunning(HIDO_VOID) { return g_cal_running; } HIDO_BOOL MotionCalibration_Start(HIDO_VOID) { /* 检查是否已经在运行 */ if (g_cal_running == HIDO_TRUE) { HIDO_Debug2("[CAL] Already running!\r\n"); return HIDO_FALSE; } /* 安全检查 */ if (Cal_SafetyCheck() == HIDO_FALSE) { HIDO_Debug2("[CAL] Safety check failed! (SBUS/CH8/GPS not ready)\r\n"); return HIDO_FALSE; } /* 初始化状态 */ g_cal_state = CAL_STATE_WARMUP; g_state_start_time = HAL_GetTick(); g_sample_count = 0U; g_cal_running = HIDO_TRUE; HIDO_Debug2("[CAL] =================================\r\n"); HIDO_Debug2("[CAL] Calibration sequence started!\r\n"); HIDO_Debug2("[CAL] Safety: CH8>1500 & GPS ready\r\n"); HIDO_Debug2("[CAL] =================================\r\n"); return HIDO_TRUE; } HIDO_VOID MotionCalibration_Stop(HIDO_VOID) { if (g_cal_running == HIDO_TRUE) { Cal_EmergencyStop(); HIDO_Debug2("[CAL] Calibration stopped by user\r\n"); } }