From e9e2f666fb077a68fed9d2c4719fa0fab1a76ae9 Mon Sep 17 00:00:00 2001 From: nitiantuhao <2062405236@qq.com> Date: Sat, 14 Mar 2026 17:17:17 +0800 Subject: [PATCH] 1.0 --- Core/Inc/f4_can_app.h | 203 +++++++-- Core/Inc/ladrc.h | 12 +- Core/Inc/stm32f4xx_it.h | 1 + Core/Src/can.c | 3 + Core/Src/f4_can_app.c | 895 ++++++++++++++++++++++++++++++++++++++++ Core/Src/ladrc.c | 213 +++++++--- Core/Src/main.c | 149 +++---- Core/Src/motor.c | 424 ++++++++++++------- Core/Src/stm32f4xx_it.c | 14 + Core/Src/tim.c | 2 +- FDR-Core.ioc | 3 +- 11 files changed, 1609 insertions(+), 310 deletions(-) diff --git a/Core/Inc/f4_can_app.h b/Core/Inc/f4_can_app.h index fce3e17..5678958 100644 --- a/Core/Inc/f4_can_app.h +++ b/Core/Inc/f4_can_app.h @@ -1,47 +1,180 @@ #ifndef __F4_CAN_APP_H #define __F4_CAN_APP_H +#include + #include "main.h" -#include "can.h" // CubeMX生成的CAN头文件，提供 hcan1 句柄 +#include "can.h" -// ---------------- 1. CAN ID 定义 (标准11位) ---------------- -#define CAN_ID_EMCY_F4 0x010 // 紧急故障 (最高优先级) -#define CAN_ID_HEARTBEAT_H7 0x080 // 上位机心跳 -#define CAN_ID_HEARTBEAT_F4 0x081 // 下位机心跳 -#define CAN_ID_RXPDO_CTRL 0x100 // 控制下发 (H7 -> F4) -#define CAN_ID_TXPDO1_ODOM 0x200 // 里程计遥测1 (F4 -> H7) -#define CAN_ID_TXPDO2_LADRC 0x201 // LADRC状态2 (F4 -> H7) +/* + * 这个文件定义“上位机 CAN 协议 + 底盘状态机 + 故障/诊断上报”的公共接口。 + * + * 设计原则： + * 1) 10ms TIM6 中断只做硬实时内容：看门狗、速度解算、LADRC、故障判定。 + * 2) CAN 接收中断只解析帧，不做复杂浮点运算。 + * 3) 主循环只做低实时性的周期发送：状态帧、诊断帧、里程计。 + * 4) 运动安全只依赖“新鲜合法的 0x100 控制帧”，0x080 心跳不再允许底盘继续沿用旧速度运动。 + */ -// ---------------- 2. 机器人底盘物理参数 ---------------- -// 【注意】请根据你实际测量的小车物理尺寸修改以下参数 -#define ROBOT_TRACK_WIDTH_M 0.195f // 左右轮距 23.6 cm (0.236 m) -#define ROBOT_WHEEL_RADIUS_M 0.040f // 车轮半径 (假设65mm轮子，即 0.0325 m) -#define ICR_COEFFICIENT 1.0f // 瞬时旋转中心漂移系数 (默认1.0，根据场地摩擦力微调) -#define PI_VALUE 3.1415926535f +/* ================== 系统状态机定义 ================== */ +typedef enum +{ + SYSTEM_BOOTING = 0, /* 上电启动，尚未收到第一帧合法速度命令。 */ + SYSTEM_OPERATIONAL, /* 正常工作，允许按目标速度运动。 */ + SYSTEM_SAFE_FAULT /* 安全保护：目标速度已被强制清零。 */ +} System_State_t; -// ---------------- 3. 状态机枚举 ---------------- -typedef enum { - F4_STATE_FAULT = 0, // 故障/急停状态 - F4_STATE_OPERATIONAL = 1 // 正常运行状态 -} F4_SystemState_t; +/* ================== 健康等级定义 ================== */ +typedef enum +{ + SYSTEM_HEALTH_OK = 0, /* 当前没有活动中的告警/故障。 */ + SYSTEM_HEALTH_WARNING = 1, /* 有警告，但系统还可继续工作。 */ + SYSTEM_HEALTH_FAULT = 2 /* 有明确故障，通常已经或应当进入保护。 */ +} System_Health_t; -// ---------------- 4. 数据解包联合体 (处理浮点数字节序) ---------------- -typedef union { - uint8_t bytes[8]; - struct { - float target_vx; // 目标线速度 (m/s) - float target_wz; // 目标角速度 (rad/s) - } data; -} RxCtrlPayload_t; +/* ================== 诊断位图定义 ================== */ +typedef enum +{ + DIAG_COMM_TIMEOUT = (1UL << 0), /* 速度控制帧超时。属于故障。 */ + DIAG_CAN_BUS_OFF = (1UL << 1), /* CAN Bus-Off。属于故障。 */ + DIAG_CMD_CRC_STORM = (1UL << 2), /* 连续 CRC 错误过多。属于故障。 */ + DIAG_CMD_CNT_STORM = (1UL << 3), /* 连续 rolling counter 错误过多。属于故障。 */ + DIAG_MOTOR_FL_STALL = (1UL << 4), /* 左前轮堵转/失效趋势。属于故障。 */ + DIAG_MOTOR_RL_STALL = (1UL << 5), /* 左后轮堵转/失效趋势。属于故障。 */ + DIAG_MOTOR_FR_STALL = (1UL << 6), /* 右前轮堵转/失效趋势。属于故障。 */ + DIAG_MOTOR_RR_STALL = (1UL << 7), /* 右后轮堵转/失效趋势。属于故障。 */ + DIAG_CONTROL_SATURATION = (1UL << 8) /* 控制输出长时间顶满。属于警告。 */ +} Chassis_DiagBits_t; -// ---------------- 5. 全局变量与函数声明 ---------------- -extern volatile F4_SystemState_t f4_fsm_state; -extern RxCtrlPayload_t rx_ctrl_cmd; +/* + * 便于快速区分“只是提醒”还是“必须当故障处理”的位掩码。 + * 上位机解析时可以直接用这两个宏做分类。 + */ +#define CHASSIS_DIAG_FATAL_MASK (DIAG_COMM_TIMEOUT | DIAG_CAN_BUS_OFF | DIAG_CMD_CRC_STORM | \ + DIAG_CMD_CNT_STORM | DIAG_MOTOR_FL_STALL | DIAG_MOTOR_RL_STALL | \ + DIAG_MOTOR_FR_STALL | DIAG_MOTOR_RR_STALL) +#define CHASSIS_DIAG_WARN_MASK (DIAG_CONTROL_SATURATION) -// 填平 CubeMX 坑的初始化函数 -void F4_CAN_Filter_And_Start(void); +/* ================== 核心控制结构体 ================== */ +typedef struct +{ + float target_vx; /* 目标线速度，单位 m/s。 */ + float target_wz; /* 目标角速度，单位 rad/s。 */ + uint8_t ctrl_flags; /* 上位机附带的控制标志位，当前透传保存。 */ + uint8_t rolling_cnt; /* 最近一次接受的速度控制帧 rolling counter。 */ + System_State_t state; /* 当前系统状态机状态。 */ +} Robot_Control_t; -// 核心任务调度函数 (需放在 TIM6 10ms 中断内调用) -void F4_CAN_Task_10ms(void); +/* ================== 上位机可读的状态快照 ================== */ +typedef struct +{ + System_State_t state; /* 当前状态机状态。 */ + System_Health_t health; /* 当前健康等级。 */ + uint32_t diag_bits; /* 当前活动中的诊断位图。 */ + uint8_t cmd_age_10ms; /* 距最近一次合法 0x100 已过去多少个 10ms tick。 */ + uint8_t last_cmd_counter; /* 最近一次接受的 rolling counter。 */ +} Chassis_StatusSnapshot_t; -#endif /* __F4_CAN_APP_H */ \ No newline at end of file +/* + * g_robot_ctrl 会在 CAN 接收中断里更新，也会在控制中断里读取， + * 因此这里声明为 volatile，避免编译器把它优化成寄存器缓存。 + */ +extern volatile Robot_Control_t g_robot_ctrl; + +/* ================== CAN 协议说明 ================== + * + * Rx: 0x080 心跳帧 + * - 仅用于链路存在性，不参与运动看门狗 + * + * Rx: 0x100 速度控制帧（8 字节） + * Byte0~1 : int16_t vx * 1000, 单位 m/s，小端 + * Byte2~3 : int16_t wz * 1000, 单位 rad/s，小端 + * Byte4 : ctrl_flags + * Byte5 : 预留 + * Byte6 : rolling counter + * Byte7 : CRC8-SAE J1850(前 7 字节) + * + * Tx: 0x181 状态帧（20ms） + * Byte0 : system_state + * Byte1 : system_health + * Byte2~5 : diag_bits, uint32 little-endian + * Byte6 : cmd_age_10ms + * Byte7 : status rolling counter + * + * Tx: 0x182 实际轮速帧（20ms） + * Byte0~1 : FL 实际 RPM, int16 little-endian + * Byte2~3 : RL 实际 RPM + * Byte4~5 : FR 实际 RPM + * Byte6~7 : RR 实际 RPM + * + * Tx: 0x183 目标轮速帧（20ms） + * Byte0~1 : FL 目标 RPM, int16 little-endian + * Byte2~3 : RL 目标 RPM + * Byte4~5 : FR 目标 RPM + * Byte6~7 : RR 目标 RPM + * + * Tx: 0x184 通信诊断帧（100ms） + * Byte0 : valid_cmd_total 低 8 位 + * Byte1 : crc_error_total 低 8 位 + * Byte2 : counter_reject_total 低 8 位 + * Byte3 : can_tx_drop_total 低 8 位 + * Byte4 : busoff_total 低 8 位 + * Byte5 : rx_overrun_total 低 8 位 + * Byte6 : last accepted rolling counter + * Byte7 : [7:4] 连续 counter 错误计数(饱和到15), [3:0] 连续 CRC 错误计数(饱和到15) + * + * Tx: 0x200 里程计增量帧（20ms） + * Byte0~1 : FL delta ticks, int16 little-endian + * Byte2~3 : RL delta ticks + * Byte4~5 : FR delta ticks + * Byte6~7 : RR delta ticks + */ + +/* ================== 对外接口 ================== */ + +/* + * 初始化 CAN 应用层： + * - 配置硬件滤波器，仅接收 0x080 / 0x100 + * - 启动 CAN 外设 + * - 打开 FIFO0 接收中断 + * - 清空通信诊断状态 + */ +void CAN_App_Init(void); + +/* + * 10ms 周期调用： + * - 维护速度控制命令看门狗 + * - 合法控制帧超时则进入 SAFE_FAULT + */ +void CAN_Safety_Watchdog_Tick(void); + +/* + * 10ms 周期调用： + * - 从最近一次完整一致的目标命令快照进行差速解算 + * - 在 SAFE_FAULT / BOOTING 下强制目标轮速为 0 + */ +void Kinematics_Update_LADRC(void); + +/* + * 10ms 周期调用： + * - 根据目标 RPM、实际 RPM、控制输出做堵转/饱和诊断 + * - 该函数应放在 LADRC 控制环执行之后，以便读取最新反馈 + */ +void Chassis_Diagnostics_10ms_Tick(void); + +/* + * 20ms 周期调用： + * - 发送 0x181 状态帧 + * - 发送 0x182 实际轮速帧 + * - 发送 0x183 目标轮速帧 + * - 发送 0x200 里程计帧 + * - 内部每 5 次额外发送 1 次 0x184 通信诊断帧 + */ +void CAN_Send_Telemetry_20ms(void); + +/* + * 获取当前快照，便于未来扩展到 USB/串口调试打印或上位机测试。 + */ +void Chassis_Get_StatusSnapshot(Chassis_StatusSnapshot_t *snapshot); + +#endif diff --git a/Core/Inc/ladrc.h b/Core/Inc/ladrc.h index 31565ca..3350cc6 100644 --- a/Core/Inc/ladrc.h +++ b/Core/Inc/ladrc.h @@ -35,15 +35,19 @@ float LADRC_Calc(LADRC_TypeDef *ladrc, float actual_val); /* ================= 针对四轮底盘的 LADRC 扩展调度层 ================= */ // 将 4 个控制器暴露出来，方便 main.c 里面调取数据用 VOFA+ 打印波形 -extern LADRC_TypeDef ladrc_motors[4]; +extern LADRC_TypeDef ladrc_motors[4]; -// 1. 初始化：一键初始化底层定时器和4个LADRC控制器 +/* 1. 初始化：一键初始化底层定时器和4个LADRC控制器 */ void FourWheel_LADRC_Init(void); -// 2. 设定目标转速：给四个轮子下发指令 +/* 2. 设定目标转速：给四个轮子下发指令 */ void FourWheel_Set_Target_RPM(float fl_rpm, float rl_rpm, float fr_rpm, float rr_rpm); -// 3. 核心闭环运算：必须且只能在 10ms 基础定时器中断里调用 +/* 3. 核心闭环运算：必须且只能在 10ms 基础定时器中断里调用 */ void FourWheel_LADRC_Control_Loop(void); +/* 4. 诊断/状态上报接口：读取每个轮子的目标转速和控制输出 */ +float FourWheel_Get_Target_RPM(Motor_ID_t id); +float FourWheel_Get_Control_Output(Motor_ID_t id); + #endif /* __LADRC_H */ \ No newline at end of file diff --git a/Core/Inc/stm32f4xx_it.h b/Core/Inc/stm32f4xx_it.h index fca404e..6e43170 100644 --- a/Core/Inc/stm32f4xx_it.h +++ b/Core/Inc/stm32f4xx_it.h @@ -56,6 +56,7 @@ void DebugMon_Handler(void); void PendSV_Handler(void); void SysTick_Handler(void); void CAN1_RX0_IRQHandler(void); +void CAN1_SCE_IRQHandler(void); void TIM6_DAC_IRQHandler(void); void OTG_FS_IRQHandler(void); /* USER CODE BEGIN EFP */ diff --git a/Core/Src/can.c b/Core/Src/can.c index a7f4fc4..751382f 100644 --- a/Core/Src/can.c +++ b/Core/Src/can.c @@ -86,6 +86,8 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle) /* CAN1 interrupt Init */ HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 0, 0); HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn); + HAL_NVIC_SetPriority(CAN1_SCE_IRQn, 1, 0); + HAL_NVIC_EnableIRQ(CAN1_SCE_IRQn); /* USER CODE BEGIN CAN1_MspInit 1 */ /* USER CODE END CAN1_MspInit 1 */ @@ -111,6 +113,7 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle) /* CAN1 interrupt Deinit */ HAL_NVIC_DisableIRQ(CAN1_RX0_IRQn); + HAL_NVIC_DisableIRQ(CAN1_SCE_IRQn); /* USER CODE BEGIN CAN1_MspDeInit 1 */ /* USER CODE END CAN1_MspDeInit 1 */ diff --git a/Core/Src/f4_can_app.c b/Core/Src/f4_can_app.c index e69de29..fb66300 100644 --- a/Core/Src/f4_can_app.c +++ b/Core/Src/f4_can_app.c @@ -0,0 +1,895 @@ +#include "f4_can_app.h" + +#include +#include + +#include "ladrc.h" +#include "motor.h" + +/* + * 下面两个 getter 由 ladrc.c 提供，用于诊断层读取每个轮子的目标值与控制输出。 + * 之所以在这里做前向声明，是为了尽量少改你原工程里现有的 ladrc.h。 + */ + + +/* ================= 机器人底盘物理参数 ================= */ +#define WHEEL_RADIUS_M 0.06f +#define WHEEL_TRACK_M 0.30f +#define PI_F 3.14159265358979323846f + +/* ================= CAN 协议参数 ================= */ +#define CAN_ID_HEARTBEAT 0x080U +#define CAN_ID_CMD_VEL 0x100U +#define CAN_ID_STATUS 0x181U +#define CAN_ID_ACTUAL_RPM 0x182U +#define CAN_ID_TARGET_RPM 0x183U +#define CAN_ID_COMM_DIAG 0x184U +#define CAN_ID_ODOM 0x200U + +#define CONTROL_LOOP_PERIOD_MS 10U +#define CMD_TIMEOUT_MS 150U +#define MAX_ACCEPTED_COUNTER_GAP 3U +#define TELEMETRY_COMM_DIAG_DIV 5U /* 20ms * 5 = 100ms */ + +/* ================= 通信故障判定参数 ================= */ +#define CRC_STORM_THRESHOLD 5U +#define COUNTER_STORM_THRESHOLD 5U + +/* ================= 执行机构诊断参数 ================= */ +#define STALL_TARGET_RPM_MIN 40.0f +#define STALL_FEEDBACK_RPM_MAX 8.0f +#define STALL_OUTPUT_ABS_MIN 850.0f +#define STALL_CONFIRM_TICKS 50U /* 50 * 10ms = 500ms */ + +#define SATURATION_TARGET_RPM_MIN 30.0f +#define SATURATION_OUTPUT_ABS_MIN 980.0f +#define SATURATION_CONFIRM_TICKS 20U /* 20 * 10ms = 200ms */ + +#define CAN_TX_SHORT_WAIT_MS 2U + +volatile Robot_Control_t g_robot_ctrl = { + .target_vx = 0.0f, + .target_wz = 0.0f, + .ctrl_flags = 0U, + .rolling_cnt = 0U, + .state = SYSTEM_BOOTING +}; + +/* + * 只用“合法且新鲜”的 0x100 控制帧喂运动看门狗。 + * 这样即使上位机还在发心跳，但速度命令停了，底盘也会在超时后安全停车。 + */ +static volatile uint16_t s_cmd_watchdog_ms = 0U; +static volatile uint8_t s_cmd_age_10ms = 0U; + +/* 最近一次接受的 rolling counter，同步状态用于首帧/故障恢复。 */ +static volatile uint8_t s_last_rx_counter = 0U; +static volatile bool s_counter_synced = false; + +/* 当前活动中的诊断位图。 */ +static volatile uint32_t s_diag_bits = 0U; + +/* 通信统计计数器：全部为累加值，方便上位机做趋势诊断。 */ +static volatile uint32_t s_valid_cmd_total = 0U; +static volatile uint32_t s_crc_error_total = 0U; +static volatile uint32_t s_counter_reject_total = 0U; +static volatile uint32_t s_can_tx_drop_total = 0U; +static volatile uint32_t s_busoff_total = 0U; +static volatile uint32_t s_rx_overrun_total = 0U; + +/* 连续错误计数，用于判断“风暴/持续异常”。 */ +static volatile uint8_t s_consecutive_crc_errors = 0U; +static volatile uint8_t s_consecutive_counter_errors = 0U; + +/* 轮子堵转与控制饱和诊断的确认计数器。 */ +static uint16_t s_stall_ticks[4] = {0U, 0U, 0U, 0U}; +static uint16_t s_saturation_ticks = 0U; + +/* 周期发送辅助计数器。 */ +static uint8_t s_status_tx_counter = 0U; +static uint8_t s_comm_diag_divider = 0U; + +/* ================= 内部辅助函数声明 ================= */ +static inline uint8_t CAN_CRC8_SAE_J1850(const uint8_t *data, uint16_t length); +static int16_t CAN_Read_I16_LE(uint8_t low_byte, uint8_t high_byte); +static void CAN_Write_I16_LE(uint8_t *dst, int16_t value); +static int16_t CAN_SaturateFloatToI16(float value); +static float CAN_AbsFloat(float value); +static void CAN_Config_StdDataFilter(uint8_t filter_bank, uint16_t std_id); +static void CAN_SetDiagBits(uint32_t bits); +static void CAN_ClearDiagBits(uint32_t bits); +static void CAN_AtomicIncU32(volatile uint32_t *value); +static void CAN_AtomicIncU8Saturated(volatile uint8_t *value, uint8_t max_value); +static void CAN_ResetMotionCommand(void); +static void CAN_EnterSafeFault(uint32_t reason_bits, bool desync_counter); +static void CAN_ProcessHeartbeat(const uint8_t *rx_data, uint8_t dlc); +static void CAN_ProcessControlFrame(const uint8_t *rx_data, uint8_t dlc); +static System_Health_t CAN_GetCurrentHealth(uint32_t diag_bits, System_State_t state); +static HAL_StatusTypeDef CAN_TxStdFrame(uint16_t std_id, const uint8_t *payload, uint8_t dlc); +static void CAN_Send_StatusFrame(void); +static void CAN_Send_ActualRpmFrame(void); +static void CAN_Send_TargetRpmFrame(void); +static void CAN_Send_CommDiagFrame(void); +static void CAN_Send_OdomFrame(void); + +/* ===================================================================== + * 基础辅助函数 + * ===================================================================== */ + +/* + * CRC8-SAE J1850（poly=0x1D, init=0xFF, xorout=0xFF, 非反射） + * + * 说明： + * - 上一版为了“先把功能修对”，用了按位循环的通用实现； + * 但这会在 CAN Rx ISR 中产生 7*8=56 次 bit 循环，属于不必要的开销。 + * - 这里改成查表版：每字节 1 次查表 + XOR，满足“ISR 内尽量不做重计算”的工程规范。 + * - 表是静态常量，放在 Flash，不占用 RAM。 + */ +static const uint8_t s_crc8_j1850_table[256] = { + 0x00U, 0x1DU, 0x3AU, 0x27U, 0x74U, 0x69U, 0x4EU, 0x53U, 0xE8U, 0xF5U, 0xD2U, 0xCFU, 0x9CU, 0x81U, 0xA6U, 0xBBU, + 0xCDU, 0xD0U, 0xF7U, 0xEAU, 0xB9U, 0xA4U, 0x83U, 0x9EU, 0x25U, 0x38U, 0x1FU, 0x02U, 0x51U, 0x4CU, 0x6BU, 0x76U, + 0x87U, 0x9AU, 0xBDU, 0xA0U, 0xF3U, 0xEEU, 0xC9U, 0xD4U, 0x6FU, 0x72U, 0x55U, 0x48U, 0x1BU, 0x06U, 0x21U, 0x3CU, + 0x4AU, 0x57U, 0x70U, 0x6DU, 0x3EU, 0x23U, 0x04U, 0x19U, 0xA2U, 0xBFU, 0x98U, 0x85U, 0xD6U, 0xCBU, 0xECU, 0xF1U, + 0x13U, 0x0EU, 0x29U, 0x34U, 0x67U, 0x7AU, 0x5DU, 0x40U, 0xFBU, 0xE6U, 0xC1U, 0xDCU, 0x8FU, 0x92U, 0xB5U, 0xA8U, + 0xDEU, 0xC3U, 0xE4U, 0xF9U, 0xAAU, 0xB7U, 0x90U, 0x8DU, 0x36U, 0x2BU, 0x0CU, 0x11U, 0x42U, 0x5FU, 0x78U, 0x65U, + 0x94U, 0x89U, 0xAEU, 0xB3U, 0xE0U, 0xFDU, 0xDAU, 0xC7U, 0x7CU, 0x61U, 0x46U, 0x5BU, 0x08U, 0x15U, 0x32U, 0x2FU, + 0x59U, 0x44U, 0x63U, 0x7EU, 0x2DU, 0x30U, 0x17U, 0x0AU, 0xB1U, 0xACU, 0x8BU, 0x96U, 0xC5U, 0xD8U, 0xFFU, 0xE2U, + 0x26U, 0x3BU, 0x1CU, 0x01U, 0x52U, 0x4FU, 0x68U, 0x75U, 0xCEU, 0xD3U, 0xF4U, 0xE9U, 0xBAU, 0xA7U, 0x80U, 0x9DU, + 0xEBU, 0xF6U, 0xD1U, 0xCCU, 0x9FU, 0x82U, 0xA5U, 0xB8U, 0x03U, 0x1EU, 0x39U, 0x24U, 0x77U, 0x6AU, 0x4DU, 0x50U, + 0xA1U, 0xBCU, 0x9BU, 0x86U, 0xD5U, 0xC8U, 0xEFU, 0xF2U, 0x49U, 0x54U, 0x73U, 0x6EU, 0x3DU, 0x20U, 0x07U, 0x1AU, + 0x6CU, 0x71U, 0x56U, 0x4BU, 0x18U, 0x05U, 0x22U, 0x3FU, 0x84U, 0x99U, 0xBEU, 0xA3U, 0xF0U, 0xEDU, 0xCAU, 0xD7U, + 0x35U, 0x28U, 0x0FU, 0x12U, 0x41U, 0x5CU, 0x7BU, 0x66U, 0xDDU, 0xC0U, 0xE7U, 0xFAU, 0xA9U, 0xB4U, 0x93U, 0x8EU, + 0xF8U, 0xE5U, 0xC2U, 0xDFU, 0x8CU, 0x91U, 0xB6U, 0xABU, 0x10U, 0x0DU, 0x2AU, 0x37U, 0x64U, 0x79U, 0x5EU, 0x43U, + 0xB2U, 0xAFU, 0x88U, 0x95U, 0xC6U, 0xDBU, 0xFCU, 0xE1U, 0x5AU, 0x47U, 0x60U, 0x7DU, 0x2EU, 0x33U, 0x14U, 0x09U, + 0x7FU, 0x62U, 0x45U, 0x58U, 0x0BU, 0x16U, 0x31U, 0x2CU, 0x97U, 0x8AU, 0xADU, 0xB0U, 0xE3U, 0xFEU, 0xD9U, 0xC4U, +}; + +static inline uint8_t CAN_CRC8_SAE_J1850(const uint8_t *data, uint16_t length) +{ + uint8_t crc = 0xFFU; + + while (length-- > 0U) + { + crc = s_crc8_j1850_table[(uint8_t)(crc ^ *data++)]; + } + + return (uint8_t)(crc ^ 0xFFU); +} + +static int16_t CAN_Read_I16_LE(uint8_t low_byte, uint8_t high_byte) +{ + return (int16_t)((uint16_t)low_byte | ((uint16_t)high_byte << 8)); +} + +static void CAN_Write_I16_LE(uint8_t *dst, int16_t value) +{ + if (dst == NULL) + { + return; + } + + dst[0] = (uint8_t)((uint16_t)value & 0xFFU); + dst[1] = (uint8_t)(((uint16_t)value >> 8) & 0xFFU); +} + +static int16_t CAN_SaturateFloatToI16(float value) +{ + if (value > 32767.0f) + { + value = 32767.0f; + } + else if (value < -32768.0f) + { + value = -32768.0f; + } + + if (value >= 0.0f) + { + value += 0.5f; + } + else + { + value -= 0.5f; + } + + return (int16_t)value; +} + +static float CAN_AbsFloat(float value) +{ + return (value >= 0.0f) ? value : -value; +} + +static void CAN_Config_StdDataFilter(uint8_t filter_bank, uint16_t std_id) +{ + CAN_FilterTypeDef filter = {0}; + + filter.FilterBank = filter_bank; + filter.FilterMode = CAN_FILTERMODE_IDMASK; + filter.FilterScale = CAN_FILTERSCALE_32BIT; + + /* + * bxCAN 32 位过滤寄存器的标准 ID 对齐方式：StdId 左移 5 位。 + * MaskHigh=0xFFE0 表示 11 位标准 ID 完全匹配。 + * MaskLow =0x0006 表示同时限定 IDE=0（标准帧）和 RTR=0（数据帧）。 + */ + filter.FilterIdHigh = (uint16_t)(std_id << 5); + filter.FilterIdLow = 0x0000U; + filter.FilterMaskIdHigh = 0xFFE0U; + filter.FilterMaskIdLow = 0x0006U; + + filter.FilterFIFOAssignment = CAN_RX_FIFO0; + filter.FilterActivation = ENABLE; + filter.SlaveStartFilterBank = 14; + + if (HAL_CAN_ConfigFilter(&hcan1, &filter) != HAL_OK) + { + Error_Handler(); + } +} + +static void CAN_SetDiagBits(uint32_t bits) +{ + uint32_t primask = __get_PRIMASK(); + __disable_irq(); + s_diag_bits |= bits; + __set_PRIMASK(primask); +} + +static void CAN_ClearDiagBits(uint32_t bits) +{ + uint32_t primask = __get_PRIMASK(); + __disable_irq(); + s_diag_bits &= ~bits; + __set_PRIMASK(primask); +} + +static void CAN_AtomicIncU32(volatile uint32_t *value) +{ + uint32_t primask; + + if (value == NULL) + { + return; + } + + primask = __get_PRIMASK(); + __disable_irq(); + *value = *value + 1U; + __set_PRIMASK(primask); +} + +static void CAN_AtomicIncU8Saturated(volatile uint8_t *value, uint8_t max_value) +{ + uint32_t primask; + + if (value == NULL) + { + return; + } + + primask = __get_PRIMASK(); + __disable_irq(); + if (*value < max_value) + { + *value = (uint8_t)(*value + 1U); + } + __set_PRIMASK(primask); +} + +static void CAN_ResetMotionCommand(void) +{ + g_robot_ctrl.target_vx = 0.0f; + g_robot_ctrl.target_wz = 0.0f; + g_robot_ctrl.ctrl_flags = 0U; +} + +static void CAN_EnterSafeFault(uint32_t reason_bits, bool desync_counter) +{ + uint32_t primask = __get_PRIMASK(); + + __disable_irq(); + + g_robot_ctrl.state = SYSTEM_SAFE_FAULT; + CAN_ResetMotionCommand(); + s_diag_bits |= reason_bits; + + if (desync_counter) + { + s_cmd_watchdog_ms = 0U; + s_counter_synced = false; + } + + __set_PRIMASK(primask); +} + +static System_Health_t CAN_GetCurrentHealth(uint32_t diag_bits, System_State_t state) +{ + if (((diag_bits & CHASSIS_DIAG_FATAL_MASK) != 0U) || (state == SYSTEM_SAFE_FAULT)) + { + return SYSTEM_HEALTH_FAULT; + } + + if (((diag_bits & CHASSIS_DIAG_WARN_MASK) != 0U) || + (state == SYSTEM_BOOTING) || + (s_consecutive_crc_errors != 0U) || + (s_consecutive_counter_errors != 0U)) + { + return SYSTEM_HEALTH_WARNING; + } + + return SYSTEM_HEALTH_OK; +} + +/* ===================================================================== + * 接收处理 + * ===================================================================== */ + +static void CAN_ProcessHeartbeat(const uint8_t *rx_data, uint8_t dlc) +{ + (void)rx_data; + (void)dlc; + + /* + * 心跳只表示“链路上仍然有人在说话”。 + * 它不会刷新运动看门狗，避免上位机停止发速度命令后底盘继续跑旧速度。 + */ +} + +static void CAN_ProcessControlFrame(const uint8_t *rx_data, uint8_t dlc) +{ + bool accept_frame = false; + uint8_t rx_cnt; + + if (dlc != 8U) + { + return; + } + + if (CAN_CRC8_SAE_J1850(rx_data, 7U) != rx_data[7]) + { + CAN_AtomicIncU32(&s_crc_error_total); + CAN_AtomicIncU8Saturated(&s_consecutive_crc_errors, 0xFFU); + + if (s_consecutive_crc_errors >= CRC_STORM_THRESHOLD) + { + CAN_EnterSafeFault(DIAG_CMD_CRC_STORM, true); + } + return; + } + + rx_cnt = rx_data[6]; + + if ((!s_counter_synced) || (g_robot_ctrl.state != SYSTEM_OPERATIONAL)) + { + accept_frame = true; + } + else + { + uint8_t diff = (uint8_t)(rx_cnt - s_last_rx_counter); + if ((diff >= 1U) && (diff <= MAX_ACCEPTED_COUNTER_GAP)) + { + accept_frame = true; + } + } + + if (!accept_frame) + { + CAN_AtomicIncU32(&s_counter_reject_total); + CAN_AtomicIncU8Saturated(&s_consecutive_counter_errors, 0xFFU); + + if (s_consecutive_counter_errors >= COUNTER_STORM_THRESHOLD) + { + CAN_EnterSafeFault(DIAG_CMD_CNT_STORM, true); + } + return; + } + + { + int16_t vx_scaled = CAN_Read_I16_LE(rx_data[0], rx_data[1]); + int16_t wz_scaled = CAN_Read_I16_LE(rx_data[2], rx_data[3]); + uint32_t primask = __get_PRIMASK(); + + __disable_irq(); + + g_robot_ctrl.target_vx = (float)vx_scaled / 1000.0f; + g_robot_ctrl.target_wz = (float)wz_scaled / 1000.0f; + g_robot_ctrl.ctrl_flags = rx_data[4]; + g_robot_ctrl.rolling_cnt = rx_cnt; + g_robot_ctrl.state = SYSTEM_OPERATIONAL; + + s_last_rx_counter = rx_cnt; + s_counter_synced = true; + s_cmd_watchdog_ms = CMD_TIMEOUT_MS; + s_cmd_age_10ms = 0U; + + /* 合法新帧到来后，允许通信相关与执行器相关的自动恢复。 */ + s_consecutive_crc_errors = 0U; + s_consecutive_counter_errors = 0U; + s_stall_ticks[MOTOR_FL] = 0U; + s_stall_ticks[MOTOR_RL] = 0U; + s_stall_ticks[MOTOR_FR] = 0U; + s_stall_ticks[MOTOR_RR] = 0U; + s_saturation_ticks = 0U; + + s_diag_bits &= ~(DIAG_COMM_TIMEOUT | + DIAG_CAN_BUS_OFF | + DIAG_CMD_CRC_STORM | + DIAG_CMD_CNT_STORM | + DIAG_MOTOR_FL_STALL | + DIAG_MOTOR_RL_STALL | + DIAG_MOTOR_FR_STALL | + DIAG_MOTOR_RR_STALL | + DIAG_CONTROL_SATURATION); + + __set_PRIMASK(primask); + } + + CAN_AtomicIncU32(&s_valid_cmd_total); +} + +/* ===================================================================== + * 初始化与中断回调 + * ===================================================================== */ + +void CAN_App_Init(void) +{ + uint32_t primask = __get_PRIMASK(); + + __disable_irq(); + + g_robot_ctrl.target_vx = 0.0f; + g_robot_ctrl.target_wz = 0.0f; + g_robot_ctrl.ctrl_flags = 0U; + g_robot_ctrl.rolling_cnt = 0U; + g_robot_ctrl.state = SYSTEM_BOOTING; + + s_cmd_watchdog_ms = 0U; + s_cmd_age_10ms = 0U; + s_last_rx_counter = 0U; + s_counter_synced = false; + + s_diag_bits = 0U; + s_valid_cmd_total = 0U; + s_crc_error_total = 0U; + s_counter_reject_total = 0U; + s_can_tx_drop_total = 0U; + s_busoff_total = 0U; + s_rx_overrun_total = 0U; + s_consecutive_crc_errors = 0U; + s_consecutive_counter_errors = 0U; + + memset(s_stall_ticks, 0, sizeof(s_stall_ticks)); + s_saturation_ticks = 0U; + s_status_tx_counter = 0U; + s_comm_diag_divider = 0U; + + __set_PRIMASK(primask); + + CAN_Config_StdDataFilter(0U, CAN_ID_HEARTBEAT); + CAN_Config_StdDataFilter(1U, CAN_ID_CMD_VEL); + + if (HAL_CAN_Start(&hcan1) != HAL_OK) + { + Error_Handler(); + } + + if (HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_ERROR_WARNING | + CAN_IT_ERROR_PASSIVE | CAN_IT_BUSOFF | CAN_IT_LAST_ERROR_CODE | + CAN_IT_ERROR) != HAL_OK) + { + Error_Handler(); + } +} + +void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) +{ + if (hcan != &hcan1) + { + return; + } + + while (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) > 0U) + { + CAN_RxHeaderTypeDef rx_header; + uint8_t rx_data[8]; + + if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data) != HAL_OK) + { + break; + } + + if ((rx_header.IDE != CAN_ID_STD) || (rx_header.RTR != CAN_RTR_DATA)) + { + continue; + } + + if (rx_header.StdId == CAN_ID_HEARTBEAT) + { + CAN_ProcessHeartbeat(rx_data, rx_header.DLC); + } + else if (rx_header.StdId == CAN_ID_CMD_VEL) + { + CAN_ProcessControlFrame(rx_data, rx_header.DLC); + } + else + { + /* 理论上硬件滤波器已拦掉其他 ID，这里只是双保险。 */ + } + } +} + +void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan) +{ + uint32_t error_code; + + if (hcan != &hcan1) + { + return; + } + + error_code = hcan->ErrorCode; + + if ((error_code & HAL_CAN_ERROR_BOF) != 0U) + { + CAN_AtomicIncU32(&s_busoff_total); + CAN_EnterSafeFault(DIAG_CAN_BUS_OFF, true); + } + +#ifdef HAL_CAN_ERROR_FOV0 + if ((error_code & HAL_CAN_ERROR_FOV0) != 0U) + { + CAN_AtomicIncU32(&s_rx_overrun_total); + } +#endif +} + +/* ===================================================================== + * 10ms 硬实时任务 + * ===================================================================== */ + +void CAN_Safety_Watchdog_Tick(void) +{ + uint32_t primask; + + if (s_cmd_age_10ms < 0xFFU) + { + 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); + } + + if (s_cmd_watchdog_ms > CONTROL_LOOP_PERIOD_MS) + { + primask = __get_PRIMASK(); + __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) + { + CAN_EnterSafeFault(DIAG_COMM_TIMEOUT, true); + } + else + { + /* BOOTING / SAFE_FAULT 期间不重复处理。 */ + } +} + +void Kinematics_Update_LADRC(void) +{ + float target_vx; + float target_wz; + System_State_t state; + float v_left; + float v_right; + float rpm_left; + float rpm_right; + uint32_t primask = __get_PRIMASK(); + + __disable_irq(); + target_vx = g_robot_ctrl.target_vx; + target_wz = g_robot_ctrl.target_wz; + state = g_robot_ctrl.state; + __set_PRIMASK(primask); + + if (state != SYSTEM_OPERATIONAL) + { + FourWheel_Set_Target_RPM(0.0f, 0.0f, 0.0f, 0.0f); + return; + } + + v_left = target_vx - target_wz * (WHEEL_TRACK_M * 0.5f); + v_right = target_vx + target_wz * (WHEEL_TRACK_M * 0.5f); + + rpm_left = (v_left / (2.0f * PI_F * WHEEL_RADIUS_M)) * 60.0f; + rpm_right = (v_right / (2.0f * PI_F * WHEEL_RADIUS_M)) * 60.0f; + + FourWheel_Set_Target_RPM(rpm_left, rpm_left, rpm_right, rpm_right); +} + +void Chassis_Diagnostics_10ms_Tick(void) +{ + uint32_t stall_bits = 0U; + System_State_t state = g_robot_ctrl.state; + uint32_t motor_stall_mask = (DIAG_MOTOR_FL_STALL | + DIAG_MOTOR_RL_STALL | + DIAG_MOTOR_FR_STALL | + DIAG_MOTOR_RR_STALL); + + if (state != SYSTEM_OPERATIONAL) + { + s_stall_ticks[MOTOR_FL] = 0U; + s_stall_ticks[MOTOR_RL] = 0U; + s_stall_ticks[MOTOR_FR] = 0U; + s_stall_ticks[MOTOR_RR] = 0U; + s_saturation_ticks = 0U; + CAN_ClearDiagBits(DIAG_CONTROL_SATURATION); + return; + } + + for (int i = 0; i < 4; ++i) + { + float target = CAN_AbsFloat(FourWheel_Get_Target_RPM((Motor_ID_t)i)); + float actual = CAN_AbsFloat(Get_Motor_RPM((Motor_ID_t)i)); + float output = CAN_AbsFloat(FourWheel_Get_Control_Output((Motor_ID_t)i)); + uint32_t bit = 0U; + + switch (i) + { + case MOTOR_FL: bit = DIAG_MOTOR_FL_STALL; break; + case MOTOR_RL: bit = DIAG_MOTOR_RL_STALL; break; + case MOTOR_FR: bit = DIAG_MOTOR_FR_STALL; break; + case MOTOR_RR: bit = DIAG_MOTOR_RR_STALL; break; + default: break; + } + + if ((target >= STALL_TARGET_RPM_MIN) && + (actual <= STALL_FEEDBACK_RPM_MAX) && + (output >= STALL_OUTPUT_ABS_MIN)) + { + if (s_stall_ticks[i] < STALL_CONFIRM_TICKS) + { + s_stall_ticks[i]++; + } + } + else + { + s_stall_ticks[i] = 0U; + CAN_ClearDiagBits(bit); + } + + if (s_stall_ticks[i] >= STALL_CONFIRM_TICKS) + { + stall_bits |= bit; + } + } + + if (stall_bits != 0U) + { + CAN_EnterSafeFault(stall_bits, false); + } + else + { + CAN_ClearDiagBits(motor_stall_mask); + } + + { + bool saturation_found = false; + + for (int i = 0; i < 4; ++i) + { + float target = CAN_AbsFloat(FourWheel_Get_Target_RPM((Motor_ID_t)i)); + float output = CAN_AbsFloat(FourWheel_Get_Control_Output((Motor_ID_t)i)); + + if ((target >= SATURATION_TARGET_RPM_MIN) && (output >= SATURATION_OUTPUT_ABS_MIN)) + { + saturation_found = true; + break; + } + } + + if (saturation_found) + { + if (s_saturation_ticks < SATURATION_CONFIRM_TICKS) + { + s_saturation_ticks++; + } + } + else + { + s_saturation_ticks = 0U; + CAN_ClearDiagBits(DIAG_CONTROL_SATURATION); + } + + if (s_saturation_ticks >= SATURATION_CONFIRM_TICKS) + { + CAN_SetDiagBits(DIAG_CONTROL_SATURATION); + } + } +} + +/* ===================================================================== + * 周期发送 + * ===================================================================== */ + +static HAL_StatusTypeDef CAN_TxStdFrame(uint16_t std_id, const uint8_t *payload, uint8_t dlc) +{ + CAN_TxHeaderTypeDef tx_header; + uint32_t tx_mailbox; + + if ((payload == NULL) || (dlc > 8U)) + { + CAN_AtomicIncU32(&s_can_tx_drop_total); + return HAL_ERROR; + } + + tx_header.StdId = std_id; + tx_header.IDE = CAN_ID_STD; + tx_header.RTR = CAN_RTR_DATA; + tx_header.DLC = dlc; + tx_header.TransmitGlobalTime = DISABLE; + + /* Telemetry 帧允许丢：为避免阻塞主循环（引入 20ms 抖动/漂移），这里不做忙等。 */ + if (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 0U) + { + CAN_AtomicIncU32(&s_can_tx_drop_total); + return HAL_BUSY; + } + +if (HAL_CAN_AddTxMessage(&hcan1, &tx_header, (uint8_t *)payload, &tx_mailbox) != HAL_OK) + { + CAN_AtomicIncU32(&s_can_tx_drop_total); + return HAL_ERROR; + } + + return HAL_OK; +} + +void Chassis_Get_StatusSnapshot(Chassis_StatusSnapshot_t *snapshot) +{ + uint32_t primask; + + if (snapshot == NULL) + { + return; + } + + primask = __get_PRIMASK(); + __disable_irq(); + + snapshot->state = g_robot_ctrl.state; + snapshot->diag_bits = s_diag_bits; + snapshot->cmd_age_10ms = s_cmd_age_10ms; + snapshot->last_cmd_counter = s_last_rx_counter; + snapshot->health = CAN_GetCurrentHealth(snapshot->diag_bits, snapshot->state); + + __set_PRIMASK(primask); +} + +static void CAN_Send_StatusFrame(void) +{ + Chassis_StatusSnapshot_t snapshot; + uint8_t tx_data[8]; + + Chassis_Get_StatusSnapshot(&snapshot); + + s_status_tx_counter++; + + tx_data[0] = (uint8_t)snapshot.state; + tx_data[1] = (uint8_t)snapshot.health; + tx_data[2] = (uint8_t)(snapshot.diag_bits & 0xFFU); + tx_data[3] = (uint8_t)((snapshot.diag_bits >> 8) & 0xFFU); + tx_data[4] = (uint8_t)((snapshot.diag_bits >> 16) & 0xFFU); + tx_data[5] = (uint8_t)((snapshot.diag_bits >> 24) & 0xFFU); + tx_data[6] = snapshot.cmd_age_10ms; + tx_data[7] = s_status_tx_counter; + + (void)CAN_TxStdFrame(CAN_ID_STATUS, tx_data, 8U); +} + +static void CAN_Send_ActualRpmFrame(void) +{ + uint8_t tx_data[8]; + + CAN_Write_I16_LE(&tx_data[0], CAN_SaturateFloatToI16(Get_Motor_RPM(MOTOR_FL))); + CAN_Write_I16_LE(&tx_data[2], CAN_SaturateFloatToI16(Get_Motor_RPM(MOTOR_RL))); + CAN_Write_I16_LE(&tx_data[4], CAN_SaturateFloatToI16(Get_Motor_RPM(MOTOR_FR))); + CAN_Write_I16_LE(&tx_data[6], CAN_SaturateFloatToI16(Get_Motor_RPM(MOTOR_RR))); + + (void)CAN_TxStdFrame(CAN_ID_ACTUAL_RPM, tx_data, 8U); +} + +static void CAN_Send_TargetRpmFrame(void) +{ + uint8_t tx_data[8]; + + CAN_Write_I16_LE(&tx_data[0], CAN_SaturateFloatToI16(FourWheel_Get_Target_RPM(MOTOR_FL))); + CAN_Write_I16_LE(&tx_data[2], CAN_SaturateFloatToI16(FourWheel_Get_Target_RPM(MOTOR_RL))); + CAN_Write_I16_LE(&tx_data[4], CAN_SaturateFloatToI16(FourWheel_Get_Target_RPM(MOTOR_FR))); + CAN_Write_I16_LE(&tx_data[6], CAN_SaturateFloatToI16(FourWheel_Get_Target_RPM(MOTOR_RR))); + + (void)CAN_TxStdFrame(CAN_ID_TARGET_RPM, tx_data, 8U); +} + +static void CAN_Send_CommDiagFrame(void) +{ + uint8_t tx_data[8]; + + tx_data[0] = (uint8_t)(s_valid_cmd_total & 0xFFU); + tx_data[1] = (uint8_t)(s_crc_error_total & 0xFFU); + tx_data[2] = (uint8_t)(s_counter_reject_total & 0xFFU); + tx_data[3] = (uint8_t)(s_can_tx_drop_total & 0xFFU); + tx_data[4] = (uint8_t)(s_busoff_total & 0xFFU); + tx_data[5] = (uint8_t)(s_rx_overrun_total & 0xFFU); + tx_data[6] = s_last_rx_counter; + tx_data[7] = (uint8_t)(((s_consecutive_counter_errors > 15U ? 15U : s_consecutive_counter_errors) << 4) | + (s_consecutive_crc_errors > 15U ? 15U : s_consecutive_crc_errors)); + + (void)CAN_TxStdFrame(CAN_ID_COMM_DIAG, tx_data, 8U); +} + +static void CAN_Send_OdomFrame(void) +{ + uint8_t tx_data[8]; + int16_t d_fl; + int16_t d_rl; + int16_t d_fr; + int16_t d_rr; + + Motor_Get_And_Clear_Delta_Ticks(&d_fl, &d_rl, &d_fr, &d_rr); + + CAN_Write_I16_LE(&tx_data[0], d_fl); + CAN_Write_I16_LE(&tx_data[2], d_rl); + CAN_Write_I16_LE(&tx_data[4], d_fr); + CAN_Write_I16_LE(&tx_data[6], d_rr); + + (void)CAN_TxStdFrame(CAN_ID_ODOM, tx_data, 8U); +} + +void CAN_Send_Telemetry_20ms(void) +{ + static uint8_t s_telem_slot = 0U; + + /* 状态帧最重要，固定每 20ms 发一次 */ + CAN_Send_StatusFrame(); + + /* 第二帧轮换发送，避免同一拍挤爆 3 个 TX mailbox */ + switch (s_telem_slot) + { + case 0: + CAN_Send_ActualRpmFrame(); /* 0x182 */ + break; + + case 1: + CAN_Send_TargetRpmFrame(); /* 0x183 */ + break; + + case 2: + CAN_Send_OdomFrame(); /* 0x200 */ + break; + + default: + s_telem_slot = 0U; + CAN_Send_ActualRpmFrame(); + break; + } + + s_telem_slot++; + if (s_telem_slot >= 3U) + { + s_telem_slot = 0U; + } + + /* 通信诊断帧保持 100ms 一次 */ + s_comm_diag_divider++; + if (s_comm_diag_divider >= TELEMETRY_COMM_DIAG_DIV) + { + s_comm_diag_divider = 0U; + CAN_Send_CommDiagFrame(); /* 0x184 */ + } +} diff --git a/Core/Src/ladrc.c b/Core/Src/ladrc.c index 026a405..71c93fa 100644 --- a/Core/Src/ladrc.c +++ b/Core/Src/ladrc.c @@ -1,76 +1,155 @@ #include "ladrc.h" -/** - * @brief 初始化单体 LADRC 参数 +/* + * 这个文件实现两层内容： + * 1) 单个电机的 LADRC 控制器。 + * 2) 四轮底盘的调度层（读取编码器 -> 滤波 -> LADRC -> PWM 输出）。 + * + * 额外升级点： + * - 提供目标 RPM / 控制输出 getter，供上层故障诊断与状态上报使用。 */ -void LADRC_Init(LADRC_TypeDef *ladrc, float wc, float wo, float b0, float h, float max) { - // 状态初始化 - ladrc->v = 0.0f; - ladrc->y = 0.0f; - ladrc->z1 = 0.0f; - ladrc->z2 = 0.0f; - ladrc->u = 0.0f; - - // 参数赋值 + +#define LADRC_CONTROL_DT_S 0.01f +#define LADRC_RPM_FILTER_ALPHA 0.30f + +#define LADRC_DEFAULT_WC 25.0f +#define LADRC_DEFAULT_WO 60.0f +#define LADRC_DEFAULT_B0 0.20f +#define LADRC_DEFAULT_OUT_MAX 1000.0f + +/* ================== 内部辅助函数 ================== */ +static float LADRC_Abs(float x); +static float LADRC_Clamp(float x, float min_value, float max_value); + +static float LADRC_Abs(float x) +{ + return (x >= 0.0f) ? x : -x; +} + +static float LADRC_Clamp(float x, float min_value, float max_value) +{ + if (x > max_value) + { + return max_value; + } + if (x < min_value) + { + return min_value; + } + return x; +} + +/** + * @brief 初始化单个 LADRC 控制器。 + * @note 顺手做基础参数保护，避免 b0/h/out_max 为 0 或负值时出问题。 + */ +void LADRC_Init(LADRC_TypeDef *ladrc, float wc, float wo, float b0, float h, float max) +{ + if (ladrc == NULL) + { + return; + } + + if (h <= 0.0f) + { + h = LADRC_CONTROL_DT_S; + } + if (LADRC_Abs(b0) < 1e-6f) + { + b0 = LADRC_DEFAULT_B0; + } + if (wc < 0.0f) + { + wc = -wc; + } + if (wo < 0.0f) + { + wo = -wo; + } + if (max <= 0.0f) + { + max = LADRC_DEFAULT_OUT_MAX; + } + + ladrc->v = 0.0f; /* 目标值 */ + ladrc->y = 0.0f; /* 测量值 */ + ladrc->z1 = 0.0f; /* 状态观测量 */ + ladrc->z2 = 0.0f; /* 总扰动观测量 */ + ladrc->u = 0.0f; /* 上一拍控制输出 */ + ladrc->wc = wc; ladrc->wo = wo; ladrc->b0 = b0; ladrc->h = h; ladrc->out_max = max; - - // 计算观测器增益 + ladrc->beta1 = 2.0f * wo; ladrc->beta2 = wo * wo; } /** - * @brief 单体 LADRC 核心运算 + * @brief 单个 LADRC 周期运算。 + * @param ladrc 控制器实例 + * @param actual_val 当前测量速度（RPM） + * @retval 限幅后的控制输出（通常可直接映射到 PWM） */ -float LADRC_Calc(LADRC_TypeDef *ladrc, float actual_val) { +float LADRC_Calc(LADRC_TypeDef *ladrc, float actual_val) +{ + float e; + float u0; + float out; + + if (ladrc == NULL) + { + return 0.0f; + } + ladrc->y = actual_val; - - // 第一部分：LESO - float e = ladrc->y - ladrc->z1; + + /* 第 1 部分：LESO（线性扩张状态观测器） */ + e = ladrc->y - ladrc->z1; ladrc->z1 += (ladrc->z2 + ladrc->b0 * ladrc->u + ladrc->beta1 * e) * ladrc->h; ladrc->z2 += (ladrc->beta2 * e) * ladrc->h; - - // 第二部分：LSEF & 扰动补偿 - float u0 = ladrc->wc * (ladrc->v - ladrc->z1); - float out = (u0 - ladrc->z2) / ladrc->b0; - - // 第三部分：输出限幅保护 - if (out > ladrc->out_max) out = ladrc->out_max; - if (out < -ladrc->out_max) out = -ladrc->out_max; - - ladrc->u = out; - + + /* 第 2 部分：LSEF + 扰动补偿 */ + u0 = ladrc->wc * (ladrc->v - ladrc->z1); + out = (u0 - ladrc->z2) / ladrc->b0; + + /* 第 3 部分：输出限幅，保护电机和驱动 */ + out = LADRC_Clamp(out, -ladrc->out_max, ladrc->out_max); + + ladrc->u = out; return out; } - /* ===================================================================== - * 针对四轮底盘的 LADRC 扩展调度层 + * 针对四轮底盘的 LADRC 调度层 * ===================================================================== */ -// 实例化 4 个控制器的实体 LADRC_TypeDef ladrc_motors[4]; /** - * @brief 一键初始化底层硬件与四个 LADRC + * @brief 一键初始化底层硬件和四个 LADRC 控制器。 */ void FourWheel_LADRC_Init(void) { - // 1. 唤醒底层电机与编码器硬件 + int i; + Motor_Init(); - // 2. 初始化 4 个控制器的参数 (注意这里的限幅是 1000，匹配底层的 PWM_LIMIT) - for(int i = 0; i < 4; i++) { - LADRC_Init(&ladrc_motors[i], 25.0f, 60.0f, 0.2f, 0.01f, 1000.0f); + for (i = 0; i < 4; ++i) + { + LADRC_Init(&ladrc_motors[i], + LADRC_DEFAULT_WC, + LADRC_DEFAULT_WO, + LADRC_DEFAULT_B0, + LADRC_CONTROL_DT_S, + LADRC_DEFAULT_OUT_MAX); } } /** - * @brief 一键下发四个轮子的目标转速 + * @brief 设置四个轮子的目标 RPM。 */ void FourWheel_Set_Target_RPM(float fl_rpm, float rl_rpm, float fr_rpm, float rr_rpm) { @@ -81,29 +160,57 @@ void FourWheel_Set_Target_RPM(float fl_rpm, float rl_rpm, float fr_rpm, float rr } /** - * @brief 四轮闭环大循环 (强烈建议放在 10ms 的定时器中断内执行) + * @brief 四轮 LADRC 大循环，建议固定 10ms 调用一次。 + * @note 调度顺序： + * 1) 更新编码器测速 + * 2) 一阶低通滤波，减小测速毛刺 + * 3) 每个轮子独立做 LADRC + * 4) 输出到 H 桥 */ void FourWheel_LADRC_Control_Loop(void) { - Motor_Update_RPM(0.01f); - - // 静态数组，用来记住上一次的平滑速度 static float filtered_rpm[4] = {0.0f, 0.0f, 0.0f, 0.0f}; - // 滤波系数 alpha (0.0 ~ 1.0)。越小越平滑，越大越敏捷。 - // 0.3 的意思是：相信 30% 的最新数据，保留 70% 的历史数据 - const float alpha = 0.3f; + int i; - for(int i = 0; i < 4; i++) + Motor_Update_RPM(LADRC_CONTROL_DT_S); + + for (i = 0; i < 4; ++i) { - // 1. 拿到底层的原始狂野转速 float raw_rpm = Get_Motor_RPM((Motor_ID_t)i); + float pwm_out; - // 2. 软件减震器：一阶低通滤波！ - filtered_rpm[i] = (1.0f - alpha) * filtered_rpm[i] + alpha * raw_rpm; - - // 3. 把过滤掉毛刺、极其顺滑的速度扔给 LADRC - float pwm_out = LADRC_Calc(&ladrc_motors[i], filtered_rpm[i]); + filtered_rpm[i] = (1.0f - LADRC_RPM_FILTER_ALPHA) * filtered_rpm[i] + + LADRC_RPM_FILTER_ALPHA * raw_rpm; + pwm_out = LADRC_Calc(&ladrc_motors[i], filtered_rpm[i]); Set_Motor_Output((Motor_ID_t)i, (int16_t)pwm_out); } -} \ No newline at end of file +} + +/** + * @brief 读取指定轮子的“当前目标 RPM”。 + * @note 该接口主要供上层做状态上报与故障诊断使用。 + */ +float FourWheel_Get_Target_RPM(Motor_ID_t id) +{ + if ((id < MOTOR_FL) || (id > MOTOR_RR)) + { + return 0.0f; + } + + return ladrc_motors[id].v; +} + +/** + * @brief 读取指定轮子的“当前控制输出”。 + * @note 该值尚未经过电机左右镜像翻转，但足够用于判断是否长期顶满。 + */ +float FourWheel_Get_Control_Output(Motor_ID_t id) +{ + if ((id < MOTOR_FL) || (id > MOTOR_RR)) + { + return 0.0f; + } + + return ladrc_motors[id].u; +} diff --git a/Core/Src/main.c b/Core/Src/main.c index 83c2d8a..8c8d16e 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -1,4 +1,5 @@ /* USER CODE BEGIN Header */ + #include #include "ladrc.h" #include "usbd_cdc_if.h" // USB 虚拟串口发送头文件 @@ -41,7 +42,7 @@ /* Private define ------------------------------------------------------------*/ /* USER CODE BEGIN PD */ - +#define TELEMETRY_TX_PERIOD_MS 20U /* USER CODE END PD */ /* Private macro -------------------------------------------------------------*/ @@ -52,10 +53,7 @@ /* Private variables ---------------------------------------------------------*/ /* USER CODE BEGIN PV */ -/* USER CODE BEGIN PV */ -CAN_RxHeaderTypeDef RxHeader; -uint8_t RxData[8]; -volatile uint8_t can_rx_flag = 0; // 告诉主程序“有数据来了”的标志位 +extern USBD_HandleTypeDef hUsbDeviceFS; /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------*/ @@ -66,24 +64,34 @@ void SystemClock_Config(void); /* Private user code ---------------------------------------------------------*/ /* USER CODE BEGIN 0 */ -extern USBD_HandleTypeDef hUsbDeviceFS; // 引入 USB 状态句柄 +int _write(int file, char *ptr, int len) +{ + uint32_t start_tick; + uint8_t result; -int _write(int file, char *ptr, int len) { - // 1. 致命拦截：如果电脑压根没连上 USB，直接丢弃数据，坚决不死等！ - if (hUsbDeviceFS.dev_state != USBD_STATE_CONFIGURED) { - return len; + (void)file; + + if ((ptr == NULL) || (len <= 0)) + { + return 0; } - uint8_t result = CDC_Transmit_FS((uint8_t*)ptr, len); - uint32_t timeout = 0; - - // 2. 极短超时：就算 USB 连着但突然卡了，最多循环 5000 次就强行放弃，保命要紧 - while(result == USBD_BUSY && timeout < 5000) { - timeout++; - result = CDC_Transmit_FS((uint8_t*)ptr, len); + if (hUsbDeviceFS.dev_state != USBD_STATE_CONFIGURED) + { + return 0; } - return len; + start_tick = HAL_GetTick(); + do + { + result = CDC_Transmit_FS((uint8_t *)ptr, (uint16_t)len); + if (result == USBD_OK) + { + return len; + } + } while ((result == USBD_BUSY) && ((HAL_GetTick() - start_tick) < 2U)); + + return 0; } /* USER CODE END 0 */ @@ -128,28 +136,14 @@ int main(void) MX_CAN1_Init(); MX_UART4_Init(); /* USER CODE BEGIN 2 */ -// 1. 结构体一定要初始化为 0！ - CAN_FilterTypeDef canFilterConfig = {0}; + FourWheel_LADRC_Init(); + CAN_App_Init(); - canFilterConfig.FilterBank = 0; - canFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; - canFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; - canFilterConfig.FilterIdHigh = 0x0000; - canFilterConfig.FilterIdLow = 0x0000; - canFilterConfig.FilterMaskIdHigh = 0x0000; - canFilterConfig.FilterMaskIdLow = 0x0000; - canFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0; - canFilterConfig.FilterActivation = ENABLE; - - // 2. 加上这极其关键的一句，把 0-13 号过滤器分给 CAN1 - canFilterConfig.SlaveStartFilterBank = 14; - - HAL_StatusTypeDef fs = HAL_CAN_ConfigFilter(&hcan1, &canFilterConfig); - HAL_StatusTypeDef ss = HAL_CAN_Start(&hcan1); - HAL_StatusTypeDef ns = HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING); - - HAL_Delay(1000); - printf("Filter: %d, Start: %d, Notify: %d\r\n", fs, ss, ns); + /* 启动 10ms 硬实时控制节拍。 */ + if (HAL_TIM_Base_Start_IT(&htim6) != HAL_OK) + { + Error_Handler(); + } /* USER CODE END 2 */ /* Infinite loop */ @@ -160,31 +154,42 @@ int main(void) /* USER CODE END WHILE */ /* USER CODE BEGIN 3 */ - // ========================================== - // 强制打印 1：每隔 1 秒无条件打印一次（心跳包） - // ========================================== + /* + * 20ms 周期发送的“无漂移（driftless）”调度器： + * + * 之前版本用 last = now 的写法： + * if (now - last >= period) { last = now; ... } + * 在主循环存在抖动/阻塞时会把“相位”带着一起漂移，表现为 20ms 周期不再对齐。 + * + * 这里改成 next_deadline += period 的写法，让发送时刻尽量锁在固定相位上； + * 若主循环被阻塞太久（落后 >= 1 个周期），则做一次“软重同步”，避免一口气补发多次造成总线突发。 + */ + static uint32_t next_telemetry_deadline = 0U; + uint32_t now = HAL_GetTick(); - // ========================================== - // 强制打印 2：原来的 CAN 接收处理逻辑 - // ========================================== - uint32_t fifo_pending = HAL_CAN_GetRxFifoFillLevel(&hcan1, CAN_RX_FIFO0); - if (fifo_pending > 0) { - printf("FIFO has %lu messages\r\n", fifo_pending); + if (next_telemetry_deadline == 0U) + { + next_telemetry_deadline = now + TELEMETRY_TX_PERIOD_MS; } - if (can_rx_flag == 1) { - can_rx_flag = 0; // 赶紧先把标志位清零 + if ((int32_t)(now - next_telemetry_deadline) >= 0) + { + next_telemetry_deadline += TELEMETRY_TX_PERIOD_MS; + CAN_Send_Telemetry_20ms(); - // 💡 修改这里：用一个数组把要发的话拼起来，只调用一次 printf！ - // 这能彻底防止 168MHz 的 CPU 把 USB 虚拟串口瞬间撑爆死机 - char usb_buf[100]; - sprintf(usb_buf, "!!! BINGO !!! Got CAN MSG! ID:0x%03lX Data: %02X %02X %02X %02X %02X %02X %02X %02X\r\n", - RxHeader.StdId, - RxData[0], RxData[1], RxData[2], RxData[3], - RxData[4], RxData[5], RxData[6], RxData[7]); - - printf("%s", usb_buf); + /* 如果主循环太慢导致仍然落后（>= 1 周期），重同步，避免连发突刺。 */ + if ((int32_t)(now - next_telemetry_deadline) >= 0) + { + next_telemetry_deadline = now + TELEMETRY_TX_PERIOD_MS; + /* 可选：在这里置一个诊断位，提示“Telemetry Late”。 */ + /* CAN_Report_TelemetryLate(); */ + } } + + /* + * 主循环保持轻量。 + * 如果后续要加 LED、串口 shell、参数配置等慢任务，尽量都放这里。 + */ /* USER CODE END WHILE */ } /* USER CODE END 3 */ @@ -236,17 +241,23 @@ void SystemClock_Config(void) } /* USER CODE BEGIN 4 */ -void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) +/** + * @brief 定时器周期中断回调。 + * @note TIM6 是整个底盘控制的“硬实时心跳”。 + * 固定顺序： + * 1) 控制命令看门狗 + * 2) 差速逆解算 + * 3) 四轮 LADRC 闭环 + * 4) 基于最新反馈做故障诊断 + */ +void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { - if (hcan->Instance == CAN1) { - // 翻转 LED 看看有没有进中断 - HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_2); - - if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, RxData) == HAL_OK) { - can_rx_flag = 1; - // 再翻转一次，确认消息取出成功 - HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_2); - } + if (htim->Instance == TIM6) + { + CAN_Safety_Watchdog_Tick(); + Kinematics_Update_LADRC(); + FourWheel_LADRC_Control_Loop(); + Chassis_Diagnostics_10ms_Tick(); } } /* USER CODE END 4 */ diff --git a/Core/Src/motor.c b/Core/Src/motor.c index ff21c3e..5eebd31 100644 --- a/Core/Src/motor.c +++ b/Core/Src/motor.c @@ -1,200 +1,330 @@ #include "motor.h" -#include "tim.h" // 必须包含此文件以使用 htim1, htim2 等外部句柄 + +#include +#include + +#include "tim.h" + +/* + * 这个文件负责两件事： + * 1) 把 LADRC 输出的有符号控制量，转换成 H 桥两路 PWM + * 2) 读取四路编码器，得到实时 RPM，并累加成供 CAN 上传的里程计增量 + * + * 约定： + * - 上层统一把“车体前进”视为正方向 + * - 由于左右电机安装方向镜像，左侧电机的硬件方向需要在底层翻转 + */ /* ================== 全局静态变量 ================== */ -// 存储四个电机上一次的定时器计数值 -static uint16_t last_count[4] = {0, 0, 0, 0}; -// 存储四个电机的实时转速 (RPM) -static float current_rpm[4] = {0.0f, 0.0f, 0.0f, 0.0f}; -// 存储供 CAN 上传的里程计脉冲累加值 -static int16_t can_delta_ticks[4] = {0, 0, 0, 0}; -/* ================== 函 数 实 现 ================== */ +static uint16_t s_last_count[4] = {0U, 0U, 0U, 0U}; +static volatile float s_current_rpm[4] = {0.0f, 0.0f, 0.0f, 0.0f}; +static volatile int32_t s_odom_acc_ticks[4] = {0, 0, 0, 0}; + +/* ================== 内部辅助函数声明 ================== */ +static inline void Motor_WriteBridge(TIM_HandleTypeDef *htim, + uint32_t channel_in1, + uint32_t channel_in2, + uint16_t pwm_in1, + uint16_t pwm_in2); +static void Motor_PrimeEncoderCounters(void); +static uint16_t Motor_ClampAbsToPwm(int32_t value); +static int16_t Motor_SaturateToI16(int32_t value); /** - * @brief 启动所有的底层定时器通道 + * @brief 给 H 桥两路输入分别写入 PWM。 + * @note AT8236 方向约定： + * - 正转：IN1 = PWM，IN2 = 0 + * - 反转：IN1 = 0， IN2 = PWM + */ +static inline void Motor_WriteBridge(TIM_HandleTypeDef *htim, + uint32_t channel_in1, + uint32_t channel_in2, + uint16_t pwm_in1, + uint16_t pwm_in2) +{ + __HAL_TIM_SET_COMPARE(htim, channel_in1, pwm_in1); + __HAL_TIM_SET_COMPARE(htim, channel_in2, pwm_in2); +} + +/** + * @brief 把任意有符号数限幅并取绝对值，转换成 PWM 占空比。 + */ +static uint16_t Motor_ClampAbsToPwm(int32_t value) +{ + if (value > PWM_LIMIT) + { + value = PWM_LIMIT; + } + else if (value < -PWM_LIMIT) + { + value = -PWM_LIMIT; + } + + if (value < 0) + { + value = -value; + } + + return (uint16_t)value; +} + +/** + * @brief int32 饱和到 int16，防止 CAN 打包时溢出回绕。 + */ +static int16_t Motor_SaturateToI16(int32_t value) +{ + if (value > 32767) + { + return 32767; + } + if (value < -32768) + { + return -32768; + } + return (int16_t)value; +} + +/** + * @brief 在编码器启动后，用当前计数值作为初值。 + * @note 这样可以避免“last_count 初始为 0”导致首个控制周期出现巨大假速度。 + */ +static void Motor_PrimeEncoderCounters(void) +{ + s_last_count[MOTOR_FL] = (uint16_t)__HAL_TIM_GET_COUNTER(&htim5); + s_last_count[MOTOR_RL] = (uint16_t)__HAL_TIM_GET_COUNTER(&htim3); + s_last_count[MOTOR_FR] = (uint16_t)__HAL_TIM_GET_COUNTER(&htim4); + s_last_count[MOTOR_RR] = (uint16_t)__HAL_TIM_GET_COUNTER(&htim1); +} + +/** + * @brief 启动所有 PWM 和编码器接口。 */ void Motor_Init(void) { - // 1. 启动左侧电机 PWM (FL使用TIM2, RL使用TIM9) - HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); // FL_IN1 (PA2) - HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); // FL_IN2 (PA3) - HAL_TIM_PWM_Start(&htim9, TIM_CHANNEL_1); // RL_IN1 (PE5) - HAL_TIM_PWM_Start(&htim9, TIM_CHANNEL_2); // RL_IN2 (PE6) + uint32_t primask = __get_PRIMASK(); - // 2. 启动右侧电机 PWM (全部使用TIM8) - HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1); // FR_IN1 (PC6) - HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_2); // FR_IN2 (PC7) - HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_3); // RR_IN1 (PC8) - HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_4); // RR_IN2 (PC9) + if (HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3) != HAL_OK) { Error_Handler(); } + if (HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4) != HAL_OK) { Error_Handler(); } + if (HAL_TIM_PWM_Start(&htim9, TIM_CHANNEL_1) != HAL_OK) { Error_Handler(); } + if (HAL_TIM_PWM_Start(&htim9, TIM_CHANNEL_2) != HAL_OK) { Error_Handler(); } + if (HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1) != HAL_OK) { Error_Handler(); } + if (HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_2) != HAL_OK) { Error_Handler(); } + if (HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_3) != HAL_OK) { Error_Handler(); } + if (HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_4) != HAL_OK) { Error_Handler(); } - // 3. 启动所有编码器接口 - HAL_TIM_Encoder_Start(&htim5, TIM_CHANNEL_ALL); // 左上 FL (PA0, PA1) - HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); // 左下 RL (PA6, PA7) - HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL); // 右上 FR (PD12, PD13) - HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_ALL); // 右下 RR (PE9, PE11) + if (HAL_TIM_Encoder_Start(&htim5, TIM_CHANNEL_ALL) != HAL_OK) { Error_Handler(); } + if (HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL) != HAL_OK) { Error_Handler(); } + if (HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL) != HAL_OK) { Error_Handler(); } + if (HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_ALL) != HAL_OK) { Error_Handler(); } + + /* 启动完成后，把输出和测速状态清零。 */ + Motor_Brake_All(); + Motor_PrimeEncoderCounters(); + + __disable_irq(); + s_current_rpm[MOTOR_FL] = 0.0f; + s_current_rpm[MOTOR_RL] = 0.0f; + s_current_rpm[MOTOR_FR] = 0.0f; + s_current_rpm[MOTOR_RR] = 0.0f; + s_odom_acc_ticks[MOTOR_FL] = 0; + s_odom_acc_ticks[MOTOR_RL] = 0; + s_odom_acc_ticks[MOTOR_FR] = 0; + s_odom_acc_ticks[MOTOR_RR] = 0; + __set_PRIMASK(primask); } /** - * @brief 将有符号的控制量转化为 AT8236 的双路输入占空比 - * @param id: 电机编号 (MOTOR_FL, MOTOR_RL, MOTOR_FR, MOTOR_RR) - * @param control_out: 闭环算法的输出控制量 (-PWM_LIMIT ~ +PWM_LIMIT) + * @brief 把有符号控制量转换为电机驱动输出。 + * @param id 电机编号 + * @param control_out 上层控制量，正值表示“希望车体前进” + * @note + * 1) 这里先按“车体坐标系”理解 control_out。 + * 2) 左侧电机由于安装镜像，硬件方向要翻转一次。 + * 3) 变量 forward 的语义始终保持一致： + * true -> IN1 = PWM, IN2 = 0 + * false -> IN1 = 0, IN2 = PWM */ void Set_Motor_Output(Motor_ID_t id, int16_t control_out) { - /* ====================================================================== - * 【物理镜像校准 - 输出层】 - * 现象：右侧电机正转（顺时针）时车体向前；左侧电机必须反转（逆时针）车体才向前。 - * 处理：当上层算法要求车体"向前"（即 control_out 为正）时， - * 我们需要在底层强行把左侧电机的指令反相，让它驱动电机逆时针转。 - * ====================================================================== */ - if (id == MOTOR_FL || id == MOTOR_RL) { - control_out = -control_out; - } + int32_t hw_command = (int32_t)control_out; + bool forward; + uint16_t pwm_val; - // 1. 限幅保护（防止算法计算溢出导致小车失控疯狂加速） - if(control_out > PWM_LIMIT) control_out = PWM_LIMIT; - if(control_out < -PWM_LIMIT) control_out = -PWM_LIMIT; - - // 2. 解析正反转与实际需要设置的 PWM 数值 - // AT8236 逻辑: 正转 -> IN1 = PWM, IN2 = 0 ; 反转 -> IN1 = 0, IN2 = PWM - uint16_t pwm_val = 0; - uint8_t dir = 0; // 1为正转，0为反转 - - if(control_out >= 0) { - dir = 0; - pwm_val = control_out; - } else { - dir = 1; - pwm_val = -control_out; // 取绝对值 - } - - // 3. 执行输出 (使用直接写寄存器宏，高频调用效率极高) - switch(id) + /* 左侧硬件方向镜像：同样的“车体前进”命令，左轮需要反向转。 */ + if ((id == MOTOR_FL) || (id == MOTOR_RL)) { - case MOTOR_FL: // 左上: TIM2_CH3, TIM2_CH4 - if(dir) { - __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, pwm_val); - __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, 0); - } else { - __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, 0); - __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, pwm_val); + hw_command = -hw_command; + } + + forward = (hw_command >= 0); + pwm_val = Motor_ClampAbsToPwm(hw_command); + + switch (id) + { + case MOTOR_FL: + if (forward) + { + Motor_WriteBridge(&htim2, TIM_CHANNEL_3, TIM_CHANNEL_4, pwm_val, 0U); + } + else + { + Motor_WriteBridge(&htim2, TIM_CHANNEL_3, TIM_CHANNEL_4, 0U, pwm_val); } break; - case MOTOR_RL: // 左下: TIM9_CH1, TIM9_CH2 - if(dir) { - __HAL_TIM_SET_COMPARE(&htim9, TIM_CHANNEL_1, pwm_val); - __HAL_TIM_SET_COMPARE(&htim9, TIM_CHANNEL_2, 0); - } else { - __HAL_TIM_SET_COMPARE(&htim9, TIM_CHANNEL_1, 0); - __HAL_TIM_SET_COMPARE(&htim9, TIM_CHANNEL_2, pwm_val); + case MOTOR_RL: + if (forward) + { + Motor_WriteBridge(&htim9, TIM_CHANNEL_1, TIM_CHANNEL_2, pwm_val, 0U); + } + else + { + Motor_WriteBridge(&htim9, TIM_CHANNEL_1, TIM_CHANNEL_2, 0U, pwm_val); } break; - case MOTOR_FR: // 右上: TIM8_CH1, TIM8_CH2 - if(dir) { - __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_1, pwm_val); - __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_2, 0); - } else { - __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_1, 0); - __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_2, pwm_val); + case MOTOR_FR: + if (forward) + { + Motor_WriteBridge(&htim8, TIM_CHANNEL_1, TIM_CHANNEL_2, pwm_val, 0U); + } + else + { + Motor_WriteBridge(&htim8, TIM_CHANNEL_1, TIM_CHANNEL_2, 0U, pwm_val); } break; - case MOTOR_RR: // 右下: TIM8_CH3, TIM8_CH4 - if(dir) { - __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_3, pwm_val); - __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_4, 0); - } else { - __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_3, 0); - __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_4, pwm_val); + case MOTOR_RR: + if (forward) + { + Motor_WriteBridge(&htim8, TIM_CHANNEL_3, TIM_CHANNEL_4, pwm_val, 0U); } + else + { + Motor_WriteBridge(&htim8, TIM_CHANNEL_3, TIM_CHANNEL_4, 0U, pwm_val); + } + break; + + default: break; } } /** - * @brief 紧急制动所有电机 + * @brief 停止四个电机输出。 + * @note 这里使用“两个方向脚都置 0”的空转停车方式。 + * 如果你的驱动板支持真正的短刹车，并且你确认接线与驱动逻辑， + * 可以单独再加一个 Brake 模式。 */ void Motor_Brake_All(void) { - Set_Motor_Output(MOTOR_FL, 0); - Set_Motor_Output(MOTOR_RL, 0); - Set_Motor_Output(MOTOR_FR, 0); - Set_Motor_Output(MOTOR_RR, 0); + Motor_WriteBridge(&htim2, TIM_CHANNEL_3, TIM_CHANNEL_4, 0U, 0U); + Motor_WriteBridge(&htim9, TIM_CHANNEL_1, TIM_CHANNEL_2, 0U, 0U); + Motor_WriteBridge(&htim8, TIM_CHANNEL_1, TIM_CHANNEL_2, 0U, 0U); + Motor_WriteBridge(&htim8, TIM_CHANNEL_3, TIM_CHANNEL_4, 0U, 0U); } /** - * @brief 读取编码器脉冲并计算实时转速 (RPM) - * @param dt_s: 两次调用此函数的时间间隔 (单位：秒) + * @brief 读取编码器脉冲并更新实时转速。 + * @param dt_s 控制周期，单位秒，例如 10ms 就传 0.01f + * @note + * - 利用 int16_t 差分天然处理 16 位编码器计数器回绕 + * - 左侧编码器输入在这里反相，保证上层统一把“车体前进”看成正 RPM */ void Motor_Update_RPM(float dt_s) { - // 如果传入的时间间隔非法（比如0），直接返回防止除以0导致硬件错误 - if(dt_s <= 0.0f) return; - - // 1. 获取当前四个编码器定时器的原始计数值 - uint16_t curr_count_FL = __HAL_TIM_GET_COUNTER(&htim5); - uint16_t curr_count_RL = __HAL_TIM_GET_COUNTER(&htim3); - uint16_t curr_count_FR = __HAL_TIM_GET_COUNTER(&htim4); - uint16_t curr_count_RR = __HAL_TIM_GET_COUNTER(&htim1); - - // 2. 计算脉冲增量 (int16_t 强转自动处理 0~65535 溢出跳变) + uint16_t curr_count_fl; + uint16_t curr_count_rl; + uint16_t curr_count_fr; + uint16_t curr_count_rr; int16_t delta[4]; - delta[MOTOR_FL] = (int16_t)(curr_count_FL - last_count[MOTOR_FL]); - delta[MOTOR_RL] = (int16_t)(curr_count_RL - last_count[MOTOR_RL]); - delta[MOTOR_FR] = (int16_t)(curr_count_FR - last_count[MOTOR_FR]); - delta[MOTOR_RR] = (int16_t)(curr_count_RR - last_count[MOTOR_RR]); + float rpm_scale; - // 3. 更新上次计数值，为下一次计算做准备 - last_count[MOTOR_FL] = curr_count_FL; - last_count[MOTOR_RL] = curr_count_RL; - last_count[MOTOR_FR] = curr_count_FR; - last_count[MOTOR_RR] = curr_count_RR; - - /* ====================================================================== - * 【物理镜像校准 - 输入层】 - * 现象：当车体整体向前移动时，右侧产生正脉冲，而左侧产生负脉冲。 - * 处理：为了让算法层统一认为 "向前走算出的 RPM 就是正数"， - * 我们必须在这里将左侧电机的脉冲增量强制取反！ - * ====================================================================== */ - delta[MOTOR_FL] = -delta[MOTOR_FL]; - delta[MOTOR_RL] = -delta[MOTOR_RL]; - // 将校准后的增量累加进 CAN 发送缓冲区 - can_delta_ticks[MOTOR_FL] += delta[MOTOR_FL]; - can_delta_ticks[MOTOR_RL] += delta[MOTOR_RL]; - can_delta_ticks[MOTOR_FR] += delta[MOTOR_FR]; - can_delta_ticks[MOTOR_RR] += delta[MOTOR_RR]; - // 4. 计算转速 RPM (转/分钟) - // 公式: (脉冲增量 / 一圈总脉冲) / 时间周期(秒) * 60 = RPM - for(int i = 0; i < 4; i++) + if (dt_s <= 0.0f) { - current_rpm[i] = ((float)delta[i] / PULSES_PER_REV) / dt_s * 60.0f; + return; } + + curr_count_fl = (uint16_t)__HAL_TIM_GET_COUNTER(&htim5); + curr_count_rl = (uint16_t)__HAL_TIM_GET_COUNTER(&htim3); + curr_count_fr = (uint16_t)__HAL_TIM_GET_COUNTER(&htim4); + curr_count_rr = (uint16_t)__HAL_TIM_GET_COUNTER(&htim1); + + delta[MOTOR_FL] = (int16_t)(curr_count_fl - s_last_count[MOTOR_FL]); + delta[MOTOR_RL] = (int16_t)(curr_count_rl - s_last_count[MOTOR_RL]); + delta[MOTOR_FR] = (int16_t)(curr_count_fr - s_last_count[MOTOR_FR]); + delta[MOTOR_RR] = (int16_t)(curr_count_rr - s_last_count[MOTOR_RR]); + + s_last_count[MOTOR_FL] = curr_count_fl; + s_last_count[MOTOR_RL] = curr_count_rl; + s_last_count[MOTOR_FR] = curr_count_fr; + s_last_count[MOTOR_RR] = curr_count_rr; + + /* 左侧输入镜像修正：让“车体前进”统一表现为正脉冲 / 正 RPM。 */ + delta[MOTOR_FL] = (int16_t)(-delta[MOTOR_FL]); + delta[MOTOR_RL] = (int16_t)(-delta[MOTOR_RL]); + + /* 用 32 位内部累加，避免偶发高速度时 16 位回绕。 */ + s_odom_acc_ticks[MOTOR_FL] += delta[MOTOR_FL]; + s_odom_acc_ticks[MOTOR_RL] += delta[MOTOR_RL]; + s_odom_acc_ticks[MOTOR_FR] += delta[MOTOR_FR]; + s_odom_acc_ticks[MOTOR_RR] += delta[MOTOR_RR]; + + rpm_scale = 60.0f / ((float)PULSES_PER_REV * dt_s); + s_current_rpm[MOTOR_FL] = (float)delta[MOTOR_FL] * rpm_scale; + s_current_rpm[MOTOR_RL] = (float)delta[MOTOR_RL] * rpm_scale; + s_current_rpm[MOTOR_FR] = (float)delta[MOTOR_FR] * rpm_scale; + s_current_rpm[MOTOR_RR] = (float)delta[MOTOR_RR] * rpm_scale; } /** - * @brief 对外提供的统一数据读取接口 - * @param id: 电机编号 - * @retval 实际物理转速 + * @brief 获取某个电机的实时 RPM。 */ float Get_Motor_RPM(Motor_ID_t id) { - return current_rpm[id]; -} -/** - * @brief 供 CAN 任务提取里程计增量，提取后自动清零 - */ -void Motor_Get_And_Clear_Delta_Ticks(int16_t* d_fl, int16_t* d_rl, int16_t* d_fr, int16_t* d_rr) -{ - *d_fl = can_delta_ticks[MOTOR_FL]; - *d_rl = can_delta_ticks[MOTOR_RL]; - *d_fr = can_delta_ticks[MOTOR_FR]; - *d_rr = can_delta_ticks[MOTOR_RR]; + if ((id < MOTOR_FL) || (id > MOTOR_RR)) + { + return 0.0f; + } - // 清零，为下一个周期重新累加 - can_delta_ticks[MOTOR_FL] = 0; - can_delta_ticks[MOTOR_RL] = 0; - can_delta_ticks[MOTOR_FR] = 0; - can_delta_ticks[MOTOR_RR] = 0; -} \ No newline at end of file + return s_current_rpm[id]; +} + +/** + * @brief 读取并清空供 CAN 上传的里程计增量。 + * @note 这个函数会被主循环调用，而累加动作发生在 10ms 中断里， + * 所以“读取 + 清零”必须放在同一个临界区，避免丢计数。 + */ +void Motor_Get_And_Clear_Delta_Ticks(int16_t *d_fl, int16_t *d_rl, int16_t *d_fr, int16_t *d_rr) +{ + int32_t fl; + int32_t rl; + int32_t fr; + int32_t rr; + uint32_t primask; + + if ((d_fl == NULL) || (d_rl == NULL) || (d_fr == NULL) || (d_rr == NULL)) + { + return; + } + + primask = __get_PRIMASK(); + __disable_irq(); + + fl = s_odom_acc_ticks[MOTOR_FL]; + rl = s_odom_acc_ticks[MOTOR_RL]; + fr = s_odom_acc_ticks[MOTOR_FR]; + rr = s_odom_acc_ticks[MOTOR_RR]; + + s_odom_acc_ticks[MOTOR_FL] = 0; + s_odom_acc_ticks[MOTOR_RL] = 0; + s_odom_acc_ticks[MOTOR_FR] = 0; + s_odom_acc_ticks[MOTOR_RR] = 0; + + __set_PRIMASK(primask); + + *d_fl = Motor_SaturateToI16(fl); + *d_rl = Motor_SaturateToI16(rl); + *d_fr = Motor_SaturateToI16(fr); + *d_rr = Motor_SaturateToI16(rr); +} diff --git a/Core/Src/stm32f4xx_it.c b/Core/Src/stm32f4xx_it.c index 30faa20..ab3fcb6 100644 --- a/Core/Src/stm32f4xx_it.c +++ b/Core/Src/stm32f4xx_it.c @@ -214,6 +214,20 @@ void CAN1_RX0_IRQHandler(void) /* USER CODE END CAN1_RX0_IRQn 1 */ } +/** + * @brief This function handles CAN1 SCE interrupt. + */ +void CAN1_SCE_IRQHandler(void) +{ + /* USER CODE BEGIN CAN1_SCE_IRQn 0 */ + + /* USER CODE END CAN1_SCE_IRQn 0 */ + HAL_CAN_IRQHandler(&hcan1); + /* USER CODE BEGIN CAN1_SCE_IRQn 1 */ + + /* USER CODE END CAN1_SCE_IRQn 1 */ +} + /** * @brief This function handles TIM6 global interrupt, DAC1 and DAC2 underrun error interrupts. */ diff --git a/Core/Src/tim.c b/Core/Src/tim.c index ff2e3be..307fe6f 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, 0, 0); + HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 2, 0); HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn); /* USER CODE BEGIN TIM6_MspInit 1 */ diff --git a/FDR-Core.ioc b/FDR-Core.ioc index 3e46d50..e3a3d48 100644 --- a/FDR-Core.ioc +++ b/FDR-Core.ioc @@ -73,6 +73,7 @@ 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.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 @@ -83,7 +84,7 @@ 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\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.TIM6_DAC_IRQn=true\:2\: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