OTA升级功能调通，准备增加boot的代码

yincheng.zhong

2 天以前 567085ead3f6adaabd884f16ab4b17c62e8f0403

 STM32H743/APL/motion_calibration_task.c

@@ -13,6 +13,7 @@
#include "GPS.h"
#include "SBUS.h"
#include "pwm_ctrol.h"
#include "motion_mode.h"

/*******************************************************************************
 *                              配置参数                                        *
@@ -23,13 +24,40 @@
#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)      // 重转向
/* 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)      // 预热
@@ -51,22 +79,43 @@
{
    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_1,            // 间歇1
    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_2,            // 间歇2
    CAL_STATE_REST_4,            // 间歇4
    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_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              // 错误（安全停止）
@@ -92,46 +141,92 @@
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"},
    {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"}
    {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才允许运行 */
/* 安全检查：SBUS CH8 > 1500 且 GPS 就绪才允许运行 */
static HIDO_BOOL Cal_SafetyCheck(HIDO_VOID)
{
    /* 检查SBUS信号有效性 */
    if (SBUS_IsSignalValid(CAL_SBUS_TIMEOUT_MS) == HIDO_FALSE)
    if (SBUS_IsSignalValid(MOTION_SBUS_TIMEOUT_MS) == HIDO_FALSE)
    {
        return HIDO_FALSE;
    }
    
    /* 检查CH8（索引7） < 1500 */
    HIDO_UINT16 ch8 = SBUS_GetChannel(7U);
    if (ch8 >= CAL_SBUS_SAFETY_THRESHOLD)
    /* 检查 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;
    }
    
@@ -145,7 +240,7 @@
    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");
    HIDO_Debug2("[CAL] EMERGENCY STOP - Safety check failed!\r\n");
}

/* 输出PWM控制 */
@@ -176,22 +271,13 @@
    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)
    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 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);
@@ -199,18 +285,18 @@
        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",
        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,
                    lat_deg, lat_frac,
                    lon_deg, lon_frac,
                    alt_int, alt_frac,
                    hdg_int, hdg_frac,
                    pitch_int, pitch_frac,
                    roll_int, roll_frac,
                    (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++;
@@ -274,7 +360,7 @@
    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");
    HIDO_Debug2("[CAL] Trigger: CH8>1500 & GPS ready to start\r\n");
    
    for (;;)
    {
@@ -287,12 +373,16 @@
        }
        else
        {
            /* 空闲时检测启动条件：CH7 > 1800 && CH8 < 1500 */
            if (SBUS_IsSignalValid(CAL_SBUS_TIMEOUT_MS) == HIDO_TRUE)
            /* 空闲时持续输出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 ch7 = SBUS_GetChannel(6U);  /* CH7索引6 */
                HIDO_UINT16 ch8 = SBUS_GetChannel(7U);  /* CH8索引7 */
                HIDO_BOOL trigger = (ch7 > 1800U) && (ch8 < CAL_SBUS_SAFETY_THRESHOLD);
                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)
@@ -348,7 +438,7 @@
    /* 安全检查 */
    if (Cal_SafetyCheck() == HIDO_FALSE)
    {
        HIDO_Debug2("[CAL] Safety check failed! CH8 >= 1500 or SBUS invalid!\r\n");
        HIDO_Debug2("[CAL] Safety check failed! (SBUS/CH8/GPS not ready)\r\n");
        return HIDO_FALSE;
    }
    
@@ -360,7 +450,7 @@
    
    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] Safety: CH8>1500 & GPS ready\r\n");
    HIDO_Debug2("[CAL] =================================\r\n");
    
    return HIDO_TRUE;