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 (线性扩展状态观测器) 状态变量
-  float z1;         // 系统输出的估计值 (估算的实际速度)
-  float z2;         // 总扰动的估计值 (包含所有未知的阻力和模型误差)
+  /* LESO 状态 */
+  float z1;
+  float z2;
 
-  // 控制器核心参数 (仅需调这 3 个)
-  float wc;         // 控制器带宽 (Controller Bandwidth)
-  float wo;         // 观测器带宽 (Observer Bandwidth)
-  float b0;         // 系统的控制增益 (非常关键，决定了 PWM 到速度的转换系数)
+  /* 核心参数 */
+  float wc;
+  float wo;
+  float b0;
 
-  // LESO 内部增益
-  float beta1;      // 观测器增益 1
-  float beta2;      // 观测器增益 2
+  /* LESO 增益 */
+  float beta1;
+  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 */
+#endif /* __LADRC_H */