Merge branch 'MC_02' of ssh://gitea.qutrobot.top:222/robofish/rm_balance into MC_02

# Conflicts:
#	User/module/cmd/cmd.c
#	User/module/config.c

User/module/balance_chassis.c

@@ -118,20 +118,9 @@ static void Chassis_ResetControllers(Chassis_t *c) {
  * @param c 底盘结构体指针
  */
 static void Chassis_SelectYawZeroPoint(Chassis_t *c) {
-  float current_yaw = c->feedback.yaw.rotor_abs_angle;
-  float zero_point_1 = c->param->mech.mech_zero_yaw;
-  float zero_point_2 = zero_point_1 + M_PI;
-
-  float error_to_zero1 = CircleError(zero_point_1, current_yaw, M_2PI);
-  float error_to_zero2 = CircleError(zero_point_2, current_yaw, M_2PI);
-
-  if (fabsf(error_to_zero1) <= fabsf(error_to_zero2)) {
-    c->yaw_control.target_yaw = zero_point_1;
-    c->yaw_control.is_reversed = false;
-  } else {
-    c->yaw_control.target_yaw = zero_point_2;
-    c->yaw_control.is_reversed = true;
-  }
+  /* 双零点已关闭，始终使用主零点 */
+  c->yaw_control.target_yaw = c->param->mech.mech_zero_yaw;
+  c->yaw_control.is_reversed = false;
 }
 
 /**
@@ -1252,20 +1241,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
   } else {
     c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
 
+    /* 双零点已关闭，始终使用主零点 */
     float manual_offset = c_cmd->move_vec.vy * M_PI_2;
-    float base_target_1 = c->param->mech.mech_zero_yaw + manual_offset;
-    float base_target_2 = c->param->mech.mech_zero_yaw + M_PI + manual_offset;
-
-    float error_to_target1 = CircleError(base_target_1, c->yaw_control.current_yaw, M_2PI);
-    float error_to_target2 = CircleError(base_target_2, c->yaw_control.current_yaw, M_2PI);
-
-    if (fabsf(error_to_target1) <= fabsf(error_to_target2)) {
-      c->yaw_control.target_yaw = base_target_1;
-      c->yaw_control.is_reversed = false;
-    } else {
-      c->yaw_control.target_yaw = base_target_2;
-      c->yaw_control.is_reversed = true;
-    }
+    c->yaw_control.target_yaw = c->param->mech.mech_zero_yaw + manual_offset;
+    c->yaw_control.is_reversed = false;
 
     c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
                                         c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);

User/module/cmd/cmd.c

@@ -6,6 +6,7 @@
 #include "module/cmd/cmd_types.h"
 #include "module/gimbal.h"
 #include "module/shoot.h"
+#include "module/gimbal.h"
 #include <stdint.h>
 #include <string.h>
 
@@ -662,7 +663,7 @@ int8_t CMD_Arbitrate(CMD_t *ctx) {
   if (ctx->input.online[CMD_SRC_NUC]) {
     if (ctx->active_source==CMD_SRC_RC) {
       if (ctx->input.rc.sw[0] == CMD_SW_DOWN) {
-        //用遥控器调自瞄的时候打开下面两行
+        // 用遥控器调自瞄的时候打开下面两行
         // ctx->output.gimbal.source = CMD_SRC_NUC;
         // ctx->output.shoot.source = CMD_SRC_NUC;
 #if CMD_ENABLE_MODULE_REFUI

User/module/config.c

@@ -545,7 +545,7 @@ Config_RobotParam_t robot_config = {
             //  .balance_sw_mid   = CHASSIS_MODE_RELAX,
             //   .balance_sw_down   = CHASSIS_MODE_RELAX,
             .balance_sw_mid  =CHASSIS_MODE_WHELL_LEG_BALANCE,
-            .balance_sw_down = CHASSIS_MODE_LEG_TEST,
+            .balance_sw_down = CHASSIS_MODE_WHELL_LEG_BALANCE,
 #endif
         },
     },