/*******************************************************************************
|
* 文件名称 : 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"
|
#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");
|
}
|
}
|
