1.0

Core/Inc/f4_can_app.h

@@ -21,7 +21,7 @@ typedef enum
 {
     SYSTEM_BOOTING = 0,   /* 上电启动，尚未收到第一帧合法速度命令。 */
     SYSTEM_OPERATIONAL,   /* 正常工作，允许按目标速度运动。 */
-    SYSTEM_SAFE_FAULT     /* 安全保护：目标速度已被强制清零。 */
+    SYSTEM_SAFE_FAULT     /* 安全保护：目标速度强制清零，控制器内部状态清零，并禁止继续驱动。 */
 } System_State_t;
 
 /* ================== 健康等级定义 ================== */
@@ -101,13 +101,13 @@ extern volatile Robot_Control_t g_robot_ctrl;
  *     Byte6   : cmd_age_10ms
  *     Byte7   : status rolling counter
  *
- * Tx: 0x182 实际轮速帧（20ms）
+ * Tx: 0x182 实际轮速帧（轮询，约 60ms）
  *     Byte0~1 : FL 实际 RPM, int16 little-endian
  *     Byte2~3 : RL 实际 RPM
  *     Byte4~5 : FR 实际 RPM
  *     Byte6~7 : RR 实际 RPM
  *
- * Tx: 0x183 目标轮速帧（20ms）
+ * Tx: 0x183 目标轮速帧（轮询，约 60ms）
  *     Byte0~1 : FL 目标 RPM, int16 little-endian
  *     Byte2~3 : RL 目标 RPM
  *     Byte4~5 : FR 目标 RPM
@@ -123,7 +123,7 @@ extern volatile Robot_Control_t g_robot_ctrl;
  *     Byte6   : last accepted rolling counter
  *     Byte7   : [7:4] 连续 counter 错误计数(饱和到15), [3:0] 连续 CRC 错误计数(饱和到15)
  *
- * Tx: 0x200 里程计增量帧（20ms）
+ * Tx: 0x200 里程计增量帧（轮询，约 60ms）
  *     Byte0~1 : FL delta ticks, int16 little-endian
  *     Byte2~3 : RL delta ticks
  *     Byte4~5 : FR delta ticks
@@ -151,7 +151,7 @@ void CAN_Safety_Watchdog_Tick(void);
 /*
  * 10ms 周期调用：
  * - 从最近一次完整一致的目标命令快照进行差速解算
- * - 在 SAFE_FAULT / BOOTING 下强制目标轮速为 0
+ * - 在 SAFE_FAULT / BOOTING 下强制目标轮速为 0，并禁止继续输出驱动
  */
 void Kinematics_Update_LADRC(void);
 

Core/Inc/ladrc.h

@@ -43,10 +43,13 @@ void FourWheel_LADRC_Init(void);
 /* 2. 设定目标转速：给四个轮子下发指令 */
 void FourWheel_Set_Target_RPM(float fl_rpm, float rl_rpm, float fr_rpm, float rr_rpm);
 
-/* 3. 核心闭环运算：必须且只能在 10ms 基础定时器中断里调用 */
+/* 3. 故障/停机时立即清零控制器内部状态并断驱 */
+void FourWheel_LADRC_ResetAll(void);
+
+/* 4. 核心闭环运算：必须且只能在 10ms 基础定时器中断里调用 */
 void FourWheel_LADRC_Control_Loop(void);
 
-/* 4. 诊断/状态上报接口：读取每个轮子的目标转速和控制输出 */
+/* 5. 诊断/状态上报接口：读取每个轮子的目标转速和控制输出 */
 float FourWheel_Get_Target_RPM(Motor_ID_t id);
 float FourWheel_Get_Control_Output(Motor_ID_t id);
 

Core/Src/can.c

@@ -84,9 +84,9 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
     HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
 
     /* CAN1 interrupt Init */
-    HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 0, 0);
+    HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 1, 0);
     HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
-    HAL_NVIC_SetPriority(CAN1_SCE_IRQn, 1, 0);
+    HAL_NVIC_SetPriority(CAN1_SCE_IRQn, 2, 0);
     HAL_NVIC_EnableIRQ(CAN1_SCE_IRQn);
   /* USER CODE BEGIN CAN1_MspInit 1 */
 

Core/Src/f4_can_app.c

@@ -34,6 +34,7 @@ volatile uint32_t g_dbg_valid_accept = 0U;
 #define CMD_TIMEOUT_MS                    150U
 #define MAX_ACCEPTED_COUNTER_GAP          3U
 #define TELEMETRY_COMM_DIAG_DIV           5U   /* 20ms * 5 = 100ms */
+#define CAN_RX_ISR_MAX_FRAMES_PER_CALL      2U
 
 /* ================= 通信故障判定参数 ================= */
 #define CRC_STORM_THRESHOLD               5U
@@ -63,8 +64,9 @@ volatile Robot_Control_t g_robot_ctrl = {
  * 只用“合法且新鲜”的 0x100 控制帧喂运动看门狗。
  * 这样即使上位机还在发心跳，但速度命令停了，底盘也会在超时后安全停车。
  */
-static volatile uint16_t s_cmd_watchdog_ms = 0U;
+static volatile uint32_t s_last_valid_cmd_tick_ms = 0U;
 static volatile uint8_t s_cmd_age_10ms = 0U;
+static volatile bool s_have_seen_valid_cmd = false;
 
 /* 最近一次接受的 rolling counter，同步状态用于首帧/故障恢复。 */
 static volatile uint8_t s_last_rx_counter = 0U;
@@ -300,11 +302,12 @@ static void CAN_EnterSafeFault(uint32_t reason_bits, bool desync_counter)
 
     if (desync_counter)
     {
-        s_cmd_watchdog_ms = 0U;
         s_counter_synced = false;
     }
 
     __set_PRIMASK(primask);
+
+    FourWheel_LADRC_ResetAll();
 }
 
 static System_Health_t CAN_GetCurrentHealth(uint32_t diag_bits, System_State_t state)
@@ -354,6 +357,7 @@ static void CAN_ProcessControlFrame(const uint8_t *rx_data, uint8_t dlc)
     if (CAN_CRC8_SAE_J1850(rx_data, 7U) != rx_data[7])
     {
         CAN_AtomicIncU32(&s_crc_error_total);
+        s_consecutive_counter_errors = 0U;
         CAN_AtomicIncU8Saturated(&s_consecutive_crc_errors, 0xFFU);
 
         if (s_consecutive_crc_errors >= CRC_STORM_THRESHOLD)
@@ -383,6 +387,7 @@ static void CAN_ProcessControlFrame(const uint8_t *rx_data, uint8_t dlc)
     {
         g_dbg_cnt_reject++;
         CAN_AtomicIncU32(&s_counter_reject_total);
+        s_consecutive_crc_errors = 0U;
         CAN_AtomicIncU8Saturated(&s_consecutive_counter_errors, 0xFFU);
 
         if (s_consecutive_counter_errors >= COUNTER_STORM_THRESHOLD)
@@ -407,7 +412,8 @@ static void CAN_ProcessControlFrame(const uint8_t *rx_data, uint8_t dlc)
 
         s_last_rx_counter = rx_cnt;
         s_counter_synced = true;
-        s_cmd_watchdog_ms = CMD_TIMEOUT_MS;
+        s_last_valid_cmd_tick_ms = HAL_GetTick();
+        s_have_seen_valid_cmd = true;
         s_cmd_age_10ms = 0U;
 
         /* 合法新帧到来后，允许通信相关与执行器相关的自动恢复。 */
@@ -453,8 +459,9 @@ void CAN_App_Init(void)
     g_robot_ctrl.rolling_cnt = 0U;
     g_robot_ctrl.state = SYSTEM_BOOTING;
 
-    s_cmd_watchdog_ms = 0U;
+    s_last_valid_cmd_tick_ms = 0U;
     s_cmd_age_10ms = 0U;
+    s_have_seen_valid_cmd = false;
     s_last_rx_counter = 0U;
     s_counter_synced = false;
 
@@ -493,12 +500,15 @@ void CAN_App_Init(void)
 
 void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
 {
+    uint8_t frames_processed = 0U;
+
     if (hcan != &hcan1)
     {
         return;
     }
 
-    while (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) > 0U)
+    while ((frames_processed < CAN_RX_ISR_MAX_FRAMES_PER_CALL) &&
+           (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) > 0U))
     {
         CAN_RxHeaderTypeDef rx_header;
         uint8_t rx_data[8];
@@ -508,6 +518,8 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
             break;
         }
 
+        frames_processed++;
+
         if ((rx_header.IDE != CAN_ID_STD) || (rx_header.RTR != CAN_RTR_DATA))
         {
             continue;
@@ -560,34 +572,29 @@ void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan)
 
 void CAN_Safety_Watchdog_Tick(void)
 {
-    uint32_t primask;
+    uint32_t elapsed_ms = 0U;
+    uint32_t age_ticks = 0U;
 
-    if (s_cmd_age_10ms < 0xFFU)
+    if (!s_have_seen_valid_cmd)
     {
-        primask = __get_PRIMASK();
+        s_cmd_age_10ms = 0U;
-        __disable_irq();
+        return;
-        if (s_cmd_age_10ms < 0xFFU)
-        {
-            s_cmd_age_10ms = (uint8_t)(s_cmd_age_10ms + 1U);
-        }
-        __set_PRIMASK(primask);
     }
 
-    if (s_cmd_watchdog_ms > CONTROL_LOOP_PERIOD_MS)
+    elapsed_ms = HAL_GetTick() - s_last_valid_cmd_tick_ms;
+    age_ticks = elapsed_ms / CONTROL_LOOP_PERIOD_MS;
+
+    if (age_ticks > 0xFFU)
     {
-        primask = __get_PRIMASK();
+        age_ticks = 0xFFU;
-        __disable_irq();
-        s_cmd_watchdog_ms = (uint16_t)(s_cmd_watchdog_ms - CONTROL_LOOP_PERIOD_MS);
-        __set_PRIMASK(primask);
     }
-    else if (s_cmd_watchdog_ms > 0U)
+
+    s_cmd_age_10ms = (uint8_t)age_ticks;
+
+    if ((g_robot_ctrl.state == SYSTEM_OPERATIONAL) && (elapsed_ms >= CMD_TIMEOUT_MS))
     {
         CAN_EnterSafeFault(DIAG_COMM_TIMEOUT, true);
     }
-    else
-    {
-        /* BOOTING / SAFE_FAULT 期间不重复处理。 */
-    }
 }
 
 void Kinematics_Update_LADRC(void)

Core/Src/ladrc.c

@@ -1,4 +1,5 @@
 #include "ladrc.h"
+#include "f4_can_app.h"
 
 /*
  * 这个文件实现两层内容：
@@ -20,6 +21,9 @@
 /* ================== 内部辅助函数 ================== */
 static float LADRC_Abs(float x);
 static float LADRC_Clamp(float x, float min_value, float max_value);
+static void LADRC_ResetStates(LADRC_TypeDef *ladrc);
+
+static float s_filtered_rpm[4] = {0.0f, 0.0f, 0.0f, 0.0f};
 
 static float LADRC_Abs(float x)
 {
@@ -39,6 +43,20 @@ static float LADRC_Clamp(float x, float min_value, float max_value)
     return x;
 }
 
+static void LADRC_ResetStates(LADRC_TypeDef *ladrc)
+{
+    if (ladrc == NULL)
+    {
+        return;
+    }
+
+    ladrc->v = 0.0f;
+    ladrc->y = 0.0f;
+    ladrc->z1 = 0.0f;
+    ladrc->z2 = 0.0f;
+    ladrc->u = 0.0f;
+}
+
 /**
  * @brief  初始化单个 LADRC 控制器。
  * @note   顺手做基础参数保护，避免 b0/h/out_max 为 0 或负值时出问题。
@@ -145,9 +163,23 @@ void FourWheel_LADRC_Init(void)
                    LADRC_DEFAULT_B0,
                    LADRC_CONTROL_DT_S,
                    LADRC_DEFAULT_OUT_MAX);
+        s_filtered_rpm[i] = 0.0f;
     }
 }
 
+void FourWheel_LADRC_ResetAll(void)
+{
+    int i;
+
+    for (i = 0; i < 4; ++i)
+    {
+        LADRC_ResetStates(&ladrc_motors[i]);
+        s_filtered_rpm[i] = 0.0f;
+    }
+
+    Motor_Brake_All();
+}
+
 /**
   * @brief  设置四个轮子的目标 RPM。
   */
@@ -169,20 +201,25 @@ void FourWheel_Set_Target_RPM(float fl_rpm, float rl_rpm, float fr_rpm, float rr
   */
 void FourWheel_LADRC_Control_Loop(void)
 {
-    static float filtered_rpm[4] = {0.0f, 0.0f, 0.0f, 0.0f};
     int i;
 
     Motor_Update_RPM(LADRC_CONTROL_DT_S);
 
+    if (g_robot_ctrl.state != SYSTEM_OPERATIONAL)
+    {
+        FourWheel_LADRC_ResetAll();
+        return;
+    }
+
     for (i = 0; i < 4; ++i)
     {
         float raw_rpm = Get_Motor_RPM((Motor_ID_t)i);
         float pwm_out;
 
-        filtered_rpm[i] = (1.0f - LADRC_RPM_FILTER_ALPHA) * filtered_rpm[i]
+        s_filtered_rpm[i] = (1.0f - LADRC_RPM_FILTER_ALPHA) * s_filtered_rpm[i]
                           + LADRC_RPM_FILTER_ALPHA * raw_rpm;
 
-        pwm_out = LADRC_Calc(&ladrc_motors[i], filtered_rpm[i]);
+        pwm_out = LADRC_Calc(&ladrc_motors[i], s_filtered_rpm[i]);
         Set_Motor_Output((Motor_ID_t)i, (int16_t)pwm_out);
     }
 }

Core/Src/tim.c

@@ -557,7 +557,7 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
     __HAL_RCC_TIM6_CLK_ENABLE();
 
     /* TIM6 interrupt Init */
-    HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 2, 0);
+    HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 0, 0);
     HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn);
   /* USER CODE BEGIN TIM6_MspInit 1 */
 

FDR-Core.ioc

@@ -72,19 +72,19 @@ Mcu.UserName=STM32F407VGTx
 MxCube.Version=6.15.0
 MxDb.Version=DB.6.0.150
 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
-NVIC.CAN1_RX0_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
+NVIC.CAN1_RX0_IRQn=true\:1\:0\:true\:false\:true\:true\:true\:true
-NVIC.CAN1_SCE_IRQn=true\:1\:0\:true\:false\:true\:true\:true\:true
+NVIC.CAN1_SCE_IRQn=true\:2\:0\:true\:false\:true\:true\:true\:true
 NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.ForceEnableDMAVector=true
 NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
-NVIC.OTG_FS_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true
+NVIC.OTG_FS_IRQn=true\:3\:0\:true\:false\:true\:false\:true\:true
 NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4
 NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:false\:true\:false
-NVIC.TIM6_DAC_IRQn=true\:2\:0\:true\:false\:true\:true\:true\:true
+NVIC.TIM6_DAC_IRQn=true\:0\:0\:true\:false\:true\:true\:true\:true
 NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 PA0-WKUP.GPIOParameters=GPIO_Label
 PA0-WKUP.GPIO_Label=FL_ENC_A

USB_DEVICE/Target/usbd_conf.c

@@ -91,7 +91,7 @@ void HAL_PCD_MspInit(PCD_HandleTypeDef* pcdHandle)
     __HAL_RCC_USB_OTG_FS_CLK_ENABLE();
 
     /* Peripheral interrupt init */
-    HAL_NVIC_SetPriority(OTG_FS_IRQn, 0, 0);
+    HAL_NVIC_SetPriority(OTG_FS_IRQn, 3, 0);
     HAL_NVIC_EnableIRQ(OTG_FS_IRQn);
   /* USER CODE BEGIN USB_OTG_FS_MspInit 1 */