修改vmc

Chassis_Task.h

@@ -0,0 +1,299 @@
+#ifndef _CHASSIS_TASK
+#define _CHASSIS_TASK
+
+#include "main.h"
+#include "struct_typedef.h"
+#include "pid.h"
+#include "bsp_can.h"
+// ------------- Limit info ------------- 
+#define MAX_ACCL 13000.0f
+#define MAX_ACCL_JOINT 15.0f
+#define MAX_FOOT_OUTPUT 2048
+
+//  ------------- Target value info ------------- 
+#define SIT_MODE_HEIGHT_SET            0.18f
+#define NORMAL_MODE_HEIGHT_SET         0.10f
+#define HIGH_MODE_HEIGHT_SET           0.27f
+#define EXTREMELY_HIGH_MODE_HEIGHT_SET 0.30f
+
+// ------------- Mech info ------------- 
+#define L1 0.15f
+#define L2 0.25f
+#define L3 0.25f
+#define L4 0.15f
+#define L5 0.1f
+
+#define WHEEL_PERIMETER 0.56547
+#define WHEEL_RADIUS 0.09f
+#define LEG_OFFSET       0.3790855135f // 水平位置到上限位的夹角
+#define LOWER_SUPPORT_FORCE_FOR_JUMP 5.0f
+#define LOWER_SUPPORT_FORCE 0.0f
+#define MOVE_LOWER_BOUND 0.3f
+#define EXIT_PITCH_ANGLE 0.1f
+#define DANGER_PITCH_ANGLE 0.25f
+
+#define FEED_f 7.5f
+#define FEED_f_1 3.5f
+
+#define NORMAL_MODE_WEIGHT_DISTANCE_OFFSET -0.0f
+
+#define MOTOR_POS_UPPER_BOUND 0.05f
+#define MOTOR_POS_LOWER_BOUND 1.4f
+#define LIMITED_TORQUE 0.5f
+#define UNLIMITED_TORQUE 200.0f
+
+// ------------- Time info ------------- 
+#define CHASSIS_TASK_INIT_TIME  500
+#define TASK_RUN_TIME           0.002f
+
+// ------------- Transfer info ------------- 
+#define MOTOR_ECD_TO_RAD     0.00019174779      // 2*PI / 32767
+#define HALF_ECD_RANGE                14383
+#define HALF_POSITION_RANGE 3.0f
+// #define CC                          0.00512f 
+// #define CC                          1/494.0f
+#define TORQ_K                      494.483818182
+// ------------- Math info ------------- 
+#define PI2					  6.28318530717959f
+#define PI					  3.14159265358979f
+#define PI_2				  1.57079632679489f
+#define PI_4				  0.78539816339744f
+
+
+
+typedef enum
+{
+    ENABLE_CHASSIS = 0,
+    DISABLE_CHASSIS,
+} chassis_mode_e;
+
+typedef enum
+{
+    NO_FORCE,
+    FOOT_LAUNCHING,
+    JOINT_LAUNCHING,
+    BALANCING_READY,
+    JOINT_REDUCING,
+} chassis_balancing_mode_e;
+
+typedef enum
+{
+    NONE,
+    NORMAL_MOVING_MODE,
+    ABNORMAL_MOVING_MODE,
+    JUMPING_MODE,
+    CAP_MODE,
+    FLY_MODE,
+    TK_MODE,
+} sport_mode_e;
+
+typedef enum
+{
+    READY_TO_JUMP,
+    PREPARING_LANDING,
+    PREPARING_STAND_JUMPING,
+    CONSTACTING_LEGS,
+    EXTENDING_LEGS,
+    CONSTACTING_LEGS_2,
+    FINISHED,
+} jumping_stage_e;
+
+typedef enum
+{
+    NOT_DEFINE,
+    STANDING_JUMP,
+    MOVING_JUMP,
+} jumping_mode_e;
+
+typedef enum
+{
+    SIT_MODE = 0,
+    NORMAL_MODE,
+    HIGH_MODE,
+    EXTREMELY_HIGH_MODE,
+    CHANGING_HIGH,
+} chassis_high_mode_e;
+
+typedef enum
+{
+    MOTOR_NO_FORCE = 0,
+    MOTOR_FORCE,
+} chassis_motor_mode_e;
+
+typedef enum
+{
+	ON_GROUND = 0,
+	OFF_GROUND = 1,
+} suspend_flag_e;
+
+typedef struct
+{
+    chassis_mode_e           chassis_mode, last_chassis_mode;
+    chassis_balancing_mode_e chassis_balancing_mode, last_chassis_balancing_mode;
+    sport_mode_e             sport_mode, last_sport_mode; 
+    
+    jumping_mode_e           jumping_mode, last_jumping_mode;
+    jumping_stage_e          jumping_stage, last_jumping_stage;
+
+    chassis_high_mode_e       chassis_high_mode, last_chassis_high_mode;
+
+} mode_t;
+
+typedef struct
+{
+    const fp32 *chassis_INS_angle_point;
+  	const fp32 *chassis_INS_gyro_point;
+    const fp32 *chassis_INS_accel_point;
+    fp32 yaw_angle, pitch_angle, roll_angle;
+    fp32 yaw_gyro, pitch_gyro, roll_gyro;
+    fp32 yaw_accel, pitch_accel, roll_accel;
+
+    fp32 yaw_angle_sett, pitch_angle_set, roll_angle_set;
+    fp32 yaw_gyro_set, pitch_gyro_set, roll_gyro_set;
+
+    fp32 ideal_high;
+    fp32 leg_length_L, last_leg_length_L, leg_length_L_set;
+    fp32 leg_length_R, last_leg_length_R, leg_length_R_set;
+    fp32 leg_dlength_L;
+    fp32 leg_dlength_R;
+
+    fp32 foot_roll_angle;
+    fp32 leg_angle_L, last_leg_angle_L, leg_angle_L_set;
+    fp32 leg_angle_R, last_leg_angle_R, leg_angle_R_set;
+    fp32 leg_gyro_L, leg_gyro_R;
+
+    fp32 foot_distance, foot_distance_K, foot_distance_set;
+    fp32 foot_speed, foot_speed_K, foot_speed_set;
+
+    fp32 supportive_force_L, supportive_force_R;
+
+} chassis_posture_info_t;
+
+typedef struct
+{
+    // -------- horizontal force -------- 
+    fp32 joint_balancing_torque_L, joint_balancing_torque_R;
+    fp32 foot_balancing_torque_L,  foot_balancing_torque_R;
+	
+	fp32 foot_moving_torque_L,  foot_moving_torque_R;
+    fp32 joint_moving_torque_L, joint_moving_torque_R;
+
+    fp32 joint_prevent_splits_torque_L, joint_prevent_splits_torque_R;
+
+    fp32 joint_horizontal_torque_L, joint_horizontal_torque_R;
+    fp32 foot_horizontal_torque_L,  foot_horizontal_torque_R;
+
+    fp32 joint_horizontal_torque_temp1_R, joint_horizontal_torque_temp2_R;
+    fp32 joint_horizontal_torque_temp1_L, joint_horizontal_torque_temp2_L;
+
+    fp32 yaw_torque;
+
+    // -------- vertical force -------- 
+    fp32 joint_roll_torque_L,  joint_roll_torque_R;
+    fp32 joint_stand_torque_L, joint_stand_torque_R;
+
+    fp32 joint_vertical_torque_L,      joint_vertical_torque_R;
+    fp32 joint_real_vertical_torque_L, joint_real_vertical_torque_R;
+
+    fp32 joint_vertical_torque_temp1_R, joint_vertical_torque_temp2_R;
+    fp32 joint_vertical_torque_temp1_L, joint_vertical_torque_temp2_L;
+
+} torque_info_t;
+
+typedef struct 
+{
+    fp32 J1_L,J2_L;
+	fp32 J3_L,J4_L;
+	fp32 J1_R,J2_R;
+	fp32 J3_R,J4_R;
+	fp32 invJ1_L,invJ2_L;
+	fp32 invJ3_L,invJ4_L;
+	fp32 invJ1_R,invJ2_R;
+	fp32 invJ3_R,invJ4_R;
+
+} mapping_info_t;
+
+typedef struct
+{
+    const HT_motor_measure_t *motor_measure;
+    chassis_motor_mode_e motor_mode, last_motor_mode;
+
+    bool_t offline_flag;
+
+    fp32 position;
+    fp32 init_position;
+    fp32 position_offset;
+
+    fp32 velocity;
+    fp32 torque_out, torque_get;
+    fp32 max_torque, min_torque;
+} joint_motor_t;
+
+typedef struct
+{
+    motor_measure_t motor_measure;
+    chassis_motor_mode_e motor_mode, last_motor_mode;
+
+    bool_t offline_flag;
+
+    fp32 distance, distance_offset, last_position, position, turns;
+    fp32 speed;
+    fp32 torque_out, torque_get;
+
+} foot_motor_t;
+
+typedef struct
+{
+    bool_t init_flag;
+	suspend_flag_e suspend_flag_L, last_suspend_flag_L;
+    suspend_flag_e suspend_flag_R, last_suspend_flag_R;
+    bool_t Ignore_Off_Ground;
+    bool_t abnormal_flag;
+    bool_t static_flag, last_static_flag;
+    bool_t moving_flag, last_moving_flag;
+    bool_t rotation_flag;
+    bool_t controlling_flag;
+    bool_t set_pos_after_moving;
+    bool_t overpower_warning_flag;
+    bool_t last_overpower_warning_flag;
+    bool_t stablize_high_flag;
+    bool_t last_stablize_high_flag;
+} flag_info_t;
+
+typedef struct 
+{
+	pid_type_def buffer_control_pid;
+	pid_type_def cap_pid;
+	pid_type_def scale_down_pid;
+} pid_info_t;
+
+typedef struct
+{
+    mode_t                  mode;
+    chassis_posture_info_t  chassis_posture_info;
+    torque_info_t           torque_info;
+    mapping_info_t          mapping_info;
+    flag_info_t             flag_info;
+	pid_info_t              pid_info;
+    const Gimbal_ctrl_t     *chassis_rc_ctrl;
+
+    joint_motor_t joint_motor_1, joint_motor_2, joint_motor_3, joint_motor_4;
+    foot_motor_t foot_motor_L, foot_motor_R;
+
+}chassis_control_t;
+
+enum Chassis_Mode
+{
+  Chassis_No_Force = 0,
+  Follow_Gimbal,
+  Follow_Gimbal_90Deg,
+  No_Follow,
+  Rotate,
+//   TK_MODE,
+};
+
+extern enum Chassis_Mode chassis_mode;
+extern chassis_control_t chassis_control;
+extern fp32 roll_PD[2];
+
+#endif

Core/Src/freertos.c

@@ -27,6 +27,7 @@
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
 
+#include "task/user_task.h"
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -125,7 +126,8 @@ void MX_FREERTOS_Init(void) {
 
   /* USER CODE BEGIN RTOS_THREADS */
   /* add threads, ... */
-  /* USER CODE END RTOS_THREADS */
+   osThreadNew(Task_Init, NULL, &attr_init); // 创建初始化任务
+/* USER CODE END RTOS_THREADS */
 
   /* USER CODE BEGIN RTOS_EVENTS */
   /* add events, ... */
@@ -145,12 +147,8 @@ void StartDefaultTask(void *argument)
   /* init code for USB_DEVICE */
   MX_USB_DEVICE_Init();
   /* USER CODE BEGIN StartDefaultTask */
-  /* Infinite loop */
+    osThreadTerminate(osThreadGetId());
-  for(;;)
+/* USER CODE END StartDefaultTask */
-  {
-    osDelay(1);
-  }
-  /* USER CODE END StartDefaultTask */
 }
 
 /* Private application code --------------------------------------------------*/

Core/Src/stm32f4xx_it.c

@@ -25,6 +25,7 @@
 #include "task.h"
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
+#include "bsp/uart.h"
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -502,6 +503,7 @@ void USART6_IRQHandler(void)
   /* USER CODE END USART6_IRQn 0 */
   HAL_UART_IRQHandler(&huart6);
   /* USER CODE BEGIN USART6_IRQn 1 */
+  BSP_UART_IRQHandler(&huart6);
 
   /* USER CODE END USART6_IRQn 1 */
 }

HerKules_VOCAL_SJ_LQR_v4_with_data.m

@@ -0,0 +1,321 @@
+% v1：这份LQR程序是参考我之前写的哈工程LQR程序以及小周写的AB矩阵求解器优化后写出来的，感谢周神（2024/05/07）
+% v2：添加了可以专门调试定腿长的功能（2024/05/08）
+% v3：优化部分注释，添加单位说明（2024/05/15）
+% v4: 优化了输出，输出矩阵K的系数可以真正的复制到C里（2024/05/16）
+
+% 以下所有变量含义参考2023上交轮腿电控开源（https://bbs.robomaster.com/forum.php?mod=viewthread&tid=22756）所使用符号含义
+
+%%%%%%%%%%%%%%%%%%%%%%%%%Step 0：重置程序，定义变量%%%%%%%%%%%%%%%%%%%%%%%%%
+tic
+clear all
+clc
+
+% 定义机器人机体参数
+syms R_w                % 驱动轮半径
+syms R_l                % 驱动轮轮距/2
+syms l_l l_r            % 左右腿长
+syms l_wl l_wr          % 驱动轮质心到左右腿部质心距离
+syms l_bl l_br          % 机体质心到左右腿部质心距离
+syms l_c                % 机体质心到腿部关节中心点距离
+syms m_w m_l m_b        % 驱动轮质量 腿部质量 机体质量
+syms I_w                % 驱动轮转动惯量           (自然坐标系法向)
+syms I_ll I_lr          % 驱动轮左右腿部转动惯量    (自然坐标系法向，实际上会变化)
+syms I_b                % 机体转动惯量             (自然坐标系法向)
+syms I_z                % 机器人z轴转动惯量        (简化为常量)
+
+% 定义其他独立变量并补充其导数
+syms theta_wl   theta_wr   % 左右驱动轮转角
+syms dtheta_wl  dtheta_wr 
+syms ddtheta_wl ddtheta_wr ddtheta_ll ddtheta_lr ddtheta_b
+
+% 定义状态向量
+syms s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
+
+% 定义控制向量
+syms T_wl T_wr T_bl T_br
+
+% 输入物理参数：重力加速度
+syms g
+
+
+
+%%%%%%%%%%%%%%%%%%%%%%%Step 1：解方程，求控制矩阵A，B%%%%%%%%%%%%%%%%%%%%%%%
+
+% 通过原文方程组(3.11)-(3.15)，求出ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b表达式
+eqn1 = (I_w*l_l/R_w+m_w*R_w*l_l+m_l*R_w*l_bl)*ddtheta_wl+(m_l*l_wl*l_bl-I_ll)*ddtheta_ll+(m_l*l_wl+m_b*l_l/2)*g*theta_ll+T_bl-T_wl*(1+l_l/R_w)==0;
+eqn2 = (I_w*l_r/R_w+m_w*R_w*l_r+m_l*R_w*l_br)*ddtheta_wr+(m_l*l_wr*l_br-I_lr)*ddtheta_lr+(m_l*l_wr+m_b*l_r/2)*g*theta_lr+T_br-T_wr*(1+l_r/R_w)==0;
+eqn3 = -(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wl-(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wr-(m_l*R_w*l_wl+m_b*R_w*l_l/2)*ddtheta_ll-(m_l*R_w*l_wr+m_b*R_w*l_r/2)*ddtheta_lr+T_wl+T_wr==0;
+eqn4 = (m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wl+(m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wr+m_l*l_wl*l_c*ddtheta_ll+m_l*l_wr*l_c*ddtheta_lr-I_b*ddtheta_b+m_b*g*l_c*theta_b-(T_wl+T_wr)*l_c/R_w-(T_bl+T_br)==0;
+eqn5 = ((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wl-((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wr+(I_z*l_l)/(2*R_l)*ddtheta_ll-(I_z*l_r)/(2*R_l)*ddtheta_lr-T_wl*R_l/R_w+T_wr*R_l/R_w==0;
+[ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b] = solve(eqn1,eqn2,eqn3,eqn4,eqn5,ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b);
+
+
+% 通过计算雅可比矩阵的方法得出控制矩阵A，B所需要的全部偏导数
+J_A = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[theta_ll,theta_lr,theta_b]);
+J_B = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[T_wl,T_wr,T_bl,T_br]);
+
+% 定义矩阵A，B，将指定位置的数值根据上述偏导数计算出来并填入
+A = sym('A',[10 10]);
+B = sym('B',[10 4]);
+
+% 填入A数据：a25,a27,a29,a45,a47,a49,a65,a67,a69,a85,a87,a89,a105,a107,a109
+for p = 5:2:9
+    A_index = (p - 3)/2;
+    A(2,p) = R_w*(J_A(1,A_index) + J_A(2,A_index))/2;
+    A(4,p) = (R_w*(- J_A(1,A_index) + J_A(2,A_index)))/(2*R_l) - (l_l*J_A(3,A_index))/(2*R_l) + (l_r*J_A(4,A_index))/(2*R_l);
+    for q = 6:2:10
+        A(q,p) = J_A(q/2,A_index);
+    end
+end
+
+% A的以下数值为1：a12,a34,a56,a78,a910，其余数值为0
+for r = 1:10
+    if rem(r,2) == 0
+        A(r,1) = 0; A(r,2) = 0; A(r,3) = 0; A(r,4) = 0; A(r,6) = 0; A(r,8) = 0; A(r,10) = 0;
+    else
+        A(r,:) = zeros(1,10);
+        A(r,r+1) = 1;
+    end
+end
+
+% 填入B数据：b21,b22,b23,b24,b41,b42,b43,b44,b61,b62,b63,b64,b81,b82,b83,b84,b101,b102,b103,b104,
+for h = 1:4
+    B(2,h) = R_w*(J_B(1,h) + J_B(2,h))/2;
+    B(4,h) = (R_w*(- J_B(1,h) + J_B(2,h)))/(2*R_l) - (l_l*J_B(3,h))/(2*R_l) + (l_r*J_B(4,h))/(2*R_l);
+    for f = 6:2:10
+        B(f,h) = J_B(f/2,h);
+    end
+end
+
+% B的其余数值为0
+for e = 1:2:9
+    B(e,:) = zeros(1,4);
+end
+
+
+
+%%%%%%%%%%%%%%%%%%%%%Step 2：输入参数（可以修改的部分）%%%%%%%%%%%%%%%%%%%%%
+
+% 物理参数赋值（唯一此处不可改变！），后面的数据通过增加后缀_ac区分模型符号和实际数据
+g_ac = 9.81;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%                      此处可以输入机器人机体基本参数                      %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人机体与轮部参数%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+R_w_ac = 0.9;                           % 驱动轮半径                  （单位：m）
+R_l_ac = 0.25;                            % 两个驱动轮之间距离/2         （单位：m）
+l_c_ac = 0.037;                            % 机体质心到腿部关节中心点距离  （单位：m）
+m_w_ac = 0.8; m_l_ac = 1.6183599; m_b_ac = 11.542;    % 驱动轮质量 腿部质量 机体质量  （单位：kg）
+I_w_ac = (3510000)*10^(-7);                            % 驱动轮转动惯量               （单位：kg m^2）
+I_b_ac = 0.260;                            % 机体转动惯量(自然坐标系法向)  （单位：kg m^2）
+I_z_ac = 0.226;                            % 机器人z轴转动惯量            （单位：kg m^2）
+
+%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%%%%
+
+% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉
+% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
+
+l_l_ac = 0.18;        % 左腿摆杆长度                      （左腿对应数据）  （单位：m）
+l_wl_ac = 0.05;       % 左驱动轮质心到左腿摆杆质心距离                      （单位：m）
+l_bl_ac = 0.13;       % 机体转轴到左腿摆杆质心距离                          （单位：m）
+I_ll_ac = 0.02054500;      % 左腿摆杆转动惯量                                   （单位：kg m^2）
+l_r_ac = 0.18;        % 右腿摆杆长度                      （右腿对应数据）  （单位：m）
+l_wr_ac = 0.05;       % 右驱动轮质心到右腿摆杆质心距离                      （单位：m）
+l_br_ac = 0.13;       % 机体转轴到右腿摆杆质心距离                          （单位：m）
+I_lr_ac = 0.02054500;      % 右腿摆杆转动惯量                                   （单位：kg m^2）
+
+% 机体转轴定义参考哈工程开源（https://zhuanlan.zhihu.com/p/563048952），是左右
+% 两侧两个关节电机之间的中间点相连所形成的轴
+% （如果目的是小板凳，考虑使左右腿相关数据一致）
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数数据集%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% 根据不同腿长长度，先针对左腿测量出对应的l_wl，l_bl，和I_ll
+% 通过以下方式记录数据: 矩阵分4列，
+% 第一列为左腿腿长范围区间中所有小数点精度0.01的长度，例如：0.09，0.18，单位：m
+% 第二列为l_wl，单位：m
+% 第三列为l_bl，单位：m
+% 第四列为I_ll，单位：kg m^2
+% （注意单位别搞错！）
+% 行数根据L_0范围区间（，单位cm时）的整数数量进行调整
+
+Leg_data_l = [0.11,  0.0480,  0.0620,  0.01819599;
+              0.12,  0.0470,  0.0730,  0.01862845;
+              0.13,  0.0476,  0.0824,  0.01898641;
+              0.14,  0.0480,  0.0920,  0.01931342;
+              0.15,  0.0490,  0.1010,  0.01962521;
+              0.16,  0.0500,  0.1100,  0.01993092;
+              0.17,  0.0510,  0.1190,  0.02023626;
+              0.18,  0.0525,  0.1275,  0.02054500;
+              0.19,  0.0539,  0.1361,  0.02085969;
+              0.20,  0.0554,  0.1446,  0.02118212;
+              0.21,  0.0570,  0.1530,  0.02151357;
+              0.22,  0.0586,  0.1614,  0.02185496;
+              0.23,  0.0600,  0.1700,  0.02220695;
+              0.24,  0.0621,  0.1779,  0.02256999;
+              0.25,  0.0639,  0.1861,  0.02294442;
+              0.26,  0.0657,  0.1943,  0.02333041;
+              0.27,  0.0676,  0.2024,  0.02372806;
+              0.28,  0.0700,  0.2100,  0.02413735;
+              0.29,  0.0713,  0.2187,  0.02455817;
+              0.30,  0.0733,  0.2267,  0.02499030];
+% 以上数据应通过实际测量或sw图纸获得
+
+% 由于左右腿部数据通常完全相同，我们通过复制的方式直接定义右腿的全部数据集
+% 矩阵分4列，第一列为右腿腿长范围区间中（，单位cm时）的整数腿长l_r*0.01，第二列为l_wr，第三列为l_br，第四列为I_lr）
+Leg_data_r = Leg_data_l;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%                           此处可以输入QR矩阵                           %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% 矩阵Q中，以下列分别对应：
+%        s     ds     phi     dphi     theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
+lqr_Q = [1,    0,     0,      0,       0,       0,        0,       0,        0,      0;
+         0,    2,     0,      0,       0,       0,        0,       0,        0,      0;
+         0,    0,     12000,      0,       0,       0,        0,       0,        0,      0;
+         0,    0,     0,      200,       0,       0,        0,       0,        0,      0;
+         0,    0,     0,      0,       1000,       0,        0,       0,        0,      0;
+         0,    0,     0,      0,       0,       1,        0,       0,        0,      0;
+         0,    0,     0,      0,       0,       0,        1000,       0,        0,      0;
+         0,    0,     0,      0,       0,       0,        0,       1,        0,      0;
+         0,    0,     0,      0,       0,       0,        0,       0,        20000,   0;
+         0,    0,     0,      0,       0,       0,        0,       0,        0,      1];
+% 其中：
+% s       : 自然坐标系下机器人水平方向移动距离，单位：m，ds为其导数
+% phi     ：机器人水平方向移动时yaw偏航角度，dphi为其导数
+% theta_ll：左腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_ll为其导数
+% theta_lr：右腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_lr为其导数
+% theta_b ：机体与自然坐标系水平夹角，dtheta_b为其导数
+
+% 矩阵中，以下列分别对应：
+%        T_wl    T_wr     T_bl     T_br
+lqr_R = [0.25,      0,       0,       0;
+         0,      0.25,       0,       0;
+         0,      0,       1.5,       0;
+         0,      0,       0,       1.5];
+% 其中：
+% T_wl: 左侧驱动轮输出力矩
+% T_wr：右侧驱动轮输出力矩
+% T_bl：左侧髋关节输出力矩
+% T_br：右腿髋关节输出力矩
+% 单位皆为Nm
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+
+%%%%%%%%%%%%%%%%%%%%%Step 2.5：求解矩阵（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%
+
+% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉，
+% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
+K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
+            R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
+            l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
+            A,B,lqr_Q,lqr_R)
+K = sprintf([strjoin(repmat({'%.5g'},1,size(K,2)),',  ') '\n'], K.')
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%Step 3：拟合控制律函数%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+sample_size = size(Leg_data_l,1)^2; % 单个K_ij拟合所需要的样本数
+
+length = size(Leg_data_l,1); % 测量腿部数据集的行数
+
+% 定义所有K_ij依据l_l,l_r变化的表格，每一个表格有3列，第一列是l_l，第二列
+% 是l_r，第三列是对应的K_ij的数值
+K_sample = zeros(sample_size,3,40); % 40是因为增益矩阵K应该是4行10列。
+
+for i = 1:length
+    for j = 1:length
+        index = (i - 1)*length + j;
+        l_l_ac = Leg_data_l(i,1); % 提取左腿对应的数据
+        l_wl_ac = Leg_data_l(i,2);
+        l_bl_ac = Leg_data_l(i,3);
+        I_ll_ac = Leg_data_l(i,4);
+        l_r_ac = Leg_data_r(j,1); % 提取右腿对应的数据
+        l_wr_ac = Leg_data_r(j,2);
+        l_br_ac = Leg_data_r(j,3);
+        I_lr_ac = Leg_data_r(j,4);
+        for k = 1:40
+            K_sample(index,1,k) = l_l_ac;
+            K_sample(index,2,k) = l_r_ac;
+        end
+        K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
+            R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
+            l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
+            A,B,lqr_Q,lqr_R);
+        % 根据指定的l_l,l_r输入对应的K_ij的数值
+        for l = 1:4
+            for m = 1:10
+                K_sample(index,3,(l - 1)*10 + m) = K(l,m);
+            end
+        end
+    end
+end
+
+% 创建收集全部K_ij的多项式拟合的全部系数的集合
+K_Fit_Coefficients = zeros(40,6);
+for n = 1:40
+    K_Surface_Fit = fit([K_sample(:,1,n),K_sample(:,2,n)],K_sample(:,3,n),'poly22');
+    K_Fit_Coefficients(n,:) = coeffvalues(K_Surface_Fit); % 拟合并提取出拟合的系数结果
+end
+Polynomial_expression = formula(K_Surface_Fit)
+
+% 最终返回的结果K_Fit_Coefficients是一个40行6列矩阵，每一行分别对应一个K_ij的多项式拟合的全部系数
+% 每一行和K_ij的对应关系如下：
+% - 第1行对应K_1,1
+% - 第14行对应K_2,4
+% - 第22行对应K_3,2
+% - 第37行对应K_4,7
+% ... 其他行对应关系类似
+% 拟合出的函数表达式为 p(x,y) = p00 + p10*x + p01*y + p20*x^2 + p11*x*y + p02*y^2
+% 其中x对应左腿腿长l_l，y对应右腿腿长l_r
+% K_Fit_Coefficients每一列分别对应全部K_ij的多项式拟合的单个系数
+% 每一列和系数pij的对应关系如下：
+% - 第1列对应p00
+% - 第2列对应p10
+% - 第3列对应p01
+% - 第4列对应p20
+% - 第5列对应p11
+% - 第6列对应p02
+K_Fit_Coefficients = sprintf([strjoin(repmat({'%.5g'},1,size(K_Fit_Coefficients,2)),',  ') '\n'], K_Fit_Coefficients.')
+
+% 正确食用方法：
+% 1.在C代码中写出控制律K矩阵的全部多项式，其中每一个多项式的表达式为：
+% p(l_l,l_r) = p00 + p10*l_l + p01*l_r + p20*l_l^2 + p11*l_l*l_r + p02*l_r^2
+% 2.并填入对应系数即可
+
+toc
+
+%%%%%%%%%%%%%%%%%%%%%%%%%以下信息仅供参考，可忽略%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% 如有需要可以把所有K_ij画出图来参考，可以去掉以下注释
+% 此版本只能同时查看其中一个K_ij，同时查看多个的功能下次更新
+% （前面的蛆，以后再来探索吧（bushi
+
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%得出控制矩阵K使用的函数%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
+            R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
+            l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
+            A,B,lqr_Q,lqr_R)
+    % 基于机体以及物理参数，获得控制矩阵A，B的全部数值
+    A_ac = subs(A,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ...
+        [R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ...
+        m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]);
+    B_ac = subs(B,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ...
+        [R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ...
+        m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]);
+
+    % 根据以上信息和提供的矩阵Q和R求解Riccati方程，获得增益矩阵K
+    % P为Riccati方程的解，矩阵L可以无视
+    [P,K,L_k] = icare(A_ac,B_ac,lqr_Q,lqr_R,[],[],[]);
+end
+

LQR_修正建议.md

@@ -0,0 +1,67 @@
+## LQR控制器修正建议
+
+### 问题总结：
+1. LQR增益计算方式不一致（3次多项式 vs 2次多项式）
+2. 状态向量维度错误（12维 vs 10维）
+3. 控制律映射不正确
+4. 状态定义与MATLAB模型不匹配
+
+### 修正建议：
+
+#### 1. 修正LQR_K_calc函数
+应该使用2次多项式而不是3次：
+```cpp
+float VMC::LQR_K_calc(float *coe, float l_l, float l_r) {
+    // 使用MATLAB中定义的2次多项式
+    // p(l_l,l_r) = p00 + p10*l_l + p01*l_r + p20*l_l^2 + p11*l_l*l_r + p02*l_r^2
+    return coe[0] + coe[1]*l_l + coe[2]*l_r + coe[3]*l_l*l_l + coe[4]*l_l*l_r + coe[5]*l_r*l_r;
+}
+```
+
+#### 2. 修正增益矩阵维度
+LQR增益矩阵应该是4×10，总共40个系数：
+```cpp
+// 为每条腿分配40个LQR系数，而不是12个
+float LQR_K[40];  // 4×10矩阵，展开为一维数组
+```
+
+#### 3. 修正状态向量定义
+确保状态向量与MATLAB模型一致：
+```cpp
+// 状态向量：[s, ds, phi, dphi, theta_ll, dtheta_ll, theta_lr, dtheta_lr, theta_b, dtheta_b]
+float state_error[10] = {
+    move_argu_.xhat - move_argu_.target_x,           // s误差
+    move_argu_.x_dot_hat - move_argu_.target_dot_x,  // ds误差
+    this->yaw_ - 0.0f,                               // phi误差
+    this->gyro_.z - 0.0f,                            // dphi误差
+    leg_argu_[0].theta - 平衡角度,                    // theta_ll误差
+    leg_argu_[0].d_theta - 0.0f,                     // dtheta_ll误差
+    leg_argu_[1].theta - 平衡角度,                    // theta_lr误差
+    leg_argu_[1].d_theta - 0.0f,                     // dtheta_lr误差
+    this->pit_ - 0.0f,                               // theta_b误差
+    this->gyro_.x - 0.0f                             // dtheta_b误差
+};
+```
+
+#### 4. 修正控制律计算
+使用标准的LQR控制律：
+```cpp
+// 计算控制输出：u = -K * (x - x_ref)
+float control[4] = {0}; // [T_wl, T_wr, T_bl, T_br]
+
+for(int i = 0; i < 4; i++) {
+    for(int j = 0; j < 10; j++) {
+        control[i] += LQR_K[i*10 + j] * state_error[j];
+    }
+    control[i] = -control[i]; // 负反馈
+}
+
+leg_argu_[0].Tw = control[0]; // T_wl
+leg_argu_[1].Tw = control[1]; // T_wr  
+leg_argu_[0].Tp = control[2]; // T_bl
+leg_argu_[1].Tp = control[3]; // T_br
+```
+
+### 总结：
+当前的LQR实现在数学原理上有偏差，需要按照标准的LQR控制理论和MATLAB模型进行修正。
+主要是要确保状态向量定义、增益矩阵维度和控制律计算都与理论模型一致。

MDK-ARM/DevC.uvprojx

@@ -11,6 +11,7 @@
       <ToolsetNumber>0x4</ToolsetNumber>
       <ToolsetName>ARM-ADS</ToolsetName>
       <pArmCC>6160000::V6.16::ARMCLANG</pArmCC>
+      <pCCUsed>6160000::V6.16::ARMCLANG</pCCUsed>
       <uAC6>1</uAC6>
       <TargetOption>
         <TargetCommonOption>
@@ -81,7 +82,7 @@
           </BeforeMake>
           <AfterMake>
             <RunUserProg1>0</RunUserProg1>
-            <RunUserProg2>1</RunUserProg2>
+            <RunUserProg2>0</RunUserProg2>
             <UserProg1Name></UserProg1Name>
             <UserProg2Name></UserProg2Name>
             <UserProg1Dos16Mode>0</UserProg1Dos16Mode>
@@ -313,7 +314,7 @@
           </ArmAdsMisc>
           <Cads>
             <interw>1</interw>
-            <Optim>4</Optim>
+            <Optim>2</Optim>
             <oTime>0</oTime>
             <SplitLS>0</SplitLS>
             <OneElfS>1</OneElfS>
@@ -339,7 +340,7 @@
               <MiscControls></MiscControls>
               <Define>USE_HAL_DRIVER,STM32F407xx</Define>
               <Undefine></Undefine>
-              <IncludePath>../Core/Inc;../USB_DEVICE/App;../USB_DEVICE/Target;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Middlewares/ST/STM32_USB_Device_Library/Core/Inc;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include</IncludePath>
+              <IncludePath>../Core/Inc;../USB_DEVICE/App;../USB_DEVICE/Target;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Middlewares/ST/STM32_USB_Device_Library/Core/Inc;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include;..\User</IncludePath>
             </VariousControls>
           </Cads>
           <Aads>
@@ -730,6 +731,191 @@
             </File>
           </Files>
         </Group>
+        <Group>
+          <GroupName>User/bsp</GroupName>
+          <Files>
+            <File>
+              <FileName>can.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\bsp\can.c</FilePath>
+            </File>
+            <File>
+              <FileName>dwt.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\bsp\dwt.c</FilePath>
+            </File>
+            <File>
+              <FileName>gpio.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\bsp\gpio.c</FilePath>
+            </File>
+            <File>
+              <FileName>mm.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\bsp\mm.c</FilePath>
+            </File>
+            <File>
+              <FileName>pwm.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\bsp\pwm.c</FilePath>
+            </File>
+            <File>
+              <FileName>spi.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\bsp\spi.c</FilePath>
+            </File>
+            <File>
+              <FileName>time.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\bsp\time.c</FilePath>
+            </File>
+            <File>
+              <FileName>uart.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\bsp\uart.c</FilePath>
+            </File>
+          </Files>
+        </Group>
+        <Group>
+          <GroupName>User/component</GroupName>
+          <Files>
+            <File>
+              <FileName>ahrs.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\component\ahrs.c</FilePath>
+            </File>
+            <File>
+              <FileName>cmd.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\component\cmd.c</FilePath>
+            </File>
+            <File>
+              <FileName>filter.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\component\filter.c</FilePath>
+            </File>
+            <File>
+              <FileName>limiter.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\component\limiter.c</FilePath>
+            </File>
+            <File>
+              <FileName>pid.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\component\pid.c</FilePath>
+            </File>
+            <File>
+              <FileName>user_math.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\component\user_math.c</FilePath>
+            </File>
+            <File>
+              <FileName>kinematics.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\component\kinematics.c</FilePath>
+            </File>
+            <File>
+              <FileName>lqr.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\component\lqr.c</FilePath>
+            </File>
+            <File>
+              <FileName>vmc.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\component\vmc.c</FilePath>
+            </File>
+          </Files>
+        </Group>
+        <Group>
+          <GroupName>User/device</GroupName>
+          <Files>
+            <File>
+              <FileName>buzzer.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\device\buzzer.c</FilePath>
+            </File>
+            <File>
+              <FileName>dm_imu.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\device\dm_imu.c</FilePath>
+            </File>
+            <File>
+              <FileName>dr16.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\device\dr16.c</FilePath>
+            </File>
+            <File>
+              <FileName>motor.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\device\motor.c</FilePath>
+            </File>
+            <File>
+              <FileName>motor_lk.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\device\motor_lk.c</FilePath>
+            </File>
+            <File>
+              <FileName>motor_lz.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\device\motor_lz.c</FilePath>
+            </File>
+          </Files>
+        </Group>
+        <Group>
+          <GroupName>User/module</GroupName>
+          <Files>
+            <File>
+              <FileName>config.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\module\config.c</FilePath>
+            </File>
+            <File>
+              <FileName>balance_chassis.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\module\balance_chassis.c</FilePath>
+            </File>
+          </Files>
+        </Group>
+        <Group>
+          <GroupName>User/task</GroupName>
+          <Files>
+            <File>
+              <FileName>blink.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\task\blink.c</FilePath>
+            </File>
+            <File>
+              <FileName>init.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\task\init.c</FilePath>
+            </File>
+            <File>
+              <FileName>rc.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\task\rc.c</FilePath>
+            </File>
+            <File>
+              <FileName>user_task.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\task\user_task.c</FilePath>
+            </File>
+            <File>
+              <FileName>user_task.h</FileName>
+              <FileType>5</FileType>
+              <FilePath>..\User\task\user_task.h</FilePath>
+            </File>
+            <File>
+              <FileName>atti_esti.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\task\atti_esti.c</FilePath>
+            </File>
+            <File>
+              <FileName>ctrl_chassis.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\task\ctrl_chassis.c</FilePath>
+            </File>
+          </Files>
+        </Group>
         <Group>
           <GroupName>::CMSIS</GroupName>
         </Group>
@@ -750,4 +936,13 @@
     <files/>
   </RTE>
 
+  <LayerInfo>
+    <Layers>
+      <Layer>
+        <LayName>DevC</LayName>
+        <LayPrjMark>1</LayPrjMark>
+      </Layer>
+    </Layers>
+  </LayerInfo>
+
 </Project>

MDK-ARM/DevC/DevC.lnp

@@ -61,6 +61,37 @@
 "devc\usbd_ctlreq.o"
 "devc\usbd_ioreq.o"
 "devc\usbd_cdc.o"
+"devc\can_1.o"
+"devc\dwt.o"
+"devc\gpio_1.o"
+"devc\mm.o"
+"devc\pwm.o"
+"devc\spi_1.o"
+"devc\time.o"
+"devc\uart.o"
+"devc\ahrs.o"
+"devc\cmd.o"
+"devc\filter.o"
+"devc\limiter.o"
+"devc\pid.o"
+"devc\user_math.o"
+"devc\kinematics.o"
+"devc\lqr.o"
+"devc\vmc.o"
+"devc\buzzer.o"
+"devc\dm_imu.o"
+"devc\dr16.o"
+"devc\motor.o"
+"devc\motor_lk.o"
+"devc\motor_lz.o"
+"devc\config.o"
+"devc\balance_chassis.o"
+"devc\blink.o"
+"devc\init.o"
+"devc\rc.o"
+"devc\user_task.o"
+"devc\atti_esti.o"
+"devc\ctrl_chassis.o"
 --strict --scatter "DevC\DevC.sct"
 --summary_stderr --info summarysizes --map --load_addr_map_info --xref --callgraph --symbols
 --info sizes --info totals --info unused --info veneers

MDK-ARM/DevC/ExtDll.iex

@@ -0,0 +1,2 @@
+[EXTDLL]
+Count=0

MDK-ARM/EventRecorderStub.scvd

@@ -0,0 +1,9 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<component_viewer schemaVersion="0.1" xmlns:xs="http://www.w3.org/2001/XMLSchema-instance" xs:noNamespaceSchemaLocation="Component_Viewer.xsd">
+
+<component name="EventRecorderStub" version="1.0.0"/>       <!--name and version of the component-->
+  <events>
+  </events>
+
+</component_viewer>

User/README.md

@@ -0,0 +1,297 @@
+# 轮腿机器人LQR平衡控制系统
+
+这是一个完整的轮腿机器人LQR+VMC平衡控制系统，支持6个电机（2个轮电机+4个关节电机）的协调控制。
+
+## 系统架构
+
+```
+┌─────────────────┐    ┌──────────────────┐    ┌─────────────────┐
+│   传感器输入     │    │   平衡控制器      │    │   电机控制输出   │
+│                │    │                  │    │                │
+│ • IMU姿态数据   │────▶│  LQR控制器       │────▶│ • 轮电机力矩    │
+│ • 电机反馈数据   │    │  VMC控制器       │    │ • 关节电机力矩   │
+│ • 遥控器命令     │    │  状态估计器       │    │ • MIT控制参数   │
+└─────────────────┘    └──────────────────┘    └─────────────────┘
+```
+
+## 文件结构
+
+### 核心控制文件
+- `balance_control.h/c` - 主控制器，整合LQR和VMC
+- `lqr.h/c` - LQR控制器实现
+- `vmc.h/c` - 虚拟模型控制器
+- `kinematics.h/c` - 运动学计算
+
+### 使用示例
+- `balance_control_example.c` - 完整的使用示例
+
+### MATLAB工具
+- `lqr_design_optimized.m` - LQR参数设计和系数生成
+- `vmc.m` - VMC控制器设计参考
+
+## 快速开始
+
+### 1. 硬件连接
+确保以下硬件正确连接：
+- IMU传感器（提供机体姿态）
+- 2个轮电机（支持转矩电流控制）
+- 4个关节电机（支持MIT控制模式）
+- CAN总线通信
+
+### 2. 参数配置
+
+#### 2.1 运行MATLAB脚本生成LQR参数
+```matlab
+% 在MATLAB中运行
+run('utils/lqr_design_optimized.m');
+```
+这将生成K矩阵的拟合系数，复制到`lqr.c`中的`K_fit_coefficients`数组。
+
+#### 2.2 修改机器人物理参数
+在`lqr.h`中修改您的机器人参数：
+```c
+#define R_W 0.09f           // 驱动轮半径 (m)
+#define R_L 0.25f           // 两个驱动轮之间距离/2 (m)
+#define M_W 0.8f            // 驱动轮质量 (kg)
+#define M_L 1.6183599f      // 腿部质量 (kg)
+#define M_B 11.542f         // 机体质量 (kg)
+// ... 其他参数
+```
+
+#### 2.3 配置电机ID和CAN接口
+在`balance_control_example.c`中修改电机ID：
+```c
+#define WHEEL_LEFT_ID       0x01
+#define WHEEL_RIGHT_ID      0x02
+#define HIP_LEFT_ID         0x03
+#define HIP_RIGHT_ID        0x04
+#define KNEE_LEFT_ID        0x05
+#define KNEE_RIGHT_ID       0x06
+```
+
+### 3. 代码集成
+
+#### 3.1 在main函数中初始化
+```c
+#include "balance_control.h"
+
+int main(void)
+{
+    // HAL初始化
+    HAL_Init();
+    SystemClock_Config();
+    
+    // 初始化外设
+    MX_CAN1_Init();
+    MX_TIM1_Init();
+    
+    // 初始化平衡控制系统
+    balance_system_init();
+    
+    // 启动控制循环
+    HAL_TIM_Base_Start_IT(&htim1);  // 1kHz控制频率
+    
+    while(1)
+    {
+        // 主循环处理其他任务
+        HAL_Delay(100);
+    }
+}
+```
+
+#### 3.2 定时器中断处理
+```c
+void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
+{
+    if (htim->Instance == TIM1) {
+        balance_control_task();  // 1kHz控制任务
+    }
+}
+```
+
+### 4. 传感器数据接口
+
+#### 4.1 实现IMU数据读取
+在`balance_control_example.c`中实现`read_imu_data()`函数：
+```c
+void read_imu_data(imu_data_t* imu)
+{
+    // 从您的IMU传感器读取数据
+    imu->pitch = get_imu_pitch();
+    imu->roll = get_imu_roll();
+    imu->yaw = get_imu_yaw();
+    imu->pitch_rate = get_imu_pitch_rate();
+    imu->roll_rate = get_imu_roll_rate();
+    imu->yaw_rate = get_imu_yaw_rate();
+    // ... 其他数据
+}
+```
+
+#### 4.2 实现电机反馈读取
+```c
+void read_motor_feedback(motor_feedback_t* motor_fb)
+{
+    // 从CAN总线读取电机反馈
+    motor_fb->wheel_left_angle = get_motor_angle(WHEEL_LEFT_ID);
+    motor_fb->wheel_right_angle = get_motor_angle(WHEEL_RIGHT_ID);
+    motor_fb->hip_left_angle = get_motor_angle(HIP_LEFT_ID);
+    // ... 其他电机数据
+}
+```
+
+#### 4.3 实现电机控制输出
+```c
+void send_motor_control(const motor_control_t* motor_ctrl)
+{
+    // 发送轮电机转矩控制指令
+    send_wheel_torque_cmd(WHEEL_LEFT_ID, motor_ctrl->wheel_left_torque_cmd);
+    send_wheel_torque_cmd(WHEEL_RIGHT_ID, motor_ctrl->wheel_right_torque_cmd);
+    
+    // 发送关节电机MIT控制指令
+    send_mit_control_cmd(HIP_LEFT_ID, 
+                        0,  // 位置指令
+                        0,  // 速度指令  
+                        motor_ctrl->hip_left_kp,
+                        motor_ctrl->hip_left_kd,
+                        motor_ctrl->hip_left_torque_cmd);
+    // ... 其他关节电机
+}
+```
+
+## 控制模式
+
+### 1. 平衡模式 (ROBOT_STATE_BALANCE)
+- 自动保持机体平衡
+- 响应遥控器前进/转向命令
+- 自动调节腿长以适应地形
+
+### 2. 运动模式 (ROBOT_STATE_MOVE)  
+- 在平衡基础上执行运动控制
+- 支持前进、后退、转向
+- 支持高度调节
+
+### 3. 紧急停止 (ROBOT_STATE_EMERGENCY)
+- 立即停止所有电机输出
+- 安全保护机制
+
+## 控制参数调节
+
+### LQR参数调节
+在MATLAB脚本`lqr_design_optimized.m`中修改Q和R矩阵：
+```matlab
+% 状态权重矩阵Q - 增大数值提高控制精度
+Q = diag([
+    10,      % s - 水平位移
+    5,       % ds - 水平速度  
+    20,      % phi - 偏航角
+    10,      % dphi - 偏航角速度
+    100,     % theta_ll - 左腿角
+    50,      % dtheta_ll - 左腿角速度
+    100,     % theta_lr - 右腿角  
+    50,      % dtheta_lr - 右腿角速度
+    200,     % theta_b - 机体倾斜角 (最重要)
+    100      % dtheta_b - 机体角速度
+]);
+
+% 控制权重矩阵R - 增大数值降低控制量
+R = diag([
+    0.1,     % T_wl - 左轮力矩
+    0.1,     % T_wr - 右轮力矩  
+    1.0,     % T_bl - 左腿力矩
+    1.0      % T_br - 右腿力矩
+]);
+```
+
+### VMC参数调节
+在`balance_control.c`中修改VMC参数：
+```c
+vmc_params_t vmc_params = {
+    .K_spring = 800.0f,      // 径向弹簧刚度 - 控制腿长刚度
+    .K_damper = 80.0f,       // 径向阻尼系数 - 控制腿长阻尼
+    .K_theta = 150.0f,       // 角度刚度 - 控制腿角刚度
+    .K_dtheta = 15.0f,       // 角速度阻尼 - 控制腿角阻尼
+    .max_radial_force = 1000.0f,     // 最大径向力
+    .max_tangential_force = 500.0f,  // 最大切向力
+};
+```
+
+## 安全机制
+
+### 1. 倾斜角保护
+```c
+#define MAX_TILT_ANGLE 0.5f  // 最大倾斜角 (rad)
+```
+
+### 2. 腿长保护
+```c
+#define LEG_MIN_REACH 0.05f  // 最小腿长 (m)
+#define LEG_MAX_REACH 0.28f  // 最大腿长 (m)
+```
+
+### 3. 力矩限制
+```c
+#define MAX_WHEEL_TORQUE 50.0f   // 最大轮子力矩 (N*m)
+#define MAX_JOINT_TORQUE 100.0f  // 最大关节力矩 (N*m)
+```
+
+## 调试和监控
+
+### 1. 串口调试输出
+系统会定期输出关键状态信息：
+```
+=== Balance Control Status ===
+Task Count: 1000
+Robot State: 1
+Safety Flag: 1
+IMU Pitch: 0.050 rad (2.9 deg)
+Left Leg Length: 0.200 m
+Right Leg Length: 0.200 m
+==============================
+```
+
+### 2. 状态监控
+可以通过以下变量监控系统状态：
+- `g_balance_ctrl.state` - 机器人状态
+- `g_balance_ctrl.safety_flag` - 安全标志
+- `g_balance_ctrl.robot_state` - 完整机器人状态
+
+## 常见问题
+
+### Q: 机器人启动后不稳定？
+A: 
+1. 检查IMU标定是否正确
+2. 调整LQR的Q矩阵，增大机体角度权重
+3. 检查电机正负方向是否正确
+
+### Q: 腿部抖动？
+A:
+1. 降低VMC的刚度参数
+2. 增加VMC的阻尼参数
+3. 检查关节电机的刚度设置
+
+### Q: 轮子打滑？
+A:
+1. 降低LQR的轮子力矩输出
+2. 增加R矩阵中轮子力矩的权重
+3. 检查地面摩擦条件
+
+### Q: 系统响应慢？
+A:
+1. 增加控制频率（目前为1kHz）
+2. 调整LQR的Q矩阵权重
+3. 检查CAN通信延迟
+
+## 技术支持
+
+如需技术支持或有疑问，请：
+1. 检查上述常见问题
+2. 确认硬件连接正确
+3. 验证参数配置无误
+4. 查看串口调试输出
+
+## 更新日志
+
+- v1.0 (2025-08-30): 初始版本，支持LQR+VMC协调控制
+- 完整的6电机控制接口
+- MATLAB参数设计工具
+- 完善的安全保护机制

User/bsp/bsp.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BSP_OK (0)
+#define BSP_ERR (-1)
+#define BSP_ERR_NULL (-2)
+#define BSP_ERR_INITED (-3)
+#define BSP_ERR_NO_DEV (-4)
+#define BSP_ERR_TIMEOUT (-5)
+
+#ifdef __cplusplus
+}
+#endif

User/bsp/bsp_config.yaml

@@ -0,0 +1,132 @@
+can:
+  devices:
+  - instance: CAN1
+    name: '1'
+  - instance: CAN2
+    name: '2'
+  enabled: true
+dwt:
+  enabled: true
+gpio:
+  configs:
+  - custom_name: USER_KEY
+    has_exti: true
+    ioc_label: USER_KEY
+    pin: PA0-WKUP
+    type: EXTI
+  - custom_name: ACCL_CS
+    has_exti: false
+    ioc_label: ACCL_CS
+    pin: PA4
+    type: OUTPUT
+  - custom_name: GYRO_CS
+    has_exti: false
+    ioc_label: GYRO_CS
+    pin: PB0
+    type: OUTPUT
+  - custom_name: SPI2_CS
+    has_exti: false
+    ioc_label: SPI2_CS
+    pin: PB12
+    type: OUTPUT
+  - custom_name: HW0
+    has_exti: false
+    ioc_label: HW0
+    pin: PC0
+    type: INPUT
+  - custom_name: HW1
+    has_exti: false
+    ioc_label: HW1
+    pin: PC1
+    type: INPUT
+  - custom_name: HW2
+    has_exti: false
+    ioc_label: HW2
+    pin: PC2
+    type: INPUT
+  - custom_name: ACCL_INT
+    has_exti: true
+    ioc_label: ACCL_INT
+    pin: PC4
+    type: EXTI
+  - custom_name: GYRO_INT
+    has_exti: true
+    ioc_label: GYRO_INT
+    pin: PC5
+    type: EXTI
+  - custom_name: CMPS_INT
+    has_exti: true
+    ioc_label: CMPS_INT
+    pin: PG3
+    type: EXTI
+  - custom_name: CMPS_RST
+    has_exti: false
+    ioc_label: CMPS_RST
+    pin: PG6
+    type: OUTPUT
+  enabled: true
+mm:
+  enabled: true
+pwm:
+  configs:
+  - channel: TIM_CHANNEL_1
+    custom_name: TIM8_CH1
+    label: TIM8_CH1
+    timer: TIM8
+  - channel: TIM_CHANNEL_3
+    custom_name: LASER
+    label: LASER
+    timer: TIM3
+  - channel: TIM_CHANNEL_3
+    custom_name: BUZZER
+    label: BUZZER
+    timer: TIM4
+  - channel: TIM_CHANNEL_2
+    custom_name: TIM1_CH2
+    label: TIM1_CH2
+    timer: TIM1
+  - channel: TIM_CHANNEL_3
+    custom_name: TIM1_CH3
+    label: TIM1_CH3
+    timer: TIM1
+  - channel: TIM_CHANNEL_4
+    custom_name: TIM1_CH4
+    label: TIM1_CH4
+    timer: TIM1
+  - channel: TIM_CHANNEL_1
+    custom_name: TIM1_CH1
+    label: TIM1_CH1
+    timer: TIM1
+  - channel: TIM_CHANNEL_1
+    custom_name: IMU_HEAT_PWM
+    label: IMU_HEAT_PWM
+    timer: TIM10
+  - channel: TIM_CHANNEL_1
+    custom_name: LED_B
+    label: LED_B
+    timer: TIM5
+  - channel: TIM_CHANNEL_2
+    custom_name: LED_G
+    label: LED_G
+    timer: TIM5
+  - channel: TIM_CHANNEL_3
+    custom_name: LED_R
+    label: LED_R
+    timer: TIM5
+  - channel: TIM_CHANNEL_2
+    custom_name: TIM8_CH2
+    label: TIM8_CH2
+    timer: TIM8
+  enabled: true
+spi:
+  devices:
+  - instance: SPI1
+    name: BMI088
+  enabled: true
+time:
+  enabled: true
+uart:
+  devices:
+  - instance: USART3
+    name: DR16
+  enabled: true

User/bsp/can.c

@@ -0,0 +1,701 @@
+/* Includes ----------------------------------------------------------------- */
+#include "bsp/can.h"
+#include "bsp/bsp.h"
+#include <can.h>
+#include <cmsis_os.h>
+#include <string.h>
+
+/* Private define ----------------------------------------------------------- */
+#define CAN_QUEUE_MUTEX_TIMEOUT         100   /* 队列互斥锁超时时间(ms) */
+#define CAN_TX_SEMAPHORE_TIMEOUT        1000  /* 发送信号量超时时间(ms) */
+#define CAN_TX_MAILBOX_NUM              3     /* CAN发送邮箱数量 */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+typedef struct BSP_CAN_QueueNode {
+    BSP_CAN_t can;         /* CAN通道 */
+    uint32_t can_id; /* 解析后的CAN ID */
+    osMessageQueueId_t queue; /* 消息队列ID */
+    uint8_t queue_size; /* 队列大小 */
+    struct BSP_CAN_QueueNode *next; /* 指向下一个节点的指针 */
+} BSP_CAN_QueueNode_t;
+
+/* Private variables -------------------------------------------------------- */
+static BSP_CAN_QueueNode_t *queue_list = NULL;
+static osMutexId_t queue_mutex = NULL;
+static osSemaphoreId_t tx_semaphore[BSP_CAN_NUM] = {NULL}; /* 发送信号量，用于控制发送邮箱访问 */
+static void (*CAN_Callback[BSP_CAN_NUM][BSP_CAN_CB_NUM])(void);
+static bool inited = false;
+static BSP_CAN_IdParser_t id_parser = NULL; /* ID解析器 */
+
+/* Private function prototypes ---------------------------------------------- */
+static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan);
+static osMessageQueueId_t BSP_CAN_FindQueue(BSP_CAN_t can, uint32_t can_id);
+static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size);
+static int8_t BSP_CAN_DeleteIdQueue(BSP_CAN_t can, uint32_t can_id);
+static void BSP_CAN_RxFifo0Callback(void);
+static void BSP_CAN_RxFifo1Callback(void);
+static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header);
+static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
+
+/* Private functions -------------------------------------------------------- */
+
+/**
+ * @brief 根据CAN句柄获取BSP_CAN实例
+ */
+static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan) {
+    if (hcan == NULL) return BSP_CAN_ERR;
+    
+    if (hcan->Instance == CAN1)
+        return BSP_CAN_1;
+    else if (hcan->Instance == CAN2)
+        return BSP_CAN_2;
+    else
+        return BSP_CAN_ERR;
+}
+
+/**
+ * @brief 查找指定CAN ID的消息队列
+ * @note 调用前需要获取互斥锁
+ */
+static osMessageQueueId_t BSP_CAN_FindQueue(BSP_CAN_t can, uint32_t can_id) {
+    BSP_CAN_QueueNode_t *node = queue_list;
+    while (node != NULL) {
+        if (node->can == can && node->can_id == can_id) {
+            return node->queue;
+        }
+        node = node->next;
+    }
+    return NULL;
+}
+
+/**
+ * @brief 创建指定CAN ID的消息队列
+ * @note 内部函数，已包含互斥锁保护
+ */
+static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size) {
+    if (queue_size == 0) {
+        queue_size = BSP_CAN_DEFAULT_QUEUE_SIZE;
+    }
+    if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) {
+        return BSP_ERR_TIMEOUT;
+    }
+    BSP_CAN_QueueNode_t *node = queue_list;
+    while (node != NULL) {
+        if (node->can == can && node->can_id == can_id) {
+            osMutexRelease(queue_mutex);
+            return BSP_ERR;  // 已存在
+        }
+        node = node->next;
+    }
+    BSP_CAN_QueueNode_t *new_node = (BSP_CAN_QueueNode_t *)BSP_Malloc(sizeof(BSP_CAN_QueueNode_t));
+    if (new_node == NULL) {
+        osMutexRelease(queue_mutex);
+        return BSP_ERR_NULL;
+    }
+    new_node->queue = osMessageQueueNew(queue_size, sizeof(BSP_CAN_Message_t), NULL);
+    if (new_node->queue == NULL) {
+        BSP_Free(new_node);
+        osMutexRelease(queue_mutex);
+        return BSP_ERR;
+    }
+    new_node->can = can;
+    new_node->can_id = can_id;
+    new_node->queue_size = queue_size;
+    new_node->next = queue_list;
+    queue_list = new_node;
+    osMutexRelease(queue_mutex);
+    return BSP_OK;
+}
+
+/**
+ * @brief 删除指定CAN ID的消息队列
+ * @note 内部函数，已包含互斥锁保护
+ */
+static int8_t BSP_CAN_DeleteIdQueue(BSP_CAN_t can, uint32_t can_id) {
+    if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) {
+        return BSP_ERR_TIMEOUT;
+    }
+    BSP_CAN_QueueNode_t **current = &queue_list;
+    while (*current != NULL) {
+        if ((*current)->can == can && (*current)->can_id == can_id) {
+            BSP_CAN_QueueNode_t *to_delete = *current;
+            *current = (*current)->next;
+            osMessageQueueDelete(to_delete->queue);
+            BSP_Free(to_delete);
+            osMutexRelease(queue_mutex);
+            return BSP_OK;
+        }
+        current = &(*current)->next;
+    }
+    osMutexRelease(queue_mutex);
+    return BSP_ERR;  // 未找到
+}
+/**
+ * @brief 获取帧类型
+ */
+static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header) {
+    if (header->RTR == CAN_RTR_REMOTE) {
+        return (header->IDE == CAN_ID_EXT) ? BSP_CAN_FRAME_EXT_REMOTE : BSP_CAN_FRAME_STD_REMOTE;
+    } else {
+        return (header->IDE == CAN_ID_EXT) ? BSP_CAN_FRAME_EXT_DATA : BSP_CAN_FRAME_STD_DATA;
+    }
+}
+
+/**
+ * @brief 默认ID解析器（直接返回原始ID）
+ */
+static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
+    (void)frame_type;  // 避免未使用参数警告
+    return original_id;
+}
+
+/**
+ * @brief FIFO0接收处理函数
+ */
+static void BSP_CAN_RxFifo0Callback(void) {
+    CAN_RxHeaderTypeDef rx_header;
+    uint8_t rx_data[BSP_CAN_MAX_DLC];
+    for (int can_idx = 0; can_idx < BSP_CAN_NUM; can_idx++) {
+        CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle((BSP_CAN_t)can_idx);
+        if (hcan == NULL) continue;
+        while (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) > 0) {
+            if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data) == HAL_OK) {
+                uint32_t original_id = (rx_header.IDE == CAN_ID_STD) ? rx_header.StdId : rx_header.ExtId;
+                BSP_CAN_FrameType_t frame_type = BSP_CAN_GetFrameType(&rx_header);
+                uint32_t parsed_id = BSP_CAN_ParseId(original_id, frame_type);
+                osMessageQueueId_t queue = BSP_CAN_FindQueue((BSP_CAN_t)can_idx, parsed_id);
+                if (queue != NULL) {
+                    BSP_CAN_Message_t msg = {0};
+                    msg.frame_type = frame_type;
+                    msg.original_id = original_id;
+                    msg.parsed_id = parsed_id;
+                    msg.dlc = rx_header.DLC;
+                    if (rx_header.RTR == CAN_RTR_DATA) {
+                        memcpy(msg.data, rx_data, rx_header.DLC);
+                    }
+                    msg.timestamp = HAL_GetTick();
+                    osMessageQueuePut(queue, &msg, 0, BSP_CAN_TIMEOUT_IMMEDIATE);
+                }
+            }
+        }
+    }
+}
+
+/**
+ * @brief FIFO1接收处理函数
+ */
+static void BSP_CAN_RxFifo1Callback(void) {
+    CAN_RxHeaderTypeDef rx_header;
+    uint8_t rx_data[BSP_CAN_MAX_DLC];
+    for (int can_idx = 0; can_idx < BSP_CAN_NUM; can_idx++) {
+        CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle((BSP_CAN_t)can_idx);
+        if (hcan == NULL) continue;
+        while (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO1) > 0) {
+            if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO1, &rx_header, rx_data) == HAL_OK) {
+                uint32_t original_id = (rx_header.IDE == CAN_ID_STD) ? rx_header.StdId : rx_header.ExtId;
+                BSP_CAN_FrameType_t frame_type = BSP_CAN_GetFrameType(&rx_header);
+                uint32_t parsed_id = BSP_CAN_ParseId(original_id, frame_type);
+                osMessageQueueId_t queue = BSP_CAN_FindQueue((BSP_CAN_t)can_idx, parsed_id);
+                if (queue != NULL) {
+                    BSP_CAN_Message_t msg = {0};
+                    msg.frame_type = frame_type;
+                    msg.original_id = original_id;
+                    msg.parsed_id = parsed_id;
+                    msg.dlc = rx_header.DLC;
+                    if (rx_header.RTR == CAN_RTR_DATA) {
+                        memcpy(msg.data, rx_data, rx_header.DLC);
+                    }
+                    msg.timestamp = HAL_GetTick();
+                    osMessageQueuePut(queue, &msg, 0, BSP_CAN_TIMEOUT_IMMEDIATE);
+                }
+            }
+        }
+    }
+}
+
+/* HAL Callback Functions --------------------------------------------------- */
+void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    // 释放发送信号量
+    if (tx_semaphore[bsp_can] != NULL) {
+      osSemaphoreRelease(tx_semaphore[bsp_can]);
+    }
+    
+    if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_CPLT_CB])
+      CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_CPLT_CB]();
+  }
+}
+
+void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    // 释放发送信号量
+    if (tx_semaphore[bsp_can] != NULL) {
+      osSemaphoreRelease(tx_semaphore[bsp_can]);
+    }
+    
+    if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_CPLT_CB])
+      CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_CPLT_CB]();
+  }
+}
+
+void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    // 释放发送信号量
+    if (tx_semaphore[bsp_can] != NULL) {
+      osSemaphoreRelease(tx_semaphore[bsp_can]);
+    }
+    
+    if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_CPLT_CB])
+      CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_CPLT_CB]();
+  }
+}
+
+void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    // 释放发送信号量（发送中止也要释放）
+    if (tx_semaphore[bsp_can] != NULL) {
+      osSemaphoreRelease(tx_semaphore[bsp_can]);
+    }
+    
+    if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_ABORT_CB])
+      CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_ABORT_CB]();
+  }
+}
+
+void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    // 释放发送信号量（发送中止也要释放）
+    if (tx_semaphore[bsp_can] != NULL) {
+      osSemaphoreRelease(tx_semaphore[bsp_can]);
+    }
+    
+    if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_ABORT_CB])
+      CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_ABORT_CB]();
+  }
+}
+
+void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    // 释放发送信号量（发送中止也要释放）
+    if (tx_semaphore[bsp_can] != NULL) {
+      osSemaphoreRelease(tx_semaphore[bsp_can]);
+    }
+    
+    if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_ABORT_CB])
+      CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_ABORT_CB]();
+  }
+}
+
+void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_MSG_PENDING_CB])
+      CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_MSG_PENDING_CB]();
+  }
+}
+
+void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_FULL_CB])
+      CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_FULL_CB]();
+  }
+}
+
+void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_MSG_PENDING_CB])
+      CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_MSG_PENDING_CB]();
+  }
+}
+
+void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_FULL_CB])
+      CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_FULL_CB]();
+  }
+}
+
+void HAL_CAN_SleepCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    if (CAN_Callback[bsp_can][HAL_CAN_SLEEP_CB])
+      CAN_Callback[bsp_can][HAL_CAN_SLEEP_CB]();
+  }
+}
+
+void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    if (CAN_Callback[bsp_can][HAL_CAN_WAKEUP_FROM_RX_MSG_CB])
+      CAN_Callback[bsp_can][HAL_CAN_WAKEUP_FROM_RX_MSG_CB]();
+  }
+}
+
+void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    if (CAN_Callback[bsp_can][HAL_CAN_ERROR_CB])
+      CAN_Callback[bsp_can][HAL_CAN_ERROR_CB]();
+  }
+}
+
+/* Exported functions ------------------------------------------------------- */
+
+int8_t BSP_CAN_Init(void) {
+    if (inited) {
+        return BSP_ERR_INITED;
+    }
+    
+    // 清零回调函数数组
+    memset(CAN_Callback, 0, sizeof(CAN_Callback));
+    
+    // 初始化ID解析器为默认解析器
+    id_parser = BSP_CAN_DefaultIdParser;
+    
+    // 创建互斥锁
+    queue_mutex = osMutexNew(NULL);
+    if (queue_mutex == NULL) {
+        return BSP_ERR;
+    }
+    
+    // 创建发送信号量，每个CAN通道有3个发送邮箱
+    for (int i = 0; i < BSP_CAN_NUM; i++) {
+        tx_semaphore[i] = osSemaphoreNew(CAN_TX_MAILBOX_NUM, CAN_TX_MAILBOX_NUM, NULL);
+        if (tx_semaphore[i] == NULL) {
+            // 清理已创建的信号量
+            for (int j = 0; j < i; j++) {
+                if (tx_semaphore[j] != NULL) {
+                    osSemaphoreDelete(tx_semaphore[j]);
+                    tx_semaphore[j] = NULL;
+                }
+            }
+            if (queue_mutex != NULL) {
+                osMutexDelete(queue_mutex);
+                queue_mutex = NULL;
+            }
+            return BSP_ERR;
+        }
+    }
+    
+    // 先设置初始化标志，以便后续回调注册能通过检查
+    inited = true;
+
+    // 初始化 CAN1 - 使用 FIFO0
+    CAN_FilterTypeDef can1_filter = {0};
+    can1_filter.FilterBank = 0;
+    can1_filter.FilterIdHigh = 0;
+    can1_filter.FilterIdLow = 0;
+    can1_filter.FilterMode = CAN_FILTERMODE_IDMASK;
+    can1_filter.FilterScale = CAN_FILTERSCALE_32BIT;
+    can1_filter.FilterMaskIdHigh = 0;
+    can1_filter.FilterMaskIdLow = 0;
+    can1_filter.FilterActivation = ENABLE;
+    can1_filter.SlaveStartFilterBank = 14;
+    can1_filter.FilterFIFOAssignment = CAN_RX_FIFO0;
+    HAL_CAN_ConfigFilter(&hcan1, &can1_filter);
+    HAL_CAN_Start(&hcan1);
+
+    // 注册CAN1回调函数
+    BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback);
+
+    HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING | 
+                                        CAN_IT_TX_MAILBOX_EMPTY);  // 激活发送邮箱空中断
+
+    // 初始化 CAN2 - 使用 FIFO1
+    can1_filter.FilterBank = 14;
+    can1_filter.FilterFIFOAssignment = CAN_RX_FIFO1;
+    HAL_CAN_ConfigFilter(&hcan2, &can1_filter);  // 通过 CAN1 配置
+    HAL_CAN_Start(&hcan2);
+
+    // 注册CAN2回调函数
+    BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RxFifo1Callback);
+    HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING | 
+                                        CAN_IT_TX_MAILBOX_EMPTY);  // 激活发送邮箱空中断
+
+    
+    inited = true;
+    return BSP_OK;
+}
+
+int8_t BSP_CAN_DeInit(void) {
+    if (!inited) {
+        return BSP_ERR;
+    }
+    
+    // 删除所有队列
+    if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) == osOK) {
+        BSP_CAN_QueueNode_t *current = queue_list;
+        while (current != NULL) {
+            BSP_CAN_QueueNode_t *next = current->next;
+            osMessageQueueDelete(current->queue);
+            BSP_Free(current);
+            current = next;
+        }
+        queue_list = NULL;
+        osMutexRelease(queue_mutex);
+    }
+    
+    // 删除发送信号量
+    for (int i = 0; i < BSP_CAN_NUM; i++) {
+        if (tx_semaphore[i] != NULL) {
+            osSemaphoreDelete(tx_semaphore[i]);
+            tx_semaphore[i] = NULL;
+        }
+    }
+    
+    // 删除互斥锁
+    if (queue_mutex != NULL) {
+        osMutexDelete(queue_mutex);
+        queue_mutex = NULL;
+    }
+    
+    // 清零回调函数数组
+    memset(CAN_Callback, 0, sizeof(CAN_Callback));
+    
+    // 重置ID解析器
+    id_parser = NULL;
+    
+    inited = false;
+    return BSP_OK;
+}
+
+CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) {
+        return NULL;
+    }
+    
+    switch (can) {
+    case BSP_CAN_1:
+      return &hcan1;
+    case BSP_CAN_2:
+      return &hcan2;
+        default:
+            return NULL;
+    }
+}
+
+int8_t BSP_CAN_RegisterCallback(BSP_CAN_t can, BSP_CAN_Callback_t type,
+                                void (*callback)(void)) {
+    if (!inited) {
+        return BSP_ERR_INITED;
+    }
+    if (callback == NULL) {
+        return BSP_ERR_NULL;
+    }
+    if (can >= BSP_CAN_NUM) {
+        return BSP_ERR;
+    }
+    if (type >= BSP_CAN_CB_NUM) {
+        return BSP_ERR;
+    }
+    
+    CAN_Callback[can][type] = callback;
+    return BSP_OK;
+}
+
+int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format,
+                        uint32_t id, uint8_t *data, uint8_t dlc) {
+    if (!inited) {
+        return BSP_ERR_INITED;
+    }
+    if (can >= BSP_CAN_NUM) {
+        return BSP_ERR;
+    }
+    if (data == NULL && format != BSP_CAN_FORMAT_STD_REMOTE && format != BSP_CAN_FORMAT_EXT_REMOTE) {
+        return BSP_ERR_NULL;
+    }
+    if (dlc > BSP_CAN_MAX_DLC) {
+        return BSP_ERR;
+    }
+    
+    // 获取发送信号量，确保有可用的发送邮箱
+    if (tx_semaphore[can] == NULL) {
+        return BSP_ERR;
+    }
+    
+    osStatus_t sem_status = osSemaphoreAcquire(tx_semaphore[can], CAN_TX_SEMAPHORE_TIMEOUT);
+    if (sem_status != osOK) {
+        return BSP_ERR_TIMEOUT;  // 获取信号量超时，表示发送邮箱已满
+    }
+    
+    CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can);
+    if (hcan == NULL) {
+        // 如果获取句柄失败，需要释放信号量
+        osSemaphoreRelease(tx_semaphore[can]);
+        return BSP_ERR_NULL;
+    }
+    
+    CAN_TxHeaderTypeDef header = {0};
+    uint32_t mailbox;
+    
+    switch (format) {
+        case BSP_CAN_FORMAT_STD_DATA:
+            header.StdId = id;
+            header.IDE = CAN_ID_STD;
+            header.RTR = CAN_RTR_DATA;
+            break;
+        case BSP_CAN_FORMAT_EXT_DATA:
+            header.ExtId = id;
+            header.IDE = CAN_ID_EXT;
+            header.RTR = CAN_RTR_DATA;
+            break;
+        case BSP_CAN_FORMAT_STD_REMOTE:
+            header.StdId = id;
+            header.IDE = CAN_ID_STD;
+            header.RTR = CAN_RTR_REMOTE;
+            break;
+        case BSP_CAN_FORMAT_EXT_REMOTE:
+            header.ExtId = id;
+            header.IDE = CAN_ID_EXT;
+            header.RTR = CAN_RTR_REMOTE;
+            break;
+        default:
+            // 如果格式错误，需要释放信号量
+            osSemaphoreRelease(tx_semaphore[can]);
+            return BSP_ERR;
+    }
+    
+    header.DLC = dlc;
+    header.TransmitGlobalTime = DISABLE;
+    
+    HAL_StatusTypeDef result = HAL_CAN_AddTxMessage(hcan, &header, data, &mailbox);
+    
+    if (result != HAL_OK) {
+        // 如果发送失败，需要释放信号量
+        osSemaphoreRelease(tx_semaphore[can]);
+        return BSP_ERR;
+    }
+    
+    // 发送成功，信号量将在发送完成回调中释放
+    return BSP_OK;
+}
+
+int8_t BSP_CAN_TransmitStdDataFrame(BSP_CAN_t can, BSP_CAN_StdDataFrame_t *frame) {
+    if (frame == NULL) {
+        return BSP_ERR_NULL;
+    }
+    return BSP_CAN_Transmit(can, BSP_CAN_FORMAT_STD_DATA, frame->id, frame->data, frame->dlc);
+}
+
+int8_t BSP_CAN_TransmitExtDataFrame(BSP_CAN_t can, BSP_CAN_ExtDataFrame_t *frame) {
+    if (frame == NULL) {
+        return BSP_ERR_NULL;
+    }
+    return BSP_CAN_Transmit(can, BSP_CAN_FORMAT_EXT_DATA, frame->id, frame->data, frame->dlc);
+}
+
+int8_t BSP_CAN_TransmitRemoteFrame(BSP_CAN_t can, BSP_CAN_RemoteFrame_t *frame) {
+    if (frame == NULL) {
+        return BSP_ERR_NULL;
+    }
+    BSP_CAN_Format_t format = frame->is_extended ? BSP_CAN_FORMAT_EXT_REMOTE : BSP_CAN_FORMAT_STD_REMOTE;
+    return BSP_CAN_Transmit(can, format, frame->id, NULL, frame->dlc);
+}
+
+int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size) {
+    if (!inited) {
+        return BSP_ERR_INITED;
+    }
+    return BSP_CAN_CreateIdQueue(can, can_id, queue_size);
+}
+
+int8_t BSP_CAN_UnregisterIdQueue(BSP_CAN_t can, uint32_t can_id) {
+    if (!inited) {
+        return BSP_ERR_INITED;
+    }
+    return BSP_CAN_DeleteIdQueue(can, can_id);
+}
+
+int8_t BSP_CAN_GetMessage(BSP_CAN_t can, uint32_t can_id, BSP_CAN_Message_t *msg, uint32_t timeout) {
+    if (!inited) {
+        return BSP_ERR_INITED;
+    }
+    if (msg == NULL) {
+        return BSP_ERR_NULL;
+    }
+    if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) {
+        return BSP_ERR_TIMEOUT;
+    }
+    osMessageQueueId_t queue = BSP_CAN_FindQueue(can, can_id);
+    osMutexRelease(queue_mutex);
+    if (queue == NULL) {
+        return BSP_ERR_NO_DEV;
+    }
+    osStatus_t result = osMessageQueueGet(queue, msg, NULL, timeout);
+    return (result == osOK) ? BSP_OK : BSP_ERR;
+}
+
+int32_t BSP_CAN_GetQueueCount(BSP_CAN_t can, uint32_t can_id) {
+    if (!inited) {
+        return -1;
+    }
+    if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) {
+        return -1;
+    }
+    osMessageQueueId_t queue = BSP_CAN_FindQueue(can, can_id);
+    osMutexRelease(queue_mutex);
+    if (queue == NULL) {
+        return -1;
+    }
+    return (int32_t)osMessageQueueGetCount(queue);
+}
+
+int8_t BSP_CAN_FlushQueue(BSP_CAN_t can, uint32_t can_id) {
+    if (!inited) {
+        return BSP_ERR_INITED;
+    }
+    if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) {
+        return BSP_ERR_TIMEOUT;
+    }
+    osMessageQueueId_t queue = BSP_CAN_FindQueue(can, can_id);
+    osMutexRelease(queue_mutex);
+    if (queue == NULL) {
+        return BSP_ERR_NO_DEV;
+    }
+    BSP_CAN_Message_t temp_msg;
+    while (osMessageQueueGet(queue, &temp_msg, NULL, BSP_CAN_TIMEOUT_IMMEDIATE) == osOK) {
+        // 清空
+    }
+    return BSP_OK;
+}
+
+int8_t BSP_CAN_RegisterIdParser(BSP_CAN_IdParser_t parser) {
+    if (!inited) {
+        return BSP_ERR_INITED;
+    }
+    if (parser == NULL) {
+        return BSP_ERR_NULL;
+    }
+    
+    id_parser = parser;
+    return BSP_OK;
+}
+
+int8_t BSP_CAN_UnregisterIdParser(void) {
+    if (!inited) {
+        return BSP_ERR_INITED;
+    }
+    
+    id_parser = BSP_CAN_DefaultIdParser;
+    return BSP_OK;
+}
+
+uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
+    if (id_parser != NULL) {
+        return id_parser(original_id, frame_type);
+    }
+    return BSP_CAN_DefaultIdParser(original_id, frame_type);
+}
+
+/* USER CAN FUNCTIONS BEGIN */
+/* USER CAN FUNCTIONS END */

User/bsp/can.h

@@ -0,0 +1,233 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <can.h>
+#include "bsp/bsp.h"
+#include "bsp/mm.h"
+#include <stdint.h>
+#include <stdbool.h>
+#include <cmsis_os.h>
+
+/* Exported constants ------------------------------------------------------- */
+#define BSP_CAN_MAX_DLC                 8
+#define BSP_CAN_DEFAULT_QUEUE_SIZE      10
+#define BSP_CAN_TIMEOUT_IMMEDIATE       0
+#define BSP_CAN_TIMEOUT_FOREVER         osWaitForever
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef enum {
+  BSP_CAN_1,
+  BSP_CAN_2,
+  BSP_CAN_NUM,
+  BSP_CAN_ERR,
+} BSP_CAN_t;
+
+typedef enum {
+  HAL_CAN_TX_MAILBOX0_CPLT_CB,
+  HAL_CAN_TX_MAILBOX1_CPLT_CB,
+  HAL_CAN_TX_MAILBOX2_CPLT_CB,
+  HAL_CAN_TX_MAILBOX0_ABORT_CB,
+  HAL_CAN_TX_MAILBOX1_ABORT_CB,
+  HAL_CAN_TX_MAILBOX2_ABORT_CB,
+  HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
+  HAL_CAN_RX_FIFO0_FULL_CB,
+  HAL_CAN_RX_FIFO1_MSG_PENDING_CB,
+  HAL_CAN_RX_FIFO1_FULL_CB,
+  HAL_CAN_SLEEP_CB,
+  HAL_CAN_WAKEUP_FROM_RX_MSG_CB,
+  HAL_CAN_ERROR_CB,
+  BSP_CAN_CB_NUM,
+} BSP_CAN_Callback_t;
+
+/* CAN消息格式枚举 - 用于发送和接收消息时指定格式 */
+typedef enum {
+  BSP_CAN_FORMAT_STD_DATA,    /* 标准数据帧 */
+  BSP_CAN_FORMAT_EXT_DATA,    /* 扩展数据帧 */
+  BSP_CAN_FORMAT_STD_REMOTE,  /* 标准远程帧 */
+  BSP_CAN_FORMAT_EXT_REMOTE,  /* 扩展远程帧 */
+} BSP_CAN_Format_t;
+
+/* CAN帧类型枚举 - 用于区分不同类型的CAN帧 */
+typedef enum {
+  BSP_CAN_FRAME_STD_DATA,     /* 标准数据帧 */
+  BSP_CAN_FRAME_EXT_DATA,     /* 扩展数据帧 */
+  BSP_CAN_FRAME_STD_REMOTE,   /* 标准远程帧 */
+  BSP_CAN_FRAME_EXT_REMOTE,   /* 扩展远程帧 */
+} BSP_CAN_FrameType_t;
+
+/* CAN消息结构体 - 支持不同类型帧 */
+typedef struct {
+    BSP_CAN_FrameType_t frame_type;     /* 帧类型 */
+    uint32_t original_id;               /* 原始ID（未解析） */
+    uint32_t parsed_id;                 /* 解析后的实际ID */
+    uint8_t dlc;                        /* 数据长度 */
+    uint8_t data[BSP_CAN_MAX_DLC];      /* 数据 */
+    uint32_t timestamp;                 /* 时间戳（可选） */
+} BSP_CAN_Message_t;
+
+/* 标准数据帧结构 */
+typedef struct {
+    uint32_t id;                        /* CAN ID */
+    uint8_t dlc;                        /* 数据长度 */
+    uint8_t data[BSP_CAN_MAX_DLC];      /* 数据 */
+} BSP_CAN_StdDataFrame_t;
+
+/* 扩展数据帧结构 */
+typedef struct {
+    uint32_t id;                        /* 扩展CAN ID */
+    uint8_t dlc;                        /* 数据长度 */
+    uint8_t data[BSP_CAN_MAX_DLC];      /* 数据 */
+} BSP_CAN_ExtDataFrame_t;
+
+/* 远程帧结构 */
+typedef struct {
+    uint32_t id;                        /* CAN ID */
+    uint8_t dlc;                        /* 请求的数据长度 */
+    bool is_extended;                   /* 是否为扩展帧 */
+} BSP_CAN_RemoteFrame_t;
+
+/* ID解析回调函数类型 */
+typedef uint32_t (*BSP_CAN_IdParser_t)(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 初始化 CAN 模块
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_Init(void);
+
+/**
+ * @brief 反初始化 CAN 模块
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_DeInit(void);
+
+/**
+ * @brief 获取 CAN 句柄
+ * @param can CAN 枚举
+ * @return CAN_HandleTypeDef 指针，失败返回 NULL
+ */
+CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can);
+
+/**
+ * @brief 注册 CAN 回调函数
+ * @param can CAN 枚举
+ * @param type 回调类型
+ * @param callback 回调函数指针
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_RegisterCallback(BSP_CAN_t can, BSP_CAN_Callback_t type,
+                                void (*callback)(void));
+
+/**
+ * @brief 发送 CAN 消息
+ * @param can CAN 枚举
+ * @param format 消息格式
+ * @param id CAN ID
+ * @param data 数据指针
+ * @param dlc 数据长度
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format,
+                        uint32_t id, uint8_t *data, uint8_t dlc);
+
+/**
+ * @brief 发送标准数据帧
+ * @param can CAN 枚举
+ * @param frame 标准数据帧指针
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_TransmitStdDataFrame(BSP_CAN_t can, BSP_CAN_StdDataFrame_t *frame);
+
+/**
+ * @brief 发送扩展数据帧
+ * @param can CAN 枚举
+ * @param frame 扩展数据帧指针
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_TransmitExtDataFrame(BSP_CAN_t can, BSP_CAN_ExtDataFrame_t *frame);
+
+/**
+ * @brief 发送远程帧
+ * @param can CAN 枚举
+ * @param frame 远程帧指针
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_TransmitRemoteFrame(BSP_CAN_t can, BSP_CAN_RemoteFrame_t *frame);
+
+/**
+ * @brief 注册 CAN ID 接收队列
+ * @param can CAN 枚举
+ * @param can_id 解析后的CAN ID
+ * @param queue_size 队列大小，0使用默认值
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size);
+
+/**
+ * @brief 注销 CAN ID 接收队列
+ * @param can CAN 枚举
+ * @param can_id 解析后的CAN ID
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_UnregisterIdQueue(BSP_CAN_t can, uint32_t can_id);
+
+/**
+ * @brief 获取 CAN 消息
+ * @param can CAN 枚举
+ * @param can_id 解析后的CAN ID
+ * @param msg 存储消息的结构体指针
+ * @param timeout 超时时间（毫秒），0为立即返回，osWaitForever为永久等待
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_GetMessage(BSP_CAN_t can, uint32_t can_id, BSP_CAN_Message_t *msg, uint32_t timeout);
+
+/**
+ * @brief 获取指定ID队列中的消息数量
+ * @param can CAN 枚举
+ * @param can_id 解析后的CAN ID
+ * @return 消息数量，-1表示队列不存在
+ */
+int32_t BSP_CAN_GetQueueCount(BSP_CAN_t can, uint32_t can_id);
+
+/**
+ * @brief 清空指定ID队列中的所有消息
+ * @param can CAN 枚举
+ * @param can_id 解析后的CAN ID
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_FlushQueue(BSP_CAN_t can, uint32_t can_id);
+
+/**
+ * @brief 注册ID解析器
+ * @param parser ID解析回调函数
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_RegisterIdParser(BSP_CAN_IdParser_t parser);
+
+/**
+ * @brief 注销ID解析器
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_UnregisterIdParser(void);
+
+/**
+ * @brief 解析CAN ID
+ * @param original_id 原始ID
+ * @param frame_type 帧类型
+ * @return 解析后的ID
+ */
+uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
+
+/* USER CAN FUNCTIONS BEGIN */
+/* USER CAN FUNCTIONS END */
+
+#ifdef __cplusplus
+}
+#endif

User/bsp/dwt.c

@@ -0,0 +1,122 @@
+/**
+ ******************************************************************************
+ * @file	dwt.c
+ * @author  Wang Hongxi
+ * @version V1.1.0
+ * @date    2022/3/8
+ * @brief
+ ******************************************************************************
+ * @attention
+ *
+ ******************************************************************************
+ */
+#include "bsp/dwt.h"
+
+DWT_Time_t SysTime;
+static uint32_t CPU_FREQ_Hz, CPU_FREQ_Hz_ms, CPU_FREQ_Hz_us;
+static uint32_t CYCCNT_RountCount;
+static uint32_t CYCCNT_LAST;
+uint64_t CYCCNT64;
+static void DWT_CNT_Update(void);
+
+void DWT_Init(uint32_t CPU_Freq_mHz)
+{
+    /* 使能DWT外设 */
+    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
+
+    /* DWT CYCCNT寄存器计数清0 */
+    DWT->CYCCNT = (uint32_t)0u;
+
+    /* 使能Cortex-M DWT CYCCNT寄存器 */
+    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
+
+    CPU_FREQ_Hz = CPU_Freq_mHz * 1000000;
+    CPU_FREQ_Hz_ms = CPU_FREQ_Hz / 1000;
+    CPU_FREQ_Hz_us = CPU_FREQ_Hz / 1000000;
+    CYCCNT_RountCount = 0;
+}
+
+float DWT_GetDeltaT(uint32_t *cnt_last)
+{
+    volatile uint32_t cnt_now = DWT->CYCCNT;
+    float dt = ((uint32_t)(cnt_now - *cnt_last)) / ((float)(CPU_FREQ_Hz));
+    *cnt_last = cnt_now;
+
+    DWT_CNT_Update();
+
+    return dt;
+}
+
+double DWT_GetDeltaT64(uint32_t *cnt_last)
+{
+    volatile uint32_t cnt_now = DWT->CYCCNT;
+    double dt = ((uint32_t)(cnt_now - *cnt_last)) / ((double)(CPU_FREQ_Hz));
+    *cnt_last = cnt_now;
+
+    DWT_CNT_Update();
+
+    return dt;
+}
+
+void DWT_SysTimeUpdate(void)
+{
+    volatile uint32_t cnt_now = DWT->CYCCNT;
+    static uint64_t CNT_TEMP1, CNT_TEMP2, CNT_TEMP3;
+
+    DWT_CNT_Update();
+
+    CYCCNT64 = (uint64_t)CYCCNT_RountCount * (uint64_t)UINT32_MAX + (uint64_t)cnt_now;
+    CNT_TEMP1 = CYCCNT64 / CPU_FREQ_Hz;
+    CNT_TEMP2 = CYCCNT64 - CNT_TEMP1 * CPU_FREQ_Hz;
+    SysTime.s = CNT_TEMP1;
+    SysTime.ms = CNT_TEMP2 / CPU_FREQ_Hz_ms;
+    CNT_TEMP3 = CNT_TEMP2 - SysTime.ms * CPU_FREQ_Hz_ms;
+    SysTime.us = CNT_TEMP3 / CPU_FREQ_Hz_us;
+}
+
+float DWT_GetTimeline_s(void)
+{
+    DWT_SysTimeUpdate();
+
+    float DWT_Timelinef32 = SysTime.s + SysTime.ms * 0.001f + SysTime.us * 0.000001f;
+
+    return DWT_Timelinef32;
+}
+
+float DWT_GetTimeline_ms(void)
+{
+    DWT_SysTimeUpdate();
+
+    float DWT_Timelinef32 = SysTime.s * 1000 + SysTime.ms + SysTime.us * 0.001f;
+
+    return DWT_Timelinef32;
+}
+
+uint64_t DWT_GetTimeline_us(void)
+{
+    DWT_SysTimeUpdate();
+
+    uint64_t DWT_Timelinef32 = SysTime.s * 1000000 + SysTime.ms * 1000 + SysTime.us;
+
+    return DWT_Timelinef32;
+}
+
+static void DWT_CNT_Update(void)
+{
+    volatile uint32_t cnt_now = DWT->CYCCNT;
+
+    if (cnt_now < CYCCNT_LAST)
+        CYCCNT_RountCount++;
+
+    CYCCNT_LAST = cnt_now;
+}
+
+void DWT_Delay(float Delay)
+{
+    uint32_t tickstart = DWT->CYCCNT;
+    float wait = Delay;
+
+    while ((DWT->CYCCNT - tickstart) < wait * (float)CPU_FREQ_Hz)
+    {
+    }
+}

User/bsp/dwt.h

@@ -0,0 +1,37 @@
+/**
+ ******************************************************************************
+ * @file	dwt.h
+ * @author  Wang Hongxi
+ * @version V1.1.0
+ * @date    2022/3/8
+ * @brief
+ ******************************************************************************
+ * @attention
+ *
+ ******************************************************************************
+ */
+#ifndef _DWT_H
+#define _DWT_H
+
+#include "main.h"
+#include "stdint.h"
+
+typedef struct
+{
+    uint32_t s;
+    uint16_t ms;
+    uint16_t us;
+} DWT_Time_t;
+
+void DWT_Init(uint32_t CPU_Freq_mHz);
+float DWT_GetDeltaT(uint32_t *cnt_last);
+double DWT_GetDeltaT64(uint32_t *cnt_last);
+float DWT_GetTimeline_s(void);
+float DWT_GetTimeline_ms(void);
+uint64_t DWT_GetTimeline_us(void);
+void DWT_Delay(float Delay);
+void DWT_SysTimeUpdate(void);
+
+extern DWT_Time_t SysTime;
+
+#endif /* DWT_H_ */

User/bsp/gpio.c

@@ -0,0 +1,114 @@
+/* Includes ----------------------------------------------------------------- */
+#include "bsp/gpio.h"
+
+#include <gpio.h>
+#include <main.h>
+
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+typedef struct {
+  uint16_t pin;
+  GPIO_TypeDef *gpio;
+} BSP_GPIO_MAP_t;
+
+/* Private variables -------------------------------------------------------- */
+static const BSP_GPIO_MAP_t GPIO_Map[BSP_GPIO_NUM] = {
+    {USER_KEY_Pin, USER_KEY_GPIO_Port},
+    {ACCL_CS_Pin, ACCL_CS_GPIO_Port},
+    {GYRO_CS_Pin, GYRO_CS_GPIO_Port},
+    {SPI2_CS_Pin, SPI2_CS_GPIO_Port},
+    {HW0_Pin, HW0_GPIO_Port},
+    {HW1_Pin, HW1_GPIO_Port},
+    {HW2_Pin, HW2_GPIO_Port},
+    {ACCL_INT_Pin, ACCL_INT_GPIO_Port},
+    {GYRO_INT_Pin, GYRO_INT_GPIO_Port},
+    {CMPS_INT_Pin, CMPS_INT_GPIO_Port},
+    {CMPS_RST_Pin, CMPS_RST_GPIO_Port},
+};
+
+static void (*GPIO_Callback[16])(void);
+
+/* Private function  -------------------------------------------------------- */
+void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
+  for (uint8_t i = 0; i < 16; i++) {
+    if (GPIO_Pin & (1 << i)) {
+      if (GPIO_Callback[i]) {
+        GPIO_Callback[i]();
+      }
+    }
+  }
+}
+
+/* Exported functions ------------------------------------------------------- */
+int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, void (*callback)(void)) {
+  if (callback == NULL) return BSP_ERR_NULL;
+  if (gpio >= BSP_GPIO_NUM) return BSP_ERR;
+
+  // 从GPIO映射中获取对应的pin值
+  uint16_t pin = GPIO_Map[gpio].pin;
+  
+  for (uint8_t i = 0; i < 16; i++) {
+    if (pin & (1 << i)) {
+      GPIO_Callback[i] = callback;
+      break;
+    }
+  }
+  return BSP_OK;
+}
+
+int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) {
+  switch (gpio) {
+    case BSP_GPIO_USER_KEY:
+      HAL_NVIC_EnableIRQ(USER_KEY_EXTI_IRQn);
+      break;
+    case BSP_GPIO_ACCL_INT:
+      HAL_NVIC_EnableIRQ(ACCL_INT_EXTI_IRQn);
+      break;
+    case BSP_GPIO_GYRO_INT:
+      HAL_NVIC_EnableIRQ(GYRO_INT_EXTI_IRQn);
+      break;
+    case BSP_GPIO_CMPS_INT:
+      HAL_NVIC_EnableIRQ(CMPS_INT_EXTI_IRQn);
+      break;
+    default:
+      return BSP_ERR;
+  }
+  return BSP_OK;
+}
+
+int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) {
+  switch (gpio) {
+    case BSP_GPIO_USER_KEY:
+      HAL_NVIC_DisableIRQ(USER_KEY_EXTI_IRQn);
+      break;
+    case BSP_GPIO_ACCL_INT:
+      HAL_NVIC_DisableIRQ(ACCL_INT_EXTI_IRQn);
+      break;
+    case BSP_GPIO_GYRO_INT:
+      HAL_NVIC_DisableIRQ(GYRO_INT_EXTI_IRQn);
+      break;
+    case BSP_GPIO_CMPS_INT:
+      HAL_NVIC_DisableIRQ(CMPS_INT_EXTI_IRQn);
+      break;
+    default:
+      return BSP_ERR;
+  }
+  return BSP_OK;
+}
+int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value){
+  if (gpio >= BSP_GPIO_NUM) return BSP_ERR;
+  HAL_GPIO_WritePin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin, value);
+  return BSP_OK;
+}
+
+int8_t BSP_GPIO_TogglePin(BSP_GPIO_t gpio){
+  if (gpio >= BSP_GPIO_NUM) return BSP_ERR;
+  HAL_GPIO_TogglePin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin);
+  return BSP_OK;
+}
+
+bool BSP_GPIO_ReadPin(BSP_GPIO_t gpio){
+  if (gpio >= BSP_GPIO_NUM) return false;
+  return HAL_GPIO_ReadPin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin) == GPIO_PIN_SET;
+}

User/bsp/gpio.h

@@ -0,0 +1,45 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "bsp/bsp.h"
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef enum {
+  BSP_GPIO_USER_KEY,
+  BSP_GPIO_ACCL_CS,
+  BSP_GPIO_GYRO_CS,
+  BSP_GPIO_SPI2_CS,
+  BSP_GPIO_HW0,
+  BSP_GPIO_HW1,
+  BSP_GPIO_HW2,
+  BSP_GPIO_ACCL_INT,
+  BSP_GPIO_GYRO_INT,
+  BSP_GPIO_CMPS_INT,
+  BSP_GPIO_CMPS_RST,
+  BSP_GPIO_NUM,
+  BSP_GPIO_ERR,
+} BSP_GPIO_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, void (*callback)(void));
+
+int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio);
+int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio);
+
+int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value);
+int8_t BSP_GPIO_TogglePin(BSP_GPIO_t gpio);
+
+bool BSP_GPIO_ReadPin(BSP_GPIO_t gpio);
+
+#ifdef __cplusplus
+}
+#endif

User/bsp/mm.c

@@ -0,0 +1,14 @@
+/* Includes ----------------------------------------------------------------- */
+#include "bsp\mm.h"
+
+#include "FreeRTOS.h"
+
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+/* Private function  -------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+inline void *BSP_Malloc(size_t size) { return pvPortMalloc(size); }
+
+inline void BSP_Free(void *pv) { vPortFree(pv); }

User/bsp/mm.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stddef.h>
+#include <stdint.h>
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+/* Exported functions prototypes -------------------------------------------- */
+void *BSP_Malloc(size_t size);
+void BSP_Free(void *pv);
+
+#ifdef __cplusplus
+}
+#endif

User/bsp/pwm.c

@@ -0,0 +1,109 @@
+/* Includes ----------------------------------------------------------------- */
+#include "tim.h"
+#include "bsp/pwm.h"
+#include "bsp.h"
+
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+typedef struct {
+  TIM_HandleTypeDef *tim;
+  uint16_t channel;
+} BSP_PWM_Config_t;
+
+/* Private variables -------------------------------------------------------- */
+static const BSP_PWM_Config_t PWM_Map[BSP_PWM_NUM] = {
+  {&htim8, TIM_CHANNEL_1},
+  {&htim3, TIM_CHANNEL_3},
+  {&htim4, TIM_CHANNEL_3},
+  {&htim1, TIM_CHANNEL_2},
+  {&htim1, TIM_CHANNEL_3},
+  {&htim1, TIM_CHANNEL_4},
+  {&htim1, TIM_CHANNEL_1},
+  {&htim10, TIM_CHANNEL_1},
+  {&htim5, TIM_CHANNEL_1},
+  {&htim5, TIM_CHANNEL_2},
+  {&htim5, TIM_CHANNEL_3},
+  {&htim8, TIM_CHANNEL_2},
+};
+
+/* Private function  -------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch) {
+  if (ch >= BSP_PWM_NUM) return BSP_ERR;
+  
+  HAL_TIM_PWM_Start(PWM_Map[ch].tim, PWM_Map[ch].channel);
+  return BSP_OK;
+}
+
+int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle) {
+  if (ch >= BSP_PWM_NUM) return BSP_ERR;
+  
+  if (duty_cycle > 1.0f) {
+    duty_cycle = 1.0f;
+  }
+  if (duty_cycle < 0.0f) {
+    duty_cycle = 0.0f;
+  }
+  
+  // 获取ARR值（周期值）
+  uint32_t arr = __HAL_TIM_GET_AUTORELOAD(PWM_Map[ch].tim);
+  
+  // 计算比较值：CCR = duty_cycle * (ARR + 1)
+  uint32_t ccr = (uint32_t)(duty_cycle * (arr + 1));
+  
+  __HAL_TIM_SET_COMPARE(PWM_Map[ch].tim, PWM_Map[ch].channel, ccr);
+  return BSP_OK;
+}
+
+int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq) {
+  if (ch >= BSP_PWM_NUM) return BSP_ERR;
+
+  uint32_t timer_clock = HAL_RCC_GetPCLK1Freq(); // Get the timer clock frequency
+  uint32_t prescaler = PWM_Map[ch].tim->Init.Prescaler;
+  uint32_t period = (timer_clock / (prescaler + 1)) / freq - 1;
+  
+  if (period > UINT16_MAX) {
+    return BSP_ERR; // Frequency too low
+  }
+  __HAL_TIM_SET_AUTORELOAD(PWM_Map[ch].tim, period);
+  
+  return BSP_OK;
+}
+
+int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch) {
+  if (ch >= BSP_PWM_NUM) return BSP_ERR;
+  
+  HAL_TIM_PWM_Stop(PWM_Map[ch].tim, PWM_Map[ch].channel);
+  return BSP_OK;
+}
+
+uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch) {
+    if (ch >= BSP_PWM_NUM) return BSP_ERR;
+    return PWM_Map[ch].tim->Init.AutoReloadPreload;  
+}
+
+TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch) {
+  return PWM_Map[ch].tim;
+}
+
+
+uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch) {
+  if (ch >= BSP_PWM_NUM) return BSP_ERR;
+  return PWM_Map[ch].channel;
+}
+
+int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length) {
+  if (ch >= BSP_PWM_NUM) return BSP_ERR;
+  
+  HAL_TIM_PWM_Start_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel, pData, Length);
+  return BSP_OK;
+}
+
+int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch) {
+  if (ch >= BSP_PWM_NUM) return BSP_ERR;
+  
+  HAL_TIM_PWM_Stop_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel);
+  return BSP_OK;
+}

User/bsp/pwm.h

@@ -0,0 +1,47 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdint.h>
+#include "tim.h"
+#include "bsp.h"
+
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+/* PWM通道 */
+typedef enum {
+  BSP_PWM_TIM8_CH1,
+  BSP_PWM_LASER,
+  BSP_PWM_BUZZER,
+  BSP_PWM_TIM1_CH2,
+  BSP_PWM_TIM1_CH3,
+  BSP_PWM_TIM1_CH4,
+  BSP_PWM_TIM1_CH1,
+  BSP_PWM_IMU_HEAT_PWM,
+  BSP_PWM_LED_B,
+  BSP_PWM_LED_G,
+  BSP_PWM_LED_R,
+  BSP_PWM_TIM8_CH2,
+  BSP_PWM_NUM,
+  BSP_PWM_ERR,
+} BSP_PWM_Channel_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch);
+int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle);
+int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq);
+int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch);
+uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch);
+uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch);
+TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch);
+int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length);
+int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch);
+
+#ifdef __cplusplus
+}
+#endif

User/bsp/spi.c

@@ -0,0 +1,165 @@
+/* Includes ----------------------------------------------------------------- */
+#include <spi.h>
+#include "bsp/spi.h"
+
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+static void (*SPI_Callback[BSP_SPI_NUM][BSP_SPI_CB_NUM])(void);
+
+/* Private function  -------------------------------------------------------- */
+static BSP_SPI_t SPI_Get(SPI_HandleTypeDef *hspi) {
+  if (hspi->Instance == SPI1)
+    return BSP_SPI_BMI088;
+  else
+    return BSP_SPI_ERR;
+}
+
+void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi) {
+  BSP_SPI_t bsp_spi = SPI_Get(hspi);
+  if (bsp_spi != BSP_SPI_ERR) {
+    if (SPI_Callback[bsp_spi][BSP_SPI_TX_CPLT_CB]) {
+      SPI_Callback[bsp_spi][BSP_SPI_TX_CPLT_CB]();
+    }
+  }
+}
+
+void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi) {
+  BSP_SPI_t bsp_spi = SPI_Get(hspi);
+  if (bsp_spi != BSP_SPI_ERR) {
+    if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_CPLT_CB])
+      SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_CPLT_CB]();
+  }
+}
+
+void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
+  BSP_SPI_t bsp_spi = SPI_Get(hspi);
+  if (bsp_spi != BSP_SPI_ERR) {
+    if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_CPLT_CB])
+      SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_CPLT_CB]();
+  }
+}
+
+void HAL_SPI_TxHalfCpltCallback(SPI_HandleTypeDef *hspi) {
+  BSP_SPI_t bsp_spi = SPI_Get(hspi);
+  if (bsp_spi != BSP_SPI_ERR) {
+    if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_HALF_CPLT_CB])
+      SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_HALF_CPLT_CB]();
+  }
+}
+
+void HAL_SPI_RxHalfCpltCallback(SPI_HandleTypeDef *hspi) {
+  BSP_SPI_t bsp_spi = SPI_Get(hspi);
+  if (bsp_spi != BSP_SPI_ERR) {
+    if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_HALF_CPLT_CB])
+      SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_HALF_CPLT_CB]();
+  }
+}
+
+void HAL_SPI_TxRxHalfCpltCallback(SPI_HandleTypeDef *hspi) {
+  BSP_SPI_t bsp_spi = SPI_Get(hspi);
+  if (bsp_spi != BSP_SPI_ERR) {
+    if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_HALF_CPLT_CB])
+      SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_HALF_CPLT_CB]();
+  }
+}
+
+void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi) {
+  BSP_SPI_t bsp_spi = SPI_Get(hspi);
+  if (bsp_spi != BSP_SPI_ERR) {
+    if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_ERROR_CB])
+      SPI_Callback[SPI_Get(hspi)][BSP_SPI_ERROR_CB]();
+  }
+}
+
+void HAL_SPI_AbortCpltCallback(SPI_HandleTypeDef *hspi) {
+  BSP_SPI_t bsp_spi = SPI_Get(hspi);
+  if (bsp_spi != BSP_SPI_ERR) {
+    if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_ABORT_CPLT_CB])
+      SPI_Callback[SPI_Get(hspi)][BSP_SPI_ABORT_CPLT_CB]();
+  }
+}
+
+/* Exported functions ------------------------------------------------------- */
+SPI_HandleTypeDef *BSP_SPI_GetHandle(BSP_SPI_t spi) {
+  switch (spi) {
+    case BSP_SPI_BMI088:
+      return &hspi1;
+    default:
+      return NULL;
+  }
+}
+
+int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type,
+                                void (*callback)(void)) {
+  if (callback == NULL) return BSP_ERR_NULL;
+  SPI_Callback[spi][type] = callback;
+  return BSP_OK;
+}
+
+int8_t BSP_SPI_Transmit(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma) {
+  if (spi >= BSP_SPI_NUM) return BSP_ERR;
+  SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi);
+  if (hspi == NULL) return BSP_ERR;
+
+  if (dma) {
+    return HAL_SPI_Transmit_DMA(hspi, data, size)!= HAL_OK;;
+  } else {
+    return HAL_SPI_Transmit(hspi, data, size, 20)!= HAL_OK;;
+  }
+}
+
+int8_t BSP_SPI_Receive(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma) {
+  if (spi >= BSP_SPI_NUM) return BSP_ERR;
+  SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi);
+  if (hspi == NULL) return BSP_ERR;
+
+  if (dma) {
+    return HAL_SPI_Receive_DMA(hspi, data, size)!= HAL_OK;;
+  } else {
+    return HAL_SPI_Receive(hspi, data, size, 20)!= HAL_OK;;
+  }
+}
+
+int8_t BSP_SPI_TransmitReceive(BSP_SPI_t spi, uint8_t *txData, uint8_t *rxData,
+                               uint16_t size, bool dma) {
+  if (spi >= BSP_SPI_NUM) return BSP_ERR;
+  SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi);
+  if (hspi == NULL) return BSP_ERR;
+  
+  if (dma) {
+    return HAL_SPI_TransmitReceive_DMA(hspi, txData, rxData, size)!= HAL_OK;;
+  } else {
+    return HAL_SPI_TransmitReceive(hspi, txData, rxData, size, 20)!= HAL_OK;;
+  }
+}
+
+uint8_t BSP_SPI_MemReadByte(BSP_SPI_t spi, uint8_t reg) {
+  if (spi >= BSP_SPI_NUM) return 0xFF;
+  uint8_t tmp[2] = {reg | 0x80, 0x00};
+  BSP_SPI_TransmitReceive(spi, tmp, tmp, 2u, true);
+  return tmp[1];
+}
+
+int8_t BSP_SPI_MemWriteByte(BSP_SPI_t spi, uint8_t reg, uint8_t data) {
+  if (spi >= BSP_SPI_NUM) return BSP_ERR;
+  uint8_t tmp[2] = {reg & 0x7f, data};
+  return BSP_SPI_Transmit(spi, tmp, 2u, true);
+}
+
+int8_t BSP_SPI_MemRead(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size) {
+  if (spi >= BSP_SPI_NUM) return BSP_ERR;
+  if (data == NULL || size == 0) return BSP_ERR_NULL;
+  reg = reg | 0x80;
+  BSP_SPI_Transmit(spi, &reg, 1u, true);
+  return BSP_SPI_Receive(spi, data, size, true);
+}
+
+int8_t BSP_SPI_MemWrite(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size) {
+  if (spi >= BSP_SPI_NUM) return BSP_ERR;
+  if (data == NULL || size == 0) return BSP_ERR_NULL;
+  reg = reg & 0x7f;
+  BSP_SPI_Transmit(spi, &reg, 1u, true);
+  return BSP_SPI_Transmit(spi, data, size, true);
+}

User/bsp/spi.h

@@ -0,0 +1,58 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <spi.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "bsp/bsp.h"
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+/* 要添加使用SPI的新设备，需要先在此添加对应的枚举值 */
+
+/* SPI实体枚举，与设备对应 */
+typedef enum {
+  BSP_SPI_BMI088,
+  BSP_SPI_NUM,
+  BSP_SPI_ERR,
+} BSP_SPI_t;
+
+/* SPI支持的中断回调函数类型，具体参考HAL中定义 */
+typedef enum {
+  BSP_SPI_TX_CPLT_CB,
+  BSP_SPI_RX_CPLT_CB,
+  BSP_SPI_TX_RX_CPLT_CB,
+  BSP_SPI_TX_HALF_CPLT_CB,
+  BSP_SPI_RX_HALF_CPLT_CB,
+  BSP_SPI_TX_RX_HALF_CPLT_CB,
+  BSP_SPI_ERROR_CB,
+  BSP_SPI_ABORT_CPLT_CB,
+  BSP_SPI_CB_NUM,
+} BSP_SPI_Callback_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+SPI_HandleTypeDef *BSP_SPI_GetHandle(BSP_SPI_t spi);
+int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type,
+                                void (*callback)(void));
+
+
+int8_t BSP_SPI_Transmit(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma);
+int8_t BSP_SPI_Receive(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma);
+int8_t BSP_SPI_TransmitReceive(BSP_SPI_t spi, uint8_t *txData, uint8_t *rxData,
+                               uint16_t size, bool dma);
+
+uint8_t BSP_SPI_MemReadByte(BSP_SPI_t spi, uint8_t reg);
+int8_t BSP_SPI_MemWriteByte(BSP_SPI_t spi, uint8_t reg, uint8_t data);
+int8_t BSP_SPI_MemRead(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size);
+int8_t BSP_SPI_MemWrite(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size);
+
+#ifdef __cplusplus
+}
+#endif

User/bsp/time.c

@@ -0,0 +1,65 @@
+/* Includes ----------------------------------------------------------------- */
+#include "bsp/time.h"
+#include "bsp.h"
+
+#include <cmsis_os2.h>
+#include "FreeRTOS.h"
+#include "main.h"
+#include "task.h"
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+/* Private function  -------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+uint32_t BSP_TIME_Get_ms() { return xTaskGetTickCount(); }
+
+uint64_t BSP_TIME_Get_us() {
+  uint32_t tick_freq = osKernelGetTickFreq();
+  uint32_t ticks_old = xTaskGetTickCount()*(1000/tick_freq);
+  uint32_t tick_value_old = SysTick->VAL;
+  uint32_t ticks_new = xTaskGetTickCount()*(1000/tick_freq);
+  uint32_t tick_value_new = SysTick->VAL;
+  if (ticks_old == ticks_new) {
+    return ticks_new * 1000 + 1000 - tick_value_old * 1000 / (SysTick->LOAD + 1);
+  } else {
+    return ticks_new * 1000 + 1000 - tick_value_new * 1000 / (SysTick->LOAD + 1);
+  }
+}
+
+uint64_t BSP_TIME_Get() __attribute__((alias("BSP_TIME_Get_us")));
+
+int8_t BSP_TIME_Delay_ms(uint32_t ms) {
+  uint32_t tick_period = 1000u / osKernelGetTickFreq();
+  uint32_t ticks = ms / tick_period;
+
+  switch (osKernelGetState()) {
+    case osKernelError:
+    case osKernelReserved:
+    case osKernelLocked:
+    case osKernelSuspended:
+      return BSP_ERR;
+
+    case osKernelRunning:
+      osDelay(ticks ? ticks : 1);
+      break;
+
+    case osKernelInactive:
+    case osKernelReady:
+      HAL_Delay(ms);
+      break;
+  }
+  return BSP_OK;
+}
+
+/*阻塞us延迟*/
+int8_t BSP_TIME_Delay_us(uint32_t us) {
+    uint64_t start = BSP_TIME_Get_us();
+    while (BSP_TIME_Get_us() - start < us) {
+        // 等待us时间
+    }
+    return BSP_OK;
+}
+
+int8_t BSP_TIME_Delay(uint32_t ms) __attribute__((alias("BSP_TIME_Delay_ms")));

User/bsp/time.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdint.h>
+
+#include "bsp/bsp.h"
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+/* Exported functions prototypes -------------------------------------------- */
+uint32_t BSP_TIME_Get_ms();
+
+uint64_t BSP_TIME_Get_us();
+
+uint64_t BSP_TIME_Get();
+
+int8_t BSP_TIME_Delay_ms(uint32_t ms);
+
+/*微秒阻塞延时，一般别用*/
+int8_t BSP_TIME_Delay_us(uint32_t us);
+
+int8_t BSP_TIME_Delay(uint32_t ms);
+
+#ifdef __cplusplus
+}
+#endif

User/bsp/uart.c

@@ -0,0 +1,139 @@
+/* Includes ----------------------------------------------------------------- */
+#include <usart.h>
+
+#include "bsp/uart.h"
+
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+static void (*UART_Callback[BSP_UART_NUM][BSP_UART_CB_NUM])(void);
+
+/* Private function  -------------------------------------------------------- */
+static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
+  if (huart->Instance == USART3)
+    return BSP_UART_DR16;
+  else
+    return BSP_UART_ERR;
+}
+
+void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart) {
+  BSP_UART_t bsp_uart = UART_Get(huart);
+  if (bsp_uart != BSP_UART_ERR) {
+    if (UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB]) {
+      UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB]();
+    }
+  }
+}
+
+void HAL_UART_TxHalfCpltCallback(UART_HandleTypeDef *huart) {
+  BSP_UART_t bsp_uart = UART_Get(huart);
+  if (bsp_uart != BSP_UART_ERR) {
+    if (UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB]) {
+      UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB]();
+    }
+  }
+}
+
+void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
+  BSP_UART_t bsp_uart = UART_Get(huart);
+  if (bsp_uart != BSP_UART_ERR) {
+    if (UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB]) {
+      UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB]();
+    }
+  }
+}
+
+void HAL_UART_RxHalfCpltCallback(UART_HandleTypeDef *huart) {
+  BSP_UART_t bsp_uart = UART_Get(huart);
+  if (bsp_uart != BSP_UART_ERR) {
+    if (UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB]) {
+      UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB]();
+    }
+  }
+}
+
+void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) {
+  BSP_UART_t bsp_uart = UART_Get(huart);
+  if (bsp_uart != BSP_UART_ERR) {
+    if (UART_Callback[bsp_uart][BSP_UART_ERROR_CB]) {
+      UART_Callback[bsp_uart][BSP_UART_ERROR_CB]();
+    }
+  }
+}
+
+void HAL_UART_AbortCpltCallback(UART_HandleTypeDef *huart) {
+  BSP_UART_t bsp_uart = UART_Get(huart);
+  if (bsp_uart != BSP_UART_ERR) {
+    if (UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB]) {
+      UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB]();
+    }
+  }
+}
+
+void HAL_UART_AbortTransmitCpltCallback(UART_HandleTypeDef *huart) {
+  BSP_UART_t bsp_uart = UART_Get(huart);
+  if (bsp_uart != BSP_UART_ERR) {
+    if (UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB]) {
+      UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB]();
+    }
+  }
+}
+
+void HAL_UART_AbortReceiveCpltCallback(UART_HandleTypeDef *huart) {
+  BSP_UART_t bsp_uart = UART_Get(huart);
+  if (bsp_uart != BSP_UART_ERR) {
+    if (UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB]) {
+      UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB]();
+    }
+  }
+}
+
+/* Exported functions ------------------------------------------------------- */
+void BSP_UART_IRQHandler(UART_HandleTypeDef *huart) {
+  if (__HAL_UART_GET_FLAG(huart, UART_FLAG_IDLE)) {
+    __HAL_UART_CLEAR_IDLEFLAG(huart);
+    if (UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB]) {
+      UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB]();
+    }
+  }
+}
+
+UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
+  switch (uart) {
+    case BSP_UART_DR16:
+      return &huart3;
+    default:
+      return NULL;
+  }
+}
+
+int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type,
+                                 void (*callback)(void)) {
+  if (callback == NULL) return BSP_ERR_NULL;
+  if (uart >= BSP_UART_NUM || type >= BSP_UART_CB_NUM) return BSP_ERR;
+  UART_Callback[uart][type] = callback;
+  return BSP_OK;
+}
+
+int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma) {
+  if (uart >= BSP_UART_NUM) return BSP_ERR;
+  if (data == NULL || size == 0) return BSP_ERR_NULL;
+
+  if (dma) {
+    return HAL_UART_Transmit_DMA(BSP_UART_GetHandle(uart), data, size);
+  } else {
+    return HAL_UART_Transmit_IT(BSP_UART_GetHandle(uart), data, size);
+  }
+}
+
+int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma) {
+  if (uart >= BSP_UART_NUM) return BSP_ERR;
+  if (data == NULL || size == 0) return BSP_ERR_NULL;
+
+  if (dma) {
+    return HAL_UART_Receive_DMA(BSP_UART_GetHandle(uart), data, size);
+  } else {
+    return HAL_UART_Receive_IT(BSP_UART_GetHandle(uart), data, size);
+  }
+}

User/bsp/uart.h

@@ -0,0 +1,53 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <usart.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "bsp/bsp.h"
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+/* 要添加使用UART的新设备，需要先在此添加对应的枚举值 */
+
+/* UART实体枚举，与设备对应 */
+typedef enum {
+  BSP_UART_DR16,
+  BSP_UART_NUM,
+  BSP_UART_ERR,
+} BSP_UART_t;
+
+/* UART支持的中断回调函数类型，具体参考HAL中定义 */
+typedef enum {
+  BSP_UART_TX_HALF_CPLT_CB,
+  BSP_UART_TX_CPLT_CB,
+  BSP_UART_RX_HALF_CPLT_CB,
+  BSP_UART_RX_CPLT_CB,
+  BSP_UART_ERROR_CB,
+  BSP_UART_ABORT_CPLT_CB,
+  BSP_UART_ABORT_TX_CPLT_CB,
+  BSP_UART_ABORT_RX_CPLT_CB,
+
+  BSP_UART_IDLE_LINE_CB,
+  BSP_UART_CB_NUM,
+} BSP_UART_Callback_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart);
+int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type,
+                                 void (*callback)(void));
+
+int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma);
+int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma);
+
+#ifdef __cplusplus
+}
+#endif

User/component/ahrs.c

@@ -0,0 +1,405 @@
+/*
+  开源的AHRS算法。
+  MadgwickAHRS
+*/
+
+#include "ahrs.h"
+
+#include <string.h>
+
+#include "user_math.h"
+
+#define BETA_IMU (0.033f)
+#define BETA_AHRS (0.041f)
+
+/* 2 * proportional gain (Kp) */
+static float beta = BETA_IMU;
+
+/**
+ * @brief 不使用磁力计计算姿态
+ *
+ * @param ahrs 姿态解算主结构体
+ * @param accl 加速度计数据
+ * @param gyro 陀螺仪数据
+ * @return int8_t 0对应没有错误
+ */
+static int8_t AHRS_UpdateIMU(AHRS_t *ahrs, const AHRS_Accl_t *accl,
+                             const AHRS_Gyro_t *gyro) {
+  if (ahrs == NULL) return -1;
+  if (accl == NULL) return -1;
+  if (gyro == NULL) return -1;
+
+  beta = BETA_IMU;
+
+  float ax = accl->x;
+  float ay = accl->y;
+  float az = accl->z;
+
+  float gx = gyro->x;
+  float gy = gyro->y;
+  float gz = gyro->z;
+
+  float recip_norm;
+  float s0, s1, s2, s3;
+  float q_dot1, q_dot2, q_dot3, q_dot4;
+  float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2,
+      q3q3;
+
+  /* Rate of change of quaternion from gyroscope */
+  q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy -
+                   ahrs->quat.q3 * gz);
+  q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz -
+                   ahrs->quat.q3 * gy);
+  q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz +
+                   ahrs->quat.q3 * gx);
+  q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy -
+                   ahrs->quat.q2 * gx);
+
+  /* Compute feedback only if accelerometer measurement valid (avoids NaN in
+   * accelerometer normalisation) */
+  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+    /* Normalise accelerometer measurement */
+    recip_norm = InvSqrt(ax * ax + ay * ay + az * az);
+    ax *= recip_norm;
+    ay *= recip_norm;
+    az *= recip_norm;
+
+    /* Auxiliary variables to avoid repeated arithmetic */
+    _2q0 = 2.0f * ahrs->quat.q0;
+    _2q1 = 2.0f * ahrs->quat.q1;
+    _2q2 = 2.0f * ahrs->quat.q2;
+    _2q3 = 2.0f * ahrs->quat.q3;
+    _4q0 = 4.0f * ahrs->quat.q0;
+    _4q1 = 4.0f * ahrs->quat.q1;
+    _4q2 = 4.0f * ahrs->quat.q2;
+    _8q1 = 8.0f * ahrs->quat.q1;
+    _8q2 = 8.0f * ahrs->quat.q2;
+    q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
+    q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
+    q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
+    q3q3 = ahrs->quat.q3 * ahrs->quat.q3;
+
+    /* Gradient decent algorithm corrective step */
+    s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
+    s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * ahrs->quat.q1 -
+         _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
+    s2 = 4.0f * q0q0 * ahrs->quat.q2 + _2q0 * ax + _4q2 * q3q3 -
+         _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
+    s3 = 4.0f * q1q1 * ahrs->quat.q3 - _2q1 * ax +
+         4.0f * q2q2 * ahrs->quat.q3 - _2q2 * ay;
+
+    /* normalise step magnitude */
+    recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
+
+    s0 *= recip_norm;
+    s1 *= recip_norm;
+    s2 *= recip_norm;
+    s3 *= recip_norm;
+
+    /* Apply feedback step */
+    q_dot1 -= beta * s0;
+    q_dot2 -= beta * s1;
+    q_dot3 -= beta * s2;
+    q_dot4 -= beta * s3;
+  }
+
+  /* Integrate rate of change of quaternion to yield quaternion */
+  ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq;
+  ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq;
+  ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq;
+  ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq;
+
+  /* Normalise quaternion */
+  recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 +
+                       ahrs->quat.q1 * ahrs->quat.q1 +
+                       ahrs->quat.q2 * ahrs->quat.q2 +
+                       ahrs->quat.q3 * ahrs->quat.q3);
+  ahrs->quat.q0 *= recip_norm;
+  ahrs->quat.q1 *= recip_norm;
+  ahrs->quat.q2 *= recip_norm;
+  ahrs->quat.q3 *= recip_norm;
+
+  return 0;
+}
+
+/**
+ * @brief 初始化姿态解算
+ *
+ * @param ahrs 姿态解算主结构体
+ * @param magn 磁力计数据
+ * @param sample_freq 采样频率
+ * @return int8_t 0对应没有错误
+ */
+int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq) {
+  if (ahrs == NULL) return -1;
+
+  ahrs->inv_sample_freq = 1.0f / sample_freq;
+
+  ahrs->quat.q0 = 1.0f;
+  ahrs->quat.q1 = 0.0f;
+  ahrs->quat.q2 = 0.0f;
+  ahrs->quat.q3 = 0.0f;
+
+  if (magn) {
+    float yaw = -atan2(magn->y, magn->x);
+
+    if ((magn->x == 0.0f) && (magn->y == 0.0f) && (magn->z == 0.0f)) {
+      ahrs->quat.q0 = 0.800884545f;
+      ahrs->quat.q1 = 0.00862364192f;
+      ahrs->quat.q2 = -0.00283267116f;
+      ahrs->quat.q3 = 0.598749936f;
+
+    } else if ((yaw < (M_PI / 2.0f)) || (yaw > 0.0f)) {
+      ahrs->quat.q0 = 0.997458339f;
+      ahrs->quat.q1 = 0.000336312107f;
+      ahrs->quat.q2 = -0.0057230792f;
+      ahrs->quat.q3 = 0.0740156546;
+
+    } else if ((yaw < M_PI) || (yaw > (M_PI / 2.0f))) {
+      ahrs->quat.q0 = 0.800884545f;
+      ahrs->quat.q1 = 0.00862364192f;
+      ahrs->quat.q2 = -0.00283267116f;
+      ahrs->quat.q3 = 0.598749936f;
+
+    } else if ((yaw < 90.0f) || (yaw > M_PI)) {
+      ahrs->quat.q0 = 0.800884545f;
+      ahrs->quat.q1 = 0.00862364192f;
+      ahrs->quat.q2 = -0.00283267116f;
+      ahrs->quat.q3 = 0.598749936f;
+
+    } else if ((yaw < 90.0f) || (yaw > 0.0f)) {
+      ahrs->quat.q0 = 0.800884545f;
+      ahrs->quat.q1 = 0.00862364192f;
+      ahrs->quat.q2 = -0.00283267116f;
+      ahrs->quat.q3 = 0.598749936f;
+    }
+  }
+  return 0;
+}
+
+/**
+ * @brief 姿态运算更新一次
+ * @note 输入数据必须是NED(North East Down) 参考坐标系
+ *
+ * @param ahrs 姿态解算主结构体
+ * @param accl 加速度计数据
+ * @param gyro 陀螺仪数据
+ * @param magn 磁力计数据
+ * @return int8_t 0对应没有错误
+ */
+int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl,
+                   const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn) {
+  if (ahrs == NULL) return -1;
+  if (accl == NULL) return -1;
+  if (gyro == NULL) return -1;
+
+  beta = BETA_AHRS;
+
+  float recip_norm;
+  float s0, s1, s2, s3;
+  float q_dot1, q_dot2, q_dot3, q_dot4;
+  float hx, hy;
+  float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1,
+      _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3,
+      q2q2, q2q3, q3q3;
+
+  if (magn == NULL) return AHRS_UpdateIMU(ahrs, accl, gyro);
+
+  float mx = magn->x;
+  float my = magn->y;
+  float mz = magn->z;
+
+  /* Use IMU algorithm if magnetometer measurement invalid (avoids NaN in */
+  /* magnetometer normalisation) */
+  if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
+    return AHRS_UpdateIMU(ahrs, accl, gyro);
+  }
+
+  float ax = accl->x;
+  float ay = accl->y;
+  float az = accl->z;
+
+  float gx = gyro->x;
+  float gy = gyro->y;
+  float gz = gyro->z;
+
+  /* Rate of change of quaternion from gyroscope */
+  q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy -
+                   ahrs->quat.q3 * gz);
+  q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz -
+                   ahrs->quat.q3 * gy);
+  q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz +
+                   ahrs->quat.q3 * gx);
+  q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy -
+                   ahrs->quat.q2 * gx);
+
+  /* Compute feedback only if accelerometer measurement valid (avoids NaN in
+   * accelerometer normalisation) */
+  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+    /* Normalise accelerometer measurement */
+    recip_norm = InvSqrt(ax * ax + ay * ay + az * az);
+    ax *= recip_norm;
+    ay *= recip_norm;
+    az *= recip_norm;
+
+    /* Normalise magnetometer measurement */
+    recip_norm = InvSqrt(mx * mx + my * my + mz * mz);
+    mx *= recip_norm;
+    my *= recip_norm;
+    mz *= recip_norm;
+
+    /* Auxiliary variables to avoid repeated arithmetic */
+    _2q0mx = 2.0f * ahrs->quat.q0 * mx;
+    _2q0my = 2.0f * ahrs->quat.q0 * my;
+    _2q0mz = 2.0f * ahrs->quat.q0 * mz;
+    _2q1mx = 2.0f * ahrs->quat.q1 * mx;
+    _2q0 = 2.0f * ahrs->quat.q0;
+    _2q1 = 2.0f * ahrs->quat.q1;
+    _2q2 = 2.0f * ahrs->quat.q2;
+    _2q3 = 2.0f * ahrs->quat.q3;
+    _2q0q2 = 2.0f * ahrs->quat.q0 * ahrs->quat.q2;
+    _2q2q3 = 2.0f * ahrs->quat.q2 * ahrs->quat.q3;
+    q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
+    q0q1 = ahrs->quat.q0 * ahrs->quat.q1;
+    q0q2 = ahrs->quat.q0 * ahrs->quat.q2;
+    q0q3 = ahrs->quat.q0 * ahrs->quat.q3;
+    q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
+    q1q2 = ahrs->quat.q1 * ahrs->quat.q2;
+    q1q3 = ahrs->quat.q1 * ahrs->quat.q3;
+    q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
+    q2q3 = ahrs->quat.q2 * ahrs->quat.q3;
+    q3q3 = ahrs->quat.q3 * ahrs->quat.q3;
+
+    /* Reference direction of Earth's magnetic field */
+    hx = mx * q0q0 - _2q0my * ahrs->quat.q3 +
+         _2q0mz * ahrs->quat.q2 + mx * q1q1 +
+         _2q1 * my * ahrs->quat.q2 + _2q1 * mz * ahrs->quat.q3 -
+         mx * q2q2 - mx * q3q3;
+    hy = _2q0mx * ahrs->quat.q3 + my * q0q0 -
+         _2q0mz * ahrs->quat.q1 + _2q1mx * ahrs->quat.q2 -
+         my * q1q1 + my * q2q2 + _2q2 * mz * ahrs->quat.q3 - my * q3q3;
+    // _2bx = sqrtf(hx * hx + hy * hy);
+    // 改为invsqrt
+    _2bx = 1.f / InvSqrt(hx * hx + hy * hy);
+    _2bz = -_2q0mx * ahrs->quat.q2 + _2q0my * ahrs->quat.q1 +
+           mz * q0q0 + _2q1mx * ahrs->quat.q3 - mz * q1q1 +
+           _2q2 * my * ahrs->quat.q3 - mz * q2q2 + mz * q3q3;
+    _4bx = 2.0f * _2bx;
+    _4bz = 2.0f * _2bz;
+
+    /* Gradient decent algorithm corrective step */
+    s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q1 * (2.0f * q0q1 + _2q2q3 - ay) -
+         _2bz * ahrs->quat.q2 *
+             (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (-_2bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         _2bx * ahrs->quat.q2 *
+             (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q0 * (2.0f * q0q1 + _2q2q3 - ay) -
+         4.0f * ahrs->quat.q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
+         _2bz * ahrs->quat.q3 *
+             (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (_2bx * ahrs->quat.q2 + _2bz * ahrs->quat.q0) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         (_2bx * ahrs->quat.q3 - _4bz * ahrs->quat.q1) *
+             (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q3 * (2.0f * q0q1 + _2q2q3 - ay) -
+         4.0f * ahrs->quat.q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
+         (-_4bx * ahrs->quat.q2 - _2bz * ahrs->quat.q0) *
+             (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (_2bx * ahrs->quat.q1 + _2bz * ahrs->quat.q3) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         (_2bx * ahrs->quat.q0 - _4bz * ahrs->quat.q2) *
+             (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q2 * (2.0f * q0q1 + _2q2q3 - ay) +
+         (-_4bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) *
+             (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (-_2bx * ahrs->quat.q0 + _2bz * ahrs->quat.q2) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         _2bx * ahrs->quat.q1 *
+             (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    /* normalise step magnitude */
+    recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
+    s0 *= recip_norm;
+    s1 *= recip_norm;
+    s2 *= recip_norm;
+    s3 *= recip_norm;
+
+    /* Apply feedback step */
+    q_dot1 -= beta * s0;
+    q_dot2 -= beta * s1;
+    q_dot3 -= beta * s2;
+    q_dot4 -= beta * s3;
+  }
+
+  /* Integrate rate of change of quaternion to yield quaternion */
+  ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq;
+  ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq;
+  ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq;
+  ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq;
+
+  /* Normalise quaternion */
+  recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 +
+                       ahrs->quat.q1 * ahrs->quat.q1 +
+                       ahrs->quat.q2 * ahrs->quat.q2 +
+                       ahrs->quat.q3 * ahrs->quat.q3);
+  ahrs->quat.q0 *= recip_norm;
+  ahrs->quat.q1 *= recip_norm;
+  ahrs->quat.q2 *= recip_norm;
+  ahrs->quat.q3 *= recip_norm;
+
+  return 0;
+}
+
+/**
+ * @brief 通过姿态解算主结构体中的四元数计算欧拉角
+ *
+ * @param eulr 欧拉角
+ * @param ahrs 姿态解算主结构体
+ * @return int8_t 0对应没有错误
+ */
+int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs) {
+  if (eulr == NULL) return -1;
+  if (ahrs == NULL) return -1;
+
+  const float sinr_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q1 +
+                                  ahrs->quat.q2 * ahrs->quat.q3);
+  const float cosr_cosp =
+      1.0f - 2.0f * (ahrs->quat.q1 * ahrs->quat.q1 +
+                     ahrs->quat.q2 * ahrs->quat.q2);
+  eulr->pit = atan2f(sinr_cosp, cosr_cosp);
+
+  const float sinp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q2 -
+                             ahrs->quat.q3 * ahrs->quat.q1);
+
+  if (fabsf(sinp) >= 1.0f)
+    eulr->rol = copysignf(M_PI / 2.0f, sinp);
+  else
+    eulr->rol = asinf(sinp);
+
+  const float siny_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q3 +
+                                  ahrs->quat.q1 * ahrs->quat.q2);
+  const float cosy_cosp =
+      1.0f - 2.0f * (ahrs->quat.q2 * ahrs->quat.q2 +
+                     ahrs->quat.q3 * ahrs->quat.q3);
+  eulr->yaw = atan2f(siny_cosp, cosy_cosp);
+
+#if 0
+  eulr->yaw *= M_RAD2DEG_MULT;
+  eulr->rol *= M_RAD2DEG_MULT;
+  eulr->pit *= M_RAD2DEG_MULT;
+#endif
+
+  return 0;
+}
+
+/**
+ * \brief 将对应数据置零
+ *
+ * \param eulr 被操作的数据
+ */
+void AHRS_ResetEulr(AHRS_Eulr_t *eulr) { memset(eulr, 0, sizeof(*eulr)); }

User/component/ahrs.h

@@ -0,0 +1,98 @@
+/*
+  开源的AHRS算法。
+  MadgwickAHRS
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "user_math.h"
+
+/* 欧拉角（Euler angle） */
+typedef struct {
+  float yaw; /* 偏航角（Yaw angle） */
+  float pit; /* 俯仰角（Pitch angle） */
+  float rol; /* 翻滚角（Roll angle） */
+} AHRS_Eulr_t;
+
+/* 加速度计 Accelerometer */
+typedef struct {
+  float x;
+  float y;
+  float z;
+} AHRS_Accl_t;
+
+/* 陀螺仪 Gyroscope */
+typedef struct {
+  float x;
+  float y;
+  float z;
+} AHRS_Gyro_t;
+
+/* 磁力计 Magnetometer */
+typedef struct {
+  float x;
+  float y;
+  float z;
+} AHRS_Magn_t;
+
+/* 四元数 */
+typedef struct {
+  float q0;
+  float q1;
+  float q2;
+  float q3;
+} AHRS_Quaternion_t;
+
+/* 姿态解算算法主结构体 */
+typedef struct {
+  /* 四元数 */
+  AHRS_Quaternion_t quat;
+
+  float inv_sample_freq; /* 采样频率的的倒数 */
+} AHRS_t;
+
+/**
+ * @brief 初始化姿态解算
+ *
+ * @param ahrs 姿态解算主结构体
+ * @param magn 磁力计数据
+ * @param sample_freq 采样频率
+ * @return int8_t 0对应没有错误
+ */
+int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq);
+
+/**
+ * @brief 姿态运算更新一次
+ *
+ * @param ahrs 姿态解算主结构体
+ * @param accl 加速度计数据
+ * @param gyro 陀螺仪数据
+ * @param magn 磁力计数据
+ * @return int8_t 0对应没有错误
+ */
+int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl,
+                   const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn);
+
+/**
+ * @brief 通过姿态解算主结构体中的四元数计算欧拉角
+ *
+ * @param eulr 欧拉角
+ * @param ahrs 姿态解算主结构体
+ * @return int8_t 0对应没有错误
+ */
+int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs);
+
+/**
+ * \brief 将对应数据置零
+ *
+ * \param eulr 被操作的数据
+ */
+void AHRS_ResetEulr(AHRS_Eulr_t *eulr);
+
+#ifdef __cplusplus
+}
+#endif

User/component/cmd.c

@@ -0,0 +1,375 @@
+/*
+  控制命令
+*/
+
+#include "cmd.h"
+
+#include <string.h>
+
+/**
+ * @brief 行为转换为对应按键
+ *
+ * @param cmd 主结构体
+ * @param behavior 行为
+ * @return uint16_t 行为对应的按键
+ */
+static inline CMD_KeyValue_t CMD_BehaviorToKey(CMD_t *cmd,
+                                               CMD_Behavior_t behavior) {
+  return cmd->param->map.key_map[behavior].key;
+}
+
+static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd,
+                                                    CMD_Behavior_t behavior) {
+  return cmd->param->map.key_map[behavior].active;
+}
+
+/**
+ * @brief 检查按键是否按下
+ *
+ * @param rc 遥控器数据
+ * @param key 按键名称
+ * @param stateful 是否为状态切换按键
+ * @return true 按下
+ * @return false 未按下
+ */
+static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key) {
+  /* 按下按键为鼠标左、右键 */
+  if (key == CMD_L_CLICK) {
+    return rc->mouse.l_click;
+  }
+  if (key == CMD_R_CLICK) {
+    return rc->mouse.r_click;
+  }
+  return rc->key & (1u << key);
+}
+
+static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd,
+                                   CMD_Behavior_t behavior) {
+  CMD_KeyValue_t key = CMD_BehaviorToKey(cmd, behavior);
+  CMD_ActiveType_t active = CMD_BehaviorToActive(cmd, behavior);
+
+  bool now_key_pressed, last_key_pressed;
+
+  /* 按下按键为鼠标左、右键 */
+  if (key == CMD_L_CLICK) {
+    now_key_pressed = rc->mouse.l_click;
+    last_key_pressed = cmd->mouse_last.l_click;
+  } else if (key == CMD_R_CLICK) {
+    now_key_pressed = rc->mouse.r_click;
+    last_key_pressed = cmd->mouse_last.r_click;
+  } else {
+    now_key_pressed = rc->key & (1u << key);
+    last_key_pressed = cmd->key_last & (1u << key);
+  }
+
+  switch (active) {
+    case CMD_ACTIVE_PRESSING:
+      return now_key_pressed && !last_key_pressed;
+    case CMD_ACTIVE_RASING:
+      return !now_key_pressed && last_key_pressed;
+    case CMD_ACTIVE_PRESSED:
+      return now_key_pressed;
+  }
+}
+
+/**
+ * @brief 解析pc行为逻辑
+ *
+ * @param rc 遥控器数据
+ * @param cmd 主结构体
+ * @param dt_sec 两次解析的间隔
+ */
+static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
+  cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
+
+  /* 云台设置为鼠标控制欧拉角的变化，底盘的控制向量设置为零 */
+  cmd->gimbal.delta_eulr.yaw =
+      (float)rc->mouse.x * dt_sec * cmd->param->sens_mouse;
+  cmd->gimbal.delta_eulr.pit =
+      (float)(-rc->mouse.y) * dt_sec * cmd->param->sens_mouse;
+  cmd->chassis.ctrl_vec.vx = cmd->chassis.ctrl_vec.vy = 0.0f;
+  cmd->shoot.reverse_trig = false;
+
+  /* 按键行为映射相关逻辑 */
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE)) {
+    cmd->chassis.ctrl_vec.vy += cmd->param->move.move_sense;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK)) {
+    cmd->chassis.ctrl_vec.vy -= cmd->param->move.move_sense;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT)) {
+    cmd->chassis.ctrl_vec.vx -= cmd->param->move.move_sense;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT)) {
+    cmd->chassis.ctrl_vec.vx += cmd->param->move.move_sense;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE)) {
+    cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_fast_sense;
+    cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_fast_sense;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE)) {
+    cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_slow_sense;
+    cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_slow_sense;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE)) {
+    /* 切换至开火模式，设置相应的射击频率和弹丸初速度 */
+    cmd->shoot.mode = SHOOT_MODE_LOADED;
+    cmd->shoot.fire = true;
+  } else {
+    /* 切换至准备模式，停止射击 */
+    cmd->shoot.mode = SHOOT_MODE_LOADED;
+    cmd->shoot.fire = false;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE)) {
+    /* 每按一次依次切换开火下一个模式 */
+    cmd->shoot.fire_mode++;
+    cmd->shoot.fire_mode %= FIRE_MODE_NUM;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR)) {
+    /* 切换到小陀螺模式 */
+    cmd->chassis.mode = CHASSIS_MODE_ROTOR;
+    cmd->chassis.mode_rotor = ROTOR_MODE_RAND;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER)) {
+    /* 每按一次开、关弹舱盖 */
+    cmd->shoot.cover_open = !cmd->shoot.cover_open;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF)) {
+    if (cmd->ai_status == AI_STATUS_HITSWITCH) {
+      /* 停止ai的打符模式，停用host控制 */
+      CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP);
+      cmd->host_overwrite = false;
+      cmd->ai_status = AI_STATUS_STOP;
+    } else if (cmd->ai_status == AI_STATUS_AUTOAIM) {
+      /* 自瞄模式中切换失败提醒 */
+    } else {
+      /* ai切换至打符模式，启用host控制 */
+      CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START);
+      cmd->ai_status = AI_STATUS_HITSWITCH;
+      cmd->host_overwrite = true;
+    }
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM)) {
+    if (cmd->ai_status == AI_STATUS_AUTOAIM) {
+      /* 停止ai的自瞄模式，停用host控制 */
+      cmd->host_overwrite = false;
+      cmd->ai_status = AI_STATUS_STOP;
+      CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
+    } else {
+      /* ai切换至自瞄模式，启用host控制 */
+      cmd->ai_status = AI_STATUS_AUTOAIM;
+      cmd->host_overwrite = true;
+      CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
+    }
+  } else {
+    cmd->host_overwrite = false;
+    // TODO: 修复逻辑
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG)) {
+    /* 按下拨弹反转 */
+    cmd->shoot.reverse_trig = true;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35)) {
+    cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL_35;
+  }
+  /* 保存当前按下的键位状态 */
+  cmd->key_last = rc->key;
+  memcpy(&(cmd->mouse_last), &(rc->mouse), sizeof(cmd->mouse_last));
+}
+
+/**
+ * @brief 解析rc行为逻辑
+ *
+ * @param rc 遥控器数据
+ * @param cmd 主结构体
+ * @param dt_sec 两次解析的间隔
+ */
+static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
+  switch (rc->sw_l) {
+      /* 左拨杆相应行为选择和解析 */
+    case CMD_SW_UP:
+      cmd->chassis.mode = CHASSIS_MODE_BREAK;
+      break;
+
+    case CMD_SW_MID:
+      cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
+      break;
+
+    case CMD_SW_DOWN:
+      cmd->chassis.mode = CHASSIS_MODE_ROTOR;
+      cmd->chassis.mode_rotor = ROTOR_MODE_CW;
+      break;
+
+    case CMD_SW_ERR:
+      cmd->chassis.mode = CHASSIS_MODE_RELAX;
+      break;
+  }
+  switch (rc->sw_r) {
+      /* 右拨杆相应行为选择和解析*/ 
+    case CMD_SW_UP:
+      cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
+      cmd->shoot.mode = SHOOT_MODE_SAFE;
+      break;
+
+    case CMD_SW_MID:
+      cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
+      cmd->shoot.fire = false;
+      cmd->shoot.mode = SHOOT_MODE_LOADED;
+      break;
+
+    case CMD_SW_DOWN:
+      cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
+      cmd->shoot.mode = SHOOT_MODE_LOADED;
+      cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
+      cmd->shoot.fire = true;
+      break;
+		/*
+		    case CMD_SW_UP:
+      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
+      cmd->shoot.mode = SHOOT_MODE_SAFE;
+      break;
+
+    case CMD_SW_MID:
+      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
+      cmd->shoot.fire = false;
+      cmd->shoot.mode = SHOOT_MODE_LOADED;
+      break;
+
+    case CMD_SW_DOWN:
+      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
+      cmd->shoot.mode = SHOOT_MODE_LOADED;
+      cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
+      cmd->shoot.fire = true;
+      break;
+		*/
+    case CMD_SW_ERR:
+      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
+      cmd->shoot.mode = SHOOT_MODE_RELAX;
+  }
+  /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
+  cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
+  cmd->chassis.ctrl_vec.vy = rc->ch_l_y;
+  cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc;
+  cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc;
+}
+
+/**
+ * @brief rc失控时机器人恢复放松模式
+ *
+ * @param cmd 主结构体
+ */
+static void CMD_RcLostLogic(CMD_t *cmd) {
+  /* 机器人底盘、云台、射击运行模式恢复至放松模式 */
+  cmd->chassis.mode = CHASSIS_MODE_RELAX;
+  cmd->gimbal.mode = GIMBAL_MODE_RELAX;
+  cmd->shoot.mode = SHOOT_MODE_RELAX;
+}
+
+/**
+ * @brief 初始化命令解析
+ *
+ * @param cmd 主结构体
+ * @param param 参数
+ * @return int8_t 0对应没有错误
+ */
+int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param) {
+  /* 指针检测 */
+  if (cmd == NULL) return -1;
+  if (param == NULL) return -1;
+
+  /* 设置机器人的命令参数，初始化控制方式为rc控制 */
+  cmd->pc_ctrl = false;
+  cmd->param = param;
+
+  return 0;
+}
+
+/**
+ * @brief 检查是否启用上位机控制指令覆盖
+ *
+ * @param cmd 主结构体
+ * @return true 启用
+ * @return false 不启用
+ */
+inline bool CMD_CheckHostOverwrite(CMD_t *cmd) { return cmd->host_overwrite; }
+
+/**
+ * @brief 解析命令
+ *
+ * @param rc 遥控器数据
+ * @param cmd 命令
+ * @param dt_sec 两次解析的间隔
+ * @return int8_t 0对应没有错误
+ */
+int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
+  /* 指针检测 */
+  if (rc == NULL) return -1;
+  if (cmd == NULL) return -1;
+
+  /* 在pc控制和rc控制间切换 */
+  if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
+      CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_Q))
+    cmd->pc_ctrl = true;
+
+  if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
+      CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_E))
+    cmd->pc_ctrl = false;
+  /*c当rc丢控时，恢复机器人至默认状态 */
+  if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
+    CMD_RcLostLogic(cmd);
+  } else {
+    if (cmd->pc_ctrl) {
+      CMD_PcLogic(rc, cmd, dt_sec);
+    } else {
+      CMD_RcLogic(rc, cmd, dt_sec);
+    }
+  }
+  return 0;
+}
+
+/**
+ * @brief 解析上位机命令
+ *
+ * @param host host数据
+ * @param cmd 命令
+ * @param dt_sec 两次解析的间隔
+ * @return int8_t 0对应没有错误
+ */
+int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
+  (void)dt_sec; /* 未使用dt_sec，消除警告 */
+  /* 指针检测 */
+  if (host == NULL) return -1;
+  if (cmd == NULL) return -1;
+
+  /* 云台欧拉角设置为host相应的变化的欧拉角 */
+  cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw;
+  cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit;
+
+  /* host射击命令，设置不同的射击频率和弹丸初速度 */
+  if (host->fire) {
+    cmd->shoot.mode = SHOOT_MODE_LOADED;
+    cmd->shoot.fire = true;
+  } else {
+    cmd->shoot.mode = SHOOT_MODE_SAFE;
+  }
+  return 0;
+}
+
+/**
+ * @brief 添加向Referee发送的命令
+ *
+ * @param ref 命令队列
+ * @param cmd 要添加的命令
+ * @return int8_t 0对应没有错误
+ */
+int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd) {
+  /* 指针检测 */
+  if (ref == NULL) return -1;
+  /* 越界检测 */
+  if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0) return -1;
+
+  /* 添加机器人当前行为状态到画图的命令队列中 */
+  ref->cmd[ref->counter] = cmd;
+  ref->counter++;
+  return 0;
+}

User/component/cmd.h

@@ -0,0 +1,306 @@
+/*
+  控制命令
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "ahrs.h"
+
+#define CMD_REFEREE_MAX_NUM (3) /* 发送命令限定的最大数量 */
+
+/* 机器人型号 */
+typedef enum {
+  ROBOT_MODEL_INFANTRY = 0, /* 步兵机器人 */
+  ROBOT_MODEL_HERO,         /* 英雄机器人 */
+  ROBOT_MODEL_ENGINEER,     /* 工程机器人 */
+  ROBOT_MODEL_DRONE,        /* 空中机器人 */
+  ROBOT_MODEL_SENTRY,       /* 哨兵机器人 */
+  ROBOT_MODEL_NUM,          /* 型号数量 */
+} CMD_RobotModel_t;
+
+/* 底盘运行模式 */
+typedef enum {
+  CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
+  CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */
+  CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */
+  CHASSIS_MODE_FOLLOW_GIMBAL_35, /* 通过闭环控制使车头方向35度跟随云台 */
+  CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */
+  CHASSIS_MODE_INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */
+  CHASSIS_MODE_OPEN, /* 开环模式。底盘运行不受PID控制，直接输出到电机 */
+} CMD_ChassisMode_t;
+
+/* 云台运行模式 */
+typedef enum {
+  GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
+  GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
+  GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
+} CMD_GimbalMode_t;
+
+/* 射击运行模式 */
+typedef enum {
+  SHOOT_MODE_RELAX,  /* 放松模式，电机不输出 */
+  SHOOT_MODE_SAFE,   /* 保险模式，电机闭环控制保持静止 */
+  SHOOT_MODE_LOADED, /* 上膛模式，摩擦轮开启。随时准备开火 */
+} CMD_ShootMode_t;
+
+typedef enum {
+  FIRE_MODE_SINGLE, /* 单发开火模式  */
+  FIRE_MODE_BURST,  /* N连发开火模式 */
+  FIRE_MODE_CONT,   /* 持续开火模式 */
+  FIRE_MODE_NUM,
+} CMD_FireMode_t;
+
+/* 小陀螺转动模式 */
+typedef enum {
+  ROTOR_MODE_CW,   /* 顺时针转动 */
+  ROTOR_MODE_CCW,  /* 逆时针转动 */
+  ROTOR_MODE_RAND, /* 随机转动 */
+} CMD_RotorMode_t;
+
+/* 底盘控制命令 */
+typedef struct {
+  CMD_ChassisMode_t mode;     /* 底盘运行模式 */
+  CMD_RotorMode_t mode_rotor; /* 小陀螺转动模式 */
+  MoveVector_t ctrl_vec;      /* 底盘控制向量 */
+} CMD_ChassisCmd_t;
+
+/* 云台控制命令 */
+typedef struct {
+  CMD_GimbalMode_t mode;  /* 云台运行模式 */
+  AHRS_Eulr_t delta_eulr; /* 欧拉角变化角度 */
+} CMD_GimbalCmd_t;
+
+/* 射击控制命令 */
+typedef struct {
+  CMD_ShootMode_t mode;     /* 射击运行模式 */
+  CMD_FireMode_t fire_mode; /* 开火模式 */
+  bool fire;                /*开火*/
+  bool cover_open;          /* 弹舱盖开关 */
+  bool reverse_trig;        /* 拨弹电机状态 */
+} CMD_ShootCmd_t;
+
+/* 拨杆位置 */
+typedef enum {
+  CMD_SW_ERR = 0,
+  CMD_SW_UP = 1,
+  CMD_SW_MID = 3,
+  CMD_SW_DOWN = 2,
+} CMD_SwitchPos_t;
+
+/* 键盘按键值 */
+typedef enum {
+  CMD_KEY_W = 0,
+  CMD_KEY_S,
+  CMD_KEY_A,
+  CMD_KEY_D,
+  CMD_KEY_SHIFT,
+  CMD_KEY_CTRL,
+  CMD_KEY_Q,
+  CMD_KEY_E,
+  CMD_KEY_R,
+  CMD_KEY_F,
+  CMD_KEY_G,
+  CMD_KEY_Z,
+  CMD_KEY_X,
+  CMD_KEY_C,
+  CMD_KEY_V,
+  CMD_KEY_B,
+  CMD_L_CLICK,
+  CMD_R_CLICK,
+  CMD_KEY_NUM,
+} CMD_KeyValue_t;
+
+/* 行为值序列 */
+typedef enum {
+  CMD_BEHAVIOR_FORE = 0,       /* 向前 */
+  CMD_BEHAVIOR_BACK,           /* 向后 */
+  CMD_BEHAVIOR_LEFT,           /* 向左 */
+  CMD_BEHAVIOR_RIGHT,          /* 向右 */
+  CMD_BEHAVIOR_ACCELERATE,     /* 加速 */
+  CMD_BEHAVIOR_DECELEBRATE,    /* 减速 */
+  CMD_BEHAVIOR_FIRE,           /* 开火 */
+  CMD_BEHAVIOR_FIRE_MODE,      /* 切换开火模式 */
+  CMD_BEHAVIOR_BUFF,           /* 打符模式 */
+  CMD_BEHAVIOR_AUTOAIM,        /* 自瞄模式 */
+  CMD_BEHAVIOR_OPENCOVER,      /* 弹舱盖开关 */
+  CMD_BEHAVIOR_ROTOR,          /* 小陀螺模式 */
+  CMD_BEHAVIOR_REVTRIG,        /* 反转拨弹 */
+  CMD_BEHAVIOR_FOLLOWGIMBAL35, /* 跟随云台呈35度 */
+  CMD_BEHAVIOR_NUM,
+} CMD_Behavior_t;
+
+typedef enum {
+  CMD_ACTIVE_PRESSING, /* 按下时触发 */
+  CMD_ACTIVE_RASING,   /* 抬起时触发 */
+  CMD_ACTIVE_PRESSED,  /* 按住时触发 */
+} CMD_ActiveType_t;
+
+typedef struct {
+  CMD_ActiveType_t active;
+  CMD_KeyValue_t key;
+} CMD_KeyMapItem_t;
+
+/* 行为映射的对应按键数组 */
+typedef struct {
+  CMD_KeyMapItem_t key_map[CMD_BEHAVIOR_NUM];
+} CMD_KeyMap_Params_t;
+
+/* 位移灵敏度参数 */
+typedef struct {
+  float move_sense;      /* 移动灵敏度 */
+  float move_fast_sense; /* 加速灵敏度 */
+  float move_slow_sense; /* 减速灵敏度 */
+} CMD_Move_Params_t;
+
+typedef struct {
+  uint16_t width;
+  uint16_t height;
+} CMD_Screen_t;
+
+/* 命令参数 */
+typedef struct {
+  float sens_mouse;        /* 鼠标灵敏度 */
+  float sens_rc;           /* 遥控器摇杆灵敏度 */
+  CMD_KeyMap_Params_t map; /* 按键映射行为命令 */
+  CMD_Move_Params_t move;  /* 位移灵敏度参数 */
+  CMD_Screen_t screen;     /* 屏幕分辨率参数 */
+} CMD_Params_t;
+
+/* AI行为状态 */
+typedef enum {
+  AI_STATUS_STOP,      /* 停止状态 */
+  AI_STATUS_AUTOAIM,   /* 自瞄状态 */
+  AI_STATUS_HITSWITCH, /* 打符状态 */
+  AI_STATUS_AUTOMATIC  /* 自动状态 */
+} CMD_AI_Status_t;
+
+/* UI所用行为状态 */
+typedef enum {
+  CMD_UI_NOTHING,          /* 当前无状态 */
+  CMD_UI_AUTO_AIM_START,   /* 自瞄状态开启 */
+  CMD_UI_AUTO_AIM_STOP,    /* 自瞄状态关闭 */
+  CMD_UI_HIT_SWITCH_START, /* 打符状态开启 */
+  CMD_UI_HIT_SWITCH_STOP   /* 打符状态关闭 */
+} CMD_UI_t;
+
+/*裁判系统发送的命令*/
+typedef struct {
+  CMD_UI_t cmd[CMD_REFEREE_MAX_NUM]; /* 命令数组 */
+  uint8_t counter;                   /* 命令计数 */
+} CMD_RefereeCmd_t;
+
+typedef struct {
+  bool pc_ctrl;        /* 是否使用键鼠控制 */
+  bool host_overwrite; /* 是否Host控制 */
+  uint16_t key_last;   /* 上次按键键值 */
+
+  struct {
+    int16_t x;
+    int16_t y;
+    int16_t z;
+    bool l_click; /* 左键 */
+    bool r_click; /* 右键 */
+  } mouse_last;   /* 鼠标值 */
+
+  CMD_AI_Status_t ai_status; /* AI状态 */
+
+  const CMD_Params_t *param; /* 命令参数 */
+
+  CMD_ChassisCmd_t chassis; /* 底盘控制命令 */
+  CMD_GimbalCmd_t gimbal;   /* 云台控制命令 */
+  CMD_ShootCmd_t shoot;     /* 射击控制命令 */
+  CMD_RefereeCmd_t referee; /* 裁判系统发送命令 */
+} CMD_t;
+
+typedef struct {
+  float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
+  float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
+  float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
+  float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
+
+  float ch_res; /* 第五通道值 */
+
+  CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */
+  CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */
+
+  struct {
+    int16_t x;
+    int16_t y;
+    int16_t z;
+    bool l_click; /* 左键 */
+    bool r_click; /* 右键 */
+  } mouse;        /* 鼠标值 */
+
+  uint16_t key; /* 按键值 */
+
+  uint16_t res; /* 保留，未启用 */
+} CMD_RC_t;
+
+typedef struct {
+  AHRS_Eulr_t gimbal_delta; /* 欧拉角的变化量 */
+
+  struct {
+    float vx;         /* x轴移动速度 */
+    float vy;         /* y轴移动速度 */
+    float wz;         /* z轴转动速度 */
+  } chassis_move_vec; /* 底盘移动向量 */
+
+  bool fire; /* 开火状态 */
+} CMD_Host_t;
+
+/**
+ * @brief 解析行为命令
+ *
+ * @param rc 遥控器数据
+ * @param cmd 主结构体
+ */
+int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param);
+
+/**
+ * @brief 检查是否启用上位机控制指令覆盖
+ *
+ * @param cmd 主结构体
+ * @return true 启用
+ * @return false 不启用
+ */
+bool CMD_CheckHostOverwrite(CMD_t *cmd);
+
+/**
+ * @brief 解析命令
+ *
+ * @param rc 遥控器数据
+ * @param cmd 命令
+ * @param dt_sec 两次解析的间隔
+ * @return int8_t 0对应没有错误
+ */
+int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec);
+
+/**
+ * @brief 解析上位机命令
+ *
+ * @param host host数据
+ * @param cmd 命令
+ * @param dt_sec 两次解析的间隔
+ * @return int8_t 0对应没有错误
+ */
+int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec);
+
+/**
+ * @brief 添加向Referee发送的命令
+ *
+ * @param ref 命令队列
+ * @param cmd 要添加的命令
+ * @return int8_t 0对应没有错误
+ */
+int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd);
+
+#ifdef __cplusplus
+}
+#endif

User/component/component_config.yaml

@@ -0,0 +1,21 @@
+ahrs:
+  dependencies:
+  - component/filter
+  enabled: true
+cmd:
+  dependencies: []
+  enabled: true
+filter:
+  dependencies:
+  - component/ahrs
+  enabled: true
+limiter:
+  dependencies: []
+  enabled: true
+pid:
+  dependencies:
+  - component/filter
+  enabled: true
+user_math:
+  dependencies: []
+  enabled: true

User/component/filter.c

@@ -0,0 +1,185 @@
+/*
+  各类滤波器。
+*/
+
+#include "filter.h"
+
+#include "user_math.h"
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param cutoff_freq 截止频率
+ */
+void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
+                          float cutoff_freq) {
+  if (f == NULL) return;
+
+  f->cutoff_freq = cutoff_freq;
+
+  f->delay_element_1 = 0.0f;
+  f->delay_element_2 = 0.0f;
+
+  if (f->cutoff_freq <= 0.0f) {
+    /* no filtering */
+    f->b0 = 1.0f;
+    f->b1 = 0.0f;
+    f->b2 = 0.0f;
+
+    f->a1 = 0.0f;
+    f->a2 = 0.0f;
+
+    return;
+  }
+  const float fr = sample_freq / f->cutoff_freq;
+  const float ohm = tanf(M_PI / fr);
+  const float c = 1.0f + 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm;
+
+  f->b0 = ohm * ohm / c;
+  f->b1 = 2.0f * f->b0;
+  f->b2 = f->b0;
+
+  f->a1 = 2.0f * (ohm * ohm - 1.0f) / c;
+  f->a2 = (1.0f - 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm) / c;
+}
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  /* do the filtering */
+  float delay_element_0 =
+      sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
+
+  if (isinf(delay_element_0)) {
+    /* don't allow bad values to propagate via the filter */
+    delay_element_0 = sample;
+  }
+
+  const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
+                       f->delay_element_2 * f->b2;
+
+  f->delay_element_2 = f->delay_element_1;
+  f->delay_element_1 = delay_element_0;
+
+  /* return the value. Should be no need to check limits */
+  return output;
+}
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  const float dval = sample / (f->b0 + f->b1 + f->b2);
+
+  if (isfinite(dval)) {
+    f->delay_element_1 = dval;
+    f->delay_element_2 = dval;
+
+  } else {
+    f->delay_element_1 = sample;
+    f->delay_element_2 = sample;
+  }
+
+  return LowPassFilter2p_Apply(f, sample);
+}
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param notch_freq 中心频率
+ * @param bandwidth 带宽
+ */
+void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
+                      float bandwidth) {
+  if (f == NULL) return;
+
+  f->notch_freq = notch_freq;
+  f->bandwidth = bandwidth;
+
+  f->delay_element_1 = 0.0f;
+  f->delay_element_2 = 0.0f;
+
+  if (notch_freq <= 0.0f) {
+    /* no filtering */
+    f->b0 = 1.0f;
+    f->b1 = 0.0f;
+    f->b2 = 0.0f;
+
+    f->a1 = 0.0f;
+    f->a2 = 0.0f;
+
+    return;
+  }
+
+  const float alpha = tanf(M_PI * bandwidth / sample_freq);
+  const float beta = -cosf(M_2PI * notch_freq / sample_freq);
+  const float a0_inv = 1.0f / (alpha + 1.0f);
+
+  f->b0 = a0_inv;
+  f->b1 = 2.0f * beta * a0_inv;
+  f->b2 = a0_inv;
+
+  f->a1 = f->b1;
+  f->a2 = (1.0f - alpha) * a0_inv;
+}
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+inline float NotchFilter_Apply(NotchFilter_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  /* Direct Form II implementation */
+  const float delay_element_0 =
+      sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
+  const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
+                       f->delay_element_2 * f->b2;
+
+  f->delay_element_2 = f->delay_element_1;
+  f->delay_element_1 = delay_element_0;
+
+  return output;
+}
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float NotchFilter_Reset(NotchFilter_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  float dval = sample;
+
+  if (fabsf(f->b0 + f->b1 + f->b2) > FLT_EPSILON) {
+    dval = dval / (f->b0 + f->b1 + f->b2);
+  }
+
+  f->delay_element_1 = dval;
+  f->delay_element_2 = dval;
+
+  return NotchFilter_Apply(f, sample);
+}

User/component/filter.h

@@ -0,0 +1,104 @@
+/*
+  各类滤波器。
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "user_math.h"
+
+/* 二阶低通滤波器 */
+typedef struct {
+  float cutoff_freq; /* 截止频率 */
+
+  float a1;
+  float a2;
+
+  float b0;
+  float b1;
+  float b2;
+
+  float delay_element_1;
+  float delay_element_2;
+
+} LowPassFilter2p_t;
+
+/* 带阻滤波器 */
+typedef struct {
+  float notch_freq; /* 阻止频率 */
+  float bandwidth;  /* 带宽 */
+
+  float a1;
+  float a2;
+
+  float b0;
+  float b1;
+  float b2;
+  float delay_element_1;
+  float delay_element_2;
+
+} NotchFilter_t;
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param cutoff_freq 截止频率
+ */
+void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
+                          float cutoff_freq);
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample);
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample);
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param notch_freq 中心频率
+ * @param bandwidth 带宽
+ */
+void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
+                      float bandwidth);
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float NotchFilter_Apply(NotchFilter_t *f, float sample);
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float NotchFilter_Reset(NotchFilter_t *f, float sample);
+
+#ifdef __cplusplus
+}
+#endif

User/component/kinematics.c

@@ -0,0 +1,113 @@
+/*
+一些运动学相关的计算函数
+*/
+
+#include "component/kinematics.h"
+#include "component/user_math.h"
+
+/*串联腿单腿转摆动杆正运动学*/
+/*大腿小腿均按照水平为0点，向下为正方向*/
+
+/**
+ * @brief 串联腿单腿转摆动杆正运动学
+ * @param param 腿部长度参数
+ * @param hip_angle 髋关节角度(弧度)
+ * @param knee_angle 膝关节角度(弧度)
+ * @param angle 计算到的等效摆动杆角度
+ * @param heigh 计算到的等效摆动杆高度
+ * @return 
+ */
+int8_t KIN_SerialLeg_FK(const KIN_SerialLeg_Param_t *param, const float *hip_angle,const float *knee_angle, float *angle, float *height) {
+    if (param == NULL || hip_angle == NULL || knee_angle == NULL || angle == NULL || height == NULL) {
+        return -1; // 参数错误
+    }
+    
+    float L1 = param->thigh_length;
+    float L2 = param->calf_length;
+    float q1 = *hip_angle;      // 髋关节角度
+    float q2 = *knee_angle;     // 膝关节角度
+    
+    float q4 = (M_PI - q1 - q2)/2;
+
+    // 使用余弦定理求解等效摆动杆长度
+    // L2*L2 = L1*L1 + L3*L3 - 2*L1*L3*cosf(q4);
+    // 整理得：L3*L3 - 2*L1*cosf(q4)*L3 + (L1*L1 - L2*L2) = 0
+    
+    float a = 1.0f;
+    float b = -2.0f * L1 * cosf(q4);
+    float c = L1*L1 - L2*L2;
+    
+    float discriminant = b*b - 4*a*c;
+    
+    // 检查判别式，确保有实数解
+    if (discriminant < 0) {
+        return -2; // 无实数解，配置不可达
+    }
+    
+    float sqrt_discriminant = sqrtf(discriminant);
+    float L3_1 = (-b + sqrt_discriminant) / (2*a);
+    float L3_2 = (-b - sqrt_discriminant) / (2*a);
+    
+    // 选择正的解（物理意义上的长度）
+    float L3;
+    if (L3_1 > 0 && L3_2 > 0) {
+        // 两个正解，选择较小的（通常对应机构的正常工作姿态）
+        L3 = (L3_1 < L3_2) ? L3_1 : L3_2;
+    } else if (L3_1 > 0) {
+        L3 = L3_1;
+    } else if (L3_2 > 0) {
+        L3 = L3_2;
+    } else {
+        return -3; // 无正解
+    }
+
+    *angle = q1 + q4;
+    *height = L3;
+
+    return 0; // 成功
+}
+
+/*串联腿单腿转摆动杆逆运动学*/
+/*大腿小腿均按照水平为0点，向下为正方向*/
+/**
+ * @brief 串联腿单腿转摆动杆逆运动学
+ * @param param 腿部长度参数
+ * @param angle 输入的等效摆动杆角度
+ * @param height 输入的等效摆动杆高度
+ * @param hip_angle 计算到的髋关节角度(弧度)
+ * @param knee_angle 计算到的膝关节角度(弧度)
+ * @return 
+ */
+int8_t KIN_SerialLeg_IK(const KIN_SerialLeg_Param_t *param, const float *angle,const float *height, float *hip_angle, float *knee_angle) {
+    if (param == NULL || angle == NULL || height == NULL || hip_angle == NULL || knee_angle == NULL) {
+        return -1; // 参数错误
+    }
+    float L1 = param->thigh_length;
+    float L2 = param->calf_length;
+    float q = *angle;  // 摆动杆角度
+    float h = *height; // 摆动杆长度
+
+    // 由正解可知：q = q1 + q4，h = L3
+    // 由余弦定理：L2^2 = L1^2 + h^2 - 2*L1*h*cos(q4)
+    // 整理得：cos(q4) = (L1^2 + h^2 - L2^2) / (2*L1*h)
+    float cos_q4 = (L1*L1 + h*h - L2*L2) / (2.0f * L1 * h);
+
+    // 检查 cos_q4 是否在 [-1, 1] 范围内
+    if (cos_q4 < -1.0f || cos_q4 > 1.0f) {
+        return -2; // 不可达
+    }
+
+    float q4 = acosf(cos_q4);
+
+    // 由正解：q = q1 + q4，且 q4 = (PI - q1 - q2)/2
+    // 整理得：q1 = q - q4
+    float q1 = q - q4;
+
+    // 再由 q4 = (PI - q1 - q2)/2，整理得：q2 = PI - 2*q4 - q1
+    float q2 = M_PI - 2.0f * q4 - q1;
+
+    *hip_angle = q1;
+    *knee_angle = q2;
+
+    return 0; // 成功
+}

User/component/kinematics.h

@@ -0,0 +1,43 @@
+#pragma once
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdint.h>
+#include <stdbool.h>
+#include <math.h>
+
+/**
+ * @brief 串联腿参数结构体
+ */
+typedef struct {
+    float thigh_length; // 大腿长度
+    float calf_length;  // 小腿长度
+} KIN_SerialLeg_Param_t; // ← 补上分号
+
+/**
+ * @brief 串联腿单腿转摆动杆正运动学
+ * @param param 腿部长度参数
+ * @param hip_angle 髋关节角度(弧度)
+ * @param knee_angle 膝关节角度(弧度)
+ * @param angle 计算到的等效摆动杆角度
+ * @param height 计算到的等效摆动杆高度
+ * @return 0:成功, -1:失败
+ */
+int8_t KIN_SerialLeg_FK(const KIN_SerialLeg_Param_t *param, const float *hip_angle, const float *knee_angle, float *angle, float *height);
+
+/**
+ * @brief 串联腿单腿转摆动杆逆运动学
+ * @param param 腿部长度参数
+ * @param angle 输入的等效摆动杆角度
+ * @param height 输入的等效摆动杆高度
+ * @param hip_angle 计算到的髋关节角度(弧度)
+ * @param knee_angle 计算到的膝关节角度(弧度)
+ * @return 0:成功, -1:失败
+ */
+int8_t KIN_SerialLeg_IK(const KIN_SerialLeg_Param_t *param, const float *angle, const float *height, float *hip_angle, float *knee_angle);
+
+#ifdef __cplusplus
+}
+#endif

User/component/limiter.c

@@ -0,0 +1,107 @@
+/*
+  限制器
+*/
+
+#include "limiter.h"
+
+#include <math.h>
+#include <stddef.h>
+
+#define POWER_BUFF_THRESHOLD 20
+#define CHASSIS_POWER_CHECK_FREQ 10
+#define CHASSIS_POWER_FACTOR_PASS 0.9f
+#define CHASSIS_POWER_FACTOR_NO_PASS 1.5f
+
+#define CHASSIS_MOTOR_CIRCUMFERENCE 0.12f
+
+/**
+ * @brief 限制底盘功率不超过power_limit
+ *
+ * @param power_limit 最大功率
+ * @param motor_out 电机输出值
+ * @param speed 电机转速
+ * @param len 电机数量
+ * @return int8_t 0对应没有错误
+ */
+int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out,
+                                float *speed, uint32_t len) {
+  /* power_limit小于0时不进行限制 */
+  if (motor_out == NULL || speed == NULL || power_limit < 0) return -1;
+
+  float sum_motor_out = 0.0f;
+  for (uint32_t i = 0; i < len; i++) {
+    /* 总功率计算 P=F(由转矩电流表示)*V(由转速表示) */
+    sum_motor_out +=
+        fabsf(motor_out[i]) * fabsf(speed[i]) * CHASSIS_MOTOR_CIRCUMFERENCE;
+  }
+
+  /* 保持每个电机输出值缩小时比例不变 */
+  if (sum_motor_out > power_limit) {
+    for (uint32_t i = 0; i < len; i++) {
+      motor_out[i] *= power_limit / sum_motor_out;
+    }
+  }
+
+  return 0;
+}
+
+/**
+ * @brief 电容输入功率计算
+ *
+ * @param power_in 底盘当前功率
+ * @param power_limit 裁判系统功率限制值
+ * @param power_buffer 缓冲能量
+ * @return float 裁判系统输出最大值
+ */
+float PowerLimit_CapInput(float power_in, float power_limit,
+                          float power_buffer) {
+  float target_power = 0.0f;
+
+  /* 计算下一个检测周期的剩余缓冲能量 */
+  float heat_buff = power_buffer - (float)(power_in - power_limit) /
+                                       (float)CHASSIS_POWER_CHECK_FREQ;
+  if (heat_buff < POWER_BUFF_THRESHOLD) { /* 功率限制 */
+    target_power = power_limit * CHASSIS_POWER_FACTOR_PASS;
+  } else {
+    target_power = power_limit * CHASSIS_POWER_FACTOR_NO_PASS;
+  }
+
+  return target_power;
+}
+
+/**
+ * @brief 使用缓冲能量计算底盘最大功率
+ *
+ * @param power_limit 裁判系统功率限制值
+ * @param power_buffer 缓冲能量
+ * @return float 底盘输出最大值
+ */
+float PowerLimit_TargetPower(float power_limit, float power_buffer) {
+  float target_power = 0.0f;
+
+  /* 根据剩余缓冲能量计算输出功率 */
+  target_power = power_limit * (power_buffer - 10.0f) / 20.0f;
+  if (target_power < 0.0f) target_power = 0.0f;
+
+  return target_power;
+}
+
+/**
+ * @brief 射击频率控制
+ *
+ * @param heat 当前热量
+ * @param heat_limit 热量上限
+ * @param cooling_rate 冷却速率
+ * @param heat_increase 冷却增加
+ * @param shoot_freq 经过热量限制后的射击频率
+ * @return float 射击频率
+ */
+float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
+                          float heat_increase, bool is_big) {
+  float heat_percent = heat / heat_limit;
+  float stable_freq = cooling_rate / heat_increase;
+  if (is_big)
+    return stable_freq;
+  else
+    return (heat_percent > 0.7f) ? stable_freq : 3.0f * stable_freq;
+}

User/component/limiter.h

@@ -0,0 +1,55 @@
+/*
+  限制器
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+
+/**
+ * @brief 限制底盘功率不超过power_limit
+ *
+ * @param power_limit 最大功率
+ * @param motor_out 电机输出值
+ * @param speed 电机转速
+ * @param len 电机数量
+ * @return int8_t 0对应没有错误
+ */
+int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out,
+                                float *speed, uint32_t len);
+/**
+ * @brief 电容输入功率计算
+ *
+ * @param power_in 底盘当前功率
+ * @param power_limit 裁判系统功率限制值
+ * @param power_buffer 缓冲能量
+ * @return float 裁判系统输出最大值
+ */
+float PowerLimit_CapInput(float power_in, float power_limit,
+                          float power_buffer);
+/**
+ * @brief 使用缓冲能量计算底盘最大功率
+ *
+ * @param power_limit 裁判系统功率限制值
+ * @param power_buffer 缓冲能量
+ * @return float 底盘输出最大值
+ */
+float PowerLimit_TargetPower(float power_limit, float power_buffer);
+
+/**
+ * @brief 射击频率控制
+ *
+ * @param heat 当前热量
+ * @param heat_limit 热量上限
+ * @param cooling_rate 冷却速率
+ * @param heat_increase 冷却增加
+ * @param shoot_freq 经过热量限制后的射击频率
+ * @return float 射击频率
+ */
+float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
+                          float heat_increase, bool is_big);

User/component/lqr.c

@@ -0,0 +1,229 @@
+/*
+ * LQR线性二次型最优控制器简化实现
+ * 
+ * 本文件实现了轮腿机器人的LQR (Linear Quadratic Regulator) 控制算法
+ * 主要功能包括:
+ * 1. 状态反馈控制
+ * 2. 增益矩阵K计算控制输出
+ * 3. 控制输出限幅
+ * 
+ * 系统模型:
+ * u = -K*(x - x_ref) (状态反馈)
+ */
+
+#include "lqr.h"
+#include <string.h>
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+
+#define LQR_EPSILON             (1e-6f)     // 数值计算精度
+#define LQR_DEFAULT_MAX_WHEEL   (50.0f)     // 默认最大轮毂力矩 (N*m)
+#define LQR_DEFAULT_MAX_JOINT   (30.0f)     // 默认最大关节力矩 (N*m)
+
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* Private function prototypes ---------------------------------------------- */
+
+static void LQR_MatrixMultiply(const float K[4][10], const float state_error[10], float result[4]);
+static float LQR_ComputeStateError(float current, float reference);
+
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * @brief 初始化LQR控制器
+ */
+int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque) {
+    if (lqr == NULL || max_wheel_torque <= 0 || max_joint_torque <= 0) {
+        return -1;
+    }
+    
+    // 设置力矩限制
+    lqr->max_wheel_torque = max_wheel_torque;
+    lqr->max_joint_torque = max_joint_torque;
+    
+    // 重置状态
+    LQR_Reset(lqr);
+    
+    lqr->initialized = true;
+    
+    return 0;
+}
+
+/**
+ * @brief 设置固定LQR增益矩阵
+ */
+int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, const LQR_GainMatrix_t *K) {
+    if (lqr == NULL || !lqr->initialized || K == NULL) {
+        return -1;
+    }
+    
+    // 复制增益矩阵
+    memcpy(&lqr->K, K, sizeof(LQR_GainMatrix_t));
+    
+    return 0;
+}
+
+/**
+ * @brief 更新机器人状态
+ */
+int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state) {
+    if (lqr == NULL || !lqr->initialized || state == NULL) {
+        return -1;
+    }
+    
+    // 复制状态，并对角度进行归一化
+    lqr->state = *state;
+    LQR_ANGLE_NORMALIZE(lqr->state.phi);
+    LQR_ANGLE_NORMALIZE(lqr->state.theta_ll);
+    LQR_ANGLE_NORMALIZE(lqr->state.theta_lr);
+    LQR_ANGLE_NORMALIZE(lqr->state.theta_b);
+    
+    return 0;
+}
+
+/**
+ * @brief 设置参考状态
+ */
+int8_t LQR_SetReference(LQR_Controller_t *lqr, const LQR_State_t *reference) {
+    if (lqr == NULL || !lqr->initialized || reference == NULL) {
+        return -1;
+    }
+    
+    // 复制参考状态，并对角度进行归一化
+    lqr->reference = *reference;
+    LQR_ANGLE_NORMALIZE(lqr->reference.phi);
+    LQR_ANGLE_NORMALIZE(lqr->reference.theta_ll);
+    LQR_ANGLE_NORMALIZE(lqr->reference.theta_lr);
+    LQR_ANGLE_NORMALIZE(lqr->reference.theta_b);
+    
+    return 0;
+}
+
+/**
+ * @brief 计算LQR控制输出
+ * 
+ * 实现状态反馈控制律: u = -K*(x - x_ref)
+ */
+int8_t LQR_ComputeControl(LQR_Controller_t *lqr) {
+    if (lqr == NULL || !lqr->initialized) {
+        return -1;
+    }
+    
+    // 计算状态误差向量
+    float state_error[10];
+    state_error[0] = LQR_ComputeStateError(lqr->state.s, lqr->reference.s);
+    state_error[1] = LQR_ComputeStateError(lqr->state.ds, lqr->reference.ds);
+    state_error[2] = LQR_ComputeStateError(lqr->state.phi, lqr->reference.phi);
+    state_error[3] = LQR_ComputeStateError(lqr->state.dphi, lqr->reference.dphi);
+    state_error[4] = LQR_ComputeStateError(lqr->state.theta_ll, lqr->reference.theta_ll);
+    state_error[5] = LQR_ComputeStateError(lqr->state.dtheta_ll, lqr->reference.dtheta_ll);
+    state_error[6] = LQR_ComputeStateError(lqr->state.theta_lr, lqr->reference.theta_lr);
+    state_error[7] = LQR_ComputeStateError(lqr->state.dtheta_lr, lqr->reference.dtheta_lr);
+    state_error[8] = LQR_ComputeStateError(lqr->state.theta_b, lqr->reference.theta_b);
+    state_error[9] = LQR_ComputeStateError(lqr->state.dtheta_b, lqr->reference.dtheta_b);
+    
+    // 计算控制输出: u = -K * (x - x_ref)
+    float control_vector[4];
+    LQR_MatrixMultiply(lqr->K.K, state_error, control_vector);
+    
+    // 应用负反馈并限幅
+    lqr->control.T_wl = LQR_CLAMP(-control_vector[0], -lqr->max_wheel_torque, lqr->max_wheel_torque);
+    lqr->control.T_wr = LQR_CLAMP(-control_vector[1], -lqr->max_wheel_torque, lqr->max_wheel_torque);
+    lqr->control.T_bl = LQR_CLAMP(-control_vector[2], -lqr->max_joint_torque, lqr->max_joint_torque);
+    lqr->control.T_br = LQR_CLAMP(-control_vector[3], -lqr->max_joint_torque, lqr->max_joint_torque);
+    
+    return 0;
+}
+
+/**
+ * @brief 获取控制输出
+ */
+int8_t LQR_GetControl(const LQR_Controller_t *lqr, LQR_Control_t *control) {
+    if (lqr == NULL || !lqr->initialized || control == NULL) {
+        return -1;
+    }
+    
+    *control = lqr->control;
+    return 0;
+}
+
+/**
+ * @brief 重置LQR控制器状态
+ */
+void LQR_Reset(LQR_Controller_t *lqr) {
+    if (lqr == NULL) {
+        return;
+    }
+    
+    // 清零状态和控制量
+    memset(&lqr->state, 0, sizeof(LQR_State_t));
+    memset(&lqr->reference, 0, sizeof(LQR_State_t));
+    memset(&lqr->control, 0, sizeof(LQR_Control_t));
+    memset(&lqr->K, 0, sizeof(LQR_GainMatrix_t));
+}
+
+/**
+ * @brief 从轮腿机器人传感器数据构建LQR状态
+ */
+int8_t LQR_BuildStateFromSensors(float position_x, float velocity_x,
+                                 float yaw_angle, float yaw_rate,
+                                 float left_leg_angle, float left_leg_rate,
+                                 float right_leg_angle, float right_leg_rate,
+                                 float body_pitch, float body_pitch_rate,
+                                 LQR_State_t *state) {
+    if (state == NULL) {
+        return -1;
+    }
+    
+    state->s = position_x;
+    state->ds = velocity_x;
+    state->phi = yaw_angle;
+    state->dphi = yaw_rate;
+    state->theta_ll = left_leg_angle;
+    state->dtheta_ll = left_leg_rate;
+    state->theta_lr = right_leg_angle;
+    state->dtheta_lr = right_leg_rate;
+    state->theta_b = body_pitch;
+    state->dtheta_b = body_pitch_rate;
+    
+    // 角度归一化
+    LQR_ANGLE_NORMALIZE(state->phi);
+    LQR_ANGLE_NORMALIZE(state->theta_ll);
+    LQR_ANGLE_NORMALIZE(state->theta_lr);
+    LQR_ANGLE_NORMALIZE(state->theta_b);
+    
+    return 0;
+}
+
+/* Private functions -------------------------------------------------------- */
+
+/**
+ * @brief 矩阵向量乘法: result = K * state_error
+ * 
+ * K: 4x10矩阵
+ * state_error: 10x1向量
+ * result: 4x1向量
+ */
+static void LQR_MatrixMultiply(const float K[4][10], const float state_error[10], float result[4]) {
+    for (int i = 0; i < 4; i++) {
+        result[i] = 0.0f;
+        for (int j = 0; j < 10; j++) {
+            result[i] += K[i][j] * state_error[j];
+        }
+    }
+}
+
+/**
+ * @brief 计算状态误差(考虑角度周期性)
+ */
+static float LQR_ComputeStateError(float current, float reference) {
+    float error = current - reference;
+    
+    // 对于角度状态，需要考虑周期性
+    // 这里假设大部分状态都是角度，如果需要区分可以添加参数
+    while (error > M_PI) error -= 2 * M_PI;
+    while (error < -M_PI) error += 2 * M_PI;
+    
+    return error;
+}

User/component/lqr.h

@@ -0,0 +1,168 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdint.h>
+#include <stdbool.h>
+#include <math.h>
+#include "component/user_math.h"
+
+/* Exported types ----------------------------------------------------------- */
+
+/**
+ * @brief LQR控制器状态向量定义
+ * 
+ * 状态向量维度: 10 x 1
+ * [s, ds, phi, dphi, theta_ll, dtheta_ll, theta_lr, dtheta_lr, theta_b, dtheta_b]^T
+ */
+typedef struct {
+    float s;                // 机器人水平方向移动距离 (m)
+    float ds;               // 机器人水平方向移动速度 (m/s)
+    float phi;              // 机器人水平方向移动时yaw偏航角度 (rad)
+    float dphi;             // yaw偏航角速度 (rad/s)
+    float theta_ll;         // 左腿摆杆与竖直方向夹角 (rad)
+    float dtheta_ll;        // 左腿摆杆角速度 (rad/s)
+    float theta_lr;         // 右腿摆杆与竖直方向夹角 (rad)
+    float dtheta_lr;        // 右腿摆杆角速度 (rad/s)
+    float theta_b;          // 机体与水平方向夹角 (rad)
+    float dtheta_b;         // 机体角速度 (rad/s)
+} LQR_State_t;
+
+/**
+ * @brief LQR控制器控制输入向量定义
+ * 
+ * 控制向量维度: 4 x 1
+ * [T_wl, T_wr, T_bl, T_br]^T
+ */
+typedef struct {
+    float T_wl;             // 左侧驱动轮输出力矩 (N*m)
+    float T_wr;             // 右侧驱动轮输出力矩 (N*m)
+    float T_bl;             // 左侧髋关节输出力矩 (N*m)
+    float T_br;             // 右侧髋关节输出力矩 (N*m)
+} LQR_Control_t;
+
+/**
+ * @brief LQR增益矩阵K (4x10)
+ */
+typedef struct {
+    float K[4][10];         // LQR反馈增益矩阵
+} LQR_GainMatrix_t;
+
+/**
+ * @brief 简化的LQR控制器实例结构体
+ */
+typedef struct {
+    LQR_GainMatrix_t K;              // 增益矩阵
+    LQR_State_t state;               // 当前状态
+    LQR_State_t reference;           // 参考状态
+    LQR_Control_t control;           // 控制输出
+    
+    float max_wheel_torque;          // 轮毂电机最大力矩限制 (N*m)
+    float max_joint_torque;          // 关节电机最大力矩限制 (N*m)
+    bool initialized;                // 初始化标志
+} LQR_Controller_t;
+
+/* Exported constants ------------------------------------------------------- */
+
+#define LQR_STATE_DIM           (10)        // 状态向量维度
+#define LQR_CONTROL_DIM         (4)         // 控制向量维度
+
+/* Exported macros ---------------------------------------------------------- */
+
+/**
+ * @brief 角度归一化到[-PI, PI]
+ */
+#define LQR_ANGLE_NORMALIZE(angle) do { \
+    while((angle) > M_PI) (angle) -= 2*M_PI; \
+    while((angle) < -M_PI) (angle) += 2*M_PI; \
+} while(0)
+
+/**
+ * @brief 数值限幅
+ */
+#define LQR_CLAMP(val, min, max) ((val) < (min) ? (min) : ((val) > (max) ? (max) : (val)))
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 初始化LQR控制器
+ * @param lqr LQR控制器实例
+ * @param max_wheel_torque 轮毂电机最大力矩 (N*m)
+ * @param max_joint_torque 关节电机最大力矩 (N*m)
+ * @return 0:成功, -1:失败
+ */
+int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque);
+
+/**
+ * @brief 设置固定LQR增益矩阵
+ * @param lqr LQR控制器实例
+ * @param K 增益矩阵
+ * @return 0:成功, -1:失败
+ */
+int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, const LQR_GainMatrix_t *K);
+
+/**
+ * @brief 更新机器人状态
+ * @param lqr LQR控制器实例
+ * @param state 当前状态
+ * @return 0:成功, -1:失败
+ */
+int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state);
+
+/**
+ * @brief 设置参考状态
+ * @param lqr LQR控制器实例
+ * @param reference 参考状态
+ * @return 0:成功, -1:失败
+ */
+int8_t LQR_SetReference(LQR_Controller_t *lqr, const LQR_State_t *reference);
+
+/**
+ * @brief 计算LQR控制输出
+ * @param lqr LQR控制器实例
+ * @return 0:成功, -1:失败
+ */
+int8_t LQR_ComputeControl(LQR_Controller_t *lqr);
+
+/**
+ * @brief 获取控制输出
+ * @param lqr LQR控制器实例
+ * @param control 控制输出
+ * @return 0:成功, -1:失败
+ */
+int8_t LQR_GetControl(const LQR_Controller_t *lqr, LQR_Control_t *control);
+
+/**
+ * @brief 重置LQR控制器状态
+ * @param lqr LQR控制器实例
+ */
+void LQR_Reset(LQR_Controller_t *lqr);
+
+/**
+ * @brief 从轮腿机器人传感器数据构建LQR状态
+ * @param position_x 机体x位置 (m)
+ * @param velocity_x 机体x速度 (m/s)
+ * @param yaw_angle yaw角度 (rad)
+ * @param yaw_rate yaw角速度 (rad/s)
+ * @param left_leg_angle 左腿角度 (rad)
+ * @param left_leg_rate 左腿角速度 (rad/s)
+ * @param right_leg_angle 右腿角度 (rad)
+ * @param right_leg_rate 右腿角速度 (rad/s)
+ * @param body_pitch 机体pitch角度 (rad)
+ * @param body_pitch_rate 机体pitch角速度 (rad/s)
+ * @param state 输出状态
+ * @return 0:成功, -1:失败
+ */
+int8_t LQR_BuildStateFromSensors(float position_x, float velocity_x,
+                                 float yaw_angle, float yaw_rate,
+                                 float left_leg_angle, float left_leg_rate,
+                                 float right_leg_angle, float right_leg_rate,
+                                 float body_pitch, float body_pitch_rate,
+                                 LQR_State_t *state);
+
+#ifdef __cplusplus
+}
+#endif

User/component/pid.c

@@ -0,0 +1,158 @@
+/*
+   Modified from
+   https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.cpp
+
+   参考资料：
+   https://github.com/PX4/Firmware/issues/12362
+   https://dev.px4.io/master/en/flight_stack/controller_diagrams.html
+   https://docs.px4.io/master/en/config_mc/pid_tuning_guide_multicopter.html#standard_form
+   https://www.controleng.com/articles/not-all-pid-controllers-are-the-same/
+   https://en.wikipedia.org/wiki/PID_controller
+   http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
+*/
+
+#include "pid.h"
+
+#define SIGMA 0.000001f
+
+/**
+ * @brief 初始化PID
+ *
+ * @param pid PID结构体
+ * @param mode PID模式
+ * @param sample_freq 采样频率
+ * @param param PID参数
+ * @return int8_t 0对应没有错误
+ */
+int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq,
+                const KPID_Params_t *param) {
+  if (pid == NULL) return -1;
+
+  if (!isfinite(param->p)) return -1;
+  if (!isfinite(param->i)) return -1;
+  if (!isfinite(param->d)) return -1;
+  if (!isfinite(param->i_limit)) return -1;
+  if (!isfinite(param->out_limit)) return -1;
+  pid->param = param;
+
+  float dt_min = 1.0f / sample_freq;
+  if (isfinite(dt_min))
+    pid->dt_min = dt_min;
+  else
+    return -1;
+
+  LowPassFilter2p_Init(&(pid->dfilter), sample_freq, pid->param->d_cutoff_freq);
+
+  pid->mode = mode;
+  PID_Reset(pid);
+  return 0;
+}
+
+/**
+ * @brief PID计算
+ *
+ * @param pid PID结构体
+ * @param sp 设定值
+ * @param fb 反馈值
+ * @param fb_dot 反馈值微分
+ * @param dt 间隔时间
+ * @return float 计算的输出
+ */
+float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt) {
+  if (!isfinite(sp) || !isfinite(fb) || !isfinite(fb_dot) || !isfinite(dt)) {
+    return pid->last.out;
+  }
+
+  /* 计算误差值 */
+  const float err = CircleError(sp, fb, pid->param->range);
+
+  /* 计算P项 */
+  const float k_err = err * pid->param->k;
+
+  /* 计算D项 */
+  const float k_fb = pid->param->k * fb;
+  const float filtered_k_fb = LowPassFilter2p_Apply(&(pid->dfilter), k_fb);
+
+  float d;
+  switch (pid->mode) {
+    case KPID_MODE_CALC_D:
+      /* 通过fb计算D，避免了由于sp变化导致err突变的问题 */
+      /* 当sp不变时，err的微分等于负的fb的微分 */
+      d = (filtered_k_fb - pid->last.k_fb) / fmaxf(dt, pid->dt_min);
+      break;
+
+    case KPID_MODE_SET_D:
+      d = fb_dot;
+      break;
+
+    case KPID_MODE_NO_D:
+      d = 0.0f;
+      break;
+  }
+  pid->last.err = err;
+  pid->last.k_fb = filtered_k_fb;
+
+  if (!isfinite(d)) d = 0.0f;
+
+  /* 计算PD输出 */
+  float output = (k_err * pid->param->p) - (d * pid->param->d);
+
+  /* 计算I项 */
+  const float i = pid->i + (k_err * dt);
+  const float i_out = i * pid->param->i;
+  
+  if (pid->param->i > SIGMA) {
+    /* 检查是否饱和 */
+    if (isfinite(i)) {
+      if ((fabsf(output + i_out) <= pid->param->out_limit) &&
+          (fabsf(i) <= pid->param->i_limit)) {
+        /* 未饱和，使用新积分 */
+        pid->i = i;
+      }
+    }
+  }
+  
+  /* 计算PID输出 */
+  output += i_out;
+ 
+  /* 限制输出 */
+  if (isfinite(output)) {
+    if (pid->param->out_limit > SIGMA) {
+      output = AbsClip(output, pid->param->out_limit);
+    }
+    pid->last.out = output;
+  }
+  return pid->last.out;
+}
+
+/**
+ * @brief 重置微分项
+ *
+ * @param pid PID结构体
+ * @return int8_t 0对应没有错误
+ */
+int8_t PID_ResetIntegral(KPID_t *pid) {
+  if (pid == NULL) return -1;
+
+  pid->i = 0.0f;
+
+  return 0;
+}
+
+/**
+ * @brief 重置PID
+ *
+ * @param pid PID结构体
+ * @return int8_t 0对应没有错误
+ */
+int8_t PID_Reset(KPID_t *pid) {
+  if (pid == NULL) return -1;
+
+  pid->i = 0.0f;
+  pid->last.err = 0.0f;
+  pid->last.k_fb = 0.0f;
+  pid->last.out = 0.0f;
+  LowPassFilter2p_Reset(&(pid->dfilter), 0.0f);
+
+  return 0;
+}

User/component/pid.h

@@ -0,0 +1,95 @@
+/*
+  Modified from
+  https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.h
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+#include "filter.h"
+#include "user_math.h"
+
+/* PID模式 */
+typedef enum {
+  KPID_MODE_NO_D = 0, /* 不使用微分项，PI控制器 */
+  KPID_MODE_CALC_D, /* 根据反馈的值计算离散微分，忽略PID_Calc中的fb_dot */
+  KPID_MODE_SET_D /* 直接提供微分值，PID_Calc中的fb_dot将被使用，(Gyros) */
+} KPID_Mode_t;
+
+/* PID参数 */
+typedef struct {
+  float k;             /* 控制器增益，设置为1用于并行模式 */
+  float p;             /* 比例项增益，设置为1用于标准形式 */
+  float i;             /* 积分项增益 */
+  float d;             /* 微分项增益 */
+  float i_limit;       /* 积分项上限 */
+  float out_limit;     /* 输出绝对值限制 */
+  float d_cutoff_freq; /* D项低通截止频率 */
+  float range;         /* 计算循环误差时使用，大于0时启用 */
+} KPID_Params_t;
+
+/* PID主结构体 */
+typedef struct {
+  KPID_Mode_t mode;
+  const KPID_Params_t *param;
+
+  float dt_min; /* 最小PID_Calc调用间隔 */
+  float i;      /* 积分 */
+
+  struct {
+    float err;  /* 上次误差 */
+    float k_fb; /* 上次反馈值 */
+    float out;  /* 上次输出 */
+  } last;
+
+  LowPassFilter2p_t dfilter; /* D项低通滤波器 */
+} KPID_t;
+
+/**
+ * @brief 初始化PID
+ *
+ * @param pid PID结构体
+ * @param mode PID模式
+ * @param sample_freq 采样频率
+ * @param param PID参数
+ * @return int8_t 0对应没有错误
+ */
+int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq,
+                const KPID_Params_t *param);
+
+/**
+ * @brief PID计算
+ *
+ * @param pid PID结构体
+ * @param sp 设定值
+ * @param fb 反馈值
+ * @param fb_dot 反馈值微分
+ * @param dt 间隔时间
+ * @return float 计算的输出
+ */
+float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt);
+
+/**
+ * @brief 重置微分项
+ *
+ * @param pid PID结构体
+ * @return int8_t 0对应没有错误
+ */
+int8_t PID_ResetIntegral(KPID_t *pid);
+
+/**
+ * @brief 重置PID
+ *
+ * @param pid PID结构体
+ * @return int8_t 0对应没有错误
+ */
+int8_t PID_Reset(KPID_t *pid);
+
+#ifdef __cplusplus
+}
+#endif

User/component/user_math.c

@@ -0,0 +1,132 @@
+/*
+  自定义的数学运算。
+*/
+
+#include "user_math.h"
+
+#include <string.h>
+
+inline float InvSqrt(float x) {
+//#if 0
+  /* Fast inverse square-root */
+  /* See: http://en.wikipedia.org/wiki/Fast_inverse_square_root */
+	float halfx = 0.5f * x;
+	float y = x;
+	long i = *(long*)&y;
+	i = 0x5f3759df - (i>>1);
+	y = *(float*)&i;
+	y = y * (1.5f - (halfx * y * y));
+	y = y * (1.5f - (halfx * y * y));
+	return y;	
+//#else
+//  return 1.0f / sqrtf(x);
+//#endif
+}
+
+inline float AbsClip(float in, float limit) {
+  return (in < -limit) ? -limit : ((in > limit) ? limit : in);
+}
+
+float fAbs(float in){
+  return (in > 0) ? in : -in;
+}
+
+inline void Clip(float *origin, float min, float max) {
+  if (*origin > max) *origin = max;
+  if (*origin < min) *origin = min;
+}
+
+inline float Sign(float in) { return (in > 0) ? 1.0f : 0.0f; }
+
+/**
+ * \brief 将运动向量置零
+ *
+ * \param mv 被操作的值
+ */
+inline void ResetMoveVector(MoveVector_t *mv) { memset(mv, 0, sizeof(*mv)); }
+
+/**
+ * \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
+ * 例如编码器：相差1.5PI其实等于相差-0.5PI
+ *
+ * \param sp 被操作的值
+ * \param fb 变化量
+ * \param range 被操作的值变化范围，正数时起效
+ *
+ * \return 函数运行结果
+ */
+inline float CircleError(float sp, float fb, float range) {
+  float error = sp - fb;
+  if (range > 0.0f) {
+    float half_range = range / 2.0f;
+
+    if (error > half_range)
+      error -= range;
+    else if (error < -half_range)
+      error += range;
+  }
+  return error;
+}
+
+/**
+ * \brief 循环加法，用于没有负数值，并在一定范围内变化的值
+ * 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI
+ *
+ * \param origin 被操作的值
+ * \param delta 变化量
+ * \param range 被操作的值变化范围，正数时起效
+ */
+inline void CircleAdd(float *origin, float delta, float range) {
+  float out = *origin + delta;
+  if (range > 0.0f) {
+    if (out >= range)
+      out -= range;
+    else if (out < 0.0f)
+      out += range;
+  }
+  *origin = out;
+}
+
+/**
+ * @brief 循环值取反
+ *
+ * @param origin 被操作的值
+ */
+inline void CircleReverse(float *origin) { *origin = -(*origin) + M_2PI; }
+
+/**
+ * @brief 根据目标弹丸速度计算摩擦轮转速
+ *
+ * @param bullet_speed 弹丸速度
+ * @param fric_radius 摩擦轮半径
+ * @param is17mm 是否为17mm
+ * @return 摩擦轮转速
+ */
+inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
+  if (bullet_speed == 0.0f) return 0.f;
+  if (is17mm) {
+    if (bullet_speed == 15.0f) return 4670.f;
+    if (bullet_speed == 18.0f) return 5200.f;
+    if (bullet_speed == 30.0f) return 7350.f;
+  } else {
+    if (bullet_speed == 10.0f) return 4450.f;
+    if (bullet_speed == 16.0f) return 5800.f;
+  }
+
+  /* 不为裁判系统设定值时,计算转速 */
+  return 60.0f * (float)bullet_speed / (M_2PI * fric_radius);
+}
+
+// /**
+//  * @brief 断言失败处理
+//  *
+//  * @param file 文件名
+//  * @param line 行号
+//  */
+// void VerifyFailed(const char *file, uint32_t line) {
+//   UNUSED(file);
+//   UNUSED(line);
+//   while (1) {
+//     __NOP();
+//   }
+// }

User/component/user_math.h

@@ -0,0 +1,161 @@
+/*
+  自定义的数学运算。
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <float.h>
+#include <math.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stddef.h>
+
+#define M_DEG2RAD_MULT (0.01745329251f)
+#define M_RAD2DEG_MULT (57.2957795131f)
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846f
+#endif
+
+#ifndef M_2PI
+#define M_2PI 6.28318530717958647692f
+#endif
+
+#ifndef __packed
+  #define __packed __attribute__((__packed__))
+#endif /* __packed */
+
+#define max(a, b)           \
+  ({                        \
+    __typeof__(a) _a = (a); \
+    __typeof__(b) _b = (b); \
+    _a > _b ? _a : _b;      \
+  })
+
+#define min(a, b)           \
+  ({                        \
+    __typeof__(a) _a = (a); \
+    __typeof__(b) _b = (b); \
+    _a < _b ? _a : _b;      \
+  })
+
+/* 移动向量 */
+typedef struct {
+  float vx; /* 前后平移 */
+  float vy; /* 左右平移 */
+  float wz; /* 转动 */
+} MoveVector_t;
+
+float InvSqrt(float x);
+
+float AbsClip(float in, float limit);
+
+float fAbs(float in);
+
+void Clip(float *origin, float min, float max);
+
+float Sign(float in);
+
+/**
+ * \brief 将运动向量置零
+ *
+ * \param mv 被操作的值
+ */
+void ResetMoveVector(MoveVector_t *mv);
+
+/**
+ * \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
+ * 例如编码器：相差1.5PI其实等于相差-0.5PI
+ *
+ * \param sp 被操作的值
+ * \param fb 变化量
+ * \param range 被操作的值变化范围，正数时起效
+ *
+ * \return 函数运行结果
+ */
+float CircleError(float sp, float fb, float range);
+
+/**
+ * \brief 循环加法，用于没有负数值，并在一定范围内变化的值
+ * 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI
+ *
+ * \param origin 被操作的值
+ * \param delta 变化量
+ * \param range 被操作的值变化范围，正数时起效
+ */
+void CircleAdd(float *origin, float delta, float range);
+
+/**
+ * @brief 循环值取反
+ *
+ * @param origin 被操作的值
+ */
+void CircleReverse(float *origin);
+
+/**
+ * @brief 根据目标弹丸速度计算摩擦轮转速
+ *
+ * @param bullet_speed 弹丸速度
+ * @param fric_radius 摩擦轮半径
+ * @param is17mm 是否为17mm
+ * @return 摩擦轮转速
+ */
+float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm);
+
+#ifdef __cplusplus
+}
+#endif
+
+#ifdef DEBUG
+
+/**
+ * @brief 如果表达式的值为假则运行处理函数
+ *
+ */
+#define ASSERT(expr)                    \
+  do {                                  \
+    if (!(expr)) {                      \
+      VerifyFailed(__FILE__, __LINE__); \
+    }                                   \
+  } while (0)
+#else
+
+/**
+ * @brief 未定DEBUG，表达式不会运行，断言被忽略
+ *
+ */
+#define ASSERT(expr) ((void)(0))
+#endif
+
+#ifdef DEBUG
+
+/**
+ * @brief 如果表达式的值为假则运行处理函数
+ *
+ */
+#define VERIFY(expr)                    \
+  do {                                  \
+    if (!(expr)) {                      \
+      VerifyFailed(__FILE__, __LINE__); \
+    }                                   \
+  } while (0)
+#else
+
+/**
+ * @brief 表达式会运行，忽略表达式结果
+ *
+ */
+#define VERIFY(expr) ((void)(expr))
+#endif
+
+// /**
+//  * @brief 断言失败处理
+//  *
+//  * @param file 文件名
+//  * @param line 行号
+//  */
+// void VerifyFailed(const char *file, uint32_t line);

User/component/vmc.c

@@ -0,0 +1,381 @@
+/*
+ * VMC虚拟模型控制器实现
+ * 
+ * 本文件实现了轮腿机器人的VMC (Virtual Model Control) 虚拟模型控制算法
+ * 主要功能包括:
+ * 1. 五连杆机构的正逆运动学解算
+ * 2. 虚拟力到关节力矩的映射
+ * 3. 地面接触检测
+ * 4. 等效摆动杆模型转换
+ * 
+ * 参考文献:
+ * - 韭菜的菜 知乎: 平衡步兵控制系统设计
+ * - 上交轮腿电控开源方案 (2023)
+ */
+
+#include "vmc.h"
+#include <string.h>
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+
+#define VMC_EPSILON         (1e-6f)     // 数值计算精度
+#define VMC_MAX_ITER        (10)        // 最大迭代次数
+
+/* Private macro ------------------------------------------------------------ */
+
+/**
+ * @brief 限制数值范围
+ */
+#define VMC_CLAMP(val, min, max) ((val) < (min) ? (min) : ((val) > (max) ? (max) : (val)))
+
+/**
+ * @brief 安全开方
+ */
+#define VMC_SAFE_SQRT(x) (((x) > 0) ? sqrtf(x) : 0.0f)
+
+/* Private variables -------------------------------------------------------- */
+/* Private function prototypes ---------------------------------------------- */
+
+static int8_t VMC_ValidateParams(const VMC_Param_t *param);
+static void VMC_UpdateKinematics(VMC_t *vmc, float phi1, float phi4);
+static int8_t VMC_SolveClosedLoop(VMC_t *vmc);
+static float VMC_ComputeNumericDerivative(float current, float previous, float dt);
+
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * @brief 初始化VMC控制器
+ */
+int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq) {
+    if (vmc == NULL || param == NULL || sample_freq <= 0) {
+        return -1;
+    }
+    
+    // 复制参数
+    memcpy(&vmc->param, param, sizeof(VMC_Param_t));
+    
+    // 设置控制周期
+    vmc->dt = 1.0f / sample_freq;
+    
+    // 重置状态
+    VMC_Reset(vmc);
+    
+    vmc->initialized = true;
+    
+    return 0;
+}
+
+/**
+ * @brief VMC五连杆正解算
+ * 
+ * 通过髋关节角度和机体姿态，计算足端位置和等效摆动杆参数
+ * 
+ * 坐标系定义:
+ * - x轴: 机体前进方向为正
+ * - y轴: 竖直向下为正  
+ * - 角度: 顺时针为正
+ */
+int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, float body_pitch_rate) {
+    if (vmc == NULL || !vmc->initialized) {
+        return -1;
+    }
+    
+    VMC_Leg_t *leg = &vmc->leg;
+    
+    // 保存历史值
+    leg->last_phi0 = leg->phi0;
+    leg->last_L0 = leg->L0;
+    leg->last_d_L0 = leg->d_L0;
+    leg->last_d_theta = leg->d_theta;
+    
+    // 更新关节角度
+    leg->phi1 = phi1;
+    leg->phi4 = phi4;
+    
+    // 更新运动学状态
+    VMC_UpdateKinematics(vmc, phi1, phi4);
+    
+    // 求解闭环运动学
+    if (VMC_SolveClosedLoop(vmc) != 0) {
+        return -1;
+    }
+    
+    // 计算足端坐标
+    leg->foot_x = leg->XC - vmc->param.hip_length / 2.0f;
+    leg->foot_y = leg->YC;
+    
+    // 计算等效摆动杆参数
+    leg->L0 = VMC_SAFE_SQRT(leg->foot_x * leg->foot_x + leg->foot_y * leg->foot_y);
+    leg->phi0 = atan2f(leg->foot_y, leg->foot_x);
+    
+    // 计算等效摆动杆角度(相对于机体坐标系)
+    leg->alpha = VMC_PI_2 - leg->phi0;
+    leg->theta = -(VMC_PI_2 + body_pitch - leg->phi0);
+    
+    // 角度归一化
+    VMC_ANGLE_NORMALIZE(leg->theta);
+    VMC_ANGLE_NORMALIZE(leg->phi0);
+    
+    // 计算角速度和长度变化率
+    leg->d_phi0 = VMC_ComputeNumericDerivative(leg->phi0, leg->last_phi0, vmc->dt);
+    leg->d_alpha = -leg->d_phi0;
+    leg->d_theta = body_pitch_rate + leg->d_phi0;
+    leg->d_L0 = VMC_ComputeNumericDerivative(leg->L0, leg->last_L0, vmc->dt);
+    
+    // 计算角加速度
+    leg->dd_theta = VMC_ComputeNumericDerivative(leg->d_theta, leg->last_d_theta, vmc->dt);
+    
+    return 0;
+}
+
+/**
+ * @brief VMC五连杆逆解算(力矩分配)
+ * 
+ * 根据期望的虚拟力和力矩，通过雅可比矩阵计算关节力矩
+ */
+int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual) {
+    if (vmc == NULL || !vmc->initialized) {
+        return -1;
+    }
+    
+    // 保存虚拟力和力矩
+    vmc->leg.F_virtual = -F_virtual;
+    vmc->leg.T_virtual = T_virtual;
+    
+    // 计算雅可比矩阵
+    if (VMC_ComputeJacobian(vmc) != 0) {
+        return -1;
+    }
+    
+    VMC_Leg_t *leg = &vmc->leg;
+    
+    // 通过雅可比转置计算关节力矩
+    // tau = J^T * F_virtual
+    leg->tau_hip_rear = leg->J11 * vmc->leg.F_virtual + leg->J12 * vmc->leg.T_virtual;
+    leg->tau_hip_front = leg->J21 * vmc->leg.F_virtual + leg->J22 * vmc->leg.T_virtual;
+    
+    return 0;
+}
+
+/**
+ * @brief 地面接触检测
+ * 
+ * 基于虚拟力和腿部状态估计地面法向力
+ */
+float VMC_GroundContactDetection(VMC_t *vmc) {
+    if (vmc == NULL || !vmc->initialized) {
+        return 0.0f;
+    }
+    
+    VMC_Leg_t *leg = &vmc->leg;
+    
+    // 计算地面法向力 
+    // Fn = F0*cos(theta) + Tp*sin(theta)/L0 + mg
+    leg->Fn = leg->F_virtual * cosf(leg->theta) + 
+              leg->T_virtual * sinf(leg->theta) / leg->L0 + 
+              vmc->param.wheel_mass * 9.8f;  // 添加轮子重力
+    
+    // 地面接触判断
+    leg->is_ground_contact = (leg->Fn > 10.0f);  // 10N阈值
+    
+    return leg->Fn;
+}
+
+/**
+ * @brief 获取足端位置
+ */
+int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y) {
+    if (vmc == NULL || !vmc->initialized || x == NULL || y == NULL) {
+        return -1;
+    }
+    
+    *x = vmc->leg.foot_x;
+    *y = vmc->leg.foot_y;
+    
+    return 0;
+}
+
+/**
+ * @brief 获取等效摆动杆参数
+ */
+int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, float *d_length, float *d_angle) {
+    if (vmc == NULL || !vmc->initialized) {
+        return -1;
+    }
+    
+    if (length) *length = vmc->leg.L0;
+    if (angle) *angle = vmc->leg.theta;
+    if (d_length) *d_length = vmc->leg.d_L0;
+    if (d_angle) *d_angle = vmc->leg.d_theta;
+    
+    return 0;
+}
+
+/**
+ * @brief 获取关节输出力矩
+ */
+int8_t VMC_GetJointTorques(const VMC_t *vmc, float *tau_front, float *tau_rear) {
+    if (vmc == NULL || !vmc->initialized || tau_front == NULL || tau_rear == NULL) {
+        return -1;
+    }
+    
+    *tau_front = vmc->leg.tau_hip_front;
+    *tau_rear = vmc->leg.tau_hip_rear;
+    
+    return 0;
+}
+
+/**
+ * @brief 重置VMC控制器状态
+ */
+void VMC_Reset(VMC_t *vmc) {
+    if (vmc == NULL) {
+        return;
+    }
+    
+    // 清零腿部状态
+    memset(&vmc->leg, 0, sizeof(VMC_Leg_t));
+    
+    // 设置初始值
+    vmc->leg.L0 = 0.15f;  // 默认腿长15cm
+    vmc->leg.theta = 0.0f;
+}
+
+/**
+ * @brief 设置虚拟力和力矩
+ */
+void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual) {
+    if (vmc == NULL || !vmc->initialized) {
+        return;
+    }
+    
+    vmc->leg.F_virtual = F_virtual;
+    vmc->leg.T_virtual = T_virtual;
+}
+
+/**
+ * @brief 计算雅可比矩阵
+ * 
+ * 根据当前关节配置计算从虚拟力到关节力矩的雅可比矩阵
+ */
+int8_t VMC_ComputeJacobian(VMC_t *vmc) {
+    if (vmc == NULL || !vmc->initialized) {
+        return -1;
+    }
+    
+    VMC_Leg_t *leg = &vmc->leg;
+    
+    // 检查分母不为零
+    float sin_diff = sinf(leg->phi3 - leg->phi2);
+    if (fabsf(sin_diff) < VMC_EPSILON) {
+        return -1;  // 奇异配置
+    }
+    
+    // 计算雅可比矩阵元素
+    // J11: 后髋关节到支撑力的雅可比
+    leg->J11 = (vmc->param.leg_1 * sinf(leg->phi0 - leg->phi3) * 
+                sinf(leg->phi1 - leg->phi2)) / sin_diff;
+    
+    // J12: 后髋关节到摆动力矩的雅可比  
+    leg->J12 = (vmc->param.leg_1 * cosf(leg->phi0 - leg->phi3) * 
+                sinf(leg->phi1 - leg->phi2)) / (leg->L0 * sin_diff);
+    
+    // J21: 前髋关节到支撑力的雅可比
+    leg->J21 = (vmc->param.leg_4 * sinf(leg->phi0 - leg->phi2) * 
+                sinf(leg->phi3 - leg->phi4)) / sin_diff;
+    
+    // J22: 前髋关节到摆动力矩的雅可比
+    leg->J22 = (vmc->param.leg_4 * cosf(leg->phi0 - leg->phi2) * 
+                sinf(leg->phi3 - leg->phi4)) / (leg->L0 * sin_diff);
+    
+    return 0;
+}
+
+/* Private functions -------------------------------------------------------- */
+
+/**
+ * @brief 验证VMC参数有效性
+ */
+static int8_t VMC_ValidateParams(const VMC_Param_t *param) {
+    if (param->hip_length <= 0 || param->leg_1 <= 0 || param->leg_2 <= 0 || 
+        param->leg_3 <= 0 || param->leg_4 <= 0 || param->wheel_radius <= 0) {
+        return -1;
+    }
+    
+    // 检查腿部几何约束
+    if (param->leg_2 + param->leg_3 <= param->leg_1 + param->leg_4) {
+        return -1;  // 不满足闭环几何约束
+    }
+    
+    return 0;
+}
+
+/**
+ * @brief 更新基本运动学参数
+ */
+static void VMC_UpdateKinematics(VMC_t *vmc, float phi1, float phi4) {
+    VMC_Leg_t *leg = &vmc->leg;
+    
+    // 计算关键点坐标
+    // 点B (后髋关节末端)
+    leg->XB = vmc->param.leg_1 * cosf(phi1);
+    leg->YB = vmc->param.leg_1 * sinf(phi1);
+    
+    // 点D (前髋关节末端)  
+    leg->XD = vmc->param.hip_length + vmc->param.leg_4 * cosf(phi4);
+    leg->YD = vmc->param.leg_4 * sinf(phi4);
+    
+    // 计算BD连杆长度
+    float dx = leg->XD - leg->XB;
+    float dy = leg->YD - leg->YB;
+    leg->lBD = VMC_SAFE_SQRT(dx * dx + dy * dy);
+}
+
+/**
+ * @brief 求解闭环运动学方程
+ * 
+ * 根据两个髋关节角度，求解中间关节角度
+ */
+static int8_t VMC_SolveClosedLoop(VMC_t *vmc) {
+    VMC_Leg_t *leg = &vmc->leg;
+    
+    // 使用余弦定理求解phi2
+    leg->A0 = 2 * vmc->param.leg_2 * (leg->XD - leg->XB);
+    leg->B0 = 2 * vmc->param.leg_2 * (leg->YD - leg->YB);
+    leg->C0 = vmc->param.leg_2 * vmc->param.leg_2 + 
+              leg->lBD * leg->lBD - 
+              vmc->param.leg_3 * vmc->param.leg_3;
+    
+    // 检查判别式
+    float discriminant = leg->A0 * leg->A0 + leg->B0 * leg->B0 - leg->C0 * leg->C0;
+    if (discriminant < 0) {
+        return -1;  // 无解
+    }
+    
+    float sqrt_discriminant = VMC_SAFE_SQRT(discriminant);
+    
+    // 计算phi2 (选择合适的解)
+    leg->phi2 = 2 * atan2f(leg->B0 + sqrt_discriminant, leg->A0 + leg->C0);
+    
+    // 计算phi3
+    leg->phi3 = atan2f(leg->YB - leg->YD + vmc->param.leg_2 * sinf(leg->phi2),
+                       leg->XB - leg->XD + vmc->param.leg_2 * cosf(leg->phi2));
+    
+    // 计算足端坐标点C
+    leg->XC = leg->XB + vmc->param.leg_2 * cosf(leg->phi2);
+    leg->YC = leg->YB + vmc->param.leg_2 * sinf(leg->phi2);
+    
+    return 0;
+}
+
+/**
+ * @brief 计算数值微分
+ */
+static float VMC_ComputeNumericDerivative(float current, float previous, float dt) {
+    if (dt <= 0) {
+        return 0.0f;
+    }
+    
+    return (current - previous) / dt;
+}

User/component/vmc.h

@@ -0,0 +1,196 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdint.h>
+#include <stdbool.h>
+#include <math.h>
+#include "kinematics.h"
+
+/* Exported types ----------------------------------------------------------- */
+
+/**
+ * @brief VMC虚拟模型控制参数结构体
+ */
+typedef struct {
+    float hip_length;       // 髋关节间距
+    float leg_1;           // 大腿前端长度 (L1)
+    float leg_2;           // 大腿后端长度 (L2) 
+    float leg_3;           // 小腿长度 (L3)
+    float leg_4;           // 小腿前端长度 (L4)
+    float wheel_radius;    // 轮子半径
+    float wheel_mass;      // 轮子质量
+} VMC_Param_t;
+
+/**
+ * @brief VMC腿部运动学状态结构体
+ */
+typedef struct {
+    // 关节角度
+    float phi1;            // 后髋关节角度 (rad)
+    float phi2;            // 大腿后端角度 (rad)
+    float phi3;            // 小腿角度 (rad) 
+    float phi4;            // 前髋关节角度 (rad)
+    
+    // 足端坐标
+    float foot_x;          // 足端x坐标
+    float foot_y;          // 足端y坐标
+    
+    // 等效摆动杆参数
+    float L0;              // 等效摆动杆长度
+    float d_L0;            // 等效摆动杆长度变化率
+    float theta;           // 等效摆动杆角度
+    float d_theta;         // 等效摆动杆角速度
+    float dd_theta;        // 等效摆动杆角加速度
+    
+    // 虚拟力和力矩
+    float F_virtual;       // 虚拟支撑力
+    float T_virtual;       // 虚拟摆动力矩
+    
+    // 雅可比矩阵元素
+    float J11, J12, J21, J22;
+    
+    // 输出力矩
+    float tau_hip_front;   // 前髋关节输出力矩
+    float tau_hip_rear;    // 后髋关节输出力矩
+    
+    // 内部计算变量
+    float XB, YB, XC, YC, XD, YD;  // 各关键点坐标
+    float lBD;             // BD连杆长度
+    float A0, B0, C0;      // 运动学计算中间变量
+    float phi0;            // 足端极角
+    float alpha;           // 等效摆动杆与竖直方向夹角
+    float d_phi0;          // 足端极角变化率
+    float d_alpha;         // alpha角变化率
+    
+    // 历史值（用于数值微分）
+    float last_phi0;
+    float last_L0; 
+    float last_d_L0;
+    float last_d_theta;
+    
+    // 地面接触检测
+    float Fn;              // 地面法向力
+    bool is_ground_contact; // 地面接触标志
+} VMC_Leg_t;
+
+/**
+ * @brief VMC控制器结构体
+ */
+typedef struct {
+    VMC_Param_t param;     // VMC参数
+    VMC_Leg_t leg;         // 腿部状态
+    float dt;              // 控制周期
+    bool initialized;      // 初始化标志
+} VMC_t;
+
+/* Exported constants ------------------------------------------------------- */
+
+#define VMC_PI_2        (1.5707963267948966f)
+#define VMC_PI          (3.1415926535897932f)
+#define VMC_2PI         (6.2831853071795865f)
+
+/* Exported macros ---------------------------------------------------------- */
+
+/**
+ * @brief 角度范围限制到[-PI, PI]
+ */
+#define VMC_ANGLE_NORMALIZE(angle) do { \
+    while((angle) > VMC_PI) (angle) -= VMC_2PI; \
+    while((angle) < -VMC_PI) (angle) += VMC_2PI; \
+} while(0)
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 初始化VMC控制器
+ * @param vmc VMC控制器实例
+ * @param param VMC参数
+ * @param sample_freq 采样频率 (Hz)
+ * @return 0:成功, -1:失败
+ */
+int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq);
+
+/**
+ * @brief VMC五连杆正解算
+ * @param vmc VMC控制器实例
+ * @param phi1 后髋关节角度 (rad)
+ * @param phi4 前髋关节角度 (rad)
+ * @param body_pitch 机体pitch角 (rad)
+ * @param body_pitch_rate 机体pitch角速度 (rad/s)
+ * @return 0:成功, -1:失败
+ */
+int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, float body_pitch_rate);
+
+/**
+ * @brief VMC五连杆逆解算(力矩分配)
+ * @param vmc VMC控制器实例
+ * @param F_virtual 期望虚拟支撑力 (N)
+ * @param T_virtual 期望虚拟摆动力矩 (N*m)
+ * @return 0:成功, -1:失败
+ */
+int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual);
+
+/**
+ * @brief 地面接触检测
+ * @param vmc VMC控制器实例
+ * @return 地面法向力 (N)
+ */
+float VMC_GroundContactDetection(VMC_t *vmc);
+
+/**
+ * @brief 获取足端位置(直角坐标)
+ * @param vmc VMC控制器实例
+ * @param x 足端x坐标输出
+ * @param y 足端y坐标输出
+ * @return 0:成功, -1:失败
+ */
+int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y);
+
+/**
+ * @brief 获取等效摆动杆参数
+ * @param vmc VMC控制器实例
+ * @param length 等效摆动杆长度输出
+ * @param angle 等效摆动杆角度输出
+ * @param d_length 等效摆动杆长度变化率输出
+ * @param d_angle 等效摆动杆角速度输出
+ * @return 0:成功, -1:失败
+ */
+int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, float *d_length, float *d_angle);
+
+/**
+ * @brief 获取关节输出力矩
+ * @param vmc VMC控制器实例
+ * @param tau_front 前髋关节力矩输出
+ * @param tau_rear 后髋关节力矩输出
+ * @return 0:成功, -1:失败
+ */
+int8_t VMC_GetJointTorques(const VMC_t *vmc, float *tau_front, float *tau_rear);
+
+/**
+ * @brief 重置VMC控制器状态
+ * @param vmc VMC控制器实例
+ */
+void VMC_Reset(VMC_t *vmc);
+
+/**
+ * @brief 设置虚拟力和力矩
+ * @param vmc VMC控制器实例
+ * @param F_virtual 虚拟支撑力 (N)
+ * @param T_virtual 虚拟摆动力矩 (N*m)
+ */
+void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual);
+
+/**
+ * @brief 计算雅可比矩阵
+ * @param vmc VMC控制器实例
+ * @return 0:成功, -1:失败
+ */
+int8_t VMC_ComputeJacobian(VMC_t *vmc);
+
+#ifdef __cplusplus
+}
+#endif

User/device/buzzer.c

@@ -0,0 +1,44 @@
+#include "device/buzzer.h"
+
+
+int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel) {
+    if (buzzer == NULL) return DEVICE_ERR;
+    
+    buzzer->channel = channel;
+    buzzer->header.online = true;
+    
+    BUZZER_Stop(buzzer);
+    
+    return DEVICE_OK ;
+}
+
+int8_t BUZZER_Start(BUZZER_t *buzzer) {
+    if (buzzer == NULL || !buzzer->header.online) 
+        return DEVICE_ERR;
+    
+    return (BSP_PWM_Start(buzzer->channel) == BSP_OK) ? 
+           DEVICE_OK  : DEVICE_ERR;
+}
+
+int8_t BUZZER_Stop(BUZZER_t *buzzer) {
+    if (buzzer == NULL || !buzzer->header.online) 
+        return DEVICE_ERR;
+    
+    return (BSP_PWM_Stop(buzzer->channel) == BSP_OK) ? 
+           DEVICE_OK  : DEVICE_ERR;
+}
+
+int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle) {
+    if (buzzer == NULL || !buzzer->header.online) 
+        return DEVICE_ERR;
+    
+    int result = DEVICE_OK ;
+    
+    if (BSP_PWM_SetFreq(buzzer->channel, freq) != BSP_OK) 
+        result = DEVICE_ERR;
+    
+    if (BSP_PWM_SetComp(buzzer->channel, duty_cycle) != BSP_OK) 
+        result = DEVICE_ERR;
+    
+    return result;
+}

User/device/buzzer.h

@@ -0,0 +1,35 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device.h"          
+#include "bsp/pwm.h"         
+#include <stddef.h>
+
+/* Exported constants ------------------------------------------------------- */
+
+/* Exported types ----------------------------------------------------------- */
+typedef struct {
+    DEVICE_Header_t header;    
+    BSP_PWM_Channel_t channel; 
+} BUZZER_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel);
+
+
+int8_t BUZZER_Start(BUZZER_t *buzzer);
+
+
+int8_t BUZZER_Stop(BUZZER_t *buzzer);
+
+
+int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle);
+
+#ifdef __cplusplus
+}
+#endif

User/device/device.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define DEVICE_OK (0)
+#define DEVICE_ERR (-1)
+#define DEVICE_ERR_NULL (-2)
+#define DEVICE_ERR_INITED (-3)
+#define DEVICE_ERR_NO_DEV (-4)
+
+/* AUTO GENERATED SIGNALS BEGIN */
+#define SIGNAL_DR16_RAW_REDY (1u << 0)
+/* AUTO GENERATED SIGNALS END */
+
+/* USER SIGNALS BEGIN */
+
+/* USER SIGNALS END */
+/*设备层通用Header*/
+typedef struct {
+    bool online;
+    uint64_t last_online_time;
+} DEVICE_Header_t;
+
+#ifdef __cplusplus
+}
+#endif

User/device/device_config.yaml

@@ -0,0 +1,19 @@
+buzzer:
+  bsp_config: {}
+  enabled: true
+dm_imu:
+  bsp_config: {}
+  enabled: true
+dr16:
+  bsp_config:
+    BSP_UART_DR16: BSP_UART_DR16
+  enabled: true
+motor:
+  bsp_config: {}
+  enabled: true
+motor_lk:
+  bsp_config: {}
+  enabled: true
+motor_lz:
+  bsp_config: {}
+  enabled: true

User/device/dm_imu.c

@@ -0,0 +1,271 @@
+/*
+    DM_IMU数据获取
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "dm_imu.h"
+
+#include "bsp/can.h"
+#include "bsp/time.h"
+#include "component/user_math.h"
+#include <string.h>
+
+/* Private define ----------------------------------------------------------- */
+#define DM_IMU_OFFLINE_TIMEOUT      1000    // 设备离线判定时间1000ms
+
+#define ACCEL_CAN_MAX (58.8f)
+#define ACCEL_CAN_MIN	(-58.8f)
+#define GYRO_CAN_MAX	(34.88f)
+#define GYRO_CAN_MIN	(-34.88f)
+#define PITCH_CAN_MAX	(90.0f)
+#define PITCH_CAN_MIN	(-90.0f)
+#define ROLL_CAN_MAX	(180.0f)
+#define ROLL_CAN_MIN	(-180.0f)
+#define YAW_CAN_MAX		(180.0f)
+#define YAW_CAN_MIN 	(-180.0f)
+#define TEMP_MIN			(0.0f)
+#define TEMP_MAX			(60.0f)
+#define Quaternion_MIN	(-1.0f)
+#define Quaternion_MAX	(1.0f)
+
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+/* Private function --------------------------------------------------------- */
+
+static uint8_t count = 0; // 计数器，用于判断设备是否离线
+/**
+ * @brief: 无符号整数转换为浮点数函数
+ */
+static float uint_to_float(int x_int, float x_min, float x_max, int bits)
+{
+    float span = x_max - x_min;
+    float offset = x_min;
+    return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
+}
+
+/**
+ * @brief 解析加速度计数据
+ */
+static int8_t DM_IMU_ParseAccelData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    int8_t temp = data[1];
+    uint16_t acc_x_raw = (data[3] << 8) | data[2];
+    uint16_t acc_y_raw = (data[5] << 8) | data[4];
+    uint16_t acc_z_raw = (data[7] << 8) | data[6];
+    imu->data.temp = (float)temp;
+    imu->data.accl.x = uint_to_float(acc_x_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
+    imu->data.accl.y = uint_to_float(acc_y_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
+    imu->data.accl.z = uint_to_float(acc_z_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
+    return DEVICE_OK;
+}
+/**
+ * @brief 解析陀螺仪数据
+ */
+static int8_t DM_IMU_ParseGyroData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    uint16_t gyro_x_raw = (data[3] << 8) | data[2];
+    uint16_t gyro_y_raw = (data[5] << 8) | data[4];
+    uint16_t gyro_z_raw = (data[7] << 8) | data[6];
+    imu->data.gyro.x = uint_to_float(gyro_x_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
+    imu->data.gyro.y = uint_to_float(gyro_y_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
+    imu->data.gyro.z = uint_to_float(gyro_z_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
+    return DEVICE_OK;
+}
+/**
+ * @brief 解析欧拉角数据
+ */
+static int8_t DM_IMU_ParseEulerData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    uint16_t pit_raw = (data[3] << 8) | data[2];
+    uint16_t yaw_raw = (data[5] << 8) | data[4];
+    uint16_t rol_raw = (data[7] << 8) | data[6];
+    imu->data.euler.pit = uint_to_float(pit_raw, PITCH_CAN_MIN, PITCH_CAN_MAX, 16) * M_DEG2RAD_MULT;
+    imu->data.euler.yaw = uint_to_float(yaw_raw, YAW_CAN_MIN, YAW_CAN_MAX, 16) * M_DEG2RAD_MULT;
+    imu->data.euler.rol = uint_to_float(rol_raw, ROLL_CAN_MIN, ROLL_CAN_MAX, 16) * M_DEG2RAD_MULT;
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 解析四元数数据
+ */
+static int8_t DM_IMU_ParseQuaternionData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    int w = (data[1] << 6) | ((data[2] & 0xF8) >> 2);
+    int x = ((data[2] & 0x03) << 12) | (data[3] << 4) | ((data[4] & 0xF0) >> 4);
+    int y = ((data[4] & 0x0F) << 10) | (data[5] << 2) | ((data[6] & 0xC0) >> 6);
+    int z = ((data[6] & 0x3F) << 8) | data[7];
+    imu->data.quat.q0 = uint_to_float(w, Quaternion_MIN, Quaternion_MAX, 14);
+    imu->data.quat.q1 = uint_to_float(x, Quaternion_MIN, Quaternion_MAX, 14);
+    imu->data.quat.q2 = uint_to_float(y, Quaternion_MIN, Quaternion_MAX, 14);
+    imu->data.quat.q3 = uint_to_float(z, Quaternion_MIN, Quaternion_MAX, 14);
+    return DEVICE_OK;
+}
+
+
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * @brief 初始化DM IMU设备
+ */
+int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param) {
+    if (imu == NULL || param == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    // 初始化设备头部
+    imu->header.online = false;
+    imu->header.last_online_time = 0;
+    
+    // 配置参数
+    imu->param.can = param->can;
+    imu->param.can_id = param->can_id;
+    imu->param.device_id = param->device_id;
+    imu->param.master_id = param->master_id;
+    
+    // 清零数据
+    memset(&imu->data, 0, sizeof(DM_IMU_Data_t));
+    
+    // 注册CAN接收队列，用于接收回复报文
+    int8_t result = BSP_CAN_RegisterId(imu->param.can, imu->param.master_id, 10);
+    if (result != BSP_OK) {
+        return DEVICE_ERR;
+    }
+
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 请求IMU数据
+ */
+int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) {
+    if (imu == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    // 构造发送数据：id_L, id_H(DM_IMU_ID), RID, 0xcc
+    uint8_t tx_data[4] = {
+        imu->param.device_id & 0xFF,        // id_L
+        (imu->param.device_id >> 8) & 0xFF, // id_H
+        (uint8_t)rid,                       // RID
+        0xCC                                // 固定值
+    };
+    
+    // 发送标准数据帧
+    BSP_CAN_StdDataFrame_t frame = {
+        .id = imu->param.can_id,
+        .dlc = 4,
+    };
+    memcpy(frame.data, tx_data, 4);
+    
+    int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame);
+    return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR;
+}
+
+/**
+ * @brief 更新IMU数据（从CAN中获取所有数据并解析）
+ */
+int8_t DM_IMU_Update(DM_IMU_t *imu) {
+    if (imu == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    BSP_CAN_Message_t msg;
+    int8_t result;
+    bool data_received = false;
+    
+    // 持续接收所有可用消息
+    while ((result = BSP_CAN_GetMessage(imu->param.can, imu->param.master_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE)) == BSP_OK) {
+        // 验证回复数据格式（至少检查数据长度）
+        if (msg.dlc < 3) {
+            continue; // 跳过无效消息
+        }
+        
+        // 根据数据位的第0位确定反馈报文类型
+        uint8_t rid = msg.data[0] & 0x0F; // 取第0位的低4位作为RID
+        
+        // 根据RID类型解析数据
+        int8_t parse_result = DEVICE_ERR;
+        switch (rid) {
+            case 0x01: // RID_ACCL
+                parse_result = DM_IMU_ParseAccelData(imu, msg.data, msg.dlc);
+                break;
+            case 0x02: // RID_GYRO
+                parse_result = DM_IMU_ParseGyroData(imu, msg.data, msg.dlc);
+                break;
+            case 0x03: // RID_EULER
+                parse_result = DM_IMU_ParseEulerData(imu, msg.data, msg.dlc);
+                break;
+            case 0x04: // RID_QUATERNION
+                parse_result = DM_IMU_ParseQuaternionData(imu, msg.data, msg.dlc);
+                break;
+            default:
+                continue; // 跳过未知类型的消息
+        }
+        
+        // 如果解析成功，标记为收到数据
+        if (parse_result == DEVICE_OK) {
+            data_received = true;
+        }
+    }
+    
+    // 如果收到任何有效数据，更新设备状态
+    if (data_received) {
+        imu->header.online = true;
+        imu->header.last_online_time = BSP_TIME_Get_ms();
+        return DEVICE_OK;
+    }
+    
+    return DEVICE_ERR;
+}
+
+/** 
+ * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz）
+ */ 
+int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){
+    if (imu == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    switch (count) {
+        case 0:
+            DM_IMU_Request(imu, RID_ACCL);
+            break;
+        case 1:
+            DM_IMU_Request(imu, RID_GYRO);
+            break;
+        case 2:
+            DM_IMU_Request(imu, RID_EULER);
+            break;
+        case 3:
+            DM_IMU_Request(imu, RID_QUATERNION);
+            DM_IMU_Update(imu); // 更新所有数据
+            break;
+    }
+    count++;
+    if (count >= 4) {
+        count = 0; // 重置计数器
+    }
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 检查设备是否在线
+ */
+bool DM_IMU_IsOnline(DM_IMU_t *imu) {
+    if (imu == NULL) {
+        return false;
+    }
+    
+    uint32_t current_time = BSP_TIME_Get_ms();
+    return imu->header.online && 
+           (current_time - imu->header.last_online_time < DM_IMU_OFFLINE_TIMEOUT);
+}

User/device/dm_imu.h

@@ -0,0 +1,90 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/device.h"
+#include "component/ahrs.h"
+#include "bsp/can.h"
+/* Exported constants ------------------------------------------------------- */
+
+#define DM_IMU_CAN_ID_DEFAULT 0x6FF
+#define DM_IMU_ID_DEFAULT 0x42
+#define DM_IMU_MST_ID_DEFAULT 0x43
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+typedef struct {
+  BSP_CAN_t can;       // CAN总线句柄
+  uint16_t can_id;     // CAN通信ID
+  uint8_t device_id;   // 设备ID
+  uint8_t master_id;   // 主机ID
+} DM_IMU_Param_t;
+typedef enum {
+  RID_ACCL = 0x01, // 加速度计
+  RID_GYRO = 0x02, // 陀螺仪
+  RID_EULER = 0x03, // 欧拉角
+  RID_QUATERNION = 0x04, // 四元数
+} DM_IMU_RID_t;
+
+typedef struct {
+  AHRS_Accl_t accl; // 加速度计
+  AHRS_Gyro_t gyro; // 陀螺仪
+  AHRS_Eulr_t euler; // 欧拉角
+  AHRS_Quaternion_t quat; // 四元数
+  float temp; // 温度
+} DM_IMU_Data_t;
+
+typedef struct {
+  DEVICE_Header_t header;
+  DM_IMU_Param_t param; // IMU参数配置
+  DM_IMU_Data_t data; // IMU数据
+} DM_IMU_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 初始化DM IMU设备
+ * @param imu DM IMU设备结构体指针
+ * @param param IMU参数配置指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param);
+
+/**
+ * @brief 请求IMU数据
+ * @param imu DM IMU设备结构体指针
+ * @param rid 请求的数据类型
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid);
+
+
+/**
+ * @brief 更新IMU数据（从CAN中获取所有数据并解析）
+ * @param imu DM IMU设备结构体指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_Update(DM_IMU_t *imu);
+
+/**
+ * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz，运行4次才有完整数据）
+ * @param imu DM IMU设备结构体指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu);
+
+/**
+ * @brief 检查设备是否在线
+ * @param imu DM IMU设备结构体指针
+ * @return true 在线，false 离线
+ */
+bool DM_IMU_IsOnline(DM_IMU_t *imu);
+
+
+#ifdef __cplusplus
+}
+#endif

User/device/dr16.c

@@ -0,0 +1,85 @@
+/*
+        DR16接收机
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "dr16.h"
+
+#include <string.h>
+
+#include "bsp/uart.h"
+#include "bsp/time.h"
+/* Private define ----------------------------------------------------------- */
+#define DR16_CH_VALUE_MIN (364u)
+#define DR16_CH_VALUE_MID (1024u)
+#define DR16_CH_VALUE_MAX (1684u)
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+
+static osThreadId_t thread_alert;
+static bool inited = false;
+
+/* Private function  -------------------------------------------------------- */
+static void DR16_RxCpltCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY);
+}
+
+static bool DR16_DataCorrupted(const DR16_t *dr16) {
+  if (dr16 == NULL) return DEVICE_ERR_NULL;
+
+  if ((dr16->data.ch_r_x < DR16_CH_VALUE_MIN) ||
+      (dr16->data.ch_r_x > DR16_CH_VALUE_MAX))
+    return true;
+
+  if ((dr16->data.ch_r_y < DR16_CH_VALUE_MIN) ||
+      (dr16->data.ch_r_y > DR16_CH_VALUE_MAX))
+    return true;
+
+  if ((dr16->data.ch_l_x < DR16_CH_VALUE_MIN) ||
+      (dr16->data.ch_l_x > DR16_CH_VALUE_MAX))
+    return true;
+
+  if ((dr16->data.ch_l_y < DR16_CH_VALUE_MIN) ||
+      (dr16->data.ch_l_y > DR16_CH_VALUE_MAX))
+    return true;
+
+  if (dr16->data.sw_l == 0) return true;
+
+  if (dr16->data.sw_r == 0) return true;
+
+  return false;
+}
+
+/* Exported functions ------------------------------------------------------- */
+int8_t DR16_Init(DR16_t *dr16) {
+  if (dr16 == NULL) return DEVICE_ERR_NULL;
+  if (inited) return DEVICE_ERR_INITED;
+  if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
+
+  BSP_UART_RegisterCallback(BSP_UART_DR16, BSP_UART_RX_CPLT_CB,
+                            DR16_RxCpltCallback);
+
+  inited = true;
+  return DEVICE_OK;
+}
+
+int8_t DR16_Restart(void) {
+  __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_DR16));
+  __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_DR16));
+  return DEVICE_OK;
+}
+
+int8_t DR16_StartDmaRecv(DR16_t *dr16) {
+  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_DR16),
+                           (uint8_t *)&(dr16->data),
+                           sizeof(dr16->data)) == HAL_OK)
+    return DEVICE_OK;
+  return DEVICE_ERR;
+}
+
+bool DR16_WaitDmaCplt(uint32_t timeout) {
+  return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) ==
+          SIGNAL_DR16_RAW_REDY);
+}

User/device/dr16.h

@@ -0,0 +1,47 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <cmsis_os2.h>
+
+#include "component/user_math.h"
+#include "device/device.h"
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef struct __packed {
+  uint16_t ch_r_x : 11;
+  uint16_t ch_r_y : 11;
+  uint16_t ch_l_x : 11;
+  uint16_t ch_l_y : 11;
+  uint8_t sw_r : 2;
+  uint8_t sw_l : 2;
+  int16_t x;
+  int16_t y;
+  int16_t z;
+  uint8_t press_l;
+  uint8_t press_r;
+  uint16_t key;
+  uint16_t res;
+} DR16_Data_t;
+
+typedef struct {
+  DEVICE_Header_t header;
+  DR16_Data_t data;
+} DR16_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+int8_t DR16_Init(DR16_t *dr16);
+int8_t DR16_Restart(void);
+
+int8_t DR16_StartDmaRecv(DR16_t *dr16);
+bool DR16_WaitDmaCplt(uint32_t timeout);
+
+
+#ifdef __cplusplus
+}
+#endif

User/device/motor.c

@@ -0,0 +1,39 @@
+/*
+        DR16接收机
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "motor.h"
+
+#include <string.h>
+
+
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+
+/* Private function  -------------------------------------------------------- */
+
+/* Exported functions ------------------------------------------------------- */
+float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor) {
+  if (motor == NULL) return DEVICE_ERR_NULL;
+    return motor->feedback.rotor_abs_angle;
+}
+
+float MOTOR_GetRotorSpeed(const MOTOR_t *motor) {
+  if (motor == NULL) return DEVICE_ERR_NULL;
+    return motor->feedback.rotor_speed;
+  
+}
+
+float MOTOR_GetTorqueCurrent(const MOTOR_t *motor) {
+  if (motor == NULL) return DEVICE_ERR_NULL;
+    return motor->feedback.torque_current;
+  
+}
+
+float MOTOR_GetTemp(const MOTOR_t *motor) {
+  if (motor == NULL) return DEVICE_ERR_NULL;
+  return motor->feedback.temp;
+}

User/device/motor.h

@@ -0,0 +1,52 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/device.h"
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef struct {
+    float rotor_abs_angle; /* 转子绝对角度 */
+    float rotor_speed; /* 实际转子转速 */
+    float torque_current; /* 转矩电流 */
+    float temp; /* 温度 */
+} MOTOR_Feedback_t;
+
+/**
+ * @brief mit电机输出参数结构体
+ */
+typedef struct {
+    float torque;      /* 目标力矩 */
+    float velocity;    /* 目标速度 */
+    float angle;       /* 目标位置 */
+    float kp;         /* 位置环增益 */
+    float kd;         /* 速度环增益 */
+} MOTOR_MIT_Output_t;
+
+/**
+ * @brief 转矩电流控制模式参数结构体
+ */
+typedef struct {
+    float current;    /* 目标电流 */
+} MOTOR_Current_Output_t;
+
+typedef struct {
+    DEVICE_Header_t header;
+    bool reverse; /* 是否反装 true表示反装 */
+    MOTOR_Feedback_t feedback;
+} MOTOR_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor);
+float MOTOR_GetRotorSpeed(const MOTOR_t *motor);
+float MOTOR_GetTorqueCurrent(const MOTOR_t *motor);
+float MOTOR_GetTemp(const MOTOR_t *motor);
+
+#ifdef __cplusplus
+}
+#endif

User/device/motor_lk.c

@@ -0,0 +1,316 @@
+/*
+  LK电机驱动
+*/
+/* Includes ----------------------------------------------------------------- */
+#include "motor_lk.h"
+#include <stdbool.h>
+#include <string.h>
+#include "bsp/can.h"
+#include "bsp/mm.h"
+#include "bsp/time.h"
+#include "component/user_math.h"
+
+/* Private define ----------------------------------------------------------- */
+#define LK_CTRL_ID_BASE        (0x140)
+#define LK_FB_ID_BASE          (0x240)
+
+// LK电机命令字节定义
+#define LK_CMD_FEEDBACK        (0x9C)    // 反馈命令字节
+#define LK_CMD_MOTOR_OFF       (0x80)    // 电机关闭命令
+#define LK_CMD_MOTOR_ON        (0x88)    // 电机运行命令
+#define LK_CMD_TORQUE_CTRL     (0xA1)    // 转矩闭环控制命令
+
+// LK电机参数定义
+#define LK_CURR_LSB_MF         (33.0f / 4096.0f)   // MF电机转矩电流分辨率 A/LSB
+#define LK_CURR_LSB_MG         (66.0f / 4096.0f)   // MG电机转矩电流分辨率 A/LSB
+#define LK_POWER_RANGE_MS      (1000)              // MS电机功率范围 ±1000
+#define LK_TORQUE_RANGE        (2048)              // 转矩控制值范围 ±2048
+#define LK_TORQUE_CURRENT_MF   (16.5f)             // MF电机最大转矩电流 A
+#define LK_TORQUE_CURRENT_MG   (33.0f)             // MG电机最大转矩电流 A
+
+#define MOTOR_TX_BUF_SIZE      (8)
+#define MOTOR_RX_BUF_SIZE      (8)
+
+// 编码器分辨率定义
+#define LK_ENC_14BIT_MAX       (16383)   // 14位编码器最大值
+#define LK_ENC_15BIT_MAX       (32767)   // 15位编码器最大值  
+#define LK_ENC_16BIT_MAX       (65535)   // 16位编码器最大值
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+static MOTOR_LK_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
+
+/* Private functions -------------------------------------------------------- */
+static float MOTOR_LK_GetCurrentLSB(MOTOR_LK_Module_t module) {
+    switch (module) {
+        case MOTOR_LK_MF9025:
+        case MOTOR_LK_MF9035:
+            return LK_CURR_LSB_MF;
+        default:
+            return LK_CURR_LSB_MG;  // 默认使用MG的分辨率
+    }
+}
+
+static uint16_t MOTOR_LK_GetEncoderMax(MOTOR_LK_Module_t module) {
+    // 根据电机型号返回编码器最大值，这里假设都使用16位编码器
+    // 实际使用时需要根据具体电机型号配置
+    return LK_ENC_16BIT_MAX;
+}
+
+static MOTOR_LK_CANManager_t* MOTOR_LK_GetCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return NULL;
+    return can_managers[can];
+}
+
+static int8_t MOTOR_LK_CreateCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return DEVICE_ERR;
+    if (can_managers[can] != NULL) return DEVICE_OK;
+    
+    can_managers[can] = (MOTOR_LK_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LK_CANManager_t));
+    if (can_managers[can] == NULL) return DEVICE_ERR;
+    
+    memset(can_managers[can], 0, sizeof(MOTOR_LK_CANManager_t));
+    can_managers[can]->can = can;
+    return DEVICE_OK;
+}
+
+static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) {
+
+    // 检查命令字节是否为反馈命令
+    if (msg->data[0] != LK_CMD_FEEDBACK) {
+        // 如果不是标准反馈命令，可能是其他格式的数据
+        // 临时跳过命令字节检查，直接解析数据
+        // return;
+    }
+    
+    // 解析温度 (DATA[1])
+    motor->motor.feedback.temp = (int8_t)msg->data[1];
+
+    // 解析转矩电流值或功率值 (DATA[2], DATA[3])
+    int16_t raw_current_or_power = (int16_t)((msg->data[3] << 8) | msg->data[2]);
+
+    // 根据电机类型解析电流或功率
+    switch (motor->param.module) {
+        case MOTOR_LK_MF9025:
+        case MOTOR_LK_MF9035:
+            motor->motor.feedback.torque_current = raw_current_or_power * MOTOR_LK_GetCurrentLSB(motor->param.module);
+            break;
+        default:
+            motor->motor.feedback.torque_current = (float)raw_current_or_power;
+            break;
+    }
+
+    // 解析转速 (DATA[4], DATA[5]) - 单位：1dps/LSB
+    int16_t raw_speed = (int16_t)((msg->data[5] << 8) | msg->data[4]);
+    motor->motor.feedback.rotor_speed = motor->param.reverse ? -raw_speed : raw_speed;
+
+    // 解析编码器值 (DATA[6], DATA[7])
+    uint16_t raw_encoder = (uint16_t)((msg->data[7] << 8) | msg->data[6]);
+    uint16_t encoder_max = MOTOR_LK_GetEncoderMax(motor->param.module);
+
+    // 将编码器值转换为弧度 (0 ~ 2π)
+    float angle = (float)raw_encoder / (float)encoder_max * M_2PI;
+    motor->motor.feedback.rotor_abs_angle = motor->param.reverse ? (M_2PI - angle) : angle;
+}
+
+/* Exported functions ------------------------------------------------------- */
+
+int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    if (MOTOR_LK_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
+    MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR;
+    
+    // 检查是否已注册
+    for (int i = 0; i < manager->motor_count; i++) {
+        if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
+            return DEVICE_ERR_INITED;
+        }
+    }
+    
+    // 检查数量
+    if (manager->motor_count >= MOTOR_LK_MAX_MOTORS) return DEVICE_ERR;
+    
+    // 创建新电机实例
+    MOTOR_LK_t *new_motor = (MOTOR_LK_t*)BSP_Malloc(sizeof(MOTOR_LK_t));
+    if (new_motor == NULL) return DEVICE_ERR;
+    
+    memcpy(&new_motor->param, param, sizeof(MOTOR_LK_Param_t));
+    memset(&new_motor->motor, 0, sizeof(MOTOR_t));
+    new_motor->motor.reverse = param->reverse;
+    
+    // 对于某些LK电机，反馈数据可能通过命令ID发送
+    // 根据实际测试，使用命令ID接收反馈数据
+    uint16_t feedback_id = param->id;  // 使用命令ID作为反馈ID
+    
+    // 注册CAN接收ID
+    if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) {
+        BSP_Free(new_motor);
+        return DEVICE_ERR;
+    }
+    
+    manager->motors[manager->motor_count] = new_motor;
+    manager->motor_count++;
+    return DEVICE_OK;
+}
+
+int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR_NO_DEV;
+    
+    for (int i = 0; i < manager->motor_count; i++) {
+        MOTOR_LK_t *motor = manager->motors[i];
+        if (motor && motor->param.id == param->id) {
+            // 对于某些LK电机，反馈数据通过命令ID发送
+            uint16_t feedback_id = param->id;
+            
+            BSP_CAN_Message_t rx_msg;
+            if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
+                uint64_t now_time = BSP_TIME_Get();
+                if (now_time - motor->motor.header.last_online_time > 1000) {
+                    motor->motor.header.online = false;
+                    return DEVICE_ERR_NO_DEV;
+                }
+                return DEVICE_ERR;
+            }
+            
+            motor->motor.header.online = true;
+            motor->motor.header.last_online_time = BSP_TIME_Get();
+            MOTOR_LK_Decode(motor, &rx_msg);
+            return DEVICE_OK;
+        }
+    }
+    return DEVICE_ERR_NO_DEV;
+}
+
+int8_t MOTOR_LK_UpdateAll(void) {
+    int8_t ret = DEVICE_OK;
+    for (int can = 0; can < BSP_CAN_NUM; can++) {
+        MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager((BSP_CAN_t)can);
+        if (manager == NULL) continue;
+        
+        for (int i = 0; i < manager->motor_count; i++) {
+            MOTOR_LK_t *motor = manager->motors[i];
+            if (motor != NULL) {
+                if (MOTOR_LK_Update(&motor->param) != DEVICE_OK) {
+                    ret = DEVICE_ERR;
+                }
+            }
+        }
+    }
+    return ret;
+}
+
+int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+
+    MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR_NO_DEV;
+
+    // 限制输出值范围
+    if (value > 1.0f) value = 1.0f;
+    if (value < -1.0f) value = -1.0f;
+
+    MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param);
+    if (motor == NULL) return DEVICE_ERR_NO_DEV;
+
+    // 根据反转参数调整输出
+    float output = param->reverse ? -value : value;
+
+    // 转矩闭环控制命令 - 将输出值转换为转矩控制值
+    int16_t torque_control = (int16_t)(output * (float)LK_TORQUE_RANGE);
+
+    // 构建CAN帧
+    BSP_CAN_StdDataFrame_t tx_frame;
+    tx_frame.id = param->id;
+    tx_frame.dlc = MOTOR_TX_BUF_SIZE;
+
+    tx_frame.data[0] = LK_CMD_TORQUE_CTRL;
+    tx_frame.data[1] = 0x00;
+    tx_frame.data[2] = 0x00;
+    tx_frame.data[3] = 0x00;
+    tx_frame.data[4] = (uint8_t)(torque_control & 0xFF);
+    tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF);
+    tx_frame.data[6] = 0x00;
+    tx_frame.data[7] = 0x00;
+
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param) {
+    // 对于LK电机，每次设置输出时就直接发送控制命令
+    // 这个函数可以用于发送其他控制命令，如电机开启/关闭
+    return DEVICE_OK;
+}
+
+int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    BSP_CAN_StdDataFrame_t tx_frame;
+    tx_frame.id = param->id;
+    tx_frame.dlc = MOTOR_TX_BUF_SIZE;
+    
+    // 电机运行命令
+    tx_frame.data[0] = LK_CMD_MOTOR_ON;     // 命令字节
+    tx_frame.data[1] = 0x00;
+    tx_frame.data[2] = 0x00;
+    tx_frame.data[3] = 0x00;
+    tx_frame.data[4] = 0x00;
+    tx_frame.data[5] = 0x00;
+    tx_frame.data[6] = 0x00;
+    tx_frame.data[7] = 0x00;
+    
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    BSP_CAN_StdDataFrame_t tx_frame;
+    tx_frame.id = param->id;
+    tx_frame.dlc = MOTOR_TX_BUF_SIZE;
+    
+    // 电机关闭命令
+    tx_frame.data[0] = LK_CMD_MOTOR_OFF;    // 命令字节
+    tx_frame.data[1] = 0x00;
+    tx_frame.data[2] = 0x00;
+    tx_frame.data[3] = 0x00;
+    tx_frame.data[4] = 0x00;
+    tx_frame.data[5] = 0x00;
+    tx_frame.data[6] = 0x00;
+    tx_frame.data[7] = 0x00;
+    
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return NULL;
+    
+    MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
+    if (manager == NULL) return NULL;
+    
+    for (int i = 0; i < manager->motor_count; i++) {
+        MOTOR_LK_t *motor = manager->motors[i];
+        if (motor && motor->param.id == param->id) {
+            return motor;
+        }
+    }
+    return NULL;
+}
+
+int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param) {
+    return MOTOR_LK_SetOutput(param, 0.0f);
+}
+
+int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param) {
+    MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param);
+    if (motor) {
+        motor->motor.header.online = false;
+        return DEVICE_OK;
+    }
+    return DEVICE_ERR_NO_DEV;
+}

User/device/motor_lk.h

@@ -0,0 +1,119 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/device.h"
+#include "device/motor.h"
+#include "bsp/can.h"
+
+/* Exported constants ------------------------------------------------------- */
+#define MOTOR_LK_MAX_MOTORS 32
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef enum {
+    MOTOR_LK_MF9025,
+    MOTOR_LK_MF9035,
+} MOTOR_LK_Module_t;
+
+
+/*每个电机需要的参数*/
+typedef struct {
+    BSP_CAN_t can;
+    uint16_t id;
+    MOTOR_LK_Module_t module;
+    bool reverse;
+} MOTOR_LK_Param_t;
+
+/*电机实例*/
+typedef struct{
+    MOTOR_LK_Param_t param;
+    MOTOR_t motor;
+} MOTOR_LK_t;
+
+/*CAN管理器，管理一个CAN总线上所有的电机*/
+typedef struct {
+    BSP_CAN_t can;
+    MOTOR_LK_t *motors[MOTOR_LK_MAX_MOTORS];
+    uint8_t motor_count;
+} MOTOR_LK_CANManager_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 注册一个LK电机
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 更新指定电机数据
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 设置一个电机的输出
+ * @param param 电机参数
+ * @param value 输出值，范围[-1.0, 1.0]
+ * @return 
+ */
+int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value);
+
+/**
+ * @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 发送电机开启命令
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 发送电机关闭命令
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 获取指定电机的实例指针
+ * @param param 电机参数
+ * @return 
+ */
+MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 使电机松弛（设置输出为0）
+ * @param param 
+ * @return 
+ */
+int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 使电机离线（设置在线状态为false）
+ * @param param 
+ * @return 
+ */
+int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 
+ * @param  
+ * @return 
+ */
+int8_t MOTOR_LK_UpdateAll(void);
+
+#ifdef __cplusplus
+}
+#endif

User/device/motor_lz.c

@@ -0,0 +1,478 @@
+/*
+  灵足电机驱动
+  
+  灵足电机通信协议：
+  - CAN 2.0通信接口，波特率1Mbps
+  - 采用扩展帧格式(29位ID)
+  - ID格式：Bit28~24(通信类型) + Bit23~8(数据区2) + Bit7~0(目标地址)
+*/
+/* Includes ----------------------------------------------------------------- */
+#include "motor_lz.h"
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include "bsp/can.h"
+#include "bsp/mm.h"
+#include "bsp/time.h"
+#include "component/user_math.h"
+
+/* Private define ----------------------------------------------------------- */
+// 灵足电机协议参数
+#define LZ_ANGLE_RANGE_RAD          (12.57f)        /* 角度范围 ±12.57 rad */
+#define LZ_VELOCITY_RANGE_RAD_S     (20.0f)         /* 角速度范围 ±20 rad/s */
+#define LZ_TORQUE_RANGE_NM          (60.0f)         /* 力矩范围 ±60 Nm */
+#define LZ_KP_MAX                   (5000.0f)       /* Kp最大值 */
+#define LZ_KD_MAX                   (100.0f)        /* Kd最大值 */
+
+#define LZ_RAW_VALUE_MAX            (65535)         /* 16位原始值最大值 */
+#define LZ_TEMP_SCALE               (10.0f)         /* 温度缩放因子 */
+
+#define LZ_MAX_RECOVER_DIFF_RAD    (0.28f)
+#define MOTOR_TX_BUF_SIZE           (8)
+#define MOTOR_RX_BUF_SIZE           (8)
+
+/* Private macro ------------------------------------------------------------ */
+
+MOTOR_LZ_MotionParam_t lz_recover_param = {
+    .target_angle = 0.0f,
+    .target_velocity = 0.0f,
+    .kp = 30.0f,
+    .kd = 1.0f,
+    .torque = 0.0f,
+};
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+static MOTOR_LZ_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
+
+/* Private function prototypes ---------------------------------------------- */
+static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can);
+static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can);
+static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg);
+static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id);
+static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value);
+static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value);
+static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc);
+static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
+
+/* Private functions -------------------------------------------------------- */
+
+/**
+ * @brief 获取CAN管理器
+ */
+static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return NULL;
+    return can_managers[can];
+}
+
+/**
+ * @brief 创建CAN管理器
+ */
+static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return DEVICE_ERR;
+    if (can_managers[can] != NULL) return DEVICE_OK;
+    
+    can_managers[can] = (MOTOR_LZ_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LZ_CANManager_t));
+    if (can_managers[can] == NULL) return DEVICE_ERR;
+    
+    memset(can_managers[can], 0, sizeof(MOTOR_LZ_CANManager_t));
+    can_managers[can]->can = can;
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 构建扩展ID
+ */
+static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id) {
+    uint32_t ext_id = 0;
+    ext_id |= ((uint32_t)cmd_type & 0x1F) << 24;      // Bit28~24: 通信类型
+    ext_id |= ((uint32_t)data2 & 0xFFFF) << 8;        // Bit23~8: 数据区2
+    ext_id |= ((uint32_t)target_id & 0xFF);           // Bit7~0: 目标地址
+    return ext_id;
+}
+
+/**
+ * @brief 浮点值转换为原始值（对称范围：-max_value ~ +max_value）
+ */
+static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value) {
+    // 限制范围
+    if (value > max_value) value = max_value;
+    if (value < -max_value) value = -max_value;
+    
+    // 转换为0~65535范围，对应-max_value~max_value
+    return (uint16_t)((value + max_value) / (2.0f * max_value) * (float)LZ_RAW_VALUE_MAX);
+}
+
+/**
+ * @brief 浮点值转换为原始值（单向范围：0 ~ +max_value）
+ */
+static uint16_t MOTOR_LZ_FloatToRawPositive(float value, float max_value) {
+    // 限制范围
+    if (value > max_value) value = max_value;
+    if (value < 0.0f) value = 0.0f;
+    
+    // 转换为0~65535范围，对应0~max_value
+    return (uint16_t)(value / max_value * (float)LZ_RAW_VALUE_MAX);
+}
+
+/**
+ * @brief 原始值转换为浮点值
+ */
+static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value) {
+    // 将0~65535范围转换为-max_value~max_value
+    return ((float)raw_value / (float)LZ_RAW_VALUE_MAX) * (2.0f * max_value) - max_value;
+}
+
+/**
+ * @brief 发送扩展帧
+ */
+static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc) {
+    BSP_CAN_ExtDataFrame_t tx_frame;
+    tx_frame.id = ext_id;
+    tx_frame.dlc = dlc;
+    if (data != NULL) {
+        memcpy(tx_frame.data, data, dlc);
+    } else {
+        memset(tx_frame.data, 0, dlc);
+    }
+    
+    return BSP_CAN_TransmitExtDataFrame(can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+/**
+ * @brief 灵足电机ID解析器
+ * @param original_id 原始CAN ID（29位扩展帧）
+ * @param frame_type 帧类型
+ * @return 解析后的ID（用于队列匹配）
+ * 
+ * 灵足电机扩展ID格式：
+ * Bit28~24: 通信类型 (0x1=运控控制, 0x2=反馈数据, 0x3=使能, 0x4=停止, 0x6=设零位)
+ * Bit23~8:  数据区2 (根据通信类型而定)
+ * Bit7~0:   目标地址 (目标电机CAN ID)
+ */
+static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
+    // 只处理扩展数据帧
+    if (frame_type != BSP_CAN_FRAME_EXT_DATA) {
+        return original_id; // 非扩展帧直接返回原始ID
+    }
+    
+    // 解析扩展ID各个字段
+    uint8_t cmd_type = (original_id >> 24) & 0x1F;    // Bit28~24: 通信类型
+    uint16_t data2 = (original_id >> 8) & 0xFFFF;     // Bit23~8: 数据区2
+    uint8_t host_id = (uint8_t)(original_id & 0xFF);  // Bit7~0: 主机CAN ID
+    
+    // 对于反馈数据帧，我们使用特殊的解析规则
+    if (cmd_type == MOTOR_LZ_CMD_FEEDBACK) {
+        // 反馈数据的data2字段包含：
+        // Bit8~15: 当前电机CAN ID
+        // Bit16~21: 故障信息
+        // Bit22~23: 模式状态
+        uint8_t motor_can_id = data2 & 0xFF;          // bit8~15: 当前电机CAN ID
+        
+        // 返回格式化的ID，便于匹配
+        // 格式：0x02HHMMTT (02=反馈命令, HH=主机ID, MM=电机ID, TT=主机ID)
+        return (0x02000000) | (host_id << 16) | (motor_can_id << 8) | host_id;
+    }
+    
+    // 对于其他命令类型，直接返回原始ID
+    return original_id;
+}
+
+/**
+ * @brief 解码灵足电机反馈数据
+ */
+static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) {
+    if (motor == NULL || msg == NULL) return;
+    uint8_t cmd_type = (msg->original_id >> 24) & 0x1F;
+    if (cmd_type != MOTOR_LZ_CMD_FEEDBACK) return;
+    uint16_t id_data2 = (msg->original_id >> 8) & 0xFFFF;
+    uint8_t motor_can_id = id_data2 & 0xFF;
+    uint8_t fault_info = (id_data2 >> 8) & 0x3F;
+    uint8_t mode_state = (id_data2 >> 14) & 0x03;
+    motor->lz_feedback.motor_can_id = motor_can_id;
+    motor->lz_feedback.fault.under_voltage = (fault_info & 0x01) != 0;
+    motor->lz_feedback.fault.driver_fault = (fault_info & 0x02) != 0;
+    motor->lz_feedback.fault.over_temp = (fault_info & 0x04) != 0;
+    motor->lz_feedback.fault.encoder_fault = (fault_info & 0x08) != 0;
+    motor->lz_feedback.fault.stall_overload = (fault_info & 0x10) != 0;
+    motor->lz_feedback.fault.uncalibrated = (fault_info & 0x20) != 0;
+    motor->lz_feedback.state = (MOTOR_LZ_State_t)mode_state;
+
+    // 反馈解码并自动反向
+    uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]);
+    float angle = MOTOR_LZ_RawToFloat(raw_angle, LZ_ANGLE_RANGE_RAD);
+    uint16_t raw_velocity = (uint16_t)((msg->data[2] << 8) | msg->data[3]);
+    float velocity = MOTOR_LZ_RawToFloat(raw_velocity, LZ_VELOCITY_RANGE_RAD_S);
+    uint16_t raw_torque = (uint16_t)((msg->data[4] << 8) | msg->data[5]);
+    float torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM);
+
+    while (angle <0){
+        angle += M_2PI;
+    }
+    while (angle > M_2PI){
+        angle -= M_2PI;
+    }
+    // 自动反向
+    if (motor->param.reverse) {
+        angle = M_2PI - angle;
+        velocity = -velocity;
+        torque = -torque;
+    }
+
+    motor->lz_feedback.current_angle = angle;
+    motor->lz_feedback.current_velocity = velocity;
+    motor->lz_feedback.current_torque = torque;
+
+    uint16_t raw_temp = (uint16_t)((msg->data[6] << 8) | msg->data[7]);
+    motor->lz_feedback.temperature = (float)raw_temp / LZ_TEMP_SCALE;
+
+    motor->motor.feedback.rotor_abs_angle = angle;
+    motor->motor.feedback.rotor_speed = velocity; 
+    motor->motor.feedback.torque_current = torque;
+    motor->motor.feedback.temp = (int8_t)motor->lz_feedback.temperature;
+    motor->motor.header.online = true;
+    motor->motor.header.last_online_time = BSP_TIME_Get();
+}
+
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * @brief 初始化灵足电机驱动系统
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Init(void) {
+    // 注册灵足电机专用的ID解析器
+    return BSP_CAN_RegisterIdParser(MOTOR_LZ_IdParser) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+/**
+ * @brief 反初始化灵足电机驱动系统
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_DeInit(void) {
+    // 注销ID解析器
+    return BSP_CAN_UnregisterIdParser() == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    if (MOTOR_LZ_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
+    MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR;
+    
+    // 检查是否已注册
+    for (int i = 0; i < manager->motor_count; i++) {
+        if (manager->motors[i] && manager->motors[i]->param.motor_id == param->motor_id) {
+            return DEVICE_ERR; // 已注册
+        }
+    }
+    
+    // 检查数量
+    if (manager->motor_count >= MOTOR_LZ_MAX_MOTORS) return DEVICE_ERR;
+    
+    // 创建新电机实例
+    MOTOR_LZ_t *new_motor = (MOTOR_LZ_t*)BSP_Malloc(sizeof(MOTOR_LZ_t));
+    if (new_motor == NULL) return DEVICE_ERR;
+    
+    memcpy(&new_motor->param, param, sizeof(MOTOR_LZ_Param_t));
+    memset(&new_motor->motor, 0, sizeof(MOTOR_t));
+    memset(&new_motor->lz_feedback, 0, sizeof(MOTOR_LZ_Feedback_t));
+    memset(&new_motor->motion_param, 0, sizeof(MOTOR_LZ_MotionParam_t));
+    
+    new_motor->motor.reverse = param->reverse;
+    
+    // 注册CAN接收ID - 使用解析后的反馈数据ID
+    // 构建反馈数据的原始扩展ID
+    // 反馈数据：data2包含电机ID(bit8~15)，target_id是主机ID
+    uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
+    // 通过ID解析器得到解析后的ID
+    uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
+    
+    if (BSP_CAN_RegisterId(param->can, parsed_feedback_id, 3) != BSP_OK) {
+        BSP_Free(new_motor);
+        return DEVICE_ERR;
+    }
+    
+    manager->motors[manager->motor_count] = new_motor;
+    manager->motor_count++;
+    return DEVICE_OK;
+}
+
+int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR_NO_DEV;
+    
+    for (int i = 0; i < manager->motor_count; i++) {
+        MOTOR_LZ_t *motor = manager->motors[i];
+        if (motor && motor->param.motor_id == param->motor_id) {
+            // 获取反馈数据 - 使用解析后的ID
+            uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
+            uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
+            BSP_CAN_Message_t msg;
+            
+            while (BSP_CAN_GetMessage(param->can, parsed_feedback_id, &msg, 0) == BSP_OK) {
+                MOTOR_LZ_Decode(motor, &msg);
+            }
+            return DEVICE_OK;
+        }
+    }
+    return DEVICE_ERR_NO_DEV;
+}
+
+int8_t MOTOR_LZ_UpdateAll(void) {
+    int8_t ret = DEVICE_OK;
+    for (int can = 0; can < BSP_CAN_NUM; can++) {
+        MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager((BSP_CAN_t)can);
+        if (manager == NULL) continue;
+        
+        for (int i = 0; i < manager->motor_count; i++) {
+            MOTOR_LZ_t *motor = manager->motors[i];
+            if (motor) {
+                if (MOTOR_LZ_Update(&motor->param) != DEVICE_OK) {
+                    ret = DEVICE_ERR;
+                }
+            }
+        }
+    }
+    return ret;
+}
+
+int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param) {
+    if (param == NULL || motion_param == NULL) return DEVICE_ERR_NULL;
+    MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
+    if (motor == NULL) return DEVICE_ERR_NO_DEV;
+
+    // 自动反向控制
+    MOTOR_LZ_MotionParam_t send_param = *motion_param;
+    if (param->reverse) {
+        send_param.target_angle = -send_param.target_angle;
+        send_param.target_velocity = -send_param.target_velocity;
+        send_param.torque = -send_param.torque;
+    }
+
+    memcpy(&motor->motion_param, motion_param, sizeof(MOTOR_LZ_MotionParam_t));
+
+    uint16_t raw_torque = MOTOR_LZ_FloatToRaw(send_param.torque, LZ_TORQUE_RANGE_NM);
+    uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_MOTION, raw_torque, param->motor_id);
+    uint8_t data[8];
+    uint16_t raw_angle = MOTOR_LZ_FloatToRaw(send_param.target_angle, LZ_ANGLE_RANGE_RAD);
+    data[0] = (raw_angle >> 8) & 0xFF;
+    data[1] = raw_angle & 0xFF;
+    uint16_t raw_velocity = MOTOR_LZ_FloatToRaw(send_param.target_velocity, LZ_VELOCITY_RANGE_RAD_S);
+    data[2] = (raw_velocity >> 8) & 0xFF;
+    data[3] = raw_velocity & 0xFF;
+    uint16_t raw_kp = MOTOR_LZ_FloatToRawPositive(send_param.kp, LZ_KP_MAX);
+    data[4] = (raw_kp >> 8) & 0xFF;
+    data[5] = raw_kp & 0xFF;
+    uint16_t raw_kd = MOTOR_LZ_FloatToRawPositive(send_param.kd, LZ_KD_MAX);
+    data[6] = (raw_kd >> 8) & 0xFF;
+    data[7] = raw_kd & 0xFF;
+
+    return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
+}
+
+
+int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    // 构建扩展ID - 使能命令
+    uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_ENABLE, param->host_id, param->motor_id);
+    
+    // 数据区清零
+    uint8_t data[8] = {0};
+    
+    return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
+}
+
+int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    // 构建扩展ID - 停止命令
+    uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_DISABLE, param->host_id, param->motor_id);
+    
+    // 数据区
+    uint8_t data[8] = {0};
+    if (clear_fault) {
+        data[0] = 1; // Byte[0]=1时清故障
+    }
+    
+    return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
+}
+
+int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    // 构建扩展ID - 设置零位命令
+    uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_SET_ZERO, param->host_id, param->motor_id);
+    
+    // 数据区 - Byte[0]=1
+    uint8_t data[8] = {1, 0, 0, 0, 0, 0, 0, 0};
+    
+    return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
+}
+
+MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param) {
+    if (param == NULL) return NULL;
+    
+    MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
+    if (manager == NULL) return NULL;
+    
+    for (int i = 0; i < manager->motor_count; i++) {
+        MOTOR_LZ_t *motor = manager->motors[i];
+        if (motor && motor->param.motor_id == param->motor_id) {
+            return motor;
+        }
+    }
+    return NULL;
+}
+
+int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param) {
+    return MOTOR_LZ_Disable(param, false);
+}
+
+int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param) {
+    MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
+    if (motor) {
+        motor->motor.header.online = false;
+        return DEVICE_OK;
+    }
+    return DEVICE_ERR_NO_DEV;
+}
+
+static MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param) {
+    MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
+    if (motor && motor->motor.header.online) {
+        return &motor->lz_feedback;
+    }
+    return NULL;
+}
+
+
+int8_t MOTOR_LZ_RecoverToZero(MOTOR_LZ_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
+    if (motor == NULL) return DEVICE_ERR_NO_DEV;
+    
+    // 获取当前角度
+    MOTOR_LZ_Feedback_t *feedback = MOTOR_LZ_GetFeedback(param);
+    if (feedback == NULL) return DEVICE_ERR_NO_DEV;
+    
+    float current_angle = feedback->current_angle;
+    
+    // 计算目标角度为0时的最短路径
+    float angle_diff = -current_angle; // 目标是0，所以差值就是-current_angle
+    // 限制最大差值，防止过大跳变
+    if (angle_diff > LZ_MAX_RECOVER_DIFF_RAD) angle_diff = LZ_MAX_RECOVER_DIFF_RAD;
+    if (angle_diff < -LZ_MAX_RECOVER_DIFF_RAD) angle_diff = -LZ_MAX_RECOVER_DIFF_RAD;
+    
+    float target_angle = current_angle + angle_diff;
+    
+    // 创建运控参数，设置位置和速度限制
+    MOTOR_LZ_MotionParam_t motion_param = lz_recover_param; // 使用预设的恢复参数
+    motion_param.target_angle = target_angle;
+
+    return MOTOR_LZ_MotionControl(param, &motion_param);
+}

User/device/motor_lz.h

@@ -0,0 +1,195 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/device.h"
+#include "device/motor.h"
+#include "bsp/can.h"
+
+/* Exported constants ------------------------------------------------------- */
+#define MOTOR_LZ_MAX_MOTORS 32
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef enum {
+    MOTOR_LZ_RSO0,
+    MOTOR_LZ_RSO1,
+    MOTOR_LZ_RSO2,
+    MOTOR_LZ_RSO3,
+    MOTOR_LZ_RSO4,
+    MOTOR_LZ_RSO5,
+    MOTOR_LZ_RSO6,
+} MOTOR_LZ_Module_t;
+
+/* 灵足电机控制模式 */
+typedef enum {
+    MOTOR_LZ_MODE_MOTION = 0x1,     /* 运控模式 */
+    MOTOR_LZ_MODE_CURRENT = 0x2,    /* 电流模式 */
+    MOTOR_LZ_MODE_VELOCITY = 0x3,   /* 速度模式 */
+    MOTOR_LZ_MODE_POSITION = 0x4,   /* 位置模式 */
+} MOTOR_LZ_ControlMode_t;
+
+/* 灵足电机通信类型 */
+typedef enum {
+    MOTOR_LZ_CMD_MOTION = 0x1,      /* 运控模式控制 */
+    MOTOR_LZ_CMD_FEEDBACK = 0x2,    /* 电机反馈数据 */
+    MOTOR_LZ_CMD_ENABLE = 0x3,      /* 电机使能运行 */
+    MOTOR_LZ_CMD_DISABLE = 0x4,     /* 电机停止运行 */
+    MOTOR_LZ_CMD_SET_ZERO = 0x6,    /* 设置电机机械零位 */
+} MOTOR_LZ_CmdType_t;
+
+/* 灵足电机运行状态 */
+typedef enum {
+    MOTOR_LZ_STATE_RESET = 0,       /* Reset模式[复位] */
+    MOTOR_LZ_STATE_CALI = 1,        /* Cali模式[标定] */
+    MOTOR_LZ_STATE_MOTOR = 2,       /* Motor模式[运行] */
+} MOTOR_LZ_State_t;
+
+/* 灵足电机故障信息 */
+typedef struct {
+    bool uncalibrated;      /* bit21: 未标定 */
+    bool stall_overload;    /* bit20: 堵转过载故障 */
+    bool encoder_fault;     /* bit19: 磁编码故障 */
+    bool over_temp;         /* bit18: 过温 */
+    bool driver_fault;      /* bit17: 驱动故障 */
+    bool under_voltage;     /* bit16: 欠压故障 */
+} MOTOR_LZ_Fault_t;
+
+/* 灵足电机运控参数 */
+typedef struct {
+    float target_angle;     /* 目标角度 (-12.57f~12.57f rad) */
+    float target_velocity;  /* 目标角速度 (-20~20 rad/s) */
+    float kp;               /* 位置增益 (0.0~5000.0) */
+    float kd;               /* 微分增益 (0.0~100.0) */
+    float torque;           /* 力矩 (-60~60 Nm) */
+} MOTOR_LZ_MotionParam_t;
+
+/*每个电机需要的参数*/
+typedef struct {
+    BSP_CAN_t can;              /* CAN总线 */
+    uint8_t motor_id;           /* 电机CAN ID */
+    uint8_t host_id;            /* 主机CAN ID */
+    MOTOR_LZ_Module_t module;   /* 电机型号 */
+    bool reverse;               /* 是否反向 */
+    MOTOR_LZ_ControlMode_t mode; /* 控制模式 */
+} MOTOR_LZ_Param_t;
+
+/*电机反馈信息扩展*/
+typedef struct {
+    float current_angle;        /* 当前角度 (-12.57f~12.57f rad) */
+    float current_velocity;     /* 当前角速度 (-20~20 rad/s) */
+    float current_torque;       /* 当前力矩 (-60~60 Nm) */
+    float temperature;          /* 当前温度 (摄氏度) */
+    MOTOR_LZ_State_t state;     /* 运行状态 */
+    MOTOR_LZ_Fault_t fault;     /* 故障信息 */
+    uint8_t motor_can_id;       /* 当前电机CAN ID */
+} MOTOR_LZ_Feedback_t;
+
+/*电机实例*/
+typedef struct {
+    MOTOR_LZ_Param_t param;
+    MOTOR_t motor;
+    MOTOR_LZ_Feedback_t lz_feedback;    /* 灵足电机特有反馈信息 */
+    MOTOR_LZ_MotionParam_t motion_param; /* 运控模式参数 */
+} MOTOR_LZ_t;
+
+/*CAN管理器，管理一个CAN总线上所有的电机*/
+typedef struct {
+    BSP_CAN_t can;
+    MOTOR_LZ_t *motors[MOTOR_LZ_MAX_MOTORS];
+    uint8_t motor_count;
+} MOTOR_LZ_CANManager_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 初始化灵足电机驱动系统
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Init(void);
+
+/**
+ * @brief 反初始化灵足电机驱动系统
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_DeInit(void);
+
+/**
+ * @brief 注册一个灵足电机
+ * @param param 电机参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param);
+
+/**
+ * @brief 更新指定电机数据
+ * @param param 电机参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param);
+
+/**
+ * @brief 更新所有电机数据
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_UpdateAll(void);
+
+/**
+ * @brief 运控模式控制电机
+ * @param param 电机参数
+ * @param motion_param 运控参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param);
+
+/**
+ * @brief 电机使能运行
+ * @param param 电机参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param);
+
+/**
+ * @brief 电机停止运行
+ * @param param 电机参数
+ * @param clear_fault 是否清除故障
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault);
+
+/**
+ * @brief 设置电机机械零位
+ * @param param 电机参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param);
+
+/**
+ * @brief 获取指定电机的实例指针
+ * @param param 电机参数
+ * @return 电机实例指针，失败返回NULL
+ */
+MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param);
+
+/**
+ * @brief 使电机松弛（发送停止命令）
+ * @param param 电机参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param);
+
+/**
+ * @brief 使电机离线（设置在线状态为false）
+ * @param param 电机参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param);
+
+int8_t MOTOR_LZ_RecoverToZero(MOTOR_LZ_Param_t *param);
+
+#ifdef __cplusplus
+}
+#endif

User/module/balance_chassis.c

@@ -0,0 +1,339 @@
+#include "module/balance_chassis.h"
+#include "bsp/time.h"
+#include "bsp/can.h"
+#include "component/user_math.h"
+#include "component/kinematics.h"
+#include <math.h>
+#include <string.h>
+
+
+float fn=0.0f;
+float tp=0.0f;
+
+float t1=0.0f;
+float t2=0.0f;
+
+
+
+static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
+  if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
+  if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */
+
+  MOTOR_LK_MotorOn(&c->param->wheel_motors[0]);
+  MOTOR_LK_MotorOn(&c->param->wheel_motors[1]);
+  for (int i = 0; i < 4; i++) {
+    MOTOR_LZ_Enable(&c->param->joint_motors[i]);
+  }
+
+  c->mode = mode;
+  c->state = 0;  // 重置状态，确保每次切换模式时都重新初始化
+
+  return CHASSIS_OK;
+}
+
+/* 更新机体状态估计 */
+static void Chassis_UpdateChassisState(Chassis_t *c) {
+    if (c == NULL) return;
+    
+    // 从轮子编码器估计机体速度 (参考C++代码)
+    float left_wheel_speed_dps = c->feedback.wheel[0].rotor_speed; // dps (度每秒)
+    float right_wheel_speed_dps = c->feedback.wheel[1].rotor_speed; // dps (度每秒)
+    
+    // 将dps转换为rad/s
+    float left_wheel_speed = left_wheel_speed_dps * M_PI / 180.0f; // rad/s
+    float right_wheel_speed = right_wheel_speed_dps * M_PI / 180.0f; // rad/s
+    
+    float wheel_radius = 0.072f;
+    
+    float left_wheel_linear_vel = left_wheel_speed * wheel_radius;
+    float right_wheel_linear_vel = right_wheel_speed * wheel_radius;
+    
+    // 机体x方向速度 (轮子中心速度)
+    c->chassis_state.last_velocity_x = c->chassis_state.velocity_x;
+    c->chassis_state.velocity_x = (left_wheel_linear_vel + right_wheel_linear_vel) / 2.0f;
+    
+    // 积分得到位置
+    c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
+}
+
+/* 执行LQR控制 */
+static int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
+    if (c == NULL || c_cmd == NULL) return -1;
+    
+    // 构建当前状态
+    LQR_State_t current_state;
+    float left_leg_length, left_leg_angle, left_leg_d_length, left_leg_d_angle;
+    float right_leg_length, right_leg_angle, right_leg_d_length, right_leg_d_angle;
+    
+    // 获取等效摆动杆状态
+    VMC_GetVirtualLegState(&c->vmc_[0], &left_leg_length, &left_leg_angle, &left_leg_d_length, &left_leg_d_angle);
+    VMC_GetVirtualLegState(&c->vmc_[1], &right_leg_length, &right_leg_angle, &right_leg_d_length, &right_leg_d_angle);
+    
+    LQR_BuildStateFromSensors(
+        c->chassis_state.position_x,
+        c->chassis_state.velocity_x,
+        c->feedback.imu.euler.yaw,
+        c->feedback.imu.gyro.z,
+        left_leg_angle,
+        left_leg_d_angle,
+        right_leg_angle,
+        right_leg_d_angle,
+        c->feedback.imu.euler.pit,
+        c->feedback.imu.gyro.y,
+        &current_state
+    );
+    
+    // 设置参考状态
+    LQR_State_t reference_state = {0};
+    reference_state.s = 0.0f; // 期望位移设为0（相对平衡位置）
+    reference_state.ds = c_cmd->move_vec.vx; // 期望速度
+    reference_state.phi = 0.0f; // 期望yaw角度
+    reference_state.dphi = c_cmd->move_vec.wz; // 期望yaw角速度
+    // 其他状态保持为0（平衡状态）
+    
+    // 更新LQR控制器状态
+    LQR_UpdateState(&c->lqr, &current_state);
+    LQR_SetReference(&c->lqr, &reference_state);
+    
+    // 计算控制输出
+    if (LQR_ComputeControl(&c->lqr) != 0) {
+        return -1;
+    }
+    
+    // 获取控制输出
+    LQR_Control_t lqr_output;
+    LQR_GetControl(&c->lqr, &lqr_output);
+    
+    // 分配力矩到电机
+    // 轮毂电机 (考虑减速比)
+    // float wheel_gear_ratio = 19.2f;
+    // MOTOR_LK_SetTorque(&c->param->wheel_motors[0], lqr_output.T_wl / wheel_gear_ratio);
+    // MOTOR_LK_SetTorque(&c->param->wheel_motors[1], lqr_output.T_wr / wheel_gear_ratio);
+    c->output.wheel[0] = lqr_output.T_wl/2.5; // 轮子电机输出
+    c->output.wheel[1] = lqr_output.T_wr/2.5;
+    // 通过VMC将虚拟力转换为关节力矩
+    // 左腿
+    float F_virtual_left = lqr_output.T_bl; // 简化映射，实际需要更复杂的转换
+    // float T_virtual_left = 0.0f;
+    float T_virtual_left = lqr_output.T_wl;   // 左腿虚拟摆动力矩
+    VMC_InverseSolve(&c->vmc_[0], F_virtual_left, T_virtual_left);
+    
+    float tau_left_front, tau_left_rear;
+    VMC_GetJointTorques(&c->vmc_[0], &tau_left_front, &tau_left_rear);
+    
+    // 右腿
+    float F_virtual_right = lqr_output.T_br;
+    // float T_virtual_right = 0.0f;
+    float T_virtual_right = lqr_output.T_wr;  // 右腿虚拟摆动力矩
+    VMC_InverseSolve(&c->vmc_[1], F_virtual_right, T_virtual_right);
+    
+    float tau_right_front, tau_right_rear;
+    VMC_GetJointTorques(&c->vmc_[1], &tau_right_front, &tau_right_rear);
+    
+    // 输出到关节电机
+    // MOTOR_LZ_SetTorque(&c->param->joint_motors[0], tau_left_rear);  // 左后
+    // MOTOR_LZ_SetTorque(&c->param->joint_motors[1], tau_left_front); // 左前
+    // MOTOR_LZ_SetTorque(&c->param->joint_motors[2], tau_right_front);// 右前
+    // MOTOR_LZ_SetTorque(&c->param->joint_motors[3], tau_right_rear); // 右后
+    c->output.joint[0].torque = tau_left_rear;
+    c->output.joint[1].torque = tau_left_front;
+    c->output.joint[2].torque = tau_right_front;
+    c->output.joint[3].torque = tau_right_rear;
+    return 0;
+}
+
+
+int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){
+    if (c == NULL || param == NULL || target_freq <= 0.0f) {
+        return -1; // 参数错误
+    }
+    c->param = param;
+    
+    /*初始化can*/
+    BSP_CAN_Init();
+
+    /*初始化和注册所有电机*/
+    MOTOR_LZ_Init();
+
+    for (int i = 0; i < 4; i++) {
+      MOTOR_LZ_Register(&c->param->joint_motors[i]);
+    }
+    for (int i = 0; i < 2; i++) {
+        MOTOR_LK_Register(&c->param->wheel_motors[i]);
+    }
+    
+    /*初始化VMC控制器*/
+    VMC_Init(&c->vmc_[0], &param->vmc_param[0], target_freq);
+    VMC_Init(&c->vmc_[1], &param->vmc_param[1], target_freq);
+    
+    /*初始化LQR控制器*/
+    LQR_Init(&c->lqr, param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque);
+    LQR_SetGainMatrix(&c->lqr, &param->lqr_gains);
+    
+    /*初始化机体状态*/
+    c->chassis_state.position_x = 0.0f;
+    c->chassis_state.velocity_x = 0.0f;
+    c->chassis_state.last_velocity_x = 0.0f;
+
+    return CHASSIS_OK;
+}
+
+int8_t Chassis_UpdateFeedback(Chassis_t *c){
+    if (c == NULL) {
+        return -1; // 参数错误
+    }
+    /*更新电机反馈*/
+    for (int i = 0; i < 4; i++) {
+        MOTOR_LZ_Update(&c->param->joint_motors[i]);
+    }
+    MOTOR_LK_Update(&c->param->wheel_motors[0]);
+    MOTOR_LK_Update(&c->param->wheel_motors[1]);
+    
+    /*将电机反馈数据赋值到标准反馈结构体，并检查是否离线*/
+    // 更新关节电机反馈并检查离线状态
+    for (int i = 0; i < 4; i++) {
+        MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_motors[i]);
+        if (joint_motor != NULL) {
+            c->feedback.joint[i] = joint_motor->motor.feedback;
+            c->feedback.joint[i].rotor_abs_angle = joint_motor->motor.feedback.rotor_abs_angle - M_PI; // 机械零点调整
+        } 
+    }
+    
+    // 更新轮子电机反馈并检查离线状态
+    for (int i = 0; i < 2; i++) {
+        MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_motors[i]);
+        if (wheel_motor != NULL) {
+            c->feedback.wheel[i] = wheel_motor->motor.feedback;
+        }
+    }
+    
+    // 更新机体状态估计
+    Chassis_UpdateChassisState(c);
+    
+    return 0;
+}
+
+int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu){
+    if (c == NULL) {
+        return -1; // 参数错误
+    }
+    c->feedback.imu = imu;
+    // c->feedback.imu.euler.pit = - c->feedback.imu.euler.pit;
+    return 0;
+}
+
+int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
+    if (c == NULL || c_cmd == NULL) {
+        return CHASSIS_ERR_NULL; // 参数错误
+    }
+    c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / 1000000.0f; /* 计算两次调用的时间间隔，单位秒 */
+    c->lask_wakeup = BSP_TIME_Get_us();
+
+    /*设置底盘模式*/
+    if (Chassis_SetMode(c, c_cmd->mode) != CHASSIS_OK) {
+        return CHASSIS_ERR_MODE; // 设置模式失败
+    }
+
+    /*根据底盘模式执行不同的控制逻辑*/
+    switch (c->mode) {
+        case CHASSIS_MODE_RELAX:
+            // 放松模式，电机不输出
+            MOTOR_LZ_Relax(&c->param->joint_motors[0]);
+            MOTOR_LZ_Relax(&c->param->joint_motors[1]);
+            MOTOR_LZ_Relax(&c->param->joint_motors[2]);
+            MOTOR_LZ_Relax(&c->param->joint_motors[3]);
+            MOTOR_LK_Relax(&c->param->wheel_motors[0]);
+            MOTOR_LK_Relax(&c->param->wheel_motors[1]);
+
+            // 更新VMC正解算用于状态估计
+            VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
+                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
+            VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
+                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
+
+            VMC_InverseSolve(&c->vmc_[0], fn, tp);
+
+            VMC_GetJointTorques(&c->vmc_[0], &t1, &t2);
+            
+            // MOTOR_LZ_MotionControl(&c->param->joint_motors[0], &(MOTOR_LZ_MotionParam_t){.torque = t1});
+
+            
+
+            // Chassis_LQRControl(c, c_cmd); // 即使在放松模式下也执行LQR以保持状态更新
+
+            break;
+
+        case CHASSIS_MODE_RECOVER:
+            // 恢复模式，使用简单的关节位置控制回到初始姿态
+            // TODO: 实现恢复逻辑
+            break;
+            
+        case CHASSIS_MODE_WHELL_BALANCE:
+            // 更新VMC正解算用于状态估计
+            // MOTOR_LZ_Relax(&c->param->joint_motors[0]);
+            // MOTOR_LZ_Relax(&c->param->joint_motors[1]);
+            // MOTOR_LZ_Relax(&c->param->joint_motors[2]);
+            // MOTOR_LZ_Relax(&c->param->joint_motors[3]);
+            // MOTOR_LK_Relax(&c->param->wheel_motors[0]);
+            // MOTOR_LK_Relax(&c->param->wheel_motors[1]);
+            for (int i = 0; i < 4; i++) {
+                c->output.joint[i].torque = 0.0f;
+            }
+            for (int i = 0; i < 2; i++) {
+                c->output.wheel[i] = 0.0f;
+            }
+
+            // 更新VMC正解算用于状态估计
+            VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
+                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
+            VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
+                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
+
+            VMC_InverseSolve(&c->vmc_[0], fn, tp);
+            VMC_GetJointTorques(&c->vmc_[0], &t1, &t2);
+            
+            c->output.joint[0].torque = t1;
+            c->output.joint[1].torque = t2;
+
+            // Chassis_LQRControl(c, c_cmd); // 即使在放松模式下也执行LQR以保持状态更新
+
+            Chassis_Output(c); // 统一输出
+            break;
+            
+        case CHASSIS_MODE_WHELL_LEG_BALANCE:
+            // 轮腿平衡模式，使用LQR控制
+            
+            // // 更新VMC正解算
+            // VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
+            //                     c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
+            // VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
+            //                     c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
+            
+            // // 执行LQR控制
+            // if (Chassis_LQRControl(c, c_cmd) != 0) {
+            //     // LQR控制失败，切换到安全模式
+            //     return CHASSIS_ERR;
+            // }
+            break;
+            
+        default:
+            return CHASSIS_ERR_MODE;
+    }
+    
+    return CHASSIS_OK;
+}
+
+void Chassis_Output(Chassis_t *c) {
+    if (c == NULL) return;
+    for (int i = 0; i < 4; i++) {
+        MOTOR_LZ_MotionParam_t param = {0};
+        param.torque = c->output.joint[i].torque;
+        MOTOR_LZ_MotionControl(&c->param->joint_motors[i], &param);
+    }
+    for (int i = 0; i < 2; i++) {
+        MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]);
+    }
+    // 这个函数已经在各个模式中直接调用了电机输出函数
+    // 如果需要统一输出，可以在这里实现
+    // 现在的设计是在控制逻辑中直接输出，所以这里留空
+}

User/module/balance_chassis.h

@@ -0,0 +1,194 @@
+/*
+ * 平衡底盘模组
+ */
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+//             Front
+//         | 1       4 |
+//  (1)Left|           |Right(2)
+//         | 2       3 |
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdint.h>
+#include <stdbool.h>
+#include "component/vmc.h"
+#include "component/lqr.h"
+#include "component/ahrs.h"
+#include "component/filter.h"
+#include "component/pid.h"
+#include "device/motor.h"
+#include "device/motor_lk.h"
+#include "device/motor_lz.h"
+/* Exported constants ------------------------------------------------------- */
+#define CHASSIS_OK (0)        /* 运行正常 */
+#define CHASSIS_ERR (-1)      /* 运行时发现了其他错误 */
+#define CHASSIS_ERR_NULL (-2) /* 运行时发现NULL指针 */
+#define CHASSIS_ERR_MODE (-3) /* 运行时配置了错误的CMD_ChassisMode_t */
+#define CHASSIS_ERR_TYPE (-4) /* 运行时配置了错误的Chassis_Type_t */
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+typedef enum {
+  CHASSIS_MODE_RELAX,            /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
+  CHASSIS_MODE_RECOVER,
+  CHASSIS_MODE_WHELL_BALANCE,          /* 平衡模式，底盘自我平衡 */
+  CHASSIS_MODE_WHELL_LEG_BALANCE,      /* 轮子+腿平衡模式，底盘自我平衡 */
+} Chassis_Mode_t;
+
+typedef struct {
+  Chassis_Mode_t mode;           /* 底盘模式 */
+  MoveVector_t move_vec;         /* 底盘运动向量 */
+  float height;               /* 目标高度 */
+} Chassis_CMD_t;
+
+typedef struct {
+  AHRS_Accl_t accl;   /* IMU加速度计 */
+  AHRS_Gyro_t gyro;   /* IMU陀螺仪 */
+  AHRS_Eulr_t euler;  /* IMU欧拉角 */
+}Chassis_IMU_t;
+
+typedef struct {
+  MOTOR_Feedback_t joint[4]; /* 四个关节电机反馈 */
+  MOTOR_Feedback_t wheel[2]; /* 两个轮子电机反馈 */
+  Chassis_IMU_t imu; /* IMU数据 */
+  MOTOR_Feedback_t yaw; /* 云台Yaw轴电机反馈 */
+}Chassis_Feedback_t;
+
+typedef struct {
+  MOTOR_LZ_MotionParam_t joint[4]; /* 四个关节电机输出 */
+  float wheel[2]; /* 两个轮子电机输出 */
+}Chassis_Output_t;
+
+
+/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
+typedef struct {
+
+  VMC_Param_t vmc_param[2];        /* VMC参数 */
+  LQR_GainMatrix_t lqr_gains;      /* LQR增益矩阵 */
+
+  MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */
+  MOTOR_LK_Param_t wheel_motors[2]; /* 两个轮子电机参数 */
+
+  float mech_zero_yaw; /* 机械零点 */
+  
+  /* LQR控制器参数 */
+  struct {
+    float max_wheel_torque; /* 轮毂电机最大力矩限制 */
+    float max_joint_torque; /* 关节电机最大力矩限制 */
+  } lqr_param;
+
+  /* 低通滤波器截止频率 */
+  struct {
+    float in;  /* 输入 */
+    float out; /* 输出 */
+  } low_pass_cutoff_freq;
+} Chassis_Params_t;
+
+/*
+ * 运行的主结构体，所有这个文件里的函数都在操作这个结构体
+ * 包含了初始化参数，中间变量，输出变量
+ */
+typedef struct {
+  uint64_t lask_wakeup;
+  float dt;
+
+  Chassis_Params_t *param; /* 底盘的参数，用Chassis_Init设定 */
+  AHRS_Eulr_t *mech_zero;
+
+  /* 模块通用 */
+  Chassis_Mode_t mode; /* 底盘模式 */
+
+  /* 反馈信息 */
+  Chassis_Feedback_t feedback;
+  /* 控制信息*/
+  Chassis_Output_t output;
+
+  VMC_t vmc_[2]; /* 两条腿的VMC */
+  LQR_Controller_t lqr; /* LQR控制器 */
+
+  int8_t state;
+
+  float angle;
+  float height;
+  
+  /* 机体状态估计 */
+  struct {
+    float position_x;     /* 机体x位置 */
+    float velocity_x;     /* 机体x速度 */
+    float last_velocity_x; /* 上一次速度用于数值微分 */
+  } chassis_state;
+
+  float wz_multi; /* 小陀螺模式旋转方向 */
+
+  /* PID计算的目标值 */
+  struct {
+    AHRS_Eulr_t chassis;
+  } setpoint;
+
+  /* 反馈控制用的PID */
+  struct {
+    KPID_t left_wheel;  /* 左轮PID */
+    KPID_t right_wheel; /* 右轮PID */
+    KPID_t follow; /* 跟随云台用的PID */
+    KPID_t balance; /* 平衡用的PID */
+  } pid;
+
+  /* 滤波器 */
+  struct {
+    LowPassFilter2p_t *in;  /* 反馈值滤波器 */
+    LowPassFilter2p_t *out; /* 输出值滤波器 */
+  } filter;
+
+} Chassis_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * \brief 初始化底盘
+ *
+ * \param c 包含底盘数据的结构体
+ * \param param 包含底盘参数的结构体指针
+ * \param target_freq 任务预期的运行频率
+ *
+ * \return 函数运行结果
+ */
+int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq);
+
+/**
+ * \brief 更新底盘的反馈信息
+ *
+ * \param c 包含底盘数据的结构体
+ * \return 函数运行结果
+ */
+int8_t Chassis_UpdateFeedback(Chassis_t *c);
+
+
+int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu);
+/**
+ * \brief 运行底盘控制逻辑
+ *
+ * \param c 包含底盘数据的结构体
+ * \param c_cmd 底盘控制指令
+ *
+ * \return 函数运行结果
+ */
+int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd);
+
+
+/**
+ * \brief 底盘输出值
+ *
+ * \param s 包含底盘数据的结构体
+ * \param out CAN设备底盘输出结构体
+ */
+void Chassis_Output(Chassis_t *c);
+
+#ifdef __cplusplus
+}
+#endif

User/module/chassis.h

@@ -0,0 +1,180 @@
+/*
+ * 底盘模组
+ */
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "component\cmd.h"
+#include "component\filter.h"
+#include "component\mixer.h"
+#include "component\pid.h"
+#include "device\can.h"
+#include "device\referee.h"
+
+/* Exported constants ------------------------------------------------------- */
+#define CHASSIS_OK (0)        /* 运行正常 */
+#define CHASSIS_ERR (-1)      /* 运行时发现了其他错误 */
+#define CHASSIS_ERR_NULL (-2) /* 运行时发现NULL指针 */
+#define CHASSIS_ERR_MODE (-3) /* 运行时配置了错误的CMD_ChassisMode_t */
+#define CHASSIS_ERR_TYPE (-4) /* 运行时配置了错误的Chassis_Type_t */
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+/* 底盘类型（底盘的物理设计） */
+typedef enum {
+  CHASSIS_TYPE_MECANUM,    /* 麦克纳姆轮 */
+  CHASSIS_TYPE_PARLFIX4,   /* 平行摆设的四个驱动轮 */
+  CHASSIS_TYPE_PARLFIX2,   /* 平行摆设的两个驱动轮 */
+  CHASSIS_TYPE_OMNI_CROSS, /* 叉型摆设的四个全向轮 */
+  CHASSIS_TYPE_OMNI_PLUS,  /* 十字型摆设的四个全向轮 */
+  CHASSIS_TYPE_DRONE,      /* 底盘为无人机 */
+  CHASSIS_TYPE_SINGLE,     /* 单个摩擦轮 */
+} Chassis_Type_t;
+
+/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
+typedef struct {
+  Chassis_Type_t type; /* 底盘类型，底盘的机械设计和轮子选型 */
+  KPID_Params_t motor_pid_param;  /* 轮子控制PID的参数 */
+  KPID_Params_t follow_pid_param; /* 跟随云台PID的参数 */
+
+  /* 低通滤波器截止频率 */
+  struct {
+    float in;  /* 输入 */
+    float out; /* 输出 */
+  } low_pass_cutoff_freq;
+
+  /* 电机反装 应该和云台设置相同*/
+  struct {
+    bool yaw;
+  } reverse;
+
+} Chassis_Params_t;
+
+/*
+ * 运行的主结构体，所有这个文件里的函数都在操作这个结构体
+ * 包含了初始化参数，中间变量，输出变量
+ */
+typedef struct {
+  uint32_t lask_wakeup;
+  float dt;
+
+  const Chassis_Params_t *param; /* 底盘的参数，用Chassis_Init设定 */
+  AHRS_Eulr_t *mech_zero;
+
+  /* 模块通用 */
+  CMD_ChassisMode_t mode; /* 底盘模式 */
+
+  /* 底盘设计 */
+  int8_t num_wheel; /* 底盘轮子数量 */
+  Mixer_t mixer;    /* 混合器，移动向量->电机目标值 */
+
+  MoveVector_t move_vec; /* 底盘实际的运动向量 */
+
+  /* 反馈信息 */
+  struct {
+    float gimbal_yaw_encoder; /* 云台Yaw轴编码器角度 */
+    float *motor_rpm;         /* 电机转速的动态数组，单位：RPM */
+    float *motor_current;     /* 转矩电流 单位：A */
+  } feedback;
+
+  float wz_multi; /* 小陀螺模式旋转方向 */
+
+  /* PID计算的目标值 */
+  struct {
+    float *motor_rpm; /* 电机转速的动态数组，单位：RPM */
+  } setpoint;
+
+  /* 反馈控制用的PID */
+  struct {
+    KPID_t *motor; /* 控制轮子电机用的PID的动态数组 */
+    KPID_t follow; /* 跟随云台用的PID */
+  } pid;
+
+  /* 滤波器 */
+  struct {
+    LowPassFilter2p_t *in;  /* 反馈值滤波器 */
+    LowPassFilter2p_t *out; /* 输出值滤波器 */
+  } filter;
+
+  float *out; /* 电机最终的输出值的动态数组 */
+} Chassis_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * \brief 初始化底盘
+ *
+ * \param c 包含底盘数据的结构体
+ * \param param 包含底盘参数的结构体指针
+ * \param target_freq 任务预期的运行频率
+ *
+ * \return 函数运行结果
+ */
+int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
+                    AHRS_Eulr_t *mech_zero, float target_freq);
+
+/**
+ * \brief 更新底盘的反馈信息
+ *
+ * \param c 包含底盘数据的结构体
+ * \param can CAN设备结构体
+ *
+ * \return 函数运行结果
+ */
+int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can);
+
+/**
+ * \brief 运行底盘控制逻辑
+ *
+ * \param c 包含底盘数据的结构体
+ * \param c_cmd 底盘控制指令
+ * \param dt_sec 两次调用的时间间隔
+ *
+ * \return 函数运行结果
+ */
+int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd,
+                       uint32_t now);
+
+/**
+ * @brief 底盘功率限制
+ *
+ * @param c 底盘数据
+ * @param cap 电容数据
+ * @param ref 裁判系统数据
+ * @return 函数运行结果
+ */
+int8_t Chassis_PowerLimit(Chassis_t *c, const CAN_Capacitor_t *cap,
+                          const Referee_ForChassis_t *ref);
+
+/**
+ * \brief 复制底盘输出值
+ *
+ * \param s 包含底盘数据的结构体
+ * \param out CAN设备底盘输出结构体
+ */
+void Chassis_DumpOutput(Chassis_t *c, CAN_ChassisOutput_t *out);
+
+/**
+ * \brief 清空Chassis输出数据
+ *
+ * \param out CAN设备底盘输出结构体
+ */
+void Chassis_ResetOutput(CAN_ChassisOutput_t *out);
+
+/**
+ * @brief 导出底盘数据
+ *
+ * @param chassis 底盘数据结构体
+ * @param ui UI数据结构体
+ */
+void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui);
+
+#ifdef __cplusplus
+}
+#endif

User/module/config.c

@@ -0,0 +1,162 @@
+/*
+ * 配置相关
+ */
+
+/* Includes ----------------------------------------------------------------- */
+#include "module/config.h"
+#include "bsp/can.h"
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+
+/* Exported variables ------------------------------------------------------- */
+
+// 机器人参数配置
+Config_RobotParam_t robot_config = {
+
+    .imu_param = {
+        .can = BSP_CAN_2,
+        .can_id = 0x6FF,      
+        .device_id = 0x42,   
+        .master_id = 0x43,   
+    },
+    .joint_motors[0] = { // 左膝关节
+        .can = BSP_CAN_1,
+        .motor_id = 124,
+        .host_id = 130,
+        .module = MOTOR_LZ_RSO3,
+        .reverse = false,
+        .mode = MOTOR_LZ_MODE_MOTION,
+    },
+    .joint_motors[1] = { // 左髋关节
+        .can = BSP_CAN_1,
+        .motor_id = 125,
+        .host_id = 130,
+        .module = MOTOR_LZ_RSO3,
+        .reverse = false,
+        .mode = MOTOR_LZ_MODE_MOTION,
+    },
+    .joint_motors[2] = { // 右髋关节
+        .can = BSP_CAN_1,
+        .motor_id = 126,
+        .host_id = 130,
+        .module = MOTOR_LZ_RSO3,
+        .reverse = false,
+        .mode = MOTOR_LZ_MODE_MOTION,
+    },
+    .joint_motors[3] = { // 右膝关节
+        .can = BSP_CAN_1,
+        .motor_id = 127,
+        .host_id = 130,
+        .module = MOTOR_LZ_RSO3,
+        .reverse = false,
+        .mode = MOTOR_LZ_MODE_MOTION,
+    },
+    .wheel_motors[0] = { // 左轮电机
+        .can = BSP_CAN_1,
+        .id = 1,  // 电机ID为1，对应命令ID=0x141，反馈ID=0x181
+        .module = MOTOR_LK_MF9025,
+        .reverse = false,
+    },
+    .wheel_motors[1] = { // 右轮电机
+        .can = BSP_CAN_1,
+        .id = 2,  // 电机ID为2，对应命令ID=0x142，反馈ID=0x182
+        .module = MOTOR_LK_MF9025,
+        .reverse = true,
+    },
+    
+    .chassis_param = {
+        .low_pass_cutoff_freq = {
+            .in = 30.0f,
+            .out = 30.0f,
+        },
+        .joint_motors = {
+            { // 左髋关节
+                .can = BSP_CAN_1,
+                .motor_id = 124,
+                .host_id = 130,
+                .module = MOTOR_LZ_RSO3,
+                .reverse = true,
+                .mode = MOTOR_LZ_MODE_MOTION,
+            },
+            { // 左膝关节
+                .can = BSP_CAN_1,
+                .motor_id = 125,
+                .host_id = 130,
+                .module = MOTOR_LZ_RSO3,
+                .reverse = true,
+                .mode = MOTOR_LZ_MODE_MOTION,
+            },
+            { // 右膝关节
+                .can = BSP_CAN_1,
+                .motor_id = 126,
+                .host_id = 130,
+                .module = MOTOR_LZ_RSO3,
+                .reverse = false,
+                .mode = MOTOR_LZ_MODE_MOTION,
+            },
+            { // 右髋关节
+                .can = BSP_CAN_1,
+                .motor_id = 127,
+                .host_id = 130,
+                .module = MOTOR_LZ_RSO3,
+                .reverse = false,
+                .mode = MOTOR_LZ_MODE_MOTION,
+            },
+        },
+        .wheel_motors = {
+            { // 左轮电机
+                .can = BSP_CAN_1,
+                .id = 0x141,
+                .module = MOTOR_LK_MF9025,
+                .reverse = false,
+            },
+            { // 右轮电机
+                .can = BSP_CAN_1,
+                .id = 0x142,
+                .module = MOTOR_LK_MF9025,
+                .reverse = true,
+            },
+        },
+        .mech_zero_yaw = 0.0f,
+        .vmc_param = {
+            { // 左腿
+                .leg_1 = 0.206f,      // 前大腿长度 (m)
+                .leg_2 = 0.258f,      // 前小腿长度 (m)
+                .leg_3 = 0.206f,      // 后小腿长度 (m)
+                .leg_4 = 0.258f,      // 后大腿长度 (m)
+                .hip_length = 0.0f    // 髋宽 (m)
+            },
+            { // 右腿
+                .leg_1 = 0.206f,      // 前大腿长度 (m)
+                .leg_2 = 0.258f,      // 前小腿长度 (m)
+                .leg_3 = 0.206f,      // 后小腿长度 (m)
+                .leg_4 = 0.258f,      // 后大腿长度 (m)
+                .hip_length = 0.0f    // 髋宽 (m)
+            }
+        },
+        .lqr_gains ={
+            .K = {
+                { -1.3677,  -12.022,  4.0676,  -2.6185,  -66.132,  -4.2516,  -1.4083,  -0.051404,  -57.561,  -5.3641 },
+                { -1.3677,  -12.022,  -4.0676,  2.6185,  -1.4083,  -0.051404,  -66.132,  -4.2516,  -57.561,  -5.3641 },
+                { 0.14689,  1.2865,  -63.224,  -12.495,  6.2265,  -0.13959,  1.2635,  0.48938,  -78.822,  -5.121 },
+                { 0.14689,  1.2865,  63.224,  12.495,  1.2635,  0.48938,  6.2265,  -0.13959,  -78.822,  -5.121 }
+            }
+        },
+        .lqr_param.max_joint_torque = 20.0f,  // 关节电机最大力矩 20Nm
+        .lqr_param.max_wheel_torque = 2.5f,  // 轮毂电机最大力矩 2.5Nm
+    }
+};
+
+/* Private function prototypes ---------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * @brief 获取机器人配置参数
+ * @return 机器人配置参数指针
+ */
+Config_RobotParam_t* Config_GetRobotParam(void) {
+    return &robot_config;
+}

User/module/config.h

@@ -0,0 +1,35 @@
+/*
+ * 配置相关
+ */
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include "device/dm_imu.h"
+#include "device/motor_lz.h"
+#include "device/motor_lk.h"
+#include "module/balance_chassis.h"
+
+
+typedef struct {
+    DM_IMU_Param_t imu_param;
+    MOTOR_LZ_Param_t joint_motors[4];
+    MOTOR_LK_Param_t wheel_motors[2];
+    Chassis_Params_t chassis_param;
+} Config_RobotParam_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 获取机器人配置参数
+ * @return 机器人配置参数指针
+ */
+Config_RobotParam_t* Config_GetRobotParam(void);
+
+#ifdef __cplusplus
+}
+#endif

User/module/mod_wheelleg_chassis.cpp

@@ -0,0 +1,662 @@
+#include "mod_wheelleg_chassis.hpp"
+
+#include <cmath>
+#include <tuple>
+
+using namespace Module;
+Wheelleg::Wheelleg(Param& param)
+    : param_(param),
+      roll_pid_(param.roll_pid_param, 333.0f),
+      yaw_pid_(param.yaw_pid_param, 333.0f),
+      yaccl_pid_(param.yaccl_pid_param, 333.0f),
+      lowfilter_(333.0f, 50.0f),
+      acclfilter_(333.0f, 30.0f),
+      manfilter_(param.manfilter_param),
+
+      ctrl_lock_(true) {
+  memset(&(this->cmd_), 0, sizeof(this->cmd_));
+
+  this->hip_motor_.at(0) =
+      new Device::MitMotor(param.hip_motor_param.at(0), "hip_left_front");
+  this->hip_motor_.at(1) =
+      new Device::MitMotor(param.hip_motor_param.at(1), "hip_left_back");
+  this->hip_motor_.at(2) =
+      new Device::MitMotor(param.hip_motor_param.at(2), "hip_right_front");
+  this->hip_motor_.at(3) =
+      new Device::MitMotor(param.hip_motor_param.at(3), "hip_left_back");
+
+  this->wheel_motor_.at(0) =
+      new Device::RMMotor(param.wheel_motor_param.at(0), "wheel_left");
+  this->wheel_motor_.at(1) =
+      new Device::RMMotor(param.wheel_motor_param.at(1), "wheel_right");
+
+  this->leglength_pid_.at(0) =
+      new Component::PID(param.leglength_pid_param.at(0), 333.0f);
+  this->leglength_pid_.at(1) =
+      new Component::PID(param.leglength_pid_param.at(1), 333.0f);
+
+  this->theta_pid_.at(0) =
+      new Component::PID(param.theta_pid_param.at(0), 333.0f);
+  this->theta_pid_.at(1) =
+      new Component::PID(param.theta_pid_param.at(1), 333.0f);
+
+  this->tp_pid_.at(0) = new Component::PID(param.Tp_pid_param.at(0), 333.0);
+  this->tp_pid_.at(1) = new Component::PID(param.Tp_pid_param.at(1), 333.0);
+
+  this->leg_.at(0) = new Component::VMC(param_.leg_param.at(0), 333.0f);
+  this->leg_.at(1) = new Component::VMC(param_.leg_param.at(1), 333.0f);
+
+  auto event_callback = [](Wheelleg::ChassisEvent event, Wheelleg* chassis) {
+    chassis->ctrl_lock_.Wait(UINT32_MAX);
+    switch (event) {
+      case SET_MODE_RELAX:
+        chassis->SetMode(RELAX);
+        break;
+      case SET_MODE_STAND:
+        chassis->SetMode(STAND);
+        break;
+      case SET_MODE_ROTOR:
+        chassis->SetMode(ROTOR);
+        break;
+      case SET_MODE_RESET:
+        chassis->SetMode(RESET);
+        break;
+      default:
+        break;
+    }
+    chassis->ctrl_lock_.Post();
+  };
+
+  Component::CMD::RegisterEvent<Wheelleg*, ChassisEvent>(
+      event_callback, this, this->param_.EVENT_MAP);
+
+  auto chassis_thread = [](Wheelleg* chassis) {
+    auto cmd_sub =
+        Message::Subscriber<Component::CMD::ChassisCMD>("cmd_chassis");
+    auto eulr_sub =
+        Message::Subscriber<Component::Type::Eulr>("chassis_imu_eulr");
+    auto gyro_sub =
+        Message::Subscriber<Component::Type::Vector3>("chassis_gyro");
+
+    auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
+
+    auto accl_sub =
+        Message::Subscriber<Component::Type::Vector3>("chassis_imu_accl_abs");
+    // auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
+    uint32_t last_online_time = bsp_time_get_ms();
+
+    while (1) {
+      eulr_sub.DumpData(chassis->eulr_); /* imu */
+      gyro_sub.DumpData(chassis->gyro_); /* imu */
+      accl_sub.DumpData(chassis->accl_);
+
+      yaw_sub.DumpData(chassis->yaw_);
+      cmd_sub.DumpData(chassis->cmd_);
+      // yaw_sub.DumpData(chassis->yaw_);
+
+      chassis->ctrl_lock_.Wait(UINT32_MAX);
+      chassis->MotorSetAble();
+      chassis->UpdateFeedback();
+      chassis->Calculate();
+      chassis->Control();
+      chassis->ctrl_lock_.Post();
+
+      /* 运行结束，等待下一次唤醒 */
+      chassis->thread_.SleepUntil(3, last_online_time);
+    }
+  };
+  this->thread_.Create(chassis_thread, this, "chassis_thread", 1536,
+                       System::Thread::MEDIUM);
+}
+
+void Wheelleg::MotorSetAble() {
+  if (this->hip_motor_flag_ == 0) {
+    this->hip_motor_[0]->Relax();
+    this->hip_motor_[1]->Relax();
+    this->hip_motor_[2]->Relax();
+    this->hip_motor_[3]->Relax();
+    this->dm_motor_flag_ = 0;
+  }
+
+  else {
+    if (this->dm_motor_flag_ == 0) {
+      for (int i = 0; i < 5; i++) {
+        this->hip_motor_[0]->Enable();
+      }
+      for (int i = 0; i < 5; i++) {
+        this->hip_motor_[1]->Enable();
+      }
+      for (int i = 0; i < 5; i++) {
+        this->hip_motor_[2]->Enable();
+      }
+      for (int i = 0; i < 5; i++) {
+        this->hip_motor_[3]->Enable();
+      }
+
+      this->dm_motor_flag_ = 1;
+    }
+  };
+}
+
+void Wheelleg::UpdateFeedback() {
+  this->now_ = bsp_time_get();
+  this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
+  this->last_wakeup_ = this->now_;
+
+  this->wheel_motor_[0]->Update();
+  this->wheel_motor_[1]->Update();
+
+  this->leg_argu_[0].phi4_ =
+      this->hip_motor_[0]->GetAngle() -
+      this->param_.wheel_param.mech_zero[0];  // 前关节角度
+  this->leg_argu_[0].phi1_ =
+      this->hip_motor_[1]->GetAngle() -
+      this->param_.wheel_param.mech_zero[1];  // 后关节角度
+
+  this->leg_argu_[1].phi4_ =
+      (-this->hip_motor_[2]->GetAngle() +
+       this->param_.wheel_param.mech_zero[3]);  // 前关节角度
+  if (leg_argu_[1].phi4_ > M_PI) {
+    this->leg_argu_[1].phi4_ = this->leg_argu_[1].phi4_ - 2 * M_PI;
+  }
+
+  this->leg_argu_[1].phi1_ =
+      (-this->hip_motor_[3]->GetAngle() +
+       this->param_.wheel_param.mech_zero[2]);  // 后关节角度
+  if (leg_argu_[1].phi1_ > M_PI) {
+    this->leg_argu_[1].phi1_ = this->leg_argu_[1].phi1_ - 2 * M_PI;
+  }
+
+  this->pit_ = this->eulr_.pit;
+  if (this->pit_ > M_PI) {
+    this->pit_ = this->eulr_.pit - 2 * M_PI;
+  }
+
+  /*  注意极性 */
+  std::tuple<float, float, float, float> result0 =
+      this->leg_[0]->VMCsolve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
+                              this->pit_, -this->gyro_.x, this->dt_);
+  this->leg_argu_[0].L0 = std::get<0>(result0);
+  this->leg_argu_[0].d_L0 = std::get<1>(result0);
+  this->leg_argu_[0].theta = -std::get<2>(result0);
+  this->leg_argu_[0].d_theta = -std::get<3>(result0);
+
+  if (leg_argu_[0].theta > M_PI) {
+    leg_argu_[0].theta = leg_argu_[0].theta - 2 * M_PI;
+  }
+  if (leg_argu_[0].theta < -M_PI) {
+    leg_argu_[0].theta = leg_argu_[0].theta + 2 * M_PI;
+  }
+
+  std::tuple<float, float, float, float> result1 =
+      this->leg_[1]->VMCsolve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
+                              this->pit_, -this->gyro_.x, this->dt_);
+  this->leg_argu_[1].L0 = std::get<0>(result1);
+  this->leg_argu_[1].d_L0 = std::get<1>(result1);
+  this->leg_argu_[1].theta = -std::get<2>(result1);
+  this->leg_argu_[1].d_theta = -std::get<3>(result1);
+
+  if (leg_argu_[1].theta > M_PI) {
+    leg_argu_[1].theta = leg_argu_[1].theta - 2 * M_PI;
+  }
+  if (leg_argu_[1].theta < -M_PI) {
+    leg_argu_[1].theta = leg_argu_[1].theta + 2 * M_PI;
+  }
+
+  /* 轮子转速近似于编码器速度 由此推测机体速度 */
+  this->leg_argu_[0].single_x_dot =
+      -wheel_motor_[0]->GetSpeed() * 2 * M_PI *
+          (this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
+      leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
+      leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta);
+
+  this->leg_argu_[1].single_x_dot =
+      wheel_motor_[1]->GetSpeed() * 2 * M_PI *
+          (this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
+      leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
+      leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta);
+
+  this->move_argu_.last_x_dot = this->move_argu_.x_dot;
+  this->move_argu_.x_dot =
+
+      (this->leg_argu_[0].single_x_dot + this->leg_argu_[1].single_x_dot) / 2;
+  this->move_argu_.x_dot =
+      (-wheel_motor_[0]->GetSpeed() + wheel_motor_[1]->GetSpeed()) * M_PI *
+      this->param_.wheel_param.wheel_radius / 60.f / 15.765;
+  this->move_argu_.x = this->move_argu_.x_dot * dt_ + move_argu_.x;
+
+  this->move_argu_.delta_speed =
+      lowfilter_.Apply((move_argu_.x_dot - move_argu_.last_x_dot) / dt_);
+
+  this->move_argu_.accl = acclfilter_.Apply(this->accl_.y * 9.8f);
+
+  if (now_ > 1000000) {
+    this->move_argu_.x_dot_hat =
+        manfilter_.comp_filter(move_argu_.x_dot, move_argu_.delta_speed,
+                               this->move_argu_.last_x_dot, accl_.y * 9.8f,
+                               dt_) -
+        ((leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
+          leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta)) +
+         (leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
+          leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta))) /
+            2;
+    this->move_argu_.xhat = dt_ * move_argu_.x_dot_hat + move_argu_.xhat;
+  }
+}
+
+void Wheelleg::Calculate() {
+  switch (this->mode_) {
+    case Wheelleg::RELAX:
+      // if (fabs(move_argu_.target_dot_x - cmd_.y * 1.5f) > 0.005) {
+      //   if (move_argu_.target_dot_x > cmd_.y * 1.5f) {
+      //     move_argu_.target_dot_x -= 0.004;
+      //   }
+      //   if (move_argu_.target_dot_x < cmd_.y * 1.5f) {
+      //     move_argu_.target_dot_x += 0.004;
+      //   }
+      // } else {
+      //   move_argu_.target_dot_x = cmd_.y * 1.5f;
+      // }
+      // move_argu_.target_x = move_argu_.target_dot_x * dt_ +
+      // move_argu_.target_x;
+      move_argu_.target_x = 0;
+      move_argu_.target_dot_x = 0;
+      break;
+    case Wheelleg::STAND:
+
+      /* 0.002为最大加速度 即500hz*0.002 梯度式递增减 */
+      if (fabs(move_argu_.target_dot_x -
+               cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) {
+        if (move_argu_.target_dot_x >
+            cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
+          move_argu_.target_dot_x -= 0.004;
+        }
+        if (move_argu_.target_dot_x <
+            cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
+          move_argu_.target_dot_x += 0.004;
+        }
+      } else {
+        move_argu_.target_dot_x =
+            cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
+      }
+      move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
+
+      this->move_argu_.target_yaw = 0.0f;
+
+      /*双零点*/
+      if (this->yaw_ > M_PI_2) {
+        this->move_argu_.target_yaw = 3.14158f;
+      }
+      if (this->yaw_ < -M_PI_2) {
+        this->move_argu_.target_yaw = 3.14158f;
+      }
+      break;
+
+    case Wheelleg::ROTOR:
+      if (fabs(move_argu_.target_dot_x -
+               cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) {
+        if (move_argu_.target_dot_x >
+            cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
+          move_argu_.target_dot_x -= 0.004;
+        }
+        if (move_argu_.target_dot_x <
+            cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
+          move_argu_.target_dot_x += 0.004;
+        }
+      } else {
+        move_argu_.target_dot_x =
+            cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
+      }
+      move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
+      this->move_argu_.target_yaw = this->yaw_ + 0.01;
+
+      break;
+      // move_argu_.target_dot_x = cmd_.x;
+      // move_argu_.target_x =
+      //     move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
+      // // move_argu_.target_dot_x = sqrtf(cmd_.x * cmd_.x + cmd_.y *
+      // cmd_.y);
+      // // move_argu_.x_dot =
+      // //     move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
+      // // move_argu_.target_yaw = atan2(cmd_.x, cmd_.y);
+      // break;
+
+    case Wheelleg::RESET:
+      this->move_argu_.target_dot_x = 0;
+      move_argu_.target_x = 0;
+      this->move_argu_.target_yaw = this->eulr_.yaw;
+      this->move_argu_.xhat = 0;
+
+      //     move_argu_.target_yaw - atan2(cmd_.x, cmd_.y);
+      // if ((fabs(move_argu_.target_yaw) - fabs(eulr_.yaw)) > M_PI / 2) {
+      //   this->move_argu_.target_dot_x = -this->move_argu_.target_dot_x;
+      // }
+      break;
+
+    default:
+      XB_ASSERT(false);
+      return;
+  }
+
+  leg_argu_[0].Fn = leg_[0]->GndDetector(leg_argu_[0].Tp, leg_argu_[0].F0);
+  onground0_flag_ = false;
+  if (leg_argu_[0].Fn > 30) {
+    onground0_flag_ = true;
+  }
+  leg_argu_[1].Fn = leg_[1]->GndDetector(leg_argu_[1].Tp, leg_argu_[1].F0);
+  onground1_flag_ = false;
+  if (leg_argu_[1].Fn > 30) {
+    onground1_flag_ = true;
+  }
+  std::tuple<float, float> result3;
+  std::tuple<float, float> result4;
+
+  switch (this->mode_) {
+    case Wheelleg::RELAX:
+    case Wheelleg::ROTOR:
+    case Wheelleg::STAND:
+
+      for (int i = 0; i < 12; i++) {
+        leg_argu_[0].LQR_K[i] = this->leg_[0]->LQR_K_calc(
+            &this->param_.wheel_param.K_Poly_Coefficient_L[i][0],
+            leg_argu_[0].L0);
+      }
+
+      for (int i = 0; i < 12; i++) {
+        leg_argu_[1].LQR_K[i] = this->leg_[1]->LQR_K_calc(
+            &this->param_.wheel_param.K_Poly_Coefficient_R[i][0],
+            leg_argu_[1].L0);
+      }
+      if (now_ > 1000000)
+        if (fabs(move_argu_.target_dot_x) > 0.5 ||
+            fabs(move_argu_.target_dot_x - move_argu_.x_dot_hat) > 0.7 ||
+            ((!onground0_flag_) & (!onground1_flag_))) {
+          leg_argu_[0].LQR_K[2] = 0;
+          leg_argu_[1].LQR_K[2] = 0;
+          this->move_argu_.xhat = 0;
+          move_argu_.target_x = 0;
+        }
+
+      if (onground0_flag_) {
+        leg_argu_[0].Tw =
+            (leg_argu_[0].LQR_K[0] *
+                 (-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
+             leg_argu_[0].LQR_K[1] * (-leg_argu_[0].d_theta) +
+             leg_argu_[0].LQR_K[2] *
+                 (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+             leg_argu_[0].LQR_K[3] *
+                 (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+             leg_argu_[0].LQR_K[4] * (-this->pit_ + 0.16) +
+             leg_argu_[0].LQR_K[5] * (-this->gyro_.x + 0.0f));
+        leg_argu_[0].Tp =
+            (leg_argu_[0].LQR_K[6] *
+                 (-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
+             leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta) +
+             leg_argu_[0].LQR_K[8] *
+                 (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+             leg_argu_[0].LQR_K[9] *
+                 (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+             leg_argu_[0].LQR_K[10] * (-this->pit_ + 0.16) +
+             leg_argu_[0].LQR_K[11] * (-this->gyro_.x + 0.0f));
+      } else {
+        leg_argu_[0].Tw = 0;
+
+        leg_argu_[0].Tp =
+            (leg_argu_[0].LQR_K[6] * (-leg_argu_[0].theta + 0.0f) +
+             leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta + 0.0f));
+      }
+      if (onground1_flag_) {
+        leg_argu_[1].Tw =
+            (leg_argu_[1].LQR_K[0] *
+                 (-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
+             leg_argu_[1].LQR_K[1] * (-leg_argu_[1].d_theta + 0.0f) +
+             leg_argu_[1].LQR_K[2] *
+                 (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+             leg_argu_[1].LQR_K[3] *
+                 (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+             leg_argu_[1].LQR_K[4] * (-this->pit_ + 0.16) +
+             leg_argu_[1].LQR_K[5] * (-this->gyro_.x + 0.0f));
+        leg_argu_[1].Tp =
+            (leg_argu_[1].LQR_K[6] *
+                 (-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
+             leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f) +
+             leg_argu_[1].LQR_K[8] *
+                 (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+             leg_argu_[1].LQR_K[9] *
+                 (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+             leg_argu_[1].LQR_K[10] * (-this->pit_ + 0.16) +
+             leg_argu_[1].LQR_K[11] * (-this->gyro_.x + 0.0f));
+      } else {
+        leg_argu_[1].Tw = 0;
+        leg_argu_[1].Tp =
+            (leg_argu_[1].LQR_K[6] * (-leg_argu_[1].theta + 0.0f) +
+             leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f));
+      }
+
+      this->leg_argu_[0].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
+                                    this->param_.wheel_param.static_L0[0] +
+                                    +roll_pid_.Calculate(0, eulr_.rol, dt_);
+      this->leg_argu_[1].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
+                                    this->param_.wheel_param.static_L0[1] -
+                                    roll_pid_.Calculate(0, eulr_.rol, dt_);
+
+      leg_argu_[0].F0 =
+          leg_argu_[0].Tp * sinf(leg_argu_[0].theta) +
+          this->param_.wheel_param.static_F0[0] +
+          leglength_pid_.at(0)->Calculate(this->leg_argu_[0].Delat_L0,
+                                          this->leg_argu_[0].L0, this->dt_);
+      leg_argu_[1].F0 =
+          leg_argu_[1].Tp * sinf(leg_argu_[1].theta) +
+          this->param_.wheel_param.static_F0[1] +
+          leglength_pid_.at(1)->Calculate(this->leg_argu_[1].Delat_L0,
+                                          this->leg_argu_[1].L0, this->dt_);
+
+      this->leg_argu_[0].Delta_Tp =
+          leg_argu_[0].L0 * 10.0f *
+          tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
+                                   this->leg_argu_[0].theta, this->dt_);
+      this->leg_argu_[1].Delta_Tp =
+          -leg_argu_[1].L0 * 10.0f *
+          tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
+                                   this->leg_argu_[0].theta, this->dt_);
+
+      result3 = leg_[0]->VMCinserve(
+          -leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
+          -leg_argu_[0].Tp - this->leg_argu_[0].Delta_Tp, leg_argu_[0].F0);
+      this->leg_argu_[0].T1 = std::get<0>(result3);
+      this->leg_argu_[0].T2 = std::get<1>(result3);
+
+      result4 = leg_[1]->VMCinserve(
+          -leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
+          -leg_argu_[1].Tp - this->leg_argu_[1].Delta_Tp, leg_argu_[1].F0);
+      this->leg_argu_[1].T1 = -std::get<0>(result4);
+      this->leg_argu_[1].T2 = -std::get<1>(result4);
+
+      if (onground0_flag_ & onground1_flag_) {
+        move_argu_.yaw_force =
+            -this->yaw_pid_.Calculate(move_argu_.target_yaw, this->yaw_, dt_);
+      } else {
+        move_argu_.yaw_force = 0;
+      }
+      /*3508不带减速箱是Tw*3.2f*/
+      /*2006带减速是Tw/1.8f*/
+      /* 3508带15.7减速箱是Tw/4.9256 */
+
+      this->wheel_motor_out_[0] =
+          this->leg_argu_[0].Tw / 4.9256f - move_argu_.yaw_force;
+
+      this->wheel_motor_out_[1] =
+          this->leg_argu_[1].Tw / 4.9256f + move_argu_.yaw_force;
+
+      this->hip_motor_out_[0] = this->leg_argu_[0].T1;
+      this->hip_motor_out_[1] = this->leg_argu_[0].T2;
+      this->hip_motor_out_[2] = this->leg_argu_[1].T1;
+      this->hip_motor_out_[3] = this->leg_argu_[1].T2;
+
+      this->hip_motor_flag_ = 1;
+      break;
+
+    case Wheelleg::RESET:
+      if (fabs(pit_) > M_PI / 2 || fabs(leg_argu_[0].theta) > 1.57 ||
+          fabs(leg_argu_[1].theta) > 1.57) {
+        leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
+
+        if (fabs(pit_) > M_PI / 2) {
+          if ((leg_argu_[0].theta - leg_argu_[1].theta) > 0.03) {
+            leg_argu_[1].target_theta = leg_argu_[1].theta + 0.001;
+          }
+          if ((leg_argu_[0].theta - leg_argu_[1].theta) < -0.03) {
+            leg_argu_[1].target_theta = leg_argu_[1].theta - 0.001;
+          }
+          leg_argu_[0].F0 = 10 * leglength_pid_.at(0)->Calculate(
+                                     0.65f, this->leg_argu_[0].L0, this->dt_);
+          leg_argu_[1].F0 = 10 * leglength_pid_.at(1)->Calculate(
+                                     0.65f, this->leg_argu_[1].L0, this->dt_);
+        }
+        if (fabs(leg_argu_[0].theta) < 1.57) {
+          leg_argu_[1].target_theta = leg_argu_[1].theta + cmd_.y / 1000;
+          leg_argu_[0].target_theta = leg_argu_[0].theta;
+        }
+
+        if (fabs(leg_argu_[1].theta) < 1.57) {
+          leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
+          leg_argu_[1].target_theta = leg_argu_[1].theta;
+        }
+
+        if (leg_argu_[0].target_theta > M_PI) {
+          leg_argu_[0].target_theta -= 2 * M_PI;
+        }
+        if (leg_argu_[0].target_theta < -M_PI) {
+          leg_argu_[0].target_theta += 2 * M_PI;
+        }
+        if (leg_argu_[1].target_theta > M_PI) {
+          leg_argu_[1].target_theta -= 2 * M_PI;
+        }
+        if (leg_argu_[1].target_theta < -M_PI) {
+          leg_argu_[1].target_theta += 2 * M_PI;
+        }
+        leg_argu_[0].Tp =
+            500 * this->theta_pid_[0]->Calculate(leg_argu_[0].target_theta,
+                                                 leg_argu_[0].theta, dt_);
+        leg_argu_[1].Tp =
+            500 * this->theta_pid_[1]->Calculate(leg_argu_[1].target_theta,
+                                                 leg_argu_[1].theta, dt_);
+
+      } else {
+        leg_argu_[0].F0 = 3 * leglength_pid_.at(0)->Calculate(
+                                  0.10f, this->leg_argu_[0].L0, this->dt_);
+        leg_argu_[1].F0 = 3 * leglength_pid_.at(1)->Calculate(
+                                  0.10f, this->leg_argu_[1].L0, this->dt_);
+
+        if ((this->leg_argu_[0].L0 < 0.20) && (this->leg_argu_[1].L0 < 0.20)) {
+          leg_argu_[0].Tp = 5.5 * this->theta_pid_[0]->Calculate(
+                                      0.1, leg_argu_[0].theta, dt_);
+          leg_argu_[1].Tp = 5.5 * this->theta_pid_[1]->Calculate(
+                                      0.1, leg_argu_[1].theta, dt_);
+          clampf(&leg_argu_[0].Tp, -10, 10);
+          clampf(&leg_argu_[1].Tp, -10, 10);
+        } else {
+          leg_argu_[0].Tp = 0;
+          leg_argu_[1].Tp = 0;
+        }
+      }
+
+      result3 = leg_[0]->VMCinserve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
+                                    -leg_argu_[0].Tp, leg_argu_[0].F0);
+      this->leg_argu_[0].T1 = std::get<0>(result3);
+      this->leg_argu_[0].T2 = std::get<1>(result3);
+
+      result4 = leg_[1]->VMCinserve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
+                                    -leg_argu_[1].Tp, leg_argu_[1].F0);
+      this->leg_argu_[1].T1 = -std::get<0>(result4);
+      this->leg_argu_[1].T2 = -std::get<1>(result4);
+
+      this->hip_motor_out_[0] = this->leg_argu_[0].T1;
+      this->hip_motor_out_[1] = this->leg_argu_[0].T2;
+      this->hip_motor_out_[2] = this->leg_argu_[1].T1;
+      this->hip_motor_out_[3] = this->leg_argu_[1].T2;
+
+      this->hip_motor_flag_ = 1;
+      break;
+  }
+}
+
+void Wheelleg::Control() {
+  clampf(&wheel_motor_out_[0], -0.8f, 0.8f);
+  clampf(&wheel_motor_out_[1], -0.8f, 0.8f);
+  clampf(&hip_motor_out_[0], -20.0f, 20.0f);
+  clampf(&hip_motor_out_[1], -20.0f, 20.0f);
+  clampf(&hip_motor_out_[2], -20.0f, 20.0f);
+  clampf(&hip_motor_out_[3], -20.0f, 20.0f);
+
+  // if (fabs(wheel_motor_[0]->GetSpeed()) > 5000 ||
+  //     fabs(wheel_motor_[1]->GetSpeed()) > 5000) {
+  //   wheel_motor_out_[0] = 0;
+  //   wheel_motor_out_[1] = 0;
+
+  //   if (fabs(this->pit_) > 0.5f) {
+  //     this->hip_motor_flag_ = 0;
+  //   }
+  // }
+
+  switch (this->mode_) {
+    case Wheelleg::RELAX:
+
+      this->wheel_motor_[0]->Relax();
+      this->wheel_motor_[1]->Relax();
+      hip_motor_flag_ = 0;
+      hip_motor_[0]->SetMit(0.0f);
+      hip_motor_[1]->SetMit(0.0f);
+      hip_motor_[2]->SetMit(0.0f);
+      hip_motor_[3]->SetMit(0.0f);
+      break;
+
+    case Wheelleg::STAND:
+    case Wheelleg::ROTOR:
+      // this->wheel_motor_[0]->Relax();
+      // this->wheel_motor_[1]->Relax();
+      this->wheel_motor_[0]->Control(-wheel_motor_out_[0]);
+      this->wheel_motor_[1]->Control(wheel_motor_out_[1]);
+      hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
+      hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
+      hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
+      hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
+      // hip_motor_[0]->SetMit(0.0f);
+      // hip_motor_[1]->SetMit(0.0f);
+      // hip_motor_[2]->SetMit(0.0f);
+      // hip_motor_[3]->SetMit(0.0f);
+      break;
+
+    case Wheelleg::RESET:
+
+      this->wheel_motor_[0]->Relax();
+      this->wheel_motor_[1]->Relax();
+
+      hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
+      hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
+
+      hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
+      hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
+
+      break;
+  }
+}
+
+void Wheelleg::SetMode(Wheelleg::Mode mode) {
+  if (mode == this->mode_) {
+    return;
+  }
+
+  this->leg_[0]->Reset();
+  this->leg_[1]->Reset();
+  move_argu_.x = 0;
+  move_argu_.x_dot = 0;
+  move_argu_.last_x_dot = 0;
+  move_argu_.target_x = move_argu_.xhat;
+  move_argu_.target_yaw = this->eulr_.yaw;
+  move_argu_.target_dot_x = 0;
+  move_argu_.xhat = 0;
+  move_argu_.x_dot_hat = 0;
+  this->manfilter_.reset_comp();
+  this->mode_ = mode;
+}

User/task/atti_esti.c

@@ -0,0 +1,57 @@
+/*
+    atti_esti Task
+    
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "task/user_task.h"
+/* USER INCLUDE BEGIN */
+#include "bsp/can.h"
+#include "device/dm_imu.h"
+#include "module/config.h"
+#include "module/balance_chassis.h"
+/* USER INCLUDE END */
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+DM_IMU_t dm_imu;
+Chassis_IMU_t chassis_imu_send;
+/* USER STRUCT END */
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+void Task_atti_esti(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / ATTI_ESTI_FREQ;
+
+  osDelay(ATTI_ESTI_INIT_DELAY); /* 延时一段时间再开启任务 */
+
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  /* USER CODE INIT BEGIN */
+  BSP_CAN_Init();
+  // 初始化DM IMU设备
+  DM_IMU_Init(&dm_imu, &Config_GetRobotParam()->imu_param);
+  /* USER CODE INIT END */
+  
+  while (1) {
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    /* USER CODE BEGIN */
+    
+    if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) {
+        osMessageQueueReset(task_runtime.msgq.chassis_imu); // 重置消息队列，防止阻塞
+        chassis_imu_send.accl = dm_imu.data.accl;
+        chassis_imu_send.gyro = dm_imu.data.gyro;
+        chassis_imu_send.euler = dm_imu.data.euler;
+        osMessageQueuePut(task_runtime.msgq.chassis_imu, &chassis_imu_send, 0, 0); // 非阻塞发送IMU数据
+    }
+    /* USER CODE END */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+  
+}

User/task/blink.c

@@ -0,0 +1,50 @@
+/*
+    blink Task
+    
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "task/user_task.h"
+/* USER INCLUDE BEGIN */
+#include "bsp/pwm.h"
+#include <math.h>
+/* USER INCLUDE END */
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+void Task_blink(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / BLINK_FREQ;
+
+  osDelay(BLINK_INIT_DELAY); /* 延时一段时间再开启任务 */
+
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  /* USER CODE INIT BEGIN */
+  BSP_PWM_Stop(BSP_PWM_LED_R);
+  BSP_PWM_Stop(BSP_PWM_LED_B);
+  BSP_PWM_SetComp(BSP_PWM_LED_G, 0.0f);
+  BSP_PWM_Start(BSP_PWM_LED_G);
+  /* USER CODE INIT END */
+  
+  while (1) {
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    /* USER CODE BEGIN */
+    // 呼吸灯 - 基于tick的正弦波
+    float duty = (sinf(tick * 0.003f) + 1.0f) * 0.5f; // 0到1之间的正弦波，加快频率
+    BSP_PWM_SetComp(BSP_PWM_LED_G, duty);
+    /* USER CODE END */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+  
+}

User/task/config.yaml

@@ -0,0 +1,28 @@
+- delay: 0
+  description: ''
+  freq_control: true
+  frequency: 100.0
+  function: Task_blink
+  name: blink
+  stack: 256
+- delay: 0
+  description: ''
+  freq_control: true
+  frequency: 500.0
+  function: Task_rc
+  name: rc
+  stack: 256
+- delay: 0
+  description: ''
+  freq_control: true
+  frequency: 1000.0
+  function: Task_atti_esti
+  name: atti_esti
+  stack: 256
+- delay: 500
+  description: ''
+  freq_control: true
+  frequency: 500.0
+  function: Task_ctrl_chassis
+  name: ctrl_chassis
+  stack: 512

User/task/ctrl_chassis.c

@@ -0,0 +1,73 @@
+/*
+    ctrl_chassis Task
+    
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "task/user_task.h"
+/* USER INCLUDE BEGIN */
+#include "bsp/can.h"
+#include "device/motor_lk.h"
+#include "device/motor_lz.h"
+#include "module/config.h"
+#include "module/balance_chassis.h"
+
+/* USER INCLUDE END */
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+Chassis_t chassis;
+Chassis_CMD_t chassis_cmd = {
+    .mode = CHASSIS_MODE_RECOVER,  // 改为RECOVER模式，让电机先启动
+    .move_vec = {
+        .vx = 0.0f,
+        .vy = 0.0f,
+        .wz = 0.0f,
+    },
+    .height = 0.0f,
+};
+Chassis_IMU_t chassis_imu;
+
+MOTOR_Feedback_t left_wheel_fb;
+MOTOR_Feedback_t right_wheel_fb;
+/* USER STRUCT END */
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+void Task_ctrl_chassis(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_CHASSIS_FREQ;
+
+  osDelay(CTRL_CHASSIS_INIT_DELAY); /* 延时一段时间再开启任务 */
+
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  /* USER CODE INIT BEGIN */
+
+  Chassis_Init(&chassis, &Config_GetRobotParam()->chassis_param, CTRL_CHASSIS_FREQ);
+
+
+  /* USER CODE INIT END */
+  
+  while (1) {
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    /* USER CODE BEGIN */
+    if (osMessageQueueGet(task_runtime.msgq.chassis_imu, &chassis_imu, NULL, 0) == osOK) {
+        chassis.feedback.imu = chassis_imu;
+    }
+    if (osMessageQueueGet(task_runtime.msgq.Chassis_cmd, &chassis_cmd, NULL, 0) == osOK) {
+        // 成功接收到命令，更新底盘命令
+    }
+    
+    Chassis_UpdateFeedback(&chassis);
+    Chassis_Control(&chassis, &chassis_cmd);
+
+    
+    /* USER CODE END */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }}

User/task/init.c

@@ -0,0 +1,49 @@
+/*
+    Init Task
+    任务初始化，创建各个线程任务和消息队列
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "task/user_task.h"
+
+/* USER INCLUDE BEGIN */
+#include "component/ahrs.h"
+#include "module/config.h"
+#include "module/balance_chassis.h"
+/* USER INCLUDE BEGIN */
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * \brief 初始化
+ *
+ * \param argument 未使用
+ */
+void Task_Init(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+    /* USER CODE INIT BEGIN */
+
+    /* USER CODE INIT END */
+  osKernelLock(); /* 锁定内核，防止任务切换 */
+  
+  /* 创建任务线程 */
+  task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);
+  task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
+  task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti);
+  task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
+
+  // 创建消息队列
+  /* USER MESSAGE BEGIN */
+  task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
+  task_runtime.msgq.chassis_imu = osMessageQueueNew(2u, sizeof(Chassis_IMU_t), NULL);
+  task_runtime.msgq.Chassis_cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);
+  /* USER MESSAGE END */
+
+  osKernelUnlock(); // 解锁内核
+  osThreadTerminate(osThreadGetId()); // 任务完成后结束自身
+}

User/task/rc.c

@@ -0,0 +1,90 @@
+/*
+    rc Task
+    
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "task/user_task.h"
+/* USER INCLUDE BEGIN */
+#include "device/dr16.h"
+#include "module/balance_chassis.h"
+/* USER INCLUDE END */
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+DR16_t dr16;
+Chassis_CMD_t cmd_to_chassis;
+/* USER STRUCT END */
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+void Task_rc(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / RC_FREQ;
+
+  osDelay(RC_INIT_DELAY); /* 延时一段时间再开启任务 */
+
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  /* USER CODE INIT BEGIN */
+  DR16_Init(&dr16);
+  /* USER CODE INIT END */
+  
+  while (1) {
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    /* USER CODE BEGIN */
+    DR16_StartDmaRecv(&dr16);
+    if (DR16_WaitDmaCplt(20)) {
+          // 根据左拨杆设置底盘模式
+          switch (dr16.data.sw_l) {
+              case 1: // 上位
+                  cmd_to_chassis.mode = CHASSIS_MODE_RELAX;
+                  break;
+              case 3: // 中位  
+                  cmd_to_chassis.mode = CHASSIS_MODE_RELAX;
+                  break;
+              case 2: // 下位
+                  cmd_to_chassis.mode = CHASSIS_MODE_WHELL_BALANCE;
+                  break;
+              default:
+                  cmd_to_chassis.mode = CHASSIS_MODE_RELAX;
+                  break;
+          }
+          
+          // 解析遥控器摇杆数据为运动向量
+          // 将遥控器通道值从[364, 1684]映射到[-1.0, 1.0]
+          float ch_l_y_norm = (float)(dr16.data.ch_l_y - 1024) / 660.0f; // 前后
+          float ch_l_x_norm = (float)(dr16.data.ch_l_x - 1024) / 660.0f; // 左右
+          float ch_r_x_norm = (float)(dr16.data.ch_r_y - 1024) / 660.0f; // 旋转
+          
+          // 设置运动向量（根据需要调整增益）
+          cmd_to_chassis.move_vec.vx = ch_l_y_norm * 2.0f;  // 前后运动，增益可调
+          cmd_to_chassis.move_vec.vy = ch_l_x_norm * 2.0f;  // 左右运动，增益可调  
+          cmd_to_chassis.move_vec.wz = ch_r_x_norm * 3.0f;  // 旋转运动，增益可调
+          
+          // 设置目标高度（可根据右拨杆或其他输入调整）
+          cmd_to_chassis.height = dr16.data.res-1024; // 目标高度，范围[-1024, 1024]，可根据需要调整比例
+          
+          // 发送命令到底盘控制任务
+          osMessageQueuePut(task_runtime.msgq.Chassis_cmd, &cmd_to_chassis, 0, 0);
+
+    } else {
+        // 超时处理，发送安全命令
+        cmd_to_chassis.mode = CHASSIS_MODE_RELAX;
+        cmd_to_chassis.move_vec.vx = 0.0f;
+        cmd_to_chassis.move_vec.vy = 0.0f;
+        cmd_to_chassis.move_vec.wz = 0.0f;
+        cmd_to_chassis.height = 0.0f;
+        osMessageQueuePut(task_runtime.msgq.Chassis_cmd, &cmd_to_chassis, 0, 0);
+    }
+
+    /* USER CODE END */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+  
+}

User/task/user_task.c

@@ -0,0 +1,31 @@
+#include "task/user_task.h"
+
+Task_Runtime_t task_runtime;
+
+const osThreadAttr_t attr_init = {
+    .name = "Task_Init",
+    .priority = osPriorityRealtime,
+    .stack_size = 256 * 4,
+};
+
+/* User_task */
+const osThreadAttr_t attr_blink = {
+    .name = "blink",
+    .priority = osPriorityNormal,
+    .stack_size = 256 * 4,
+};
+const osThreadAttr_t attr_rc = {
+    .name = "rc",
+    .priority = osPriorityNormal,
+    .stack_size = 256 * 4,
+};
+const osThreadAttr_t attr_atti_esti = {
+    .name = "atti_esti",
+    .priority = osPriorityNormal,
+    .stack_size = 256 * 4,
+};
+const osThreadAttr_t attr_ctrl_chassis = {
+    .name = "ctrl_chassis",
+    .priority = osPriorityNormal,
+    .stack_size = 512 * 4,
+};

User/task/user_task.h

@@ -0,0 +1,108 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+/* Includes ----------------------------------------------------------------- */
+#include <cmsis_os2.h>
+#include "FreeRTOS.h"
+#include "task.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+/* Exported constants ------------------------------------------------------- */
+/* 任务运行频率 */
+#define BLINK_FREQ (100.0)
+#define RC_FREQ (500.0)
+#define ATTI_ESTI_FREQ (1000.0)
+#define CTRL_CHASSIS_FREQ (500.0)
+
+/* 任务初始化延时ms */
+#define TASK_INIT_DELAY (100u)
+#define BLINK_INIT_DELAY (0)
+#define RC_INIT_DELAY (0)
+#define ATTI_ESTI_INIT_DELAY (0)
+#define CTRL_CHASSIS_INIT_DELAY (500)
+
+/* Exported defines --------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+/* 任务运行时结构体 */
+typedef struct {
+    /* 各任务，也可以叫做线程 */
+    struct {
+        osThreadId_t blink;
+        osThreadId_t rc;
+        osThreadId_t atti_esti;
+        osThreadId_t ctrl_chassis;
+    } thread;
+
+    /* USER MESSAGE BEGIN */
+    struct {
+        osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */
+
+        osMessageQueueId_t chassis_imu;
+        osMessageQueueId_t Chassis_cmd;
+
+    } msgq;
+    /* USER MESSAGE END */
+
+    /* 机器人状态 */
+    struct {
+        float battery; /* 电池电量百分比 */
+        float vbat; /* 电池电压 */
+        float cpu_temp; /* CPU温度 */
+    } status;
+
+    /* USER CONFIG BEGIN */
+
+    /* USER CONFIG END */
+
+    /* 各任务的stack使用 */
+    struct {
+        UBaseType_t blink;
+        UBaseType_t rc;
+        UBaseType_t atti_esti;
+        UBaseType_t ctrl_chassis;
+    } stack_water_mark;
+
+    /* 各任务运行频率 */
+    struct {
+        float blink;
+        float rc;
+        float atti_esti;
+        float ctrl_chassis;
+    } freq;
+
+    /* 任务最近运行时间 */
+    struct {
+        float blink;
+        float rc;
+        float atti_esti;
+        float ctrl_chassis;
+    } last_up_time;
+
+} Task_Runtime_t;
+
+/* 任务运行时结构体 */
+extern Task_Runtime_t task_runtime;
+
+/* 初始化任务句柄 */
+extern const osThreadAttr_t attr_init;
+extern const osThreadAttr_t attr_blink;
+extern const osThreadAttr_t attr_rc;
+extern const osThreadAttr_t attr_atti_esti;
+extern const osThreadAttr_t attr_ctrl_chassis;
+
+/* 任务函数声明 */
+void Task_Init(void *argument);
+void Task_blink(void *argument);
+void Task_rc(void *argument);
+void Task_atti_esti(void *argument);
+void Task_ctrl_chassis(void *argument);
+
+#ifdef __cplusplus
+}
+#endif

User/整合方案说明.md

@@ -0,0 +1,244 @@
+# 轮腿机器人控制系统整合方案
+
+## 📋 现有系统分析
+
+### 🏗️ 您的系统架构（优势）
+1. **成熟的硬件接口**：已实现HT+BM电机CAN通信
+2. **完整的运动学**：四连杆正/逆运动学 + 雅可比矩阵
+3. **丰富的控制模式**：平衡、运动、跳跃、悬浮检测
+4. **详细的参数数据**：基于实际测量的腿部参数表
+5. **MATLAB设计工具**：完整的LQR参数生成流程
+
+### ⚠️ 可以改进的地方
+1. **控制逻辑分散**：LQR、VMC、MPC混合使用，逻辑复杂
+2. **参数管理困难**：大量硬编码参数分散在各处
+3. **缺乏统一接口**：不同控制算法接口不一致
+
+## 🚀 整合方案
+
+### 方案一：渐进式整合（推荐）
+
+#### 第一阶段：保持现有系统，添加新接口
+```c
+// 在Chassis_Task.c中添加
+#include "integrated_balance_control.h"
+
+void Chassis_Task(void const * argument)
+{
+    vTaskDelay(CHASSIS_TASK_INIT_TIME);
+    Chassis_Init(&chassis_control);
+    vTaskDelay(100);
+    
+    while(1)
+    {
+        Chassis_Data_Update(&chassis_control);
+        Chassis_Status_Detect(&chassis_control);
+        Chassis_Mode_Set(&chassis_control);
+        Chassis_Mode_Change_Control_Transit(&chassis_control);
+        Target_Value_Set(&chassis_control);
+        
+        // 新增：整合控制器调用
+        integrated_chassis_control_update();
+        
+        // 如果使用新控制器，跳过原有的力矩计算
+        if (!get_current_controller_type()) {
+            Chassis_Torque_Calculation(&chassis_control);
+            Chassis_Torque_Combine(&chassis_control);
+        }
+        
+        Motor_CMD_Send(&chassis_control);
+        vTaskDelay(1);
+    }
+}
+```
+
+#### 第二阶段：参数统一管理
+创建统一的参数配置文件：
+
+```c
+// robot_config.h
+#ifndef ROBOT_CONFIG_H
+#define ROBOT_CONFIG_H
+
+// 机器人物理参数（与MATLAB一致）
+#define ROBOT_R_W       0.09f           // 驱动轮半径 (m)
+#define ROBOT_R_L       0.25f           // 轮距/2 (m)
+#define ROBOT_L_C       0.037f          // 机体质心到关节距离 (m)
+#define ROBOT_M_W       0.8f            // 驱动轮质量 (kg)
+#define ROBOT_M_L       1.6183599f      // 腿部质量 (kg)
+#define ROBOT_M_B       11.542f         // 机体质量 (kg)
+
+// 腿部几何参数（与您的定义一致）
+#define LEG_L1          0.15f           // 大腿长度 (m)
+#define LEG_L2          0.25f           // 小腿长度 (m)
+#define LEG_L3          0.25f           // 
+#define LEG_L4          0.15f           //
+#define LEG_L5          0.1f            // 髋关节间距/2 (m)
+
+// 控制参数
+#define CONTROL_FREQUENCY   500.0f      // 控制频率 (Hz)
+#define MAX_WHEEL_TORQUE    50.0f       // 最大轮子力矩 (N*m)
+#define MAX_JOINT_TORQUE    18.0f       // 最大关节力矩 (N*m)
+
+// 安全参数
+#define MAX_TILT_ANGLE      0.25f       // 最大倾斜角 (rad)
+#define LEG_LENGTH_MIN      0.11f       // 最小腿长 (m)
+#define LEG_LENGTH_MAX      0.30f       // 最大腿长 (m)
+
+#endif
+```
+
+#### 第三阶段：控制器选择接口
+添加动态切换功能：
+
+```c
+// 在遥控器中添加切换功能
+void handle_controller_switch(const Gimbal_ctrl_t* rc_ctrl)
+{
+    static uint8_t last_switch_state = 0;
+    uint8_t current_switch_state = 0; // 从遥控器读取开关状态
+    
+    if (current_switch_state != last_switch_state) {
+        if (current_switch_state == 1) {
+            // 切换到新控制器
+            set_controller_type(1);
+        } else {
+            // 切换到原有控制器
+            set_controller_type(0);
+        }
+        last_switch_state = current_switch_state;
+    }
+}
+```
+
+### 方案二：完全替换（激进）
+
+直接用新的balance_control替换现有的控制逻辑：
+
+#### 修改Chassis_Task主循环
+```c
+void Chassis_Task(void const * argument)
+{
+    vTaskDelay(CHASSIS_TASK_INIT_TIME);
+    
+    // 初始化新的平衡控制器
+    balance_controller_t balance_ctrl;
+    balance_control_init(&balance_ctrl, TASK_RUN_TIME);
+    
+    vTaskDelay(100);
+    
+    while(1)
+    {
+        // 数据更新（保留）
+        Chassis_Data_Update(&chassis_control);
+        
+        // 转换数据格式
+        imu_data_t imu_data;
+        motor_feedback_t motor_fb;
+        control_command_t cmd;
+        
+        convert_chassis_to_balance_imu(&chassis_control, &imu_data);
+        convert_chassis_to_balance_motor_fb(&chassis_control, &motor_fb);
+        convert_chassis_to_balance_cmd(&chassis_control, &cmd);
+        
+        // 使用新控制器
+        balance_control_update(&balance_ctrl, &imu_data, &motor_fb, &cmd);
+        
+        // 获取控制输出
+        motor_control_t motor_ctrl;
+        get_motor_control_output(&balance_ctrl, &motor_ctrl);
+        
+        // 转换回原有格式并发送
+        convert_balance_to_chassis_output(&motor_ctrl, &chassis_control);
+        Motor_CMD_Send(&chassis_control);
+        
+        vTaskDelay(1);
+    }
+}
+```
+
+## 🔧 具体修改步骤
+
+### 1. 保留您的优势代码
+- **保留**：`Forward_kinematic_solution()` - 运动学计算
+- **保留**：`Supportive_Force_Cal()` - 支撑力计算
+- **保留**：`CAN_HT_CMD()` / `CAN_BM_CONTROL_CMD()` - 电机通信
+- **保留**：MATLAB LQR设计流程
+
+### 2. 整合控制架构
+- **替换**：分散的LQR/VMC控制逻辑 → 统一的balance_control
+- **简化**：复杂的状态机 → 清晰的模式管理
+- **统一**：参数管理 → robot_config.h
+
+### 3. 数据接口适配
+```c
+// 现有的HT电机数据
+typedef struct {
+    const HT_motor_measure_t *motor_measure;
+    fp32 position;
+    fp32 velocity; 
+    fp32 torque_out, torque_get;
+} joint_motor_t;
+
+// 转换为新系统格式
+void convert_ht_motor_to_balance_fb(const joint_motor_t* ht_motor, 
+                                   float* angle, float* velocity, float* torque)
+{
+    *angle = ht_motor->position;
+    *velocity = ht_motor->velocity;
+    *torque = ht_motor->torque_get;
+}
+```
+
+### 4. 参数迁移
+将您MATLAB中的参数直接用于新系统：
+
+```c
+// 使用您的LQR拟合系数
+const float K_fit_coefficients[40][6] = {
+    // 复制您MATLAB生成的系数
+    // ...
+};
+
+// 使用您的机器人参数
+#define R_W_ACTUAL     0.09f    // 与您的R_w_ac一致
+#define R_L_ACTUAL     0.25f    // 与您的R_l_ac一致
+// ...
+```
+
+## 🎯 推荐的实施步骤
+
+### 第1周：准备工作
+1. 备份现有代码
+2. 创建新的git分支
+3. 编译并测试现有系统确保正常
+
+### 第2周：集成准备
+1. 添加integrated_balance_control文件
+2. 修改Chassis_Task添加切换接口
+3. 保持原有控制器为默认
+
+### 第3周：参数对齐
+1. 创建robot_config.h统一参数
+2. 运行MATLAB脚本生成LQR系数
+3. 对比调试确保参数一致
+
+### 第4周：功能测试
+1. 在安全环境下测试新控制器
+2. 对比两套控制器的性能
+3. 调试和优化
+
+### 第5周：全面切换
+1. 将新控制器设为默认
+2. 移除冗余的旧代码
+3. 文档整理和代码规范化
+
+## 🛡️ 风险控制
+
+1. **渐进式切换**：保留原有系统作为备份
+2. **参数验证**：MATLAB仿真验证参数正确性
+3. **安全测试**：在安全环境下测试新控制器
+4. **性能对比**：记录并对比控制效果
+5. **回滚机制**：确保可以快速回到原有系统
+
+这样您既可以利用现有系统的优势，又能获得新架构的清晰性和可维护性。您觉得哪个方案更适合您的需求？

some_functions.c

@@ -0,0 +1,171 @@
+//             Front
+//         | 1       4 |
+//  (1)Left|           |Right(2)
+//         | 2       3 |
+
+#define HT_SLAVE_ID1        0x01
+#define HT_SLAVE_ID2        0x02
+#define HT_SLAVE_ID3        0x03
+#define HT_SLAVE_ID4        0x04
+
+#define CAN_BM_M1_ID 0x97
+#define CAN_BM_M2_ID 0x98
+#define BM_CAN hcan2
+
+#define get_HT_motor_measure(ptr, data)                                    \
+{                        \
+	(ptr)->last_ecd = (ptr)->ecd;                                   \
+	(ptr)->ecd =       uint_to_float((uint16_t)((data)[1] << 8 | (data)[2] ),P_MIN, P_MAX, 16);      \
+	(ptr)->speed_rpm = uint_to_float((uint16_t)(data[3]<<4)|(data[4]>>4), V_MIN, V_MAX, 12);\
+	(ptr)->real_torque = uint_to_float((uint16_t)(((data[4] & 0x0f) << 8) | (data)[5]), -18, +18, 12); \
+}	
+
+
+
+
+
+
+#define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x)))
+#define P_MIN -95.5f// Radians
+#define P_MAX 95.5f
+#define V_MIN -45.0f// Rad/s
+#define V_MAX 45.0f
+#define KP_MIN 0.0f// N-m/rad
+#define KP_MAX 500.0f
+#define KD_MIN 0.0f// N-m/rad/s
+#define KD_MAX 5.0f
+#define T_MIN -18.0f
+#define T_MAX 18.0f
+
+
+void Forward_kinematic_solution(chassis_control_t *feedback_update,
+											fp32 Q1,fp32 S1,fp32 Q4,fp32 S4, uint8_t ce)
+{
+	fp32 dL0=0,L0=0,Q0=0,S0=0;
+	fp32 xb,xd,yb,yd,Lbd,xc,yc;
+	fp32 A0,B0,C0,Q2,Q3,S2,S3;
+	fp32 vxb,vxd,vyb,vyd,vxc,vyc;
+	fp32 cos_Q1,cos_Q4,sin_Q1,sin_Q4;
+	fp32 sin_Q2,cos_Q2,sin_Q3,cos_Q3;
+	fp32 axb,ayb,axd,ayd,a2,axc;
+	/******************************/
+	Q1 = PI + Q1;
+	cos_Q1 = arm_cos_f32(Q1);
+	sin_Q1 = arm_sin_f32(Q1);
+	cos_Q4 = arm_cos_f32(Q4);
+	sin_Q4 = arm_sin_f32(Q4);
+	xb = -L5/2.0f + L1*cos_Q1;
+	xd =  L5/2.0f + L4*cos_Q4;
+	yb = L1*sin_Q1;
+	yd = L4*sin_Q4;
+
+	Lbd=(xd-xb)*(xd-xb)+(yd-yb)*(yd-yb);
+	A0 = 2.0f*L2*(xd-xb);
+	B0 = 2.0f*L2*(yd-yb);
+	C0 = L2*L2+Lbd-L3*L3;
+	Q2 = 2.0f *atan_tl((B0+Sqrt(A0*A0 + B0*B0 -C0*C0))/(A0+C0));
+	
+	xc = xb + arm_cos_f32(Q2)*L2;
+	yc = yb + arm_sin_f32(Q2)*L2;
+
+	L0=Sqrt( xc*xc + yc*yc );
+	Q0 = atan(yc/xc);
+	
+	vxb = -S1*L1*sin_Q1;
+	vyb = S1*L1*cos_Q1;
+	vxd = -S4*L4*sin_Q4;
+	vyd = S4*L4*cos_Q4;
+	Q3 = atan_tl((yc-yd)/(xc-xd));
+	S2 = ((vxd-vxb)*arm_cos_f32(Q3) + (vyd-vyb)*arm_sin_f32(Q3))/(L2*arm_sin_f32(Q3-Q2)); 
+	S3 = ((vxd-vxb)*arm_cos_f32(Q2) + (vyd-vyb)*arm_sin_f32(Q2))/(L2*arm_sin_f32(Q3-Q2)); 
+	vxc = vxb - S2*L2*arm_sin_f32(Q2);
+  	vyc = vyb + S2*L2*arm_cos_f32(Q2);
+	S0 = 3*(-arm_sin_f32(ABS(Q0))*vxc-arm_cos_f32(Q0)*vyc);
+
+	if( Q0 < 0 )
+		Q0 += PI;
+	/*******************************/
+	if (ce)
+	{
+		feedback_update->chassis_posture_info .leg_length_L = L0;
+		feedback_update->chassis_posture_info .leg_angle_L  = Q0;
+		// feedback_update->chassis_posture_info .leg_gyro_L   		= S0;
+	}
+	else 
+	{
+		feedback_update->chassis_posture_info .leg_length_R = L0;
+		feedback_update->chassis_posture_info .leg_angle_R  = Q0;
+		// feedback_update->chassis_posture_info .leg_gyro_R   		= S0;
+	}
+}
+
+void Supportive_Force_Cal( chassis_control_t *Suspend_Detect, fp32 Q1, fp32 Q4, fp32 ce )
+{
+	fp32 dL0=0,L0=0,Q0=0,S0=0;
+	fp32 xb,xd,yb,yd,Lbd,xc,yc;
+	fp32 A0,B0,C0,Q2,Q3,S2,S3;
+	fp32 vxb,vxd,vyb,vyd,vxc,vyc;
+	fp32 cos_Q1,cos_Q4,sin_Q1,sin_Q4;
+	fp32 sin_Q2,cos_Q2,sin_Q3,cos_Q3;
+	fp32 axb,ayb,axd,ayd,a2,axc;
+	/******************************/
+	Q1 = PI + Q1;
+	cos_Q1 = arm_cos_f32(Q1);
+	sin_Q1 = arm_sin_f32(Q1);
+	cos_Q4 = arm_cos_f32(Q4);
+	sin_Q4 = arm_sin_f32(Q4);
+	xb = -L5/2.0f + L1*cos_Q1;
+	xd =  L5/2.0f + L4*cos_Q4;
+	yb = L1*sin_Q1;
+	yd = L4*sin_Q4;
+
+	Lbd=sqrt( (xd-xb)*(xd-xb)+(yd-yb)*(yd-yb) );
+	A0 = 2.0f*L2*(xd-xb);
+	B0 = 2.0f*L2*(yd-yb);
+	C0 = L2*L2+Lbd*Lbd-L3*L3;
+	Q2 = 2.0f *atan_tl((B0+Sqrt(A0*A0 + B0*B0 -C0*C0))/(A0+C0));
+	xc = xb + arm_cos_f32(Q2)*L2;
+	yc = yb + arm_sin_f32(Q2)*L2;
+
+	L0 = Sqrt( xc*xc + yc*yc );
+	Q0 = atan_tl(yc/xc);
+	if( Q0 < 0 )
+		Q0 += PI;
+	
+	Q3  = atan_tl((yc-yd)/(xc-xd))+PI;
+
+	
+
+	fp32 J1 = ( L1 * arm_sin_f32(Q0 - Q3) * arm_sin_f32( Q1 - Q2 ) ) / (arm_sin_f32( Q3 - Q2 ));
+	fp32 J2 = ( L1 * arm_cos_f32(Q0 - Q3) * arm_sin_f32( Q1 - Q2 ) ) / (arm_sin_f32( Q3 - Q2 ) * L0);
+	fp32 J3 = ( L4 * arm_sin_f32(Q0 - Q2) * arm_sin_f32( Q3 - Q4 ) ) / (arm_sin_f32( Q3 - Q2 ));
+	fp32 J4 = ( L4 * arm_cos_f32(Q0 - Q2) * arm_sin_f32( Q3 - Q4 ) ) / (arm_sin_f32( Q3 - Q2 ) * L0);
+	
+	
+	fp32 dett = J1 * J4 - J2 * J3;
+	fp32 Inv_J1 =  J4 / dett;
+	fp32 Inv_J2 = -J2 / dett;
+	fp32 Inv_J3 = -J3 / dett;
+	fp32 Inv_J4 =  J1 / dett;
+
+	ddebug = yc;
+
+	if( ce == 1.0f )
+	{
+		Suspend_Detect->chassis_posture_info.supportive_force_L = 
+			Inv_J1 * Suspend_Detect->joint_motor_1.torque_get + 
+			Inv_J2 * Suspend_Detect->joint_motor_2.torque_get;
+		// if( Suspend_Detect->chassis_posture_info.supportive_force_L < 0.0f )
+		// 	Suspend_Detect->chassis_posture_info.supportive_force_L += 40.0f;
+	}
+	else	
+	{
+		Suspend_Detect->chassis_posture_info.supportive_force_R = 
+			Inv_J1 * Suspend_Detect->joint_motor_4.torque_get + 
+			Inv_J2 * Suspend_Detect->joint_motor_3.torque_get;
+		Suspend_Detect->chassis_posture_info.supportive_force_R *= -1.0f;
+		// if( Suspend_Detect->chassis_posture_info.supportive_force_R < 0.0f )
+		// 	Suspend_Detect->chassis_posture_info.supportive_force_R += 40.0f;
+	}
+
+}

utils/lqr.asv

@@ -0,0 +1,321 @@
+% v1：这份LQR程序是参考我之前写的哈工程LQR程序以及小周写的AB矩阵求解器优化后写出来的，感谢周神（2024/05/07）
+% v2：添加了可以专门调试定腿长的功能（2024/05/08）
+% v3：优化部分注释，添加单位说明（2024/05/15）
+% v4: 优化了输出，输出矩阵K的系数可以真正的复制到C里（2024/05/16）
+
+% 以下所有变量含义参考2023上交轮腿电控开源（https://bbs.robomaster.com/forum.php?mod=viewthread&tid=22756）所使用符号含义
+
+%%%%%%%%%%%%%%%%%%%%%%%%%Step 0：重置程序，定义变量%%%%%%%%%%%%%%%%%%%%%%%%%
+tic
+clear all
+clc
+
+% 定义机器人机体参数
+syms R_w                % 驱动轮半径
+syms R_l                % 驱动轮轮距/2
+syms l_l l_r            % 左右腿长
+syms l_wl l_wr          % 驱动轮质心到左右腿部质心距离
+syms l_bl l_br          % 机体质心到左右腿部质心距离
+syms l_c                % 机体质心到腿部关节中心点距离
+syms m_w m_l m_b        % 驱动轮质量 腿部质量 机体质量
+syms I_w                % 驱动轮转动惯量           (自然坐标系法向)
+syms I_ll I_lr          % 驱动轮左右腿部转动惯量    (自然坐标系法向，实际上会变化)
+syms I_b                % 机体转动惯量             (自然坐标系法向)
+syms I_z                % 机器人z轴转动惯量        (简化为常量)
+
+% 定义其他独立变量并补充其导数
+syms theta_wl   theta_wr   % 左右驱动轮转角
+syms dtheta_wl  dtheta_wr 
+syms ddtheta_wl ddtheta_wr ddtheta_ll ddtheta_lr ddtheta_b
+
+% 定义状态向量
+syms s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
+
+% 定义控制向量
+syms T_wl T_wr T_bl T_br
+
+% 输入物理参数：重力加速度
+syms g
+
+
+
+%%%%%%%%%%%%%%%%%%%%%%%Step 1：解方程，求控制矩阵A，B%%%%%%%%%%%%%%%%%%%%%%%
+
+% 通过原文方程组(3.11)-(3.15)，求出ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b表达式
+eqn1 = (I_w*l_l/R_w+m_w*R_w*l_l+m_l*R_w*l_bl)*ddtheta_wl+(m_l*l_wl*l_bl-I_ll)*ddtheta_ll+(m_l*l_wl+m_b*l_l/2)*g*theta_ll+T_bl-T_wl*(1+l_l/R_w)==0;
+eqn2 = (I_w*l_r/R_w+m_w*R_w*l_r+m_l*R_w*l_br)*ddtheta_wr+(m_l*l_wr*l_br-I_lr)*ddtheta_lr+(m_l*l_wr+m_b*l_r/2)*g*theta_lr+T_br-T_wr*(1+l_r/R_w)==0;
+eqn3 = -(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wl-(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wr-(m_l*R_w*l_wl+m_b*R_w*l_l/2)*ddtheta_ll-(m_l*R_w*l_wr+m_b*R_w*l_r/2)*ddtheta_lr+T_wl+T_wr==0;
+eqn4 = (m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wl+(m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wr+m_l*l_wl*l_c*ddtheta_ll+m_l*l_wr*l_c*ddtheta_lr-I_b*ddtheta_b+m_b*g*l_c*theta_b-(T_wl+T_wr)*l_c/R_w-(T_bl+T_br)==0;
+eqn5 = ((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wl-((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wr+(I_z*l_l)/(2*R_l)*ddtheta_ll-(I_z*l_r)/(2*R_l)*ddtheta_lr-T_wl*R_l/R_w+T_wr*R_l/R_w==0;
+[ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b] = solve(eqn1,eqn2,eqn3,eqn4,eqn5,ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b);
+
+
+% 通过计算雅可比矩阵的方法得出控制矩阵A，B所需要的全部偏导数
+J_A = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[theta_ll,theta_lr,theta_b]);
+J_B = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[T_wl,T_wr,T_bl,T_br]);
+
+% 定义矩阵A，B，将指定位置的数值根据上述偏导数计算出来并填入
+A = sym('A',[10 10]);
+B = sym('B',[10 4]);
+
+% 填入A数据：a25,a27,a29,a45,a47,a49,a65,a67,a69,a85,a87,a89,a105,a107,a109
+for p = 5:2:9
+    A_index = (p - 3)/2;
+    A(2,p) = R_w*(J_A(1,A_index) + J_A(2,A_index))/2;
+    A(4,p) = (R_w*(- J_A(1,A_index) + J_A(2,A_index)))/(2*R_l) - (l_l*J_A(3,A_index))/(2*R_l) + (l_r*J_A(4,A_index))/(2*R_l);
+    for q = 6:2:10
+        A(q,p) = J_A(q/2,A_index);
+    end
+end
+
+% A的以下数值为1：a12,a34,a56,a78,a910，其余数值为0
+for r = 1:10
+    if rem(r,2) == 0
+        A(r,1) = 0; A(r,2) = 0; A(r,3) = 0; A(r,4) = 0; A(r,6) = 0; A(r,8) = 0; A(r,10) = 0;
+    else
+        A(r,:) = zeros(1,10);
+        A(r,r+1) = 1;
+    end
+end
+
+% 填入B数据：b21,b22,b23,b24,b41,b42,b43,b44,b61,b62,b63,b64,b81,b82,b83,b84,b101,b102,b103,b104,
+for h = 1:4
+    B(2,h) = R_w*(J_B(1,h) + J_B(2,h))/2;
+    B(4,h) = (R_w*(- J_B(1,h) + J_B(2,h)))/(2*R_l) - (l_l*J_B(3,h))/(2*R_l) + (l_r*J_B(4,h))/(2*R_l);
+    for f = 6:2:10
+        B(f,h) = J_B(f/2,h);
+    end
+end
+
+% B的其余数值为0
+for e = 1:2:9
+    B(e,:) = zeros(1,4);
+end
+
+
+
+%%%%%%%%%%%%%%%%%%%%%Step 2：输入参数（可以修改的部分）%%%%%%%%%%%%%%%%%%%%%
+
+% 物理参数赋值（唯一此处不可改变！），后面的数据通过增加后缀_ac区分模型符号和实际数据
+g_ac = 9.81;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%                      此处可以输入机器人机体基本参数                      %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人机体与轮部参数%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+R_w_ac = 0.77;                           % 驱动轮半径                  （单位：m）
+R_l_ac = 0.210;                            % 两个驱动轮之间距离/2         （单位：m）
+l_c_ac = 0.025;                            % 机体质心到腿部关节中心点距离  （单位：m）
+m_w_ac = 0.5; m_l_ac = 2.133; m_b_ac = 4.542;    % 驱动轮质量 腿部质量 机体质量  （单位：kg）
+I_w_ac = (7630000)*10^(-7);                            % 驱动轮转动惯量               （单位：kg m^2）
+I_b_ac = 0.3470;                            % 机体转动惯量(自然坐标系法向)  （单位：kg m^2）
+I_z_ac = 0.322;                            % 机器人z轴转动惯量            （单位：kg m^2）
+
+%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%%%%
+
+% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉
+% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
+
+l_l_ac = 0.16;        % 左腿摆杆长度                      （左腿对应数据）  （单位：m）
+l_wl_ac = 0.10;       % 左驱动轮质心到左腿摆杆质心距离                      （单位：m）
+l_bl_ac = 0.4;       % 机体转轴到左腿摆杆质心距离                          （单位：m）
+I_ll_ac = 0.01886;      % 左腿摆杆转动惯量                                   （单位：kg m^2）
+l_r_ac = 0.16;        % 右腿摆杆长度                      （右腿对应数据）  （单位：m）
+l_wr_ac = 0.10;       % 右驱动轮质心到右腿摆杆质心距离                      （单位：m）
+l_br_ac = 0.4;       % 机体转轴到右腿摆杆质心距离                          （单位：m）
+I_lr_ac = 0.01886;      % 右腿摆杆转动惯量                                   （单位：kg m^2）
+
+% 机体转轴定义参考哈工程开源（https://zhuanlan.zhihu.com/p/563048952），是左右
+% 两侧两个关节电机之间的中间点相连所形成的轴
+% （如果目的是小板凳，考虑使左右腿相关数据一致）
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数数据集%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% 根据不同腿长长度，先针对左腿测量出对应的l_wl，l_bl，和I_ll
+% 通过以下方式记录数据: 矩阵分4列，
+% 第一列为左腿腿长范围区间中所有小数点精度0.01的长度，例如：0.09，0.18，单位：m
+% 第二列为l_wl，单位：m
+% 第三列为l_bl，单位：m
+% 第四列为I_ll，单位：kg m^2
+% （注意单位别搞错！）
+% 行数根据L_0范围区间（，单位cm时）的整数数量进行调整
+
+Leg_data_l = [0.11,  0.0480,  0.0620,  0.01819599;
+              0.12,  0.0470,  0.0730,  0.01862845;
+              0.13,  0.0476,  0.0824,  0.01898641;
+              0.14,  0.0480,  0.0920,  0.01931342;
+              0.15,  0.0490,  0.1010,  0.01962521;
+              0.16,  0.0500,  0.1100,  0.01993092;
+              0.17,  0.0510,  0.1190,  0.02023626;
+              0.18,  0.0525,  0.1275,  0.02054500;
+              0.19,  0.0539,  0.1361,  0.02085969;
+              0.20,  0.0554,  0.1446,  0.02118212;
+              0.21,  0.0570,  0.1530,  0.02151357;
+              0.22,  0.0586,  0.1614,  0.02185496;
+              0.23,  0.0600,  0.1700,  0.02220695;
+              0.24,  0.0621,  0.1779,  0.02256999;
+              0.25,  0.0639,  0.1861,  0.02294442;
+              0.26,  0.0657,  0.1943,  0.02333041;
+              0.27,  0.0676,  0.2024,  0.02372806;
+              0.28,  0.0700,  0.2100,  0.02413735;
+              0.29,  0.0713,  0.2187,  0.02455817;
+              0.30,  0.0733,  0.2267,  0.02499030];
+% 以上数据应通过实际测量或sw图纸获得
+
+% 由于左右腿部数据通常完全相同，我们通过复制的方式直接定义右腿的全部数据集
+% 矩阵分4列，第一列为右腿腿长范围区间中（，单位cm时）的整数腿长l_r*0.01，第二列为l_wr，第三列为l_br，第四列为I_lr）
+Leg_data_r = Leg_data_l;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%                           此处可以输入QR矩阵                           %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% 矩阵Q中，以下列分别对应：
+%        s     ds     phi     dphi     theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
+lqr_Q = [1,    0,     0,      0,       0,       0,        0,       0,        0,      0;
+         0,    2,     0,      0,       0,       0,        0,       0,        0,      0;
+         0,    0,     12000,      0,       0,       0,        0,       0,        0,      0;
+         0,    0,     0,      200,       0,       0,        0,       0,        0,      0;
+         0,    0,     0,      0,       1000,       0,        0,       0,        0,      0;
+         0,    0,     0,      0,       0,       1,        0,       0,        0,      0;
+         0,    0,     0,      0,       0,       0,        1000,       0,        0,      0;
+         0,    0,     0,      0,       0,       0,        0,       1,        0,      0;
+         0,    0,     0,      0,       0,       0,        0,       0,        20000,   0;
+         0,    0,     0,      0,       0,       0,        0,       0,        0,      1];
+% 其中：
+% s       : 自然坐标系下机器人水平方向移动距离，单位：m，ds为其导数
+% phi     ：机器人水平方向移动时yaw偏航角度，dphi为其导数
+% theta_ll：左腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_ll为其导数
+% theta_lr：右腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_lr为其导数
+% theta_b ：机体与自然坐标系水平夹角，dtheta_b为其导数
+
+% 矩阵中，以下列分别对应：
+%        T_wl    T_wr     T_bl     T_br
+lqr_R = [0.25,      0,       0,       0;
+         0,      0.25,       0,       0;
+         0,      0,       1.5,       0;
+         0,      0,       0,       1.5];
+% 其中：
+% T_wl: 左侧驱动轮输出力矩
+% T_wr：右侧驱动轮输出力矩
+% T_bl：左侧髋关节输出力矩
+% T_br：右腿髋关节输出力矩
+% 单位皆为Nm
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+
+%%%%%%%%%%%%%%%%%%%%%Step 2.5：求解矩阵（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%
+
+% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉，
+% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
+K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
+            R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
+            l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
+            A,B,lqr_Q,lqr_R)
+K = sprintf([strjoin(repmat({'%.5g'},1,size(K,2)),',  ') '\n'], K.')
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%Step 3：拟合控制律函数%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+sample_size = size(Leg_data_l,1)^2; % 单个K_ij拟合所需要的样本数
+
+length = size(Leg_data_l,1); % 测量腿部数据集的行数
+
+% 定义所有K_ij依据l_l,l_r变化的表格，每一个表格有3列，第一列是l_l，第二列
+% 是l_r，第三列是对应的K_ij的数值
+K_sample = zeros(sample_size,3,40); % 40是因为增益矩阵K应该是4行10列。
+
+for i = 1:length
+    for j = 1:length
+        index = (i - 1)*length + j;
+        l_l_ac = Leg_data_l(i,1); % 提取左腿对应的数据
+        l_wl_ac = Leg_data_l(i,2);
+        l_bl_ac = Leg_data_l(i,3);
+        I_ll_ac = Leg_data_l(i,4);
+        l_r_ac = Leg_data_r(j,1); % 提取右腿对应的数据
+        l_wr_ac = Leg_data_r(j,2);
+        l_br_ac = Leg_data_r(j,3);
+        I_lr_ac = Leg_data_r(j,4);
+        for k = 1:40
+            K_sample(index,1,k) = l_l_ac;
+            K_sample(index,2,k) = l_r_ac;
+        end
+        K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
+            R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
+            l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
+            A,B,lqr_Q,lqr_R);
+        % 根据指定的l_l,l_r输入对应的K_ij的数值
+        for l = 1:4
+            for m = 1:10
+                K_sample(index,3,(l - 1)*10 + m) = K(l,m);
+            end
+        end
+    end
+end
+
+% 创建收集全部K_ij的多项式拟合的全部系数的集合
+K_Fit_Coefficients = zeros(40,6);
+for n = 1:40
+    K_Surface_Fit = fit([K_sample(:,1,n),K_sample(:,2,n)],K_sample(:,3,n),'poly22');
+    K_Fit_Coefficients(n,:) = coeffvalues(K_Surface_Fit); % 拟合并提取出拟合的系数结果
+end
+Polynomial_expression = formula(K_Surface_Fit)
+
+% 最终返回的结果K_Fit_Coefficients是一个40行6列矩阵，每一行分别对应一个K_ij的多项式拟合的全部系数
+% 每一行和K_ij的对应关系如下：
+% - 第1行对应K_1,1
+% - 第14行对应K_2,4
+% - 第22行对应K_3,2
+% - 第37行对应K_4,7
+% ... 其他行对应关系类似
+% 拟合出的函数表达式为 p(x,y) = p00 + p10*x + p01*y + p20*x^2 + p11*x*y + p02*y^2
+% 其中x对应左腿腿长l_l，y对应右腿腿长l_r
+% K_Fit_Coefficients每一列分别对应全部K_ij的多项式拟合的单个系数
+% 每一列和系数pij的对应关系如下：
+% - 第1列对应p00
+% - 第2列对应p10
+% - 第3列对应p01
+% - 第4列对应p20
+% - 第5列对应p11
+% - 第6列对应p02
+K_Fit_Coefficients = sprintf([strjoin(repmat({'%.5g'},1,size(K_Fit_Coefficients,2)),',  ') '\n'], K_Fit_Coefficients.')
+
+% 正确食用方法：
+% 1.在C代码中写出控制律K矩阵的全部多项式，其中每一个多项式的表达式为：
+% p(l_l,l_r) = p00 + p10*l_l + p01*l_r + p20*l_l^2 + p11*l_l*l_r + p02*l_r^2
+% 2.并填入对应系数即可
+
+toc
+
+%%%%%%%%%%%%%%%%%%%%%%%%%以下信息仅供参考，可忽略%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% 如有需要可以把所有K_ij画出图来参考，可以去掉以下注释
+% 此版本只能同时查看其中一个K_ij，同时查看多个的功能下次更新
+% （前面的蛆，以后再来探索吧（bushi
+
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%得出控制矩阵K使用的函数%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
+            R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
+            l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
+            A,B,lqr_Q,lqr_R)
+    % 基于机体以及物理参数，获得控制矩阵A，B的全部数值
+    A_ac = subs(A,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ...
+        [R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ...
+        m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]);
+    B_ac = subs(B,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ...
+        [R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ...
+        m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]);
+
+    % 根据以上信息和提供的矩阵Q和R求解Riccati方程，获得增益矩阵K
+    % P为Riccati方程的解，矩阵L可以无视
+    [P,K,L_k] = icare(A_ac,B_ac,lqr_Q,lqr_R,[],[],[]);
+end
+

utils/lqr.m

@@ -0,0 +1,321 @@
+% v1：这份LQR程序是参考我之前写的哈工程LQR程序以及小周写的AB矩阵求解器优化后写出来的，感谢周神（2024/05/07）
+% v2：添加了可以专门调试定腿长的功能（2024/05/08）
+% v3：优化部分注释，添加单位说明（2024/05/15）
+% v4: 优化了输出，输出矩阵K的系数可以真正的复制到C里（2024/05/16）
+
+% 以下所有变量含义参考2023上交轮腿电控开源（https://bbs.robomaster.com/forum.php?mod=viewthread&tid=22756）所使用符号含义
+
+%%%%%%%%%%%%%%%%%%%%%%%%%Step 0：重置程序，定义变量%%%%%%%%%%%%%%%%%%%%%%%%%
+tic
+clear all
+clc
+
+% 定义机器人机体参数
+syms R_w                % 驱动轮半径
+syms R_l                % 驱动轮轮距/2
+syms l_l l_r            % 左右腿长
+syms l_wl l_wr          % 驱动轮质心到左右腿部质心距离
+syms l_bl l_br          % 机体质心到左右腿部质心距离
+syms l_c                % 机体质心到腿部关节中心点距离
+syms m_w m_l m_b        % 驱动轮质量 腿部质量 机体质量
+syms I_w                % 驱动轮转动惯量           (自然坐标系法向)
+syms I_ll I_lr          % 驱动轮左右腿部转动惯量    (自然坐标系法向，实际上会变化)
+syms I_b                % 机体转动惯量             (自然坐标系法向)
+syms I_z                % 机器人z轴转动惯量        (简化为常量)
+
+% 定义其他独立变量并补充其导数
+syms theta_wl   theta_wr   % 左右驱动轮转角
+syms dtheta_wl  dtheta_wr 
+syms ddtheta_wl ddtheta_wr ddtheta_ll ddtheta_lr ddtheta_b
+
+% 定义状态向量
+syms s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
+
+% 定义控制向量
+syms T_wl T_wr T_bl T_br
+
+% 输入物理参数：重力加速度
+syms g
+
+
+
+%%%%%%%%%%%%%%%%%%%%%%%Step 1：解方程，求控制矩阵A，B%%%%%%%%%%%%%%%%%%%%%%%
+
+% 通过原文方程组(3.11)-(3.15)，求出ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b表达式
+eqn1 = (I_w*l_l/R_w+m_w*R_w*l_l+m_l*R_w*l_bl)*ddtheta_wl+(m_l*l_wl*l_bl-I_ll)*ddtheta_ll+(m_l*l_wl+m_b*l_l/2)*g*theta_ll+T_bl-T_wl*(1+l_l/R_w)==0;
+eqn2 = (I_w*l_r/R_w+m_w*R_w*l_r+m_l*R_w*l_br)*ddtheta_wr+(m_l*l_wr*l_br-I_lr)*ddtheta_lr+(m_l*l_wr+m_b*l_r/2)*g*theta_lr+T_br-T_wr*(1+l_r/R_w)==0;
+eqn3 = -(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wl-(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wr-(m_l*R_w*l_wl+m_b*R_w*l_l/2)*ddtheta_ll-(m_l*R_w*l_wr+m_b*R_w*l_r/2)*ddtheta_lr+T_wl+T_wr==0;
+eqn4 = (m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wl+(m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wr+m_l*l_wl*l_c*ddtheta_ll+m_l*l_wr*l_c*ddtheta_lr-I_b*ddtheta_b+m_b*g*l_c*theta_b-(T_wl+T_wr)*l_c/R_w-(T_bl+T_br)==0;
+eqn5 = ((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wl-((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wr+(I_z*l_l)/(2*R_l)*ddtheta_ll-(I_z*l_r)/(2*R_l)*ddtheta_lr-T_wl*R_l/R_w+T_wr*R_l/R_w==0;
+[ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b] = solve(eqn1,eqn2,eqn3,eqn4,eqn5,ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b);
+
+
+% 通过计算雅可比矩阵的方法得出控制矩阵A，B所需要的全部偏导数
+J_A = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[theta_ll,theta_lr,theta_b]);
+J_B = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[T_wl,T_wr,T_bl,T_br]);
+
+% 定义矩阵A，B，将指定位置的数值根据上述偏导数计算出来并填入
+A = sym('A',[10 10]);
+B = sym('B',[10 4]);
+
+% 填入A数据：a25,a27,a29,a45,a47,a49,a65,a67,a69,a85,a87,a89,a105,a107,a109
+for p = 5:2:9
+    A_index = (p - 3)/2;
+    A(2,p) = R_w*(J_A(1,A_index) + J_A(2,A_index))/2;
+    A(4,p) = (R_w*(- J_A(1,A_index) + J_A(2,A_index)))/(2*R_l) - (l_l*J_A(3,A_index))/(2*R_l) + (l_r*J_A(4,A_index))/(2*R_l);
+    for q = 6:2:10
+        A(q,p) = J_A(q/2,A_index);
+    end
+end
+
+% A的以下数值为1：a12,a34,a56,a78,a910，其余数值为0
+for r = 1:10
+    if rem(r,2) == 0
+        A(r,1) = 0; A(r,2) = 0; A(r,3) = 0; A(r,4) = 0; A(r,6) = 0; A(r,8) = 0; A(r,10) = 0;
+    else
+        A(r,:) = zeros(1,10);
+        A(r,r+1) = 1;
+    end
+end
+
+% 填入B数据：b21,b22,b23,b24,b41,b42,b43,b44,b61,b62,b63,b64,b81,b82,b83,b84,b101,b102,b103,b104,
+for h = 1:4
+    B(2,h) = R_w*(J_B(1,h) + J_B(2,h))/2;
+    B(4,h) = (R_w*(- J_B(1,h) + J_B(2,h)))/(2*R_l) - (l_l*J_B(3,h))/(2*R_l) + (l_r*J_B(4,h))/(2*R_l);
+    for f = 6:2:10
+        B(f,h) = J_B(f/2,h);
+    end
+end
+
+% B的其余数值为0
+for e = 1:2:9
+    B(e,:) = zeros(1,4);
+end
+
+
+
+%%%%%%%%%%%%%%%%%%%%%Step 2：输入参数（可以修改的部分）%%%%%%%%%%%%%%%%%%%%%
+
+% 物理参数赋值（唯一此处不可改变！），后面的数据通过增加后缀_ac区分模型符号和实际数据
+g_ac = 9.81;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%                      此处可以输入机器人机体基本参数                      %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人机体与轮部参数%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+R_w_ac = 0.77;                           % 驱动轮半径                  （单位：m）
+R_l_ac = 0.210;                            % 两个驱动轮之间距离/2         （单位：m）
+l_c_ac = 0.025;                            % 机体质心到腿部关节中心点距离  （单位：m）
+m_w_ac = 0.5; m_l_ac = 2.133; m_b_ac = 4.542;    % 驱动轮质量 腿部质量 机体质量  （单位：kg）
+I_w_ac = (7630000)*10^(-7);                            % 驱动轮转动惯量               （单位：kg m^2）
+I_b_ac = 0.3470;                            % 机体转动惯量(自然坐标系法向)  （单位：kg m^2）
+I_z_ac = 0.322;                            % 机器人z轴转动惯量            （单位：kg m^2）
+
+%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%%%%
+
+% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉
+% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
+
+l_l_ac = 0.16;        % 左腿摆杆长度                      （左腿对应数据）  （单位：m）
+l_wl_ac = 0.10;       % 左驱动轮质心到左腿摆杆质心距离                      （单位：m）
+l_bl_ac = 0.4;       % 机体转轴到左腿摆杆质心距离                          （单位：m）
+I_ll_ac = 0.01886;      % 左腿摆杆转动惯量                                   （单位：kg m^2）
+l_r_ac = 0.16;        % 右腿摆杆长度                      （右腿对应数据）  （单位：m）
+l_wr_ac = 0.10;       % 右驱动轮质心到右腿摆杆质心距离                      （单位：m）
+l_br_ac = 0.4;       % 机体转轴到右腿摆杆质心距离                          （单位：m）
+I_lr_ac = 0.01886;      % 右腿摆杆转动惯量                                   （单位：kg m^2）
+
+% 机体转轴定义参考哈工程开源（https://zhuanlan.zhihu.com/p/563048952），是左右
+% 两侧两个关节电机之间的中间点相连所形成的轴
+% （如果目的是小板凳，考虑使左右腿相关数据一致）
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数数据集%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% 根据不同腿长长度，先针对左腿测量出对应的l_wl，l_bl，和I_ll
+% 通过以下方式记录数据: 矩阵分4列，
+% 第一列为左腿腿长范围区间中所有小数点精度0.01的长度，例如：0.09，0.18，单位：m
+% 第二列为l_wl，单位：m
+% 第三列为l_bl，单位：m
+% 第四列为I_ll，单位：kg m^2
+% （注意单位别搞错！）
+% 行数根据L_0范围区间（，单位cm时）的整数数量进行调整
+
+Leg_data_l = [0.11,  0.0480,  0.0620,  0.01819599;
+              0.12,  0.0470,  0.0730,  0.01862845;
+              0.13,  0.0476,  0.0824,  0.01898641;
+              0.14,  0.0480,  0.0920,  0.01931342;
+              0.15,  0.0490,  0.1010,  0.01962521;
+              0.16,  0.0500,  0.1100,  0.01993092;
+              0.17,  0.0510,  0.1190,  0.02023626;
+              0.18,  0.0525,  0.1275,  0.02054500;
+              0.19,  0.0539,  0.1361,  0.02085969;
+              0.20,  0.0554,  0.1446,  0.02118212;
+              0.21,  0.0570,  0.1530,  0.02151357;
+              0.22,  0.0586,  0.1614,  0.02185496;
+              0.23,  0.0600,  0.1700,  0.02220695;
+              0.24,  0.0621,  0.1779,  0.02256999;
+              0.25,  0.0639,  0.1861,  0.02294442;
+              0.26,  0.0657,  0.1943,  0.02333041;
+              0.27,  0.0676,  0.2024,  0.02372806;
+              0.28,  0.0700,  0.2100,  0.02413735;
+              0.29,  0.0713,  0.2187,  0.02455817;
+              0.30,  0.0733,  0.2267,  0.02499030];
+% 以上数据应通过实际测量或sw图纸获得
+
+% 由于左右腿部数据通常完全相同，我们通过复制的方式直接定义右腿的全部数据集
+% 矩阵分4列，第一列为右腿腿长范围区间中（，单位cm时）的整数腿长l_r*0.01，第二列为l_wr，第三列为l_br，第四列为I_lr）
+Leg_data_r = Leg_data_l;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%                           此处可以输入QR矩阵                           %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% 矩阵Q中，以下列分别对应：
+%        s     ds     phi     dphi     theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
+lqr_Q = [1,    0,     0,      0,       0,       0,        0,       0,        0,      0;
+         0,    2,     0,      0,       0,       0,        0,       0,        0,      0;
+         0,    0,     12000,      0,       0,       0,        0,       0,        0,      0;
+         0,    0,     0,      200,       0,       0,        0,       0,        0,      0;
+         0,    0,     0,      0,       1000,       0,        0,       0,        0,      0;
+         0,    0,     0,      0,       0,       1,        0,       0,        0,      0;
+         0,    0,     0,      0,       0,       0,        1000,       0,        0,      0;
+         0,    0,     0,      0,       0,       0,        0,       1,        0,      0;
+         0,    0,     0,      0,       0,       0,        0,       0,        20000,   0;
+         0,    0,     0,      0,       0,       0,        0,       0,        0,      1];
+% 其中：
+% s       : 自然坐标系下机器人水平方向移动距离，单位：m，ds为其导数
+% phi     ：机器人水平方向移动时yaw偏航角度，dphi为其导数
+% theta_ll：左腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_ll为其导数
+% theta_lr：右腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_lr为其导数
+% theta_b ：机体与自然坐标系水平夹角，dtheta_b为其导数
+
+% 矩阵中，以下列分别对应：
+%        T_wl    T_wr     T_bl     T_br
+lqr_R = [0.25,      0,       0,       0;
+         0,      0.25,       0,       0;
+         0,      0,       1.5,       0;
+         0,      0,       0,       1.5];
+% 其中：
+% T_wl: 左侧驱动轮输出力矩
+% T_wr：右侧驱动轮输出力矩
+% T_bl：左侧髋关节输出力矩
+% T_br：右腿髋关节输出力矩
+% 单位皆为Nm
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+
+%%%%%%%%%%%%%%%%%%%%%Step 2.5：求解矩阵（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%
+
+% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉，
+% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
+K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
+            R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
+            l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
+            A,B,lqr_Q,lqr_R)
+K = sprintf([strjoin(repmat({'%.5g'},1,size(K,2)),',  ') '\n'], K.')
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%Step 3：拟合控制律函数%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+sample_size = size(Leg_data_l,1)^2; % 单个K_ij拟合所需要的样本数
+
+length = size(Leg_data_l,1); % 测量腿部数据集的行数
+
+% 定义所有K_ij依据l_l,l_r变化的表格，每一个表格有3列，第一列是l_l，第二列
+% 是l_r，第三列是对应的K_ij的数值
+K_sample = zeros(sample_size,3,40); % 40是因为增益矩阵K应该是4行10列。
+
+for i = 1:length
+    for j = 1:length
+        index = (i - 1)*length + j;
+        l_l_ac = Leg_data_l(i,1); % 提取左腿对应的数据
+        l_wl_ac = Leg_data_l(i,2);
+        l_bl_ac = Leg_data_l(i,3);
+        I_ll_ac = Leg_data_l(i,4);
+        l_r_ac = Leg_data_r(j,1); % 提取右腿对应的数据
+        l_wr_ac = Leg_data_r(j,2);
+        l_br_ac = Leg_data_r(j,3);
+        I_lr_ac = Leg_data_r(j,4);
+        for k = 1:40
+            K_sample(index,1,k) = l_l_ac;
+            K_sample(index,2,k) = l_r_ac;
+        end
+        K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
+            R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
+            l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
+            A,B,lqr_Q,lqr_R);
+        % 根据指定的l_l,l_r输入对应的K_ij的数值
+        for l = 1:4
+            for m = 1:10
+                K_sample(index,3,(l - 1)*10 + m) = K(l,m);
+            end
+        end
+    end
+end
+
+% 创建收集全部K_ij的多项式拟合的全部系数的集合
+K_Fit_Coefficients = zeros(40,6);
+for n = 1:40
+    K_Surface_Fit = fit([K_sample(:,1,n),K_sample(:,2,n)],K_sample(:,3,n),'poly22');
+    K_Fit_Coefficients(n,:) = coeffvalues(K_Surface_Fit); % 拟合并提取出拟合的系数结果
+end
+Polynomial_expression = formula(K_Surface_Fit)
+
+% 最终返回的结果K_Fit_Coefficients是一个40行6列矩阵，每一行分别对应一个K_ij的多项式拟合的全部系数
+% 每一行和K_ij的对应关系如下：
+% - 第1行对应K_1,1
+% - 第14行对应K_2,4
+% - 第22行对应K_3,2
+% - 第37行对应K_4,7
+% ... 其他行对应关系类似
+% 拟合出的函数表达式为 p(x,y) = p00 + p10*x + p01*y + p20*x^2 + p11*x*y + p02*y^2
+% 其中x对应左腿腿长l_l，y对应右腿腿长l_r
+% K_Fit_Coefficients每一列分别对应全部K_ij的多项式拟合的单个系数
+% 每一列和系数pij的对应关系如下：
+% - 第1列对应p00
+% - 第2列对应p10
+% - 第3列对应p01
+% - 第4列对应p20
+% - 第5列对应p11
+% - 第6列对应p02
+K_Fit_Coefficients = sprintf([strjoin(repmat({'%.5g'},1,size(K_Fit_Coefficients,2)),',  ') '\n'], K_Fit_Coefficients.')
+
+% 正确食用方法：
+% 1.在C代码中写出控制律K矩阵的全部多项式，其中每一个多项式的表达式为：
+% p(l_l,l_r) = p00 + p10*l_l + p01*l_r + p20*l_l^2 + p11*l_l*l_r + p02*l_r^2
+% 2.并填入对应系数即可
+
+toc
+
+%%%%%%%%%%%%%%%%%%%%%%%%%以下信息仅供参考，可忽略%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% 如有需要可以把所有K_ij画出图来参考，可以去掉以下注释
+% 此版本只能同时查看其中一个K_ij，同时查看多个的功能下次更新
+% （前面的蛆，以后再来探索吧（bushi
+
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%得出控制矩阵K使用的函数%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
+            R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
+            l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
+            A,B,lqr_Q,lqr_R)
+    % 基于机体以及物理参数，获得控制矩阵A，B的全部数值
+    A_ac = subs(A,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ...
+        [R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ...
+        m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]);
+    B_ac = subs(B,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ...
+        [R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ...
+        m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]);
+
+    % 根据以上信息和提供的矩阵Q和R求解Riccati方程，获得增益矩阵K
+    % P为Riccati方程的解，矩阵L可以无视
+    [P,K,L_k] = icare(A_ac,B_ac,lqr_Q,lqr_R,[],[],[]);
+end
+

utils/matlab/LQR_calc.m

@@ -0,0 +1,86 @@
+clear;
+clc;
+
+syms x xd xdd     T Tp    thetadd thetad theta    phidd phid phi    P N PM NM    L LM;
+
+%% 参数设定
+% 均为标准单位制
+g = 9.81;  
+
+% 驱动轮
+R = 0.075;          %轮子半径
+mw = 0.58;          %轮子质量
+Iw = 0.00823;       %轮子转动惯量
+
+% 大腿
+l_active_leg = 0.14;
+m_active_leg = 0.174;
+% 小腿
+l_slave_leg = 0.24;
+m_slave_leg = 0.180;
+% 关节间距
+joint_distance = 0.11;
+% 摆杆
+mp = (m_active_leg + m_slave_leg)*2 + 0.728;  % 需要加上定子质量
+Ip = mp*L^2/3;  %摆杆转动惯量
+
+% 机体
+M = 12;        %机体重量
+l = -0.014;    %机体质心到关节电机转轴的距离
+IM = 0.124;
+
+%% 经典力学方程
+fu1=N-NM==mp*(xdd+L*(thetadd*cos(theta)-thetad*thetad*sin(theta)));
+fu2=P-PM-mp*g==mp*L*(-thetadd*sin(theta)-thetad*thetad*cos(theta));
+fu3=NM==M*(xdd+(L+LM)*(thetadd*cos(theta)-thetad*thetad*sin(theta))-l*(phidd*cos(phi)-phid*phid*sin(phi)));
+fu4=PM-M*g==M*((L+LM)*(-thetadd*sin(theta)-thetad*thetad*cos(theta))+l*(-phidd*sin(phi)-phid*phid*cos(phi)));
+
+%% 不同部件之间的力求解
+[N,NM,P,PM]=solve(fu1,fu2,fu3,fu4,N,NM,P,PM);
+f1=xdd==(T-N*R)/(Iw/R+mw*R);
+f2=Ip*thetadd==(P*L+PM*LM)*sin(theta)-(N*L+NM*LM)*cos(theta)-T+Tp;
+f3=IM*phidd==Tp+NM*l*cos(phi)+PM*l*sin(phi);
+[xdd,thetadd,phidd]=solve(f1,f2,f3,xdd,thetadd,phidd);
+
+%% 计算雅可比矩阵A and B
+func=[thetad,thetadd,xd,xdd,phid,phidd];
+A_lin_model=jacobian(func,[theta,thetad,x,xd,phi,phid]);
+temp_A=subs(A_lin_model,[theta,thetad,xd,phi,phid],zeros(1,5));
+final_A=simplify(temp_A);
+
+B_lin_model=jacobian(func,[T Tp]);
+temp_B=subs(B_lin_model,[theta,thetad,xd,phi,phid],zeros(1,5));
+final_B=simplify(temp_B);
+
+%% 计算不同腿长下LQR增益K
+L_var = 0.1;   % 腿质心到机体转轴距离
+
+Q_mat = diag([1,1,500,100,5000,1]);
+R_mat = diag([1,0.25]);
+K = zeros(20,12);
+leg_len = zeros(20,1);
+
+for i=1:20
+    L_var = L_var + 0.005;
+    leg_len(i) = L_var*2;
+    A = double(subs(final_A, [L LM], [L_var L_var]));
+    B = double(subs(final_B, [L LM], [L_var L_var]));
+    KK = lqrd(A, B, Q_mat, R_mat, 0.001);
+    KK_t=KK.';
+    K(i,:)=KK_t(:);
+end
+
+%% 不同腿长下二项式拟合K
+K_cons=zeros(12,3);  
+
+for i=1:12
+    res=fit(leg_len,K(:,i),"poly2");
+    K_cons(i,:)=[res.p1, res.p2, res.p3];
+end
+
+for j=1:12
+    for i=1:3
+        fprintf("%f,",K_cons(j,i));
+    end
+    fprintf("\n");
+end

utils/matlab/UseBodyVel.m

@@ -0,0 +1,103 @@
+clear;
+clc;
+syms theta phi L x x_b N N_f T T_p  M N_M P_M L_M
+syms theta_dot x_dot phi_dot theta_ddot x_ddot phi_ddot
+syms x_b_dot x_b_ddot
+
+%% 参数设定
+% 均为标准单位制
+g = 9.81;           %重力加速度
+
+% 驱动轮
+mw = 0.58;          %轮子质量
+R = 0.075;          %轮子半径
+Iw = 0.00823;       %轮子转动惯量
+
+% 大腿
+l_active_leg = 0.14;
+m_active_leg = 0.174;
+% 小腿
+l_slave_leg = 0.24;
+m_slave_leg = 0.180;
+% 关节间距
+joint_distance = 0.11;
+% 摆杆
+mp = (m_active_leg + m_slave_leg)*2 + 0.728;
+Ip = mp*L^2/3;   % 摆杆转动惯量
+
+% 机体
+M = 12;         %机体重量
+IM = 0.124;     %机体惯量,绕质心
+l = -0.014;     %机体质心到电机转轴的距离
+
+% QR设置为相同的权重
+Q_cost = diag([1,1,500,100,5000,1]);
+R_cost = diag([1,0.25]);
+useBodyVelocity = 1;
+
+%% 经典力学方程
+if useBodyVelocity
+    x_ddot = x_b_ddot - (L+L_M)*cos(theta)*theta_ddot+ (L+L_M)*sin(theta)*theta_dot^2;
+end
+N_M = M*(x_ddot+(L+L_M)*theta_ddot*cos(theta)-(L+L_M)*theta_dot^2*sin(theta)-l*phi_ddot*cos(phi)+l*phi_dot^2*sin(phi));
+P_M = M*(g-(L+L_M)*theta_ddot*sin(theta)-(L+L_M)*theta_dot^2*cos(theta)-l*phi_ddot*sin(phi)-l*phi_dot^2*cos(phi));
+N = mp*(x_ddot+L*theta_ddot*cos(theta)-L*theta_dot^2*sin(theta))+N_M;
+P = mp*(g-L*theta_dot^2*cos(theta)-L*theta_ddot*sin(theta))+P_M;
+
+eqA = x_ddot == (T-N*R)/(Iw/R+mw*R);
+eqB = Ip*theta_ddot == (P*L+P_M*L_M)*sin(theta)-(N*L+N_M*L_M)*cos(theta) - T + T_p;
+eqC = IM*phi_ddot == T_p + N_M*l*cos(phi) + P_M*l*sin(phi);
+
+%% 计算雅可比矩阵
+U = [T T_p].';
+
+if useBodyVelocity
+    model_sol = solve([eqA eqB eqC],[theta_ddot,x_b_ddot,phi_ddot]);
+    X = [theta,theta_dot,x_b,x_b_dot,phi,phi_dot].';
+    dX = [theta_dot,simplify(model_sol.theta_ddot),...
+        x_b_dot,simplify(model_sol.x_b_ddot),...
+        phi_dot,simplify(model_sol.phi_ddot)].';
+    A_sym = subs(jacobian(dX,X),[theta theta_dot x_b_dot phi phi_dot],zeros(1,5));
+    B_sym = subs(jacobian(dX,U),[theta theta_dot x_b_dot phi phi_dot],zeros(1,5));
+else
+    model_sol = solve([eqA eqB eqC],[theta_ddot,x_ddot,phi_ddot]);
+    X = [theta,theta_dot,x,x_dot,phi,phi_dot].';
+    dX = [theta_dot,simplify(model_sol.theta_ddot),...
+        x_dot,simplify(model_sol.x_ddot),...
+        phi_dot,simplify(model_sol.phi_ddot)].';
+    A_sym = subs(jacobian(dX,X),[theta theta_dot x_dot phi phi_dot],zeros(1,5));
+    B_sym = subs(jacobian(dX,U),[theta theta_dot x_dot phi phi_dot],zeros(1,5));
+end
+
+%% 计算变长度LQR
+L_var = 0.1;   % 腿质心到机体转轴距离
+
+K=zeros(20,12);
+leglen=zeros(20,1);
+
+for i=1:20
+    L_var=L_var+0.005; % 10mm线性化一次
+    leglen(i)=L_var*2;
+    trans_A=double(subs(A_sym,[L L_M],[L_var L_var]));
+    trans_B=double(subs(B_sym,[L L_M],[L_var L_var]));
+    KK=lqrd(trans_A,trans_B,Q_cost,R_cost,0.001);
+    KK_t=KK.';
+    K(i,:)=KK_t(:);
+end
+
+%% 用二项式拟合K,一共12个参数
+K_cons=zeros(12,3);
+
+for i=1:12
+    res=fit(leglen,K(:,i),"poly2");
+    K_cons(i,:)=[res.p1,res.p2,res.p3];
+end
+
+for j=1:12
+    for i=1:3
+        fprintf("%f,",K_cons(j,i));
+    end
+    fprintf("\n");
+end
+
+

utils/matlab/vmc.m

@@ -0,0 +1,39 @@
+clear;
+clc;
+
+syms phi1(t) phi2(t) phi3(t) phi4(t) l5 phi0 L0 T_Leg F_Leg
+syms phi_dot_1 phi_dot_4
+syms l1 l2 l3 l4
+
+%% 腿长解算
+x_B = l1*cos(phi1);
+y_B = l1*sin(phi1);
+x_C = x_B+l2*cos(phi2);
+y_C = y_B+l2*sin(phi2);
+x_D = l5+l4*cos(phi4);
+y_D = l4*sin(phi4);
+x_dot_B = diff(x_B,t);
+y_dot_B = diff(y_B,t);
+x_dot_C = diff(x_C,t);
+y_dot_C = diff(y_C,t);
+x_dot_D = diff(x_D,t);
+y_dot_D = diff(y_D,t);
+
+%% 速度导数求解
+phi_dot_2 = ((x_dot_D-x_dot_B)*cos(phi3)+(y_dot_D-y_dot_B)*sin(phi3))/l2/sin(phi3-phi2);
+
+x_dot_C = subs(x_dot_C,diff(phi2,t),phi_dot_2);
+x_dot_C = subs(x_dot_C,[diff(phi1,t),diff(phi4,t)],[phi_dot_1,phi_dot_4]);
+y_dot_C = subs(y_dot_C,diff(phi2,t),phi_dot_2);
+y_dot_C = subs(y_dot_C,[diff(phi1,t),diff(phi4,t)],[phi_dot_1,phi_dot_4]);
+
+%% 运动映射(虚功原理)+极坐标转换
+x_dot = [x_dot_C; y_dot_C];
+q_dot = [phi_dot_1; phi_dot_4];
+x_dot = collect(simplify(collect(x_dot,q_dot)),q_dot);
+J = simplify(jacobian(x_dot,q_dot));
+rotate = [cos(phi0-pi/2) -sin(phi0-pi/2);
+          sin(phi0-pi/2)  cos(phi0-pi/2)];
+map = [0 -1/L0;
+       1 0];
+Trans_Jacobian = simplify(J.'*rotate*map);

utils/vmc.m

@@ -0,0 +1,219 @@
+function [tau_hip, tau_knee] = vmc_control(L1, L2, q1, q2, F_virtual, theta_virtual)
+% VMC (Virtual Model Control) for serial leg mechanism
+% 基于虚拟模型控制的串联腿机构力矩计算
+%
+% 输入参数:
+%   L1: 大腿长度 (m)
+%   L2: 小腿长度 (m) 
+%   q1: 髋关节角度 (rad)
+%   q2: 膝关节角度 (rad)
+%   F_virtual: 虚拟力大小 (N)
+%   theta_virtual: 虚拟力方向角度 (rad)
+%
+% 输出:
+%   tau_hip: 髋关节所需力矩 (N*m)
+%   tau_knee: 膝关节所需力矩 (N*m)
+
+    % 计算雅可比矩阵
+    J = compute_jacobian(L1, L2, q1, q2);
+    
+    % 虚拟力向量 (笛卡尔空间)
+    F_cartesian = [F_virtual * cos(theta_virtual);
+                   F_virtual * sin(theta_virtual)];
+    
+    % 通过雅可比转置将笛卡尔力转换为关节力矩
+    tau = J' * F_cartesian;
+    
+    tau_hip = tau(1);
+    tau_knee = tau(2);
+end
+
+function J = compute_jacobian(L1, L2, q1, q2)
+% 计算串联腿机构的雅可比矩阵
+% 从关节空间到足端笛卡尔空间的映射
+%
+% 输入参数:
+%   L1: 大腿长度 (m)
+%   L2: 小腿长度 (m)
+%   q1: 髋关节角度 (rad)
+%   q2: 膝关节角度 (rad)
+%
+% 输出:
+%   J: 2x2雅可比矩阵
+
+    % 足端位置 (相对于髋关节)
+    % x = L1*cos(q1) + L2*cos(q1+q2)
+    % y = L1*sin(q1) + L2*sin(q1+q2)
+    
+    % 雅可比矩阵元素
+    J11 = -L1*sin(q1) - L2*sin(q1+q2);  % dx/dq1
+    J12 = -L2*sin(q1+q2);               % dx/dq2
+    J21 = L1*cos(q1) + L2*cos(q1+q2);   % dy/dq1
+    J22 = L2*cos(q1+q2);                % dy/dq2
+    
+    J = [J11, J12;
+         J21, J22];
+end
+
+function [x, y] = forward_kinematics(L1, L2, q1, q2)
+% 串联腿正运动学 - 足端位置
+%
+% 输入参数:
+%   L1: 大腿长度 (m)
+%   L2: 小腿长度 (m)
+%   q1: 髋关节角度 (rad)
+%   q2: 膝关节角度 (rad)
+%
+% 输出:
+%   x: 足端x坐标 (m)
+%   y: 足端y坐标 (m)
+
+    x = L1*cos(q1) + L2*cos(q1+q2);
+    y = L1*sin(q1) + L2*sin(q1+q2);
+end
+
+function [q1, q2] = inverse_kinematics(L1, L2, x, y)
+% 串联腿逆运动学 - 关节角度
+%
+% 输入参数:
+%   L1: 大腿长度 (m)
+%   L2: 小腿长度 (m)
+%   x: 足端目标x坐标 (m)
+%   y: 足端目标y坐标 (m)
+%
+% 输出:
+%   q1: 髋关节角度 (rad)
+%   q2: 膝关节角度 (rad)
+
+    % 计算到足端的距离
+    r = sqrt(x^2 + y^2);
+    
+    % 检查工作空间
+    if r > (L1 + L2) || r < abs(L1 - L2)
+        error('目标位置超出工作空间范围');
+    end
+    
+    % 使用余弦定理计算膝关节角度
+    cos_q2 = (r^2 - L1^2 - L2^2) / (2*L1*L2);
+    q2 = acos(cos_q2);  % 肘部向上的解
+    
+    % 计算髋关节角度
+    alpha = atan2(y, x);
+    beta = atan2(L2*sin(q2), L1 + L2*cos(q2));
+    q1 = alpha - beta;
+end
+
+function [angle_equiv, length_equiv] = serial_to_virtual_leg(L1, L2, q1, q2)
+% 将串联腿转换为等效摆动杆表示 (对应C代码中的正运动学)
+%
+% 输入参数:
+%   L1: 大腿长度 (m)
+%   L2: 小腿长度 (m)
+%   q1: 髋关节角度 (rad)
+%   q2: 膝关节角度 (rad)
+%
+% 输出:
+%   angle_equiv: 等效摆动杆角度 (rad)
+%   length_equiv: 等效摆动杆长度 (m)
+
+    % 对应C代码中的计算
+    q4 = (pi - q1 - q2) / 2;
+    
+    % 使用余弦定理求解等效摆动杆长度
+    a = 1.0;
+    b = -2.0 * L1 * cos(q4);
+    c = L1^2 - L2^2;
+    
+    discriminant = b^2 - 4*a*c;
+    
+    if discriminant < 0
+        error('无实数解，配置不可达');
+    end
+    
+    sqrt_discriminant = sqrt(discriminant);
+    L3_1 = (-b + sqrt_discriminant) / (2*a);
+    L3_2 = (-b - sqrt_discriminant) / (2*a);
+    
+    % 选择正的解
+    if L3_1 > 0 && L3_2 > 0
+        L3 = min(L3_1, L3_2);  % 选择较小的解
+    elseif L3_1 > 0
+        L3 = L3_1;
+    elseif L3_2 > 0
+        L3 = L3_2;
+    else
+        error('无正解');
+    end
+    
+    angle_equiv = q1 + q4;
+    length_equiv = L3;
+end
+
+function [q1, q2] = virtual_to_serial_leg(L1, L2, angle_equiv, length_equiv)
+% 将等效摆动杆转换为串联腿表示 (对应C代码中的逆运动学)
+%
+% 输入参数:
+%   L1: 大腿长度 (m)
+%   L2: 小腿长度 (m)
+%   angle_equiv: 等效摆动杆角度 (rad)
+%   length_equiv: 等效摆动杆长度 (m)
+%
+% 输出:
+%   q1: 髋关节角度 (rad)
+%   q2: 膝关节角度 (rad)
+
+    h = length_equiv;
+    q = angle_equiv;
+    
+    % 计算cos(q4)
+    cos_q4 = (L1^2 + h^2 - L2^2) / (2.0 * L1 * h);
+    
+    % 检查范围
+    if cos_q4 < -1.0 || cos_q4 > 1.0
+        error('不可达配置');
+    end
+    
+    q4 = acos(cos_q4);
+    
+    % 计算关节角度
+    q1 = q - q4;
+    q2 = pi - 2.0 * q4 - q1;
+end
+
+% 示例使用函数
+function demo_vmc_control()
+% VMC控制示例
+    
+    % 腿部参数
+    L1 = 0.15;  % 大腿长度 15cm
+    L2 = 0.15;  % 小腿长度 15cm
+    
+    % 当前关节角度
+    q1 = pi/6;   % 30度
+    q2 = pi/3;   % 60度
+    
+    % 期望的虚拟力
+    F_virtual = 50;  % 50N
+    theta_virtual = -pi/2;  % 向下的力
+    
+    % 计算所需关节力矩
+    [tau_hip, tau_knee] = vmc_control(L1, L2, q1, q2, F_virtual, theta_virtual);
+    
+    fprintf('髋关节所需力矩: %.3f N*m\n', tau_hip);
+    fprintf('膝关节所需力矩: %.3f N*m\n', tau_knee);
+    
+    % 显示雅可比矩阵
+    J = compute_jacobian(L1, L2, q1, q2);
+    fprintf('雅可比矩阵:\n');
+    disp(J);
+    
+    % 测试等效摆动杆转换
+    [angle_eq, length_eq] = serial_to_virtual_leg(L1, L2, q1, q2);
+    fprintf('等效摆动杆角度: %.3f rad (%.1f deg)\n', angle_eq, rad2deg(angle_eq));
+    fprintf('等效摆动杆长度: %.3f m\n', length_eq);
+    
+    % 验证逆变换
+    [q1_back, q2_back] = virtual_to_serial_leg(L1, L2, angle_eq, length_eq);
+    fprintf('验证 - 原始角度: q1=%.3f, q2=%.3f\n', q1, q2);
+    fprintf('验证 - 恢复角度: q1=%.3f, q2=%.3f\n', q1_back, q2_back);
+end