给底盘添加yaw

User/module/balance_chassis.c

@@ -165,13 +165,6 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) {
   c->feedback.imu.gyro = virtual_chassis.data.imu.gyro;
   c->feedback.imu.euler = virtual_chassis.data.imu.euler;
 
-  /* 更新云台电机反馈数据（yaw轴） */
-  MOTOR_DM_Update(&(c->param->yaw_motor));
-  MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(c->param->yaw_motor));
-  if (dm_motor != NULL) {
-    c->feedback.yaw = dm_motor->motor.feedback;
-  }
-
   // 更新机体状态估计
   Chassis_UpdateChassisState(c);
 
@@ -302,8 +295,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     current_state.d_theta = c->vmc_[0].leg.d_theta;
     current_state.x = xhat;
     current_state.d_x = x_dot_hat;
-    current_state.phi = -c->feedback.imu.euler.pit;
+    current_state.phi = c->feedback.imu.euler.pit;
-    current_state.d_phi = -c->feedback.imu.gyro.y;
+    current_state.d_phi = c->feedback.imu.gyro.y;
 
     LQR_UpdateState(&c->lqr[0], &current_state);
 
@@ -319,7 +312,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     LQR_State_t target_state = {0};
     target_state.theta = 0.0f;   // 目标摆杆角度
     target_state.d_theta = 0.0f; // 目标摆杆角速度
-    target_state.phi = -0.2f;  // 目标俯仰角
+    target_state.phi = 0.0f;  // 目标俯仰角
     target_state.d_phi = 0.0f; // 目标俯仰角速度
     target_state.x = c->chassis_state.target_x; // 目标位置
     target_state.d_x = target_dot_x;            // 目标速度
@@ -339,10 +332,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
                c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
 
   /* 轮毂力矩输出（参考C++版本的减速比） */
-  // c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force;
+  c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force;
-  // c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force;
+  c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force;
-  c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f;
+  // c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f;
-  c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f;
+  // c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f;
   /* 腿长控制和VMC逆解算（使用PID控制） */
   float virtual_force[2];
   float target_L0[2];
@@ -384,7 +377,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
                                 c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
 
     // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 - 横滚角补偿力（左腿减少支撑力）
-    virtual_force[0] = (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + pid_output + 10.0f ;
+    virtual_force[0] = (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + pid_output + 40.0f ;
 
     // VMC逆解算（包含摆角补偿）
     if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
@@ -405,7 +398,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
                                 c->vmc_[1].leg.L0, leg_d_length[1], c->dt);
 
     // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 + 横滚角补偿力（右腿增加支撑力）
-    virtual_force[1] = (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) + pid_output + 10.0f ;
+    virtual_force[1] = (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) + pid_output + 40.0f ;
 
     // VMC逆解算（包含摆角补偿）
     if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],

User/task/ctrl_chassis.c

@@ -58,6 +58,7 @@ void Task_ctrl_chassis(void *argument) {
     if (osMessageQueueGet(task_runtime.msgq.Chassis_cmd, &chassis_cmd, NULL, 0) != osOK) {
         // 没有新命令，保持上次命令
     }
+    osMessageQueueGet(task_runtime.msgq.chassis_yaw, &chassis.feedback.yaw, NULL, 0);
  
     Chassis_UpdateFeedback(&chassis);
 

User/task/ctrl_gimbal.c

@@ -4,6 +4,7 @@
 */
 
 /* Includes ----------------------------------------------------------------- */
+#include "cmsis_os2.h"
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
 #include "component/ahrs.h"
@@ -45,11 +46,16 @@ void Task_ctrl_gimbal(void *argument) {
     }
     osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0);
 
+
     Gimbal_UpdateFeedback(&gimbal);
 
+    // osMessageQueueReset(task_runtime.msgq.chassis_yaw);
+    osMessageQueuePut(task_runtime.msgq.chassis_yaw, &gimbal.feedback.motor.yaw, 0, 0);
+    
     Gimbal_Control(&gimbal, &gimbal_cmd);
 
     Gimbal_Output(&gimbal);
+
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }

User/task/init.c

@@ -7,6 +7,7 @@
 #include "task/user_task.h"
 
 /* USER INCLUDE BEGIN */
+#include "device/motor.h"
 #include "module/balance_chassis.h"
 #include "module/gimbal.h"
 #include "module/shoot.h"
@@ -46,6 +47,7 @@ void Task_Init(void *argument) {
   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);
+  task_runtime.msgq.chassis_yaw = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_t), NULL);
   task_runtime.msgq.gimbal.imu= osMessageQueueNew(2u, sizeof(Gimbal_IMU_t), NULL);
   task_runtime.msgq.gimbal.cmd= osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL);
   task_runtime.msgq.shoot.shoot_cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL);

User/task/user_task.h

@@ -57,6 +57,7 @@ typedef struct {
 
         osMessageQueueId_t chassis_imu;
         osMessageQueueId_t Chassis_cmd;
+        osMessageQueueId_t chassis_yaw;
         struct {
             osMessageQueueId_t imu;
             osMessageQueueId_t cmd;