diff --git a/Core/Inc/f4_can_app.h b/Core/Inc/f4_can_app.h index 5678958..7f54e17 100644 --- a/Core/Inc/f4_can_app.h +++ b/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); diff --git a/Core/Inc/ladrc.h b/Core/Inc/ladrc.h index 3350cc6..9ecd7ea 100644 --- a/Core/Inc/ladrc.h +++ b/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); diff --git a/Core/Src/can.c b/Core/Src/can.c index 94cd867..a06169f 100644 --- a/Core/Src/can.c +++ b/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 */ diff --git a/Core/Src/f4_can_app.c b/Core/Src/f4_can_app.c index b085e3d..fee79ff 100644 --- a/Core/Src/f4_can_app.c +++ b/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(); - __disable_irq(); - if (s_cmd_age_10ms < 0xFFU) - { - s_cmd_age_10ms = (uint8_t)(s_cmd_age_10ms + 1U); - } - __set_PRIMASK(primask); + s_cmd_age_10ms = 0U; + return; } - 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(); - __disable_irq(); - s_cmd_watchdog_ms = (uint16_t)(s_cmd_watchdog_ms - CONTROL_LOOP_PERIOD_MS); - __set_PRIMASK(primask); + age_ticks = 0xFFU; } - 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) diff --git a/Core/Src/ladrc.c b/Core/Src/ladrc.c index 71c93fa..fb3e25f 100644 --- a/Core/Src/ladrc.c +++ b/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] - + LADRC_RPM_FILTER_ALPHA * raw_rpm; + 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); } } diff --git a/Core/Src/tim.c b/Core/Src/tim.c index 307fe6f..ff2e3be 100644 --- a/Core/Src/tim.c +++ b/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 */ diff --git a/FDR-Core.ioc b/FDR-Core.ioc index e1b6be7..fc74f43 100644 --- a/FDR-Core.ioc +++ b/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_SCE_IRQn=true\:1\:0\:true\:false\:true\:true\:true\:true +NVIC.CAN1_RX0_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 diff --git a/USB_DEVICE/Target/usbd_conf.c b/USB_DEVICE/Target/usbd_conf.c index f36ce5d..31529e6 100644 --- a/USB_DEVICE/Target/usbd_conf.c +++ b/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 */