1.0

Core/Inc/ladrc.h

@@ -2,55 +2,67 @@
 #define __LADRC_H
 
 #include <stdint.h>
-#include "motor.h"   // 引入底层电机库以使用 Motor_ID_t 宏
+#include "motor.h"
 
-/* LADRC 结构体定义 --------------------------------------------------------*/
 typedef struct {
-  float v;          // 目标速度 (Target)
+  /* 外部命令与内部参考 */
-  float y;          // 实际速度 (Actual)
+  float v_cmd;      /* 上层下发的目标 RPM */
+  float v;          /* 内部斜坡后的参考 RPM */
+  float y;          /* 实际速度 RPM */
 
-  // LESO (线性扩展状态观测器) 状态变量
+  /* LESO 状态 */
-  float z1;         // 系统输出的估计值 (估算的实际速度)
+  float z1;
-  float z2;         // 总扰动的估计值 (包含所有未知的阻力和模型误差)
+  float z2;
 
-  // 控制器核心参数 (仅需调这 3 个)
+  /* 核心参数 */
-  float wc;         // 控制器带宽 (Controller Bandwidth)
+  float wc;
-  float wo;         // 观测器带宽 (Observer Bandwidth)
+  float wo;
-  float b0;         // 系统的控制增益 (非常关键，决定了 PWM 到速度的转换系数)
+  float b0;
 
-  // LESO 内部增益
+  /* LESO 增益 */
-  float beta1;      // 观测器增益 1
+  float beta1;
-  float beta2;      // 观测器增益 2
+  float beta2;
 
-  float h;          // 采样周期 (单位：秒，比如 10ms 就是 0.01)
+  /* 采样与限幅 */
-  float out_max;    // 输出限幅 (如 1000)
+  float h;
+  float out_max;
+
+  /* 控制输出 */
+  float u;
+
+  /* Tustin 离散矩阵 */
+  float Ad11;
+  float Ad12;
+  float Ad21;
+  float Ad22;
+
+  float Bdu1;
+  float Bdu2;
+
+  float Bdy1;
+  float Bdy2;
+
+  /* Anti-windup: 作为 z2_dot 的额外输入，并离散到同一套 Tustin 模型 */
+  float kaw;
+  float eu_prev;
+  float Bdaw1;
+  float Bdaw2;
 
-  float u;          // 最终计算出的控制量 (PWM)
 } LADRC_TypeDef;
 
-/* 原有算法函数声明 --------------------------------------------------------*/
 void LADRC_Init(LADRC_TypeDef *ladrc, float wc, float wo, float b0, float h, float max);
 float LADRC_Calc(LADRC_TypeDef *ladrc, float actual_val);
 
-/* ================= 针对四轮底盘的 LADRC 扩展调度层 ================= */
-
-// 将 4 个控制器暴露出来，方便 main.c 里面调取数据用 VOFA+ 打印波形
 extern LADRC_TypeDef ladrc_motors[4];
 
-/* 1. 初始化：一键初始化底层定时器和4个LADRC控制器 */
 void FourWheel_LADRC_Init(void);
-
-/* 2. 设定目标转速：给四个轮子下发指令 */
 void FourWheel_Set_Target_RPM(float fl_rpm, float rl_rpm, float fr_rpm, float rr_rpm);
-
-/* 3. 故障/停机时立即清零控制器内部状态并断驱 */
 void FourWheel_LADRC_ResetAll(void);
-
-/* 4. 核心闭环运算：必须且只能在 10ms 基础定时器中断里调用 */
 void FourWheel_LADRC_Control_Loop(void);
 
-/* 5. 诊断/状态上报接口：读取每个轮子的目标转速和控制输出 */
+/* 命令目标与内部参考分开暴露，便于调试 */
-float FourWheel_Get_Target_RPM(Motor_ID_t id);
+float FourWheel_Get_Target_RPM(Motor_ID_t id);     /* 返回 v_cmd */
+float FourWheel_Get_Ref_RPM(Motor_ID_t id);        /* 返回 v */
 float FourWheel_Get_Control_Output(Motor_ID_t id);
 
 #endif /* __LADRC_H */

Core/Inc/motor.h

@@ -3,42 +3,30 @@
 
 #include "main.h"
 
-/* ================== 硬件与物理参数配置 ================== */
-// PWM 满载占空比 (对应 CubeMX 中的 ARR = 1049)
 #define MAX_PWM_DUTY              1049
-
-// 为安全起见设置占空比上限，防溢出或满载锁死 (约 95%)
 #define PWM_LIMIT                 1000
 
-// 编码器与减速箱参数 (JGB37-520 11线 90减速比)
+#define ENCODER_RESOLUTION        11.0f
-#define ENCODER_RESOLUTION  11.0f   // 电机尾部编码器线数
+#define REDUCTION_RATIO           90.0f
-#define REDUCTION_RATIO     90.0f   // 减速箱减速比 (90:1)
-// 车轮转一圈的总脉冲数 = 11 * 4(倍频) * 90 = 3960
 #define PULSES_PER_REV            (ENCODER_RESOLUTION * 4.0f * REDUCTION_RATIO)
 
-/* ================== 枚举与类型定义 ================== */
+/* 控制循环 5ms，但速度估计窗口取 2 个采样点 => 10ms */
-// 定义电机 ID (与物理位置一一对应)
+#define MOTOR_SPEED_WINDOW_SAMPLES 2U
+
 typedef enum {
-  MOTOR_FL = 0, // 左上 Front-Left  (PWM: TIM2, Encoder: TIM5)
+  MOTOR_FL = 0,
-  MOTOR_RL,     // 左下 Rear-Left   (PWM: TIM9, Encoder: TIM3)
+  MOTOR_RL,
-  MOTOR_FR,     // 右上 Front-Right (PWM: TIM8, Encoder: TIM4)
+  MOTOR_FR,
-  MOTOR_RR      // 右下 Rear-Right  (PWM: TIM8, Encoder: TIM1)
+  MOTOR_RR
 } Motor_ID_t;
 
-/* ================== 函 数 声 明 ================== */
-// --- 基础控制层 ---
 void Motor_Init(void);
-// 设置电机输出 (正数代表期望车轮推动车体【向前】，负数【向后】)
 void Set_Motor_Output(Motor_ID_t id, int16_t control_out);
-// 紧急制动所有电机
 void Motor_Brake_All(void);
 
-// --- 状态观测层 ---
+/* 外部仍按 5ms 调用；内部会按滚动窗口更新速度估计 */
-// 读取脉冲并更新转速 (需周期性调用，dt_s 为调用间隔的秒数，如 10ms 则传入 0.01f)
 void Motor_Update_RPM(float dt_s);
-// 获取计算好的实时转速 (正数代表车轮正使车体【向前】行驶)
 float Get_Motor_RPM(Motor_ID_t id);
-// 获取自上次调用以来的累加脉冲增量，并自动清零
 void Motor_Get_And_Clear_Delta_Ticks(int16_t* d_fl, int16_t* d_rl, int16_t* d_fr, int16_t* d_rr);
 
 #endif /* __MOTOR_H */

Core/Inc/stm32f4xx_it.h

@@ -58,6 +58,7 @@ void SysTick_Handler(void);
 void CAN1_RX0_IRQHandler(void);
 void CAN1_SCE_IRQHandler(void);
 void TIM6_DAC_IRQHandler(void);
+void TIM7_IRQHandler(void);
 void OTG_FS_IRQHandler(void);
 /* USER CODE BEGIN EFP */
 

Core/Inc/tim.h

@@ -44,6 +44,8 @@ extern TIM_HandleTypeDef htim5;
 
 extern TIM_HandleTypeDef htim6;
 
+extern TIM_HandleTypeDef htim7;
+
 extern TIM_HandleTypeDef htim8;
 
 extern TIM_HandleTypeDef htim9;
@@ -58,6 +60,7 @@ void MX_TIM3_Init(void);
 void MX_TIM4_Init(void);
 void MX_TIM5_Init(void);
 void MX_TIM6_Init(void);
+void MX_TIM7_Init(void);
 void MX_TIM8_Init(void);
 void MX_TIM9_Init(void);
 

Core/Src/ladrc.c

@@ -1,30 +1,27 @@
 #include "ladrc.h"
 #include "f4_can_app.h"
 
-/*
+#define LADRC_CONTROL_DT_S          0.005f
- * 这个文件实现两层内容：
+#define LADRC_RPM_FILTER_ALPHA      0.20f
- * 1) 单个电机的 LADRC 控制器。
- * 2) 四轮底盘的调度层（读取编码器 -> 滤波 -> LADRC -> PWM 输出）。
- *
- * 额外升级点：
- * - 提供目标 RPM / 控制输出 getter，供上层故障诊断与状态上报使用。
- */
 
-#define LADRC_CONTROL_DT_S       0.01f
+/* 这组只是保守起始值，b0 仍建议后续辨识 */
-#define LADRC_RPM_FILTER_ALPHA   0.30f
+#define LADRC_DEFAULT_WC            12.0f
-
-#define LADRC_DEFAULT_WC         20.0f
 #define LADRC_DEFAULT_WO            25.0f
-#define LADRC_DEFAULT_B0         0.40f
+#define LADRC_DEFAULT_B0            0.80f
 #define LADRC_DEFAULT_OUT_MAX       1000.0f
 
-/* ================== 内部辅助函数 ================== */
+/* 把目标爬坡放到控制循环内部，避免依赖外部调用频率 */
+#define LADRC_TARGET_SLEW_RPM_PER_S 40.0f
+
 static float LADRC_Abs(float x);
 static float LADRC_Clamp(float x, float min_value, float max_value);
+static float Ramp_To_Target(float curr, float target, float step);
 static void LADRC_ResetStates(LADRC_TypeDef *ladrc);
 
 static float s_filtered_rpm[4] = {0.0f, 0.0f, 0.0f, 0.0f};
 
+LADRC_TypeDef ladrc_motors[4];
+
 static float LADRC_Abs(float x)
 {
     return (x >= 0.0f) ? x : -x;
@@ -43,6 +40,19 @@ static float LADRC_Clamp(float x, float min_value, float max_value)
     return x;
 }
 
+static float Ramp_To_Target(float curr, float target, float step)
+{
+    if (target > curr + step)
+    {
+        return curr + step;
+    }
+    if (target < curr - step)
+    {
+        return curr - step;
+    }
+    return target;
+}
+
 static void LADRC_ResetStates(LADRC_TypeDef *ladrc)
 {
     if (ladrc == NULL)
@@ -50,19 +60,21 @@ static void LADRC_ResetStates(LADRC_TypeDef *ladrc)
         return;
     }
 
+    ladrc->v_cmd   = 0.0f;
     ladrc->v       = 0.0f;
     ladrc->y       = 0.0f;
     ladrc->z1      = 0.0f;
     ladrc->z2      = 0.0f;
     ladrc->u       = 0.0f;
+    ladrc->eu_prev = 0.0f;
 }
 
-/**
- * @brief  初始化单个 LADRC 控制器。
- * @note   顺手做基础参数保护，避免 b0/h/out_max 为 0 或负值时出问题。
- */
 void LADRC_Init(LADRC_TypeDef *ladrc, float wc, float wo, float b0, float h, float max)
 {
+    float h_beta1_2;
+    float h2_beta2_4;
+    float delta;
+
     if (ladrc == NULL)
     {
         return;
@@ -76,6 +88,10 @@ void LADRC_Init(LADRC_TypeDef *ladrc, float wc, float wo, float b0, float h, flo
     {
         b0 = LADRC_DEFAULT_B0;
     }
+    if (b0 < 0.0f)
+    {
+        b0 = -b0;
+    }
     if (wc < 0.0f)
     {
         wc = -wc;
@@ -89,11 +105,7 @@ void LADRC_Init(LADRC_TypeDef *ladrc, float wc, float wo, float b0, float h, flo
         max = LADRC_DEFAULT_OUT_MAX;
     }
 
-    ladrc->v = 0.0f;   /* 目标值 */
+    LADRC_ResetStates(ladrc);
-    ladrc->y = 0.0f;   /* 测量值 */
-    ladrc->z1 = 0.0f;  /* 状态观测量 */
-    ladrc->z2 = 0.0f;  /* 总扰动观测量 */
-    ladrc->u = 0.0f;   /* 上一拍控制输出 */
 
     ladrc->wc      = wc;
     ladrc->wo      = wo;
@@ -103,19 +115,36 @@ void LADRC_Init(LADRC_TypeDef *ladrc, float wc, float wo, float b0, float h, flo
 
     ladrc->beta1 = 2.0f * wo;
     ladrc->beta2 = wo * wo;
+
+    h_beta1_2  = h * ladrc->beta1 * 0.5f;
+    h2_beta2_4 = h * h * ladrc->beta2 * 0.25f;
+    delta      = 1.0f + h_beta1_2 + h2_beta2_4;
+
+    /* x(k+1) = Ad*x(k) + Bdu*u(k) + Bdy*y(k) + Bdaw*eu(k-1) */
+    ladrc->Ad11 = (1.0f - h_beta1_2 - h2_beta2_4) / delta;
+    ladrc->Ad12 = h / delta;
+    ladrc->Ad21 = (-h * ladrc->beta2) / delta;
+    ladrc->Ad22 = (1.0f + h_beta1_2 - h2_beta2_4) / delta;
+
+    ladrc->Bdu1 = (h * ladrc->b0) / delta;
+    ladrc->Bdu2 = (-h * h * ladrc->beta2 * ladrc->b0 * 0.5f) / delta;
+
+    ladrc->Bdy1 = (h * (ladrc->beta1 + h * ladrc->beta2 * 0.5f)) / delta;
+    ladrc->Bdy2 = (h * ladrc->beta2) / delta;
+
+    /* e_u = u_sat - u_raw; 取负增益让饱和时对 z2 形成恢复作用 */
+    ladrc->kaw   = -(ladrc->wo * ladrc->b0);
+    ladrc->Bdaw1 = (0.5f * h * h * ladrc->kaw) / delta;
+    ladrc->Bdaw2 = (h * (1.0f + h_beta1_2) * ladrc->kaw) / delta;
 }
 
-/**
- * @brief  单个 LADRC 周期运算。
- * @param  ladrc      控制器实例
- * @param  actual_val 当前测量速度（RPM）
- * @retval 限幅后的控制输出（通常可直接映射到 PWM）
- */
 float LADRC_Calc(LADRC_TypeDef *ladrc, float actual_val)
 {
-    float e;
+    float z1_new;
+    float z2_new;
     float u0;
-    float out;
+    float u_raw;
+    float u_sat;
 
     if (ladrc == NULL)
     {
@@ -124,31 +153,29 @@ float LADRC_Calc(LADRC_TypeDef *ladrc, float actual_val)
 
     ladrc->y = actual_val;
 
-    /* 第 1 部分：LESO（线性扩张状态观测器） */
+    /* LESO + anti-windup 输入统一在同一套 Tustin 离散模型里 */
-    e = ladrc->y - ladrc->z1;
+    z1_new = ladrc->Ad11 * ladrc->z1 + ladrc->Ad12 * ladrc->z2
-    ladrc->z1 += (ladrc->z2 + ladrc->b0 * ladrc->u + ladrc->beta1 * e) * ladrc->h;
+           + ladrc->Bdy1 * ladrc->y  + ladrc->Bdu1 * ladrc->u
-    ladrc->z2 += (ladrc->beta2 * e) * ladrc->h;
+           + ladrc->Bdaw1 * ladrc->eu_prev;
+
+    z2_new = ladrc->Ad21 * ladrc->z1 + ladrc->Ad22 * ladrc->z2
+           + ladrc->Bdy2 * ladrc->y  + ladrc->Bdu2 * ladrc->u
+           + ladrc->Bdaw2 * ladrc->eu_prev;
+
+    ladrc->z1 = z1_new;
+    ladrc->z2 = z2_new;
 
-    /* 第 2 部分：LSEF + 扰动补偿 */
     u0    = ladrc->wc * (ladrc->v - ladrc->z1);
-    out = (u0 - ladrc->z2) / ladrc->b0;
+    u_raw = (u0 - ladrc->z2) / ladrc->b0;
+    u_sat = LADRC_Clamp(u_raw, -ladrc->out_max, ladrc->out_max);
 
-    /* 第 3 部分：输出限幅，保护电机和驱动 */
+    /* 当前拍算出的饱和误差，下一拍再经 Bdaw 进入观测器 */
-    out = LADRC_Clamp(out, -ladrc->out_max, ladrc->out_max);
+    ladrc->eu_prev = u_sat - u_raw;
+    ladrc->u       = u_sat;
 
-    ladrc->u = out;
+    return u_sat;
-    return out;
 }
 
-/* =====================================================================
- * 针对四轮底盘的 LADRC 调度层
- * ===================================================================== */
-
-LADRC_TypeDef ladrc_motors[4];
-
-/**
-  * @brief  一键初始化底层硬件和四个 LADRC 控制器。
-  */
 void FourWheel_LADRC_Init(void)
 {
     int i;
@@ -180,40 +207,20 @@ void FourWheel_LADRC_ResetAll(void)
     Motor_Brake_All();
 }
 
-/**
-  * @brief  设置四个轮子的目标 RPM。
-  */
-#define LADRC_TARGET_SLEW_RPM_PER_S   80.0f   // 目标斜率，先试 80 rpm/s
-
-static float Ramp_To_Target(float curr, float target, float step)
-{
-    if (target > curr + step) return curr + step;
-    if (target < curr - step) return curr - step;
-    return target;
-}
-
 void FourWheel_Set_Target_RPM(float fl_rpm, float rl_rpm, float fr_rpm, float rr_rpm)
 {
-    const float step = LADRC_TARGET_SLEW_RPM_PER_S * LADRC_CONTROL_DT_S;
+    ladrc_motors[MOTOR_FL].v_cmd = fl_rpm;
-
+    ladrc_motors[MOTOR_RL].v_cmd = rl_rpm;
-    ladrc_motors[MOTOR_FL].v = Ramp_To_Target(ladrc_motors[MOTOR_FL].v, fl_rpm, step);
+    ladrc_motors[MOTOR_FR].v_cmd = fr_rpm;
-    ladrc_motors[MOTOR_RL].v = Ramp_To_Target(ladrc_motors[MOTOR_RL].v, rl_rpm, step);
+    ladrc_motors[MOTOR_RR].v_cmd = rr_rpm;
-    ladrc_motors[MOTOR_FR].v = Ramp_To_Target(ladrc_motors[MOTOR_FR].v, fr_rpm, step);
-    ladrc_motors[MOTOR_RR].v = Ramp_To_Target(ladrc_motors[MOTOR_RR].v, rr_rpm, step);
 }
 
-/**
-  * @brief  四轮 LADRC 大循环，建议固定 10ms 调用一次。
-  * @note   调度顺序：
-  *         1) 更新编码器测速
-  *         2) 一阶低通滤波，减小测速毛刺
-  *         3) 每个轮子独立做 LADRC
-  *         4) 输出到 H 桥
-  */
 void FourWheel_LADRC_Control_Loop(void)
 {
     int i;
+    const float ref_step = LADRC_TARGET_SLEW_RPM_PER_S * LADRC_CONTROL_DT_S;
 
+    /* 5ms 调度；内部速度估计窗口在 motor.c 里做成 10ms */
     Motor_Update_RPM(LADRC_CONTROL_DT_S);
 
     if (g_robot_ctrl.state != SYSTEM_OPERATIONAL)
@@ -224,21 +231,23 @@ void FourWheel_LADRC_Control_Loop(void)
 
     for (i = 0; i < 4; ++i)
     {
-        float raw_rpm = Get_Motor_RPM((Motor_ID_t)i);
+        float raw_rpm;
         float pwm_out;
 
-        s_filtered_rpm[i] = (1.0f - LADRC_RPM_FILTER_ALPHA) * s_filtered_rpm[i]
+        ladrc_motors[i].v = Ramp_To_Target(ladrc_motors[i].v,
-                          + LADRC_RPM_FILTER_ALPHA * raw_rpm;
+                                           ladrc_motors[i].v_cmd,
+                                           ref_step);
+
+        raw_rpm = Get_Motor_RPM((Motor_ID_t)i);
+
+        /* 外部低通保留，但减为“轻滤波” */
+        s_filtered_rpm[i] += LADRC_RPM_FILTER_ALPHA * (raw_rpm - s_filtered_rpm[i]);
 
         pwm_out = LADRC_Calc(&ladrc_motors[i], s_filtered_rpm[i]);
         Set_Motor_Output((Motor_ID_t)i, (int16_t)pwm_out);
     }
 }
 
-/**
-  * @brief  读取指定轮子的“当前目标 RPM”。
-  * @note   该接口主要供上层做状态上报与故障诊断使用。
-  */
 float FourWheel_Get_Target_RPM(Motor_ID_t id)
 {
     if ((id < MOTOR_FL) || (id > MOTOR_RR))
@@ -246,13 +255,19 @@ float FourWheel_Get_Target_RPM(Motor_ID_t id)
         return 0.0f;
     }
 
+    return ladrc_motors[id].v_cmd;
+}
+
+float FourWheel_Get_Ref_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))

Core/Src/main.c

@@ -135,16 +135,23 @@ int main(void)
   MX_TIM6_Init();
   MX_CAN1_Init();
   MX_UART4_Init();
+  MX_TIM7_Init();
   /* USER CODE BEGIN 2 */
   CAN_App_Init();
   FourWheel_LADRC_Init();
 
 
-  /* 启动 10ms 硬实时控制节拍。 */
+  /* 启动 10ms 慢速/诊断节拍 */
   if (HAL_TIM_Base_Start_IT(&htim6) != HAL_OK)
   {
     Error_Handler();
   }
+
+  /* 启动 5ms 硬实时底盘控制节拍 */
+  if (HAL_TIM_Base_Start_IT(&htim7) != HAL_OK)
+  {
+    Error_Handler();
+  }
   /* USER CODE END 2 */
 
   /* Infinite loop */
@@ -242,22 +249,29 @@ void SystemClock_Config(void)
 }
 
 /* USER CODE BEGIN 4 */
-/**
-  * @brief  定时器周期中断回调。
-  * @note   TIM6 是整个底盘控制的“硬实时心跳”。
-  *         固定顺序：
-  *         1) 控制命令看门狗
-  *         2) 差速逆解算
-  *         3) 四轮 LADRC 闭环
-  *         4) 基于最新反馈做故障诊断
-  */
 void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
 {
-  if (htim->Instance == TIM6)
+  /* ==================================================
+   * [快循环] TIM7: 5ms 专用于底盘高频硬实时控制
+   * ================================================== */
+  if (htim->Instance == TIM7)
   {
-    CAN_Safety_Watchdog_Tick();
+    /* 1. 运动学逆解算：根据上层下发的底盘 vx,vy,w 计算四个轮子的目标 RPM */
     Kinematics_Update_LADRC();
+
+    /* 2. 四轮 LADRC 闭环：获取编码器 -> 滤波 -> 运算 -> 输出 PWM */
     FourWheel_LADRC_Control_Loop();
+  }
+
+  /* ==================================================
+   * [慢循环] TIM6: 10ms 专用于状态机、看门狗与故障诊断
+   * ================================================== */
+  else if (htim->Instance == TIM6)
+  {
+    /* 1. 检查上位机/遥控器指令是否超时 */
+    CAN_Safety_Watchdog_Tick();
+
+    /* 2. 基于快循环更新的状态（如超调、堵转、离线等）做诊断 */
     Chassis_Diagnostics_10ms_Tick();
   }
 }

Core/Src/motor.c

@@ -5,22 +5,15 @@
 
 #include "tim.h"
 
-/*
- * 这个文件负责两件事：
- * 1) 把 LADRC 输出的有符号控制量，转换成 H 桥两路 PWM
- * 2) 读取四路编码器，得到实时 RPM，并累加成供 CAN 上传的里程计增量
- *
- * 约定：
- * - 上层统一把“车体前进”视为正方向
- * - 由于左右电机安装方向镜像，左侧电机的硬件方向需要在底层翻转
- */
-
-/* ================== 全局静态变量 ================== */
 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};
 
-/* ================== 内部辅助函数声明 ================== */
+/* 速度估计滚动窗口：降低 5ms 单拍增量测速的量化噪声 */
+static int32_t s_speed_acc_ticks[4] = {0, 0, 0, 0};
+static uint8_t s_speed_window_count = 0U;
+static float s_speed_window_dt = 0.0f;
+
 static inline void Motor_WriteBridge(TIM_HandleTypeDef *htim,
                                      uint32_t channel_in1,
                                      uint32_t channel_in2,
@@ -30,12 +23,6 @@ static void Motor_PrimeEncoderCounters(void);
 static uint16_t Motor_ClampAbsToPwm(int32_t value);
 static int16_t Motor_SaturateToI16(int32_t value);
 
-/**
-  * @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,
@@ -46,9 +33,6 @@ static inline void Motor_WriteBridge(TIM_HandleTypeDef *htim,
     __HAL_TIM_SET_COMPARE(htim, channel_in2, pwm_in2);
 }
 
-/**
-  * @brief  把任意有符号数限幅并取绝对值，转换成 PWM 占空比。
-  */
 static uint16_t Motor_ClampAbsToPwm(int32_t value)
 {
     if (value > PWM_LIMIT)
@@ -68,9 +52,6 @@ static uint16_t Motor_ClampAbsToPwm(int32_t value)
     return (uint16_t)value;
 }
 
-/**
-  * @brief  int32 饱和到 int16，防止 CAN 打包时溢出回绕。
-  */
 static int16_t Motor_SaturateToI16(int32_t value)
 {
     if (value > 32767)
@@ -84,10 +65,6 @@ static int16_t Motor_SaturateToI16(int32_t value)
     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);
@@ -96,9 +73,6 @@ static void Motor_PrimeEncoderCounters(void)
     s_last_count[MOTOR_RR] = (uint16_t)__HAL_TIM_GET_COUNTER(&htim1);
 }
 
-/**
-  * @brief  启动所有 PWM 和编码器接口。
-  */
 void Motor_Init(void)
 {
     uint32_t primask = __get_PRIMASK();
@@ -117,7 +91,6 @@ void Motor_Init(void)
     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();
 
@@ -126,31 +99,27 @@ void Motor_Init(void)
     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;
+
+    s_speed_acc_ticks[MOTOR_FL] = 0;
+    s_speed_acc_ticks[MOTOR_RL] = 0;
+    s_speed_acc_ticks[MOTOR_FR] = 0;
+    s_speed_acc_ticks[MOTOR_RR] = 0;
+    s_speed_window_count = 0U;
+    s_speed_window_dt = 0.0f;
     __set_PRIMASK(primask);
 }
 
-/**
-  * @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)
 {
     int32_t hw_command = (int32_t)control_out;
     bool forward;
     uint16_t pwm_val;
 
-    /* 左侧硬件方向镜像：同样的“车体前进”命令，左轮需要反向转。 */
     if ((id == MOTOR_FL) || (id == MOTOR_RL))
     {
         hw_command = -hw_command;
@@ -210,12 +179,6 @@ void Set_Motor_Output(Motor_ID_t id, int16_t control_out)
     }
 }
 
-/**
-  * @brief  停止四个电机输出。
-  * @note   这里使用“两个方向脚都置 0”的空转停车方式。
-  *         如果你的驱动板支持真正的短刹车，并且你确认接线与驱动逻辑，
-  *         可以单独再加一个 Brake 模式。
-  */
 void Motor_Brake_All(void)
 {
     Motor_WriteBridge(&htim2, TIM_CHANNEL_3, TIM_CHANNEL_4, 0U, 0U);
@@ -224,13 +187,6 @@ void Motor_Brake_All(void)
     Motor_WriteBridge(&htim8, TIM_CHANNEL_3, TIM_CHANNEL_4, 0U, 0U);
 }
 
-/**
-  * @brief  读取编码器脉冲并更新实时转速。
-  * @param  dt_s 控制周期，单位秒，例如 10ms 就传 0.01f
-  * @note
-  * - 利用 int16_t 差分天然处理 16 位编码器计数器回绕
-  * - 左侧编码器输入在这里反相，保证上层统一把“车体前进”看成正 RPM
-  */
 void Motor_Update_RPM(float dt_s)
 {
     uint16_t curr_count_fl;
@@ -239,6 +195,7 @@ void Motor_Update_RPM(float dt_s)
     uint16_t curr_count_rr;
     int16_t delta[4];
     float rpm_scale;
+    int i;
 
     if (dt_s <= 0.0f)
     {
@@ -260,27 +217,37 @@ void Motor_Update_RPM(float dt_s)
     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]);
-    // delta[MOTOR_FR] = (int16_t)(-delta[MOTOR_FR]);
+
-    // delta[MOTOR_RR] = (int16_t)(-delta[MOTOR_RR]);
-    /* 用 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_speed_acc_ticks[MOTOR_FL] += delta[MOTOR_FL];
-    s_current_rpm[MOTOR_RL] = (float)delta[MOTOR_RL] * rpm_scale;
+    s_speed_acc_ticks[MOTOR_RL] += delta[MOTOR_RL];
-    s_current_rpm[MOTOR_FR] = (float)delta[MOTOR_FR] * rpm_scale;
+    s_speed_acc_ticks[MOTOR_FR] += delta[MOTOR_FR];
-    s_current_rpm[MOTOR_RR] = (float)delta[MOTOR_RR] * rpm_scale;
+    s_speed_acc_ticks[MOTOR_RR] += delta[MOTOR_RR];
+    s_speed_window_dt += dt_s;
+    s_speed_window_count++;
+
+    if (s_speed_window_count >= MOTOR_SPEED_WINDOW_SAMPLES)
+    {
+        rpm_scale = 60.0f / ((float)PULSES_PER_REV * s_speed_window_dt);
+
+        for (i = 0; i < 4; ++i)
+        {
+            s_current_rpm[i] = (float)s_speed_acc_ticks[i] * rpm_scale;
+            s_speed_acc_ticks[i] = 0;
+        }
+
+        s_speed_window_count = 0U;
+        s_speed_window_dt = 0.0f;
+    }
 }
 
-/**
-  * @brief  获取某个电机的实时 RPM。
-  */
 float Get_Motor_RPM(Motor_ID_t id)
 {
     if ((id < MOTOR_FL) || (id > MOTOR_RR))
@@ -291,11 +258,6 @@ float Get_Motor_RPM(Motor_ID_t id)
     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;

Core/Src/stm32f4xx_it.c

@@ -58,6 +58,7 @@
 extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
 extern CAN_HandleTypeDef hcan1;
 extern TIM_HandleTypeDef htim6;
+extern TIM_HandleTypeDef htim7;
 /* USER CODE BEGIN EV */
 
 /* USER CODE END EV */
@@ -242,6 +243,20 @@ void TIM6_DAC_IRQHandler(void)
   /* USER CODE END TIM6_DAC_IRQn 1 */
 }
 
+/**
+  * @brief This function handles TIM7 global interrupt.
+  */
+void TIM7_IRQHandler(void)
+{
+  /* USER CODE BEGIN TIM7_IRQn 0 */
+
+  /* USER CODE END TIM7_IRQn 0 */
+  HAL_TIM_IRQHandler(&htim7);
+  /* USER CODE BEGIN TIM7_IRQn 1 */
+
+  /* USER CODE END TIM7_IRQn 1 */
+}
+
 /**
   * @brief This function handles USB On The Go FS global interrupt.
   */

Core/Src/tim.c

@@ -30,6 +30,7 @@ TIM_HandleTypeDef htim3;
 TIM_HandleTypeDef htim4;
 TIM_HandleTypeDef htim5;
 TIM_HandleTypeDef htim6;
+TIM_HandleTypeDef htim7;
 TIM_HandleTypeDef htim8;
 TIM_HandleTypeDef htim9;
 
@@ -290,6 +291,39 @@ void MX_TIM6_Init(void)
 
   /* USER CODE END TIM6_Init 2 */
 
+}
+/* TIM7 init function */
+void MX_TIM7_Init(void)
+{
+
+  /* USER CODE BEGIN TIM7_Init 0 */
+
+  /* USER CODE END TIM7_Init 0 */
+
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+
+  /* USER CODE BEGIN TIM7_Init 1 */
+
+  /* USER CODE END TIM7_Init 1 */
+  htim7.Instance = TIM7;
+  htim7.Init.Prescaler = 84-1;
+  htim7.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim7.Init.Period = 5000-1;
+  htim7.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim7) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim7, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM7_Init 2 */
+
+  /* USER CODE END TIM7_Init 2 */
+
 }
 /* TIM8 init function */
 void MX_TIM8_Init(void)
@@ -563,6 +597,21 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
 
   /* USER CODE END TIM6_MspInit 1 */
   }
+  else if(tim_baseHandle->Instance==TIM7)
+  {
+  /* USER CODE BEGIN TIM7_MspInit 0 */
+
+  /* USER CODE END TIM7_MspInit 0 */
+    /* TIM7 clock enable */
+    __HAL_RCC_TIM7_CLK_ENABLE();
+
+    /* TIM7 interrupt Init */
+    HAL_NVIC_SetPriority(TIM7_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(TIM7_IRQn);
+  /* USER CODE BEGIN TIM7_MspInit 1 */
+
+  /* USER CODE END TIM7_MspInit 1 */
+  }
 }
 void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle)
 {
@@ -770,6 +819,20 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
 
   /* USER CODE END TIM6_MspDeInit 1 */
   }
+  else if(tim_baseHandle->Instance==TIM7)
+  {
+  /* USER CODE BEGIN TIM7_MspDeInit 0 */
+
+  /* USER CODE END TIM7_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM7_CLK_DISABLE();
+
+    /* TIM7 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(TIM7_IRQn);
+  /* USER CODE BEGIN TIM7_MspDeInit 1 */
+
+  /* USER CODE END TIM7_MspDeInit 1 */
+  }
 }
 
 /* USER CODE BEGIN 1 */

FDR-Core.ioc

@@ -19,11 +19,12 @@ Mcu.CPN=STM32F407VGT6
 Mcu.Family=STM32F4
 Mcu.IP0=CAN1
 Mcu.IP1=NVIC
-Mcu.IP10=TIM8
+Mcu.IP10=TIM7
-Mcu.IP11=TIM9
+Mcu.IP11=TIM8
-Mcu.IP12=UART4
+Mcu.IP12=TIM9
-Mcu.IP13=USB_DEVICE
+Mcu.IP13=UART4
-Mcu.IP14=USB_OTG_FS
+Mcu.IP14=USB_DEVICE
+Mcu.IP15=USB_OTG_FS
 Mcu.IP2=RCC
 Mcu.IP3=SYS
 Mcu.IP4=TIM1
@@ -32,7 +33,7 @@ Mcu.IP6=TIM3
 Mcu.IP7=TIM4
 Mcu.IP8=TIM5
 Mcu.IP9=TIM6
-Mcu.IPNb=15
+Mcu.IPNb=16
 Mcu.Name=STM32F407V(E-G)Tx
 Mcu.Package=LQFP100
 Mcu.Pin0=PE5
@@ -60,14 +61,15 @@ Mcu.Pin28=PB9
 Mcu.Pin29=VP_SYS_VS_Systick
 Mcu.Pin3=PC15-OSC32_OUT
 Mcu.Pin30=VP_TIM6_VS_ClockSourceINT
-Mcu.Pin31=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
+Mcu.Pin31=VP_TIM7_VS_ClockSourceINT
+Mcu.Pin32=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
 Mcu.Pin4=PH0-OSC_IN
 Mcu.Pin5=PH1-OSC_OUT
 Mcu.Pin6=PA0-WKUP
 Mcu.Pin7=PA1
 Mcu.Pin8=PA2
 Mcu.Pin9=PA3
-Mcu.PinsNb=32
+Mcu.PinsNb=33
 Mcu.ThirdPartyNb=0
 Mcu.UserConstants=
 Mcu.UserName=STM32F407VGTx
@@ -87,6 +89,7 @@ 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\:true\:false\:true\:true\:true\:true
+NVIC.TIM7_IRQn=true\:0\:0\:false\: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
@@ -285,6 +288,9 @@ TIM6.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE
 TIM6.IPParameters=Prescaler,Period,AutoReloadPreload
 TIM6.Period=10000-1
 TIM6.Prescaler=84-1
+TIM7.IPParameters=Prescaler,Period
+TIM7.Period=5000-1
+TIM7.Prescaler=84-1
 TIM8.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1
 TIM8.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2
 TIM8.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3
@@ -309,6 +315,8 @@ VP_SYS_VS_Systick.Mode=SysTick
 VP_SYS_VS_Systick.Signal=SYS_VS_Systick
 VP_TIM6_VS_ClockSourceINT.Mode=Enable_Timer
 VP_TIM6_VS_ClockSourceINT.Signal=TIM6_VS_ClockSourceINT
+VP_TIM7_VS_ClockSourceINT.Mode=Enable_Timer
+VP_TIM7_VS_ClockSourceINT.Signal=TIM7_VS_ClockSourceINT
 VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS.Mode=CDC_FS
 VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS.Signal=USB_DEVICE_VS_USB_DEVICE_CDC_FS
 board=custom