/*******************************************************************************
|
* 文件名称 : motion_calibration_task.c
|
* 文件说明 : 车辆运动模型自动校准任务实现
|
* 创建日期 : 2025-11-25
|
*******************************************************************************/
|
|
#include "motion_calibration_task.h"
|
#include "FreeRTOS.h"
|
#include "task.h"
|
#include "stm32h7xx_hal.h"
|
#include <string.h>
|
#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");
|
}
|
}
|
