v1.2修复了EEPROM的读写以及掉电保持参数的功能，现在丢步的误差会保存在Canopen字典的0x60F4位置，网关那边可以通过SDO读取到

2026-03-02 17:43:52 +08:00
parent 25d82ccd66
commit 8955887f98
9 changed files with 1710 additions and 1597 deletions
--- a/Application/app_main.c
+++ b/Application/app_main.c
@@ -77,7 +77,6 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
        break;
    case ENCODER1_Z_Pin:
        encoder_1.z++;
-        break;
    default:
        break;
    }
--- a/Application/app_motor.c
+++ b/Application/app_motor.c
@@ -31,6 +31,7 @@ typedef struct
    uint16_t prev_raw_cnt;
    uint8_t prev_raw_valid;
    uint8_t motion_active;
+    uint32_t prev_cmd_steps;

    /* 停止后延时上报 */
    uint8_t wait_stop_settle;
@@ -44,6 +45,7 @@ typedef struct
 static StepLossMonitor_t s_step_loss = {
    .threshold_mm = STEPLOSS_THRESHOLD_MM_DEFAULT,
    .prev_state = STOP,
+    .prev_cmd_steps = 0U,
 };

 static float Normalize_FollowingErrorThreshold(float threshold_mm, float fallback_mm)
@@ -117,6 +119,28 @@ static void Update_PP_FollowingError_StatusBit(float stop_error_mm)
    CO_UNLOCK_OD(CO->CANmodule);
 }

+static void Update_FollowingErrorActual_To_OD(float abs_error_mm)
+{
+    OD_entry_t *entry_ferr_actual = OD_find(OD, CIA402_INDEX_FOLLOWING_ERROR_ACTUAL);
+    float32_t ferr_mm = (float32_t)abs_error_mm;
+
+    if (entry_ferr_actual == NULL)
+    {
+        return;
+    }
+
+    if (CO != NULL)
+    {
+        CO_LOCK_OD(CO->CANmodule);
+        (void)OD_set_f32(entry_ferr_actual, 0, ferr_mm, true);
+        CO_UNLOCK_OD(CO->CANmodule);
+    }
+    else
+    {
+        (void)OD_set_f32(entry_ferr_actual, 0, ferr_mm, true);
+    }
+}
+
 /* 核心运动分发：先做安全判断，再按模式进入回零/位置模式处理 */
 static void Process_Motion_Logic(void)
 {
@@ -176,6 +200,7 @@ void App_Motor_Init(void)
 void App_Motor_Process(void)
 {
    Sync_FollowingErrorThreshold_From_OD();
+    Update_FollowingErrorActual_To_OD(s_step_loss.abs_error_mm);

    /* 先跑CiA402状态机，再执行业务运动逻辑 */
    Process_StateMachine();
@@ -198,17 +223,19 @@ void App_Motor_StepLossCheck(void)
 {
    /* 周期读取硬件计数与已发步数（TIM12中断调用） */
    uint16_t raw_cnt = (uint16_t)__HAL_TIM_GET_COUNTER(&htim1);
+    uint8_t rebase_now = 0U;
    s_step_loss.cmd_steps = stepper_1.step_count;

    /* 新动作开始时重置累计基准，避免跨动作串扰 */
    if (stepper_1.state != STOP)
    {
-        if (!s_step_loss.motion_active)
+        if ((!s_step_loss.motion_active) || (s_step_loss.cmd_steps < s_step_loss.prev_cmd_steps))
        {
            s_step_loss.motion_active = 1U;
            s_step_loss.accum_pulses = 0;
            s_step_loss.prev_raw_cnt = raw_cnt;
            s_step_loss.prev_raw_valid = 1U;
+            rebase_now = 1U;
        }
    }
    else if (s_step_loss.motion_active)
@@ -224,9 +251,18 @@ void App_Motor_StepLossCheck(void)
    }

    /* int16差分可自然处理16位计数器回绕（溢出/下溢） */
-    int16_t delta = (int16_t)(raw_cnt - s_step_loss.prev_raw_cnt);
-    s_step_loss.accum_pulses += (int32_t)delta;
-    s_step_loss.prev_raw_cnt = raw_cnt;
+    int16_t delta = 0;
+    if (rebase_now)
+    {
+        /* New move baseline: skip one diff sample to avoid counter-reset jump. */
+    }
+    else
+    {
+        delta = (int16_t)(raw_cnt - s_step_loss.prev_raw_cnt);
+        s_step_loss.accum_pulses += (int32_t)delta;
+        s_step_loss.prev_raw_cnt = raw_cnt;
+    }
+    s_step_loss.prev_cmd_steps = s_step_loss.cmd_steps;

    /* 输出扩展后的总脉冲给上层使用 */
    encoder_1.pulses = s_step_loss.accum_pulses;
--- a/Application/app_param_store.c
+++ b/Application/app_param_store.c
@@ -15,6 +15,7 @@
 #define APP_PARAM_VERSION 0x0001u
 #define APP_PARAM_EEPROM_ADDR 0x0000u
 #define APP_PARAM_AUTOSAVE_DEBOUNCE_MS 500u
+#define APP_PARAM_THRESHOLD_DEFAULT_MM 0.1f

 /* Log level: 0=off, 1=error, 2=info */
 #define APP_PARAM_STORE_LOG_LEVEL 2
@@ -176,9 +177,13 @@ static bool App_ParamStore_ReadCurrentFromOD(App_RunParams_t *params)
    OD_entry_t *entry_acc = OD_find(OD, CIA402_INDEX_PROFILE_ACC);
    OD_entry_t *entry_dec = OD_find(OD, CIA402_INDEX_PROFILE_DEC);
    OD_entry_t *entry_ferr = OD_find(OD, CIA402_INDEX_FOLLOWING_ERROR_WINDOW);
+    ODR_t odr_acc = ODR_DEV_INCOMPAT;
+    ODR_t odr_dec = ODR_DEV_INCOMPAT;
+    ODR_t odr_ferr = ODR_DEV_INCOMPAT;
    uint32_t acc_raw = 0u;
    uint32_t dec_raw = 0u;
    float32_t ferr_raw = 0.0f;
+    float ferr_mm = APP_PARAM_THRESHOLD_DEFAULT_MM;

    if ((params == 0) || (entry_acc == 0) || (entry_dec == 0) || (entry_ferr == 0))
    {
@@ -188,30 +193,55 @@ static bool App_ParamStore_ReadCurrentFromOD(App_RunParams_t *params)
    if (CO != 0)
    {
        CO_LOCK_OD(CO->CANmodule);
-        (void)OD_get_u32(entry_acc, 0, &acc_raw, true);
-        (void)OD_get_u32(entry_dec, 0, &dec_raw, true);
-        (void)OD_get_f32(entry_ferr, 0, &ferr_raw, true);
+        odr_acc = OD_get_u32(entry_acc, 0, &acc_raw, true);
+        odr_dec = OD_get_u32(entry_dec, 0, &dec_raw, true);
+        odr_ferr = OD_get_f32(entry_ferr, 0, &ferr_raw, true);
        CO_UNLOCK_OD(CO->CANmodule);
    }
    else
    {
-        (void)OD_get_u32(entry_acc, 0, &acc_raw, true);
-        (void)OD_get_u32(entry_dec, 0, &dec_raw, true);
-        (void)OD_get_f32(entry_ferr, 0, &ferr_raw, true);
+        odr_acc = OD_get_u32(entry_acc, 0, &acc_raw, true);
+        odr_dec = OD_get_u32(entry_dec, 0, &dec_raw, true);
+        odr_ferr = OD_get_f32(entry_ferr, 0, &ferr_raw, true);
    }

-    if ((acc_raw == 0u) || (acc_raw > 65535u) || (dec_raw == 0u) || (dec_raw > 65535u))
+    if ((odr_acc != ODR_OK) || (odr_dec != ODR_OK) || (odr_ferr != ODR_OK))
    {
+        APP_PS_LOGE("[EEPROM] read OD failed, odr_acc=%d odr_dec=%d odr_ferr=%d",
+                    (int)odr_acc, (int)odr_dec, (int)odr_ferr);
        return false;
    }
-    if (((float)ferr_raw <= 0.0f) || ((float)ferr_raw > SOFT_LIMIT_MAX_MM))
+
+    if (acc_raw == 0u)
    {
-        return false;
+        acc_raw = 1u;
+    }
+    if (dec_raw == 0u)
+    {
+        dec_raw = 1u;
+    }
+    if (acc_raw > 65535u)
+    {
+        acc_raw = 65535u;
+    }
+    if (dec_raw > 65535u)
+    {
+        dec_raw = 65535u;
+    }
+
+    ferr_mm = (float)ferr_raw;
+    if (!(ferr_mm == ferr_mm) || (ferr_mm <= 0.0f))
+    {
+        ferr_mm = APP_PARAM_THRESHOLD_DEFAULT_MM;
+    }
+    if (ferr_mm > SOFT_LIMIT_MAX_MM)
+    {
+        ferr_mm = SOFT_LIMIT_MAX_MM;
    }

    params->acc_mm_s2 = (uint16_t)acc_raw;
    params->dec_mm_s2 = (uint16_t)dec_raw;
-    params->step_loss_threshold_mm = (float)ferr_raw;
+    params->step_loss_threshold_mm = ferr_mm;
    return true;
 }

@@ -239,6 +269,8 @@ static uint8_t App_ParamStore_ParamsEqual(const App_RunParams_t *a, const App_Ru

 void App_ParamStore_Init(void)
 {
+    App_RunParams_t current;
+
    /* EEPROM驱动初始化：参数由Int_EEPROM24xx内部宏配置。 */
    if (Int_EEPROM24xx_Init() != INT_EEPROM_OK)
    {
@@ -246,7 +278,15 @@ void App_ParamStore_Init(void)
        APP_PS_LOGE("[EEPROM] init failed");
    }

-    g_last_seen_valid = 0u;
+    if (App_ParamStore_ReadCurrentFromOD(&current))
+    {
+        g_last_seen_params = current;
+        g_last_seen_valid = 1u;
+    }
+    else
+    {
+        g_last_seen_valid = 0u;
+    }
    g_autosave_dirty = 0u;
    g_autosave_due_ms = 0u;
 }