/******************************************************************************* * 文件名称 : 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" /******************************************************************************* * 配置参数 * *******************************************************************************/ /* 任务配置 */ #define CAL_TASK_STACK_WORDS (1024U) #define CAL_TASK_PRIORITY (tskIDLE_PRIORITY + 3U) #define CAL_TASK_PERIOD_MS (50U) // 20Hz采样 /* PWM配置 */ #define CAL_PWM_CENTER (1500U) #define CAL_PWM_LOW_THROTTLE (1600U) // 低速 #define CAL_PWM_MID_THROTTLE (1700U) // 中速 #define CAL_PWM_HIGH_THROTTLE (1800U) // 高速 #define CAL_PWM_LIGHT_TURN (1600U) // 轻转向 #define CAL_PWM_HEAVY_TURN (1700U) // 重转向 /* 测试时长（毫秒） */ #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_LOW, // 低速加速 CAL_STATE_CRUISE_LOW, // 低速巡航 CAL_STATE_REST_1, // 间歇1 CAL_STATE_ACCEL_MID, // 中速加速 CAL_STATE_CRUISE_MID, // 中速巡航 CAL_STATE_REST_2, // 间歇2 CAL_STATE_ACCEL_HIGH, // 高速加速 CAL_STATE_CRUISE_HIGH, // 高速巡航 CAL_STATE_REST_3, // 间歇3 CAL_STATE_TURN_LEFT_LIGHT, // 轻左转 CAL_STATE_REST_4, // 间歇4 CAL_STATE_TURN_RIGHT_LIGHT, // 轻右转 CAL_STATE_REST_5, // 间歇5 CAL_STATE_TURN_LEFT_HEAVY, // 重左转 CAL_STATE_REST_6, // 间歇6 CAL_STATE_TURN_RIGHT_HEAVY, // 重右转 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; /* 状态机配置表 */ static const ST_CalStateConfig g_state_configs[] = { {CAL_STATE_WARMUP, 0, CAL_DURATION_WARMUP, CAL_PWM_CENTER, CAL_PWM_CENTER, "WARMUP"}, {CAL_STATE_ACCEL_LOW, 0, CAL_DURATION_ACCEL, CAL_PWM_LOW_THROTTLE, CAL_PWM_CENTER, "ACCEL_LOW"}, {CAL_STATE_CRUISE_LOW, 0, CAL_DURATION_CRUISE, CAL_PWM_LOW_THROTTLE, CAL_PWM_CENTER, "CRUISE_LOW"}, {CAL_STATE_REST_1, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_1"}, {CAL_STATE_ACCEL_MID, 0, CAL_DURATION_ACCEL, CAL_PWM_MID_THROTTLE, CAL_PWM_CENTER, "ACCEL_MID"}, {CAL_STATE_CRUISE_MID, 0, CAL_DURATION_CRUISE, CAL_PWM_MID_THROTTLE, CAL_PWM_CENTER, "CRUISE_MID"}, {CAL_STATE_REST_2, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_2"}, {CAL_STATE_ACCEL_HIGH, 0, CAL_DURATION_ACCEL, CAL_PWM_HIGH_THROTTLE, CAL_PWM_CENTER, "ACCEL_HIGH"}, {CAL_STATE_CRUISE_HIGH, 0, CAL_DURATION_CRUISE, CAL_PWM_HIGH_THROTTLE, CAL_PWM_CENTER, "CRUISE_HIGH"}, {CAL_STATE_REST_3, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_3"}, {CAL_STATE_TURN_LEFT_LIGHT, 0, CAL_DURATION_TURN, CAL_PWM_LOW_THROTTLE, CAL_PWM_LIGHT_TURN, "TURN_LEFT_LIGHT"}, {CAL_STATE_REST_4, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_4"}, {CAL_STATE_TURN_RIGHT_LIGHT, 0, CAL_DURATION_TURN, CAL_PWM_LOW_THROTTLE, 1500-(CAL_PWM_LIGHT_TURN-1500), "TURN_RIGHT_LIGHT"}, {CAL_STATE_REST_5, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_5"}, {CAL_STATE_TURN_LEFT_HEAVY, 0, CAL_DURATION_TURN, CAL_PWM_LOW_THROTTLE, CAL_PWM_HEAVY_TURN, "TURN_LEFT_HEAVY"}, {CAL_STATE_REST_6, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_6"}, {CAL_STATE_TURN_RIGHT_HEAVY, 0, CAL_DURATION_TURN, CAL_PWM_LOW_THROTTLE, 1500-(CAL_PWM_HEAVY_TURN-1500), "TURN_RIGHT_HEAVY"}, {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才允许运行 */ static HIDO_BOOL Cal_SafetyCheck(HIDO_VOID) { /* 检查SBUS信号有效性 */ if (SBUS_IsSignalValid(CAL_SBUS_TIMEOUT_MS) == HIDO_FALSE) { return HIDO_FALSE; } /* 检查CH8（索引7） < 1500 */ HIDO_UINT16 ch8 = SBUS_GetChannel(7U); if (ch8 >= CAL_SBUS_SAFETY_THRESHOLD) { 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 - CH8 >= 1500 or SBUS lost!\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); /* 格式：$CAL,seq,time_ms,state,throttle_pwm,steering_pwm,lat,lon,alt,hdg,pitch,roll,gx,gy,gz,ax,ay,az*XX */ if (gps_valid == HIDO_TRUE && imu_valid == HIDO_TRUE) { /* 使用整数表示法打印 */ int lat_deg = (int)gprmi.m_dLatitude; int lat_frac = (int)((gprmi.m_dLatitude - lat_deg) * 1000000.0); int lon_deg = (int)gprmi.m_dLongitude; int lon_frac = (int)((gprmi.m_dLongitude - lon_deg) * 1000000.0); int alt_int = (int)gprmi.m_fAltitude; int alt_frac = (int)((gprmi.m_fAltitude - alt_int) * 100.0f); int hdg_int = (int)gprmi.m_fHeadingAngle; int hdg_frac = (int)((gprmi.m_fHeadingAngle - hdg_int) * 100.0f); int pitch_int = (int)gprmi.m_fPitchAngle; int pitch_frac = (int)((gprmi.m_fPitchAngle - pitch_int) * 100.0f); int roll_int = (int)gprmi.m_fRollAngle; int roll_frac = (int)((gprmi.m_fRollAngle - roll_int) * 100.0f); 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,%d.%06d,%d.%06d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d,%d,%d,%d,%d,%d\r\n", g_sample_count, now_ms, state_name, throttle_pwm, steering_pwm, lat_deg, lat_frac, lon_deg, lon_frac, alt_int, alt_frac, hdg_int, hdg_frac, pitch_int, pitch_frac, roll_int, roll_frac, 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: CH7 > 1800 && CH8 < 1500 to start\r\n"); for (;;) { vTaskDelayUntil(&last_wake, period); /* 如果正在运行，执行状态机 */ if (g_cal_running == HIDO_TRUE) { Cal_UpdateStateMachine(); } else { /* 空闲时检测启动条件：CH7 > 1800 && CH8 < 1500 */ if (SBUS_IsSignalValid(CAL_SBUS_TIMEOUT_MS) == HIDO_TRUE) { HIDO_UINT16 ch7 = SBUS_GetChannel(6U); /* CH7索引6 */ HIDO_UINT16 ch8 = SBUS_GetChannel(7U); /* CH8索引7 */ HIDO_BOOL trigger = (ch7 > 1800U) && (ch8 < CAL_SBUS_SAFETY_THRESHOLD); /* 上升沿触发（避免重复启动） */ 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! CH8 >= 1500 or SBUS invalid!\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 must < 1500 to run\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"); } }