Falling_jasmine/FDR-Core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1.0

Browse Source
This commit is contained in:
nitiantuhao
2026-03-14 17:17:17 +08:00
parent 838c121a18
commit e9e2f666fb
11 changed files with 1609 additions and 310 deletions
Download Patch File Download Diff File Expand all files Collapse all files

203
Core/Inc/f4_can_app.h
View File

@@ -1,47 +1,180 @@
#ifndef __F4_CAN_APP_H
#define __F4_CAN_APP_H
#include <stdint.h>
#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 */
/*
* 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

10
Core/Inc/ladrc.h
View File

@@ -37,13 +37,17 @@ float LADRC_Calc(LADRC_TypeDef *ladrc, float actual_val);
// 将 4 个控制器暴露出来，方便 main.c 里面调取数据用 VOFA+ 打印波形
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 */

1
Core/Inc/stm32f4xx_it.h
View File

@@ -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 */

3
Core/Src/can.c
View File

@@ -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 */

895
Core/Src/f4_can_app.c
View File

@@ -0,0 +1,895 @@
#include "f4_can_app.h"
#include <stdbool.h>
#include <string.h>
#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 */
}
}

197
Core/Src/ladrc.c
View File

@@ -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;
/* 第 2 部分：LSEF + 扰动补偿 */
u0 = ladrc->wc * (ladrc->v - ladrc->z1);
out = (u0 - ladrc->z2) / ladrc->b0;
// 第三部分：输出限幅保护
if (out > ladrc->out_max) out = ladrc->out_max;
if (out < -ladrc->out_max) out = -ladrc->out_max;
/* 第 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);
}
}
/**
* @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;
}

149
Core/Src/main.c
View File

@@ -1,4 +1,5 @@
/* USER CODE BEGIN Header */
#include <stdio.h>
#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 */

422
Core/Src/motor.c
View File

@@ -1,200 +1,330 @@
#include "motor.h"
#include "tim.h" // 必须包含此文件以使用 htim1, htim2 等外部句柄
#include <stdbool.h>
#include <stdint.h>
#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;
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);
}

14
Core/Src/stm32f4xx_it.c
View File

@@ -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.
*/

2
Core/Src/tim.c
View File

@@ -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 */

3
FDR-Core.ioc
View File

@@ -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