Merge remote-tracking branch 'origin/codex/chassis-power-control' into codex/chassis-power-control

半舵·正式发售
半舵旗舰版（功率控制有更改）
2026-10-24 01:52:20 +08:00 · 2026-08-29 19:58:09 +08:00 · 2026-08-29 19:55:45 +08:00 · 2026-07-26 11:27:00 +08:00 · 2026-05-26 19:43:23 +08:00 · 2026-04-29 19:43:54 +08:00
23 changed files with 1374 additions and 330 deletions
--- a/.Doc/CAN发送非阻塞改造隐患.md
+++ b/.Doc/CAN发送非阻塞改造隐患.md
@@ -0,0 +1,56 @@
+# CAN 非阻塞发送改造 · 隐患记录
+
+> 记录时间：2026-08-29
+> 背景：修复“拔掉 CAN1 后，CAN2 上的 pitch / 摩擦轮 / 拨盘电机明显抖动”的问题。
+
+## 一、本次改动摘要
+
+| 文件 | 改动 | 目的 |
+| --- | --- | --- |
+| bsp/can/bsp_can.c | `CANTransmit()` 由忙等改为非阻塞 | 避免某一路 CAN 离线时拖慢电机控制任务 |
+| bsp/can/bsp_can.c | `CANAddFilter()` 对 `can_filter_conf` 先 `memset` 清零 | 避免未初始化字段成为垃圾匹配 ID |
+| Src/can.c | CAN1/CAN2 的 `AutoBusOff` 改为 `ENABLE` | 断开后重新连接能自动从 bus-off 恢复 |
+
+## 二、隐患与风险
+
+### 1. 双板模式下的 `CANCommSend` 多帧发送可能丢包（当前为休眠隐患）
+
+- 相关位置：`modules/can_comm/can_comm.c` 的 `CANCommSend()`（约 106、116、121 行）。
+- `CANCommSend()` 按每 8 字节一帧连续发送。当待发送数据超过 24 字节（即超过 3 帧）时，第 4 帧起会因为 3 个发送邮箱都还在忙碌而被直接丢弃。
+- 原来的忙等版本会等待邮箱空闲后继续发完，非阻塞版本不会。
+- 当前工程为 `ONE_BOARD`，该路径未参与编译，暂无影响；一旦切换到 `CHASSIS_BOARD` / `GIMBAL_BOARD`，此问题会暴露。
+
+### 2. 丢帧变为静默，失去诊断信息
+
+- 相关位置：`bsp/can/bsp_can.c` 的 `CANTransmit()`（约 107 行）。
+- 原实现邮箱满时会打印 `LOGWARNING`；现在直接 `return 0`，不重试、不记录。
+- 未来若总线设备增多或发送频率升高出现真实拥塞，丢帧将不可见，排查难度增加。
+- 对 DJI 电机本身影响较小：500 Hz 控制帧偶尔丢一两帧，电机内部看门狗时间远大于 2 ms，不会触发保护。
+
+### 3. `timeout` 参数实际失效
+
+- `CANTransmit()` 签名仍保留 `float timeout`，但函数内部用 `(void)timeout` 忽略了它。
+- 调用方传入的 `1`、`0.2`、`0.5` 等值已不再生效，存在可读性与预期一致性隐患。
+
+### 4. `AutoBusOff` 行为变化 + CubeMX 覆盖风险
+
+- 相关位置：`Src/can.c`（约 48、80 行）。
+- 改为 `ENABLE` 后，持续总线故障（如短路）会导致控制器在 bus-off ↔ 恢复之间反复循环；当前未开启 CAN 错误中断，不会产生中断风暴，但行为不同于 `DISABLE` 时的锁死状态。
+- `Src/can.c` 是 CubeMX 生成文件，从 `basic_framework.ioc` 重新生成工程时会被覆盖回 `DISABLE`，需要同步修改 `.ioc` 中的 Automatic Bus-Off Management 选项。
+
+### 5. 过滤器配置 `memset`（基本无风险）
+
+- 相关位置：`bsp/can/bsp_can.c` 的 `CANAddFilter()`（约 32 行）。
+- 属于纯修正，无副作用。
+
+## 三、建议后续动作
+
+- [ ] 若将来启用双板，给 `CANCommSend()` 的多帧发送增加“短且有界”的等待/重试，避免丢中间帧。
+- [ ] 评估是否为 `CANTransmit()` 增加可选的丢帧统计或低频告警，在“不阻塞任务”与“可诊断”之间取得平衡。
+- [ ] 在 `basic_framework.ioc` 中同步开启两个 CAN 的 Automatic Bus-Off Management，避免重新生成代码时被覆盖。
+
+## 四、相关文件
+
+- `bsp/can/bsp_can.c`
+- `modules/can_comm/can_comm.c`
+- `Src/can.c`
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -53,7 +53,7 @@ function(include_sub_directories_recursively root_dir)
        include_directories(${root_dir})
    endif()

-    file(GLOB ALL_SUB RELATIVE ${root_dir} ${root_dir}/*) # 获得当前目录下的所有文件，让如ALL_SUB列表中
+    file(GLOB ALL_SUB CONFIGURE_DEPENDS RELATIVE ${root_dir} ${root_dir}/*) # 获得当前目录下的所有文件，让如ALL_SUB列表中
    foreach(sub ${ALL_SUB})
        if (IS_DIRECTORY ${root_dir}/${sub})
            include_sub_directories_recursively(${root_dir}/${sub}) # 对子目录递归调用，包含
@@ -68,7 +68,7 @@ include_sub_directories_recursively(${CMAKE_SOURCE_DIR}/application)
 include_sub_directories_recursively(${CMAKE_SOURCE_DIR}/Inc)

 # add source, only surfix .c
-file(GLOB_RECURSE SOURCES
+file(GLOB_RECURSE SOURCES CONFIGURE_DEPENDS
    "Drivers/*.c"
    "Src/*.c"
    "Middlewares/*.c"
--- a/Src/can.c
+++ b/Src/can.c
@@ -45,7 +45,7 @@ void MX_CAN1_Init(void)
  hcan1.Init.TimeSeg1 = CAN_BS1_10TQ;
  hcan1.Init.TimeSeg2 = CAN_BS2_3TQ;
  hcan1.Init.TimeTriggeredMode = DISABLE;
-  hcan1.Init.AutoBusOff = DISABLE;
+  hcan1.Init.AutoBusOff = ENABLE;
  hcan1.Init.AutoWakeUp = DISABLE;
  hcan1.Init.AutoRetransmission = ENABLE;
  hcan1.Init.ReceiveFifoLocked = DISABLE;
@@ -77,7 +77,7 @@ void MX_CAN2_Init(void)
  hcan2.Init.TimeSeg1 = CAN_BS1_10TQ;
  hcan2.Init.TimeSeg2 = CAN_BS2_3TQ;
  hcan2.Init.TimeTriggeredMode = DISABLE;
-  hcan2.Init.AutoBusOff = DISABLE;
+  hcan2.Init.AutoBusOff = ENABLE;
  hcan2.Init.AutoWakeUp = DISABLE;
  hcan2.Init.AutoRetransmission = ENABLE;
  hcan2.Init.ReceiveFifoLocked = DISABLE;
--- a/Src/usart.c
+++ b/Src/usart.c
@@ -46,7 +46,7 @@ void MX_USART1_UART_Init(void)

  /* USER CODE END USART1_Init 1 */
  huart1.Instance = USART1;
-  huart1.Init.BaudRate = 921600;
+  huart1.Init.BaudRate = 115200;
  huart1.Init.WordLength = UART_WORDLENGTH_8B;
  huart1.Init.StopBits = UART_STOPBITS_1;
  huart1.Init.Parity = UART_PARITY_NONE;
--- a/application/chassis/chassis.c
+++ b/application/chassis/chassis.c
@@ -1,3 +1,4 @@
+
 /**
 * @file chassis.c
 * @author NeoZeng neozng1@hnu.edu.cn
@@ -16,6 +17,9 @@
 #include "dji_motor.h"
 #include "LKMotor.h"
 #include "super_cap.h"
+#include "chassis_power.h"
+#include "power_meter.h"
+#include "vofa.h"
 #include "message_center.h"
 #include "referee_task.h"
 #include "user_lib.h"
@@ -24,15 +28,41 @@
 #include "bsp_dwt.h"
 #include "referee_UI.h"
 #include "arm_math.h"
+#include <math.h>
+
+#ifndef CHASSIS_POWER_LIMIT_ENABLE
+#define CHASSIS_POWER_LIMIT_ENABLE 0
+#endif
+
+#ifndef CHASSIS_POWER_USE_POWER_METER
+#define CHASSIS_POWER_USE_POWER_METER 1
+#endif
+
+#ifndef CHASSIS_POWER_RLS_ENABLE
+#define CHASSIS_POWER_RLS_ENABLE 0
+#endif
+
+#ifndef CHASSIS_POWER_VOFA_ENABLE
+#define CHASSIS_POWER_VOFA_ENABLE 0
+#endif
+
+#ifndef CHASSIS_POWER_VOFA_DIV
+#define CHASSIS_POWER_VOFA_DIV 10u
+#endif
+
+ChassisPowerInstance chassis_power_control;
+PowerMeterInstance *chassis_power_meter;
+ChassisPower_Limit_Input_s chassis_power_debug_input;

 /* 根据robot_def.h中的macro自动计算的参数 */
 #define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f)     // 半轴距
 #define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f)   // 半轮距
 #define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长
 #if defined(CHASSIS_MCNAMEE_WHEEL)
-#define CHASSIS_WHEEL_OFFSET 1.0f // 机器人底盘轮子修正偏移量
+//#define CHASSIS_WHEEL_OFFSET 1.0f // 机器人底盘轮子修正偏移量
 #elif defined(CHASSIS_STEERING_WHEEL)
 #define CHASSIS_WHEEL_OFFSET 30.0f       // 机器人底盘轮子修正偏移量
+//#define CHASSIS_WHEEL_OFFSET 2.0f
 #define SQRT2                1.41421356f // 根号2
 #elif defined(CHASSIS_OMNI_WHEEL)
 #define CHASSIS_WHEEL_OFFSET 0.7071f // 机器人底盘轮子修正偏移量，根号2/2，即45度，用于修正全向轮的安装位置
@@ -53,18 +83,18 @@ static Chassis_Ctrl_Cmd_s chassis_cmd_recv;         // 底盘接收到的控制
 static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据

 static referee_info_t* referee_data; // 用于获取裁判系统的数据
-static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
+static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI/

 static SuperCapInstance *cap;                                       // 超级电容
-static LKMotorInstance *motor_lb,*motor_rf;                         // 轮电机9015
-static DJIMotorInstance *motor_steering_lb,*motor_steering_rf;     // 舵电机6020
+static LKMotorInstance *motor_rf,*motor_lb;                         // 轮电机9015
+static DJIMotorInstance *motor_steering_rf,*motor_steering_lb;     // 舵电机6020
 /* 用于自旋变速策略的时间变量 */
 // static float t;

 /* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
 static float chassis_vx, chassis_vy;     // 将云台系的速度投影到底盘
-static float vt_lb, vt_rf; // 底盘速度解算后的临时输出,待进行限幅
-static float at_lb, at_rf; // 底盘的角度解算后的临时输出,待进行限幅
+static float vt_rf, vt_lb; // 底盘速度解算后的临时输出,待进行限幅
+static float at_rf, at_lb; // 底盘的角度解算后的临时输出,待进行限幅
 static PIDInstance chassis_follow_pid;   // 底盘跟随云台的PID

 void ChassisInit()
@@ -82,7 +112,7 @@ void ChassisInit()
                .MaxOut = 12000,
            },
            .current_PID = {
-                .Kp = 1, // 0.4
+                .Kp = 0.4, // 0.4
                .Ki = 0,   // 0
                .Kd = 0,
                .IntegralLimit = 10500,
@@ -102,25 +132,25 @@ void ChassisInit()

    chassis_motor_config.can_init_config.tx_id = 1;
    // chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
-    motor_rf = LKMotorInit(&chassis_motor_config);
+     motor_lb= LKMotorInit(&chassis_motor_config);

    chassis_motor_config.can_init_config.tx_id = 2;
    // chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
-    motor_lb = LKMotorInit(&chassis_motor_config);
+    motor_rf = LKMotorInit(&chassis_motor_config);

    // 舵电机6020初始化
    Motor_Init_Config_s chassis_motor_steering_config = {
        .can_init_config.can_handle   = &hcan1,
        .controller_param_init_config = {
            .angle_PID = {
-                .Kp                = 35,
-                .Ki                = 0.25,
+                .Kp                = 12,
+                .Ki                = 0.2,
                .Kd                = 0,
                .CoefA             = 3,
                .CoefB             = 0.1,
                .Improve           = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter | PID_ChangingIntegrationRate,
-                .IntegralLimit     = 1500,
-                .MaxOut            = 6000,
+                .IntegralLimit     = 1000,
+                .MaxOut            = 5000,
                .Derivative_LPF_RC = 0.001,
                .DeadBand          = 0.5,
            },
@@ -144,41 +174,61 @@ void ChassisInit()
        .motor_type = GM6020,
    };

-    chassis_motor_steering_config.can_init_config.tx_id = 1;
-    motor_steering_rf                                   = DJIMotorInit(&chassis_motor_steering_config);
-    
    chassis_motor_steering_config.can_init_config.tx_id = 2;
+    motor_steering_rf                                   = DJIMotorInit(&chassis_motor_steering_config);///
+
+    chassis_motor_steering_config.can_init_config.tx_id = 1;
    motor_steering_lb                                   = DJIMotorInit(&chassis_motor_steering_config);

    PID_Init_Config_s chassis_follow_pid_conf = {
-        .Kp                = 0.95, // 6
+        .Kp                = 1.2, // 6
        .Ki                = 0,
-        .Kd                = 0.1, // 0.495
-         .DeadBand          = 0.5f,// 启用死区，值为0.5度
-        //.CoefA             = 0.2,
-        //.CoefB             = 0.3,
-        //.Improve           = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement,
-        //.IntegralLimit     = 50, // 200
-        .MaxOut            = 45,
-        //.Derivative_LPF_RC = 0.01, // 0.01
+        .Kd                = 0.1f, // 0.5
+        .DeadBand          = 0.5,
+        // .CoefA             = 0.2,
+        // .CoefB             = 0.3,
+        // .Improve           = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement,
+        // .IntegralLimit     = 50, // 200
+        .MaxOut            = 50,
+        // .Derivative_LPF_RC = 0.01, // 0.01
    };
-    PIDInit(&chassis_follow_pid, &chassis_follow_pid_conf);
+    PIDInit(&chassis_follow_pid, &chassis_follow_pid_conf);////

    // 裁判系统初始化

-    referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI
+    referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI//

    SuperCap_Init_Config_s cap_conf = {
        .can_config = {
-            .can_handle = &hcan1, // 保持你原有的硬件CAN接口不变
-            .tx_id = 0x001,       // C板发给超级电容的ID
-            .rx_id = 0x100,       // 超级电容发给C板的ID
+            .can_handle = &hcan2,
+            .tx_id = 0x001, // 超级电容默认接收id
+            .rx_id = 0x100, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
        }};
-    cap = SuperCapInit(&cap_conf); // 超级电容初始化 todo 控制
+    cap = SuperCapInit(&cap_conf); // 超级电容初始化

-    // （新增）设置超级电容功率补偿，例如补偿5W，可根据实际需求调整 todo 控制
+    // （新增）设置超级电容功率补偿，例如补偿5W，可根据实际需求调整
    SuperCapSetPowerOffset(cap, 5);

+#if CHASSIS_POWER_USE_POWER_METER
+    chassis_power_meter = PowerMeterInitCan(&hcan1, POWER_METER_DEFAULT_CAN_ID);
+#endif
+
+    ChassisPowerMotorModel_s wheel_model = ChassisPowerGetDefault3508Model();
+    ChassisPower_Init_Config_s power_config = {
+        .wheel_model = wheel_model,
+        .default_power_limit = 60.0f,
+        .min_power_limit = 20.0f,
+        .buffer_low = 30.0f,
+        .buffer_high = 50.0f,
+        .buffer_gain = 0.25f,
+        .steering_power_reserve = 6.0f,
+        .min_scale = 0.20f,
+        .rls_lambda = 0.995f,
+        .rls_init_covariance = 1000.0f,
+        .rls_enable = CHASSIS_POWER_RLS_ENABLE,
+    };
+    ChassisPowerInit(&chassis_power_control, &power_config);
+
    // 发布订阅初始化,如果为双板,则需要can comm来传递消息
 #ifdef CHASSIS_BOARD
    Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
@@ -235,7 +285,7 @@ static void MinmizeRotation(float *angle, const float *last_angle, float *speed)
    while (norm_rotation < -180.0f) {
        norm_rotation += 360.0f;
    }
-    float threshold = 110.0f;
+    float threshold = 90.0f;
    float target_single=fmodf(target_angle,360.0f);
    if(target_single<0.0f) target_single+=360.0f;

@@ -247,12 +297,11 @@ static void MinmizeRotation(float *angle, const float *last_angle, float *speed)
        int32_t round_diff = (int32_t)((target_angle-actual_angle) / 360.0f);
        *angle = actual_angle + norm_rotation-180.0f+round_diff*360.0f;
        *speed = -(*speed);
-    }else if(norm_rotation<-threshold){
+    }else if(norm_rotation<-threshold) {
        int32_t round_diff = (int32_t)((target_angle-actual_angle) / 360.0f);
        *angle = actual_angle + norm_rotation+180.0f+round_diff*360.0f;
        *speed = -(*speed);
    }
-    // ANGLE_LIMIT_360_TO_180_ABS(*angle);
    // float rotation = *angle - *last_angle;
    // ANGLE_LIMIT_360_TO_180_ABS(rotation);
    // if (rotation > 90) {
@@ -269,59 +318,56 @@ static void MinmizeRotation(float *angle, const float *last_angle, float *speed)
 * @brief 舵轮电机角度解算
 *
 */
-    static void SteeringWheelCalculate()
+static void SteeringWheelCalculate()
 {
    float offset_rf, offset_lb;     // 用于计算舵轮的角度
    float at_rf_last, at_lb_last;   // 上次的角度
-    at_lb_last = motor_steering_lb->measure.total_angle;
-    // at_lf_last = motor_steering_lf->measure.total_angle;
    at_rf_last = motor_steering_rf->measure.total_angle;
-    // at_rb_last = motor_steering_rb->measure.total_angle;
+    at_lb_last = motor_steering_lb->measure.total_angle;
    if (chassis_vx == 0 && chassis_vy == 0 && chassis_cmd_recv.wz == 0) {
        // at_lf = at_lf_last;
        // at_rf = at_rf_last;
        // at_lb = at_lb_last;
        // at_rb = at_rb_last;
-        vt_lb = 0,  vt_rf = 0;
+        vt_rf = 0, vt_lb = 0;
    } else {
        // 生成预计算变量，减少计算量，空间换时间
-        float w      = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET * SQRT2;
-        float temp_x = chassis_vx - w, temp_y = chassis_vy - w;
+        //float w      = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET * SQRT2;
+        float w      = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET ;
+        // float temp_x = chassis_vx - w, temp_y = chassis_vy - w;
        // arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lf); // lf：y- , x-
        // temp_y = chassis_vy + w;                                 // 重复利用变量,temp_x = chassis_vy - w;与上次相同因此注释
-        arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lb); // lb: y+ , x-
-        temp_x = chassis_vx + w;
-        temp_y = chassis_vy + w;//与上次相同因此注释
+        // // arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lb); // lb: y+ , x-
+        // // temp_x = chassis_vx + w;                                 // temp_y = chassis_vx + w;与上次相同因此注释
        // arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rb); // rb: y+ , x+
        // temp_y = chassis_vy - w;                                 // temp_x = chassis_vy + w;与上次相同因此注释
+        // // arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rf); // rf: y- , x+
+        float temp_x = chassis_vx - w, temp_y = chassis_vy + w;
+        arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lb); // lb: y+ , x-
+        temp_x = chassis_vx + w;
+        temp_y = chassis_vy - w;
        arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rf); // rf: y- , x+

        // 计算角度偏移
        // offset_lf = atan2f(chassis_vy - w, chassis_vx - w) * RAD_2_DEGREE; // lf:  y- , x-
-        offset_rf = atan2f(chassis_vy - w, chassis_vx - w) * RAD_2_DEGREE; // rf:  y- , x+
-        offset_lb = atan2f(chassis_vy + w, chassis_vx + w) * RAD_2_DEGREE; // lb:  y+ , x-注意
        // offset_rb = atan2f(chassis_vy + w, chassis_vx + w) * RAD_2_DEGREE; // rb:  y+ , x+
+        offset_lb = atan2f(chassis_vy + w, chassis_vx - w) * RAD_2_DEGREE; // lb: y+ , x-
+        offset_rf = atan2f(chassis_vy - w, chassis_vx + w) * RAD_2_DEGREE; // rf: y- , x+

-        // at_lf = STEERING_CHASSIS_ALIGN_ANGLE_LF + offset_lf; // 此处似乎不需要加上真实角度，因为我们需要的是绝对角度而不是相对角度
-        at_rf = STEERING_CHASSIS_ALIGN_ANGLE_RF + offset_rf;
+        at_rf = STEERING_CHASSIS_ALIGN_ANGLE_RF + offset_rf; // 此处似乎不需要加上真实角度，因为我们需要的是绝对角度而不是相对角度
        at_lb = STEERING_CHASSIS_ALIGN_ANGLE_LB + offset_lb;
-        // at_rb = STEERING_CHASSIS_ALIGN_ANGLE_RB + offset_rb;

-        // ANGLE_LIMIT_360_TO_180_ABS(at_lf);
        ANGLE_LIMIT_360_TO_180_ABS(at_rf);
        ANGLE_LIMIT_360_TO_180_ABS(at_lb);
-        // ANGLE_LIMIT_360_TO_180_ABS(at_rb);

        // MinmizeRotation(&at_lf, &at_lf_last, &vt_lf);
-        MinmizeRotation(&at_rf, &at_rf_last, &vt_rf);
-        MinmizeRotation(&at_lb, &at_lb_last, &vt_lb);
        // MinmizeRotation(&at_rb, &at_rb_last, &vt_rb);
+        MinmizeRotation(&at_lb, &at_lb_last, &vt_lb);
+        MinmizeRotation(&at_rf, &at_rf_last, &vt_rf);
    }

-    // DJIMotorSetRef(motor_steering_lf, at_lf);
    DJIMotorSetRef(motor_steering_rf, at_rf);
    DJIMotorSetRef(motor_steering_lb, at_lb);
-    // DJIMotorSetRef(motor_steering_rb, at_rb);
 }

 /**
@@ -342,6 +388,123 @@ static void MinmizeRotation(float *angle, const float *last_angle, float *speed)
 */
 static void LimitChassisOutput()
 {
+#if CHASSIS_POWER_LIMIT_ENABLE
+    float limited_rf = vt_rf;
+    float limited_lb = vt_lb;
+    const PowerMeterInstance *meter_data = 0;
+#if CHASSIS_POWER_USE_POWER_METER
+    meter_data = PowerMeterGetData(chassis_power_meter);
+#endif
+
+    ChassisPower_Limit_Input_s power_input = {
+        .rf_speed_ref = vt_rf,
+        .lb_speed_ref = vt_lb,
+        .rf_speed = motor_rf->measure.speed_rads,
+        .lb_speed = motor_lb->measure.speed_rads,
+        .rf_current_cmd = motor_rf->speed_PID.Output,
+        .lb_current_cmd = motor_lb->speed_PID.Output,
+        .rf_output_cmd = motor_rf->current_PID.Output,
+        .lb_output_cmd = motor_lb->current_PID.Output,
+        .rf_feedback_current = motor_rf->measure.real_current,
+        .lb_feedback_current = motor_lb->measure.real_current,
+        .referee_limit = referee_data != 0 ? referee_data->GameRobotState.chassis_power_limit : 0.0f,
+        .referee_power = referee_data != 0 ? referee_data->PowerHeatData.chassis_power : 0.0f,
+        .power_buffer = referee_data != 0 ? referee_data->PowerHeatData.chassis_power_buffer : 0.0f,
+        .measured_power = meter_data != 0 ? meter_data->power : 0.0f,
+        .steering_power_estimate = 0.0f,
+        .power_meter_online = meter_data != 0 ? meter_data->online : 0u,
+    };
+    chassis_power_debug_input = power_input;
+
+    ChassisPowerUpdateMeasureState(&chassis_power_control, &power_input);
+    ChassisPowerLimitWheelRefs(&chassis_power_control, &power_input, &limited_rf, &limited_lb);
+    if (power_input.power_meter_online) {
+        ChassisPowerUpdateRLS(&chassis_power_control, &power_input, power_input.measured_power);
+    }
+
+    vt_rf = limited_rf;
+    vt_lb = limited_lb;
+
+#if CHASSIS_POWER_VOFA_ENABLE
+    static uint16_t vofa_divider = 0u;
+    if (++vofa_divider >= CHASSIS_POWER_VOFA_DIV) {
+        vofa_divider = 0u;
+        const ChassisPower_State_s *power_state = ChassisPowerGetState(&chassis_power_control);
+        float vofa_data[] = {
+            power_state->estimated_total_power,
+            power_state->allowed_power,
+            power_state->scale,
+            power_input.measured_power,
+            meter_data != 0 ? meter_data->voltage : 0.0f,
+            meter_data != 0 ? meter_data->current : 0.0f,
+            power_state->rf_ref_raw,
+            power_state->rf_ref_limited,
+            power_state->lb_ref_raw,
+            power_state->lb_ref_limited,
+            motor_rf->measure.speed_rads,
+            motor_lb->measure.speed_rads,
+            motor_rf->speed_PID.Output,
+            motor_lb->speed_PID.Output,
+            motor_rf->measure.real_current,
+            motor_lb->measure.real_current,
+        };
+        vofa_justfloat_output(vofa_data, sizeof(vofa_data) / sizeof(vofa_data[0]), &huart1);
+    }
+#endif
+#else
+    const PowerMeterInstance *meter_data = 0;
+#if CHASSIS_POWER_USE_POWER_METER
+    meter_data = PowerMeterGetData(chassis_power_meter);
+#endif
+
+    ChassisPower_Limit_Input_s power_input = {
+        .rf_speed_ref = vt_rf,
+        .lb_speed_ref = vt_lb,
+        .rf_speed = motor_rf->measure.speed_rads,
+        .lb_speed = motor_lb->measure.speed_rads,
+        .rf_current_cmd = motor_rf->speed_PID.Output,
+        .lb_current_cmd = motor_lb->speed_PID.Output,
+        .rf_output_cmd = motor_rf->current_PID.Output,
+        .lb_output_cmd = motor_lb->current_PID.Output,
+        .rf_feedback_current = motor_rf->measure.real_current,
+        .lb_feedback_current = motor_lb->measure.real_current,
+        .referee_limit = referee_data != 0 ? referee_data->GameRobotState.chassis_power_limit : 0.0f,
+        .referee_power = referee_data != 0 ? referee_data->PowerHeatData.chassis_power : 0.0f,
+        .power_buffer = referee_data != 0 ? referee_data->PowerHeatData.chassis_power_buffer : 0.0f,
+        .measured_power = meter_data != 0 ? meter_data->power : 0.0f,
+        .steering_power_estimate = 0.0f,
+        .power_meter_online = meter_data != 0 ? meter_data->online : 0u,
+    };
+    chassis_power_debug_input = power_input;
+    ChassisPowerUpdateMeasureState(&chassis_power_control, &power_input);
+
+#if CHASSIS_POWER_VOFA_ENABLE
+    static uint16_t vofa_divider = 0u;
+    if (++vofa_divider >= CHASSIS_POWER_VOFA_DIV) {
+        vofa_divider = 0u;
+        const ChassisPower_State_s *power_state = ChassisPowerGetState(&chassis_power_control);
+        float vofa_data[] = {
+            power_state->estimated_total_power,
+            power_state->allowed_power,
+            power_state->scale,
+            power_input.measured_power,
+            meter_data != 0 ? meter_data->voltage : 0.0f,
+            meter_data != 0 ? meter_data->current : 0.0f,
+            power_state->rf_ref_raw,
+            power_state->rf_ref_limited,
+            power_state->lb_ref_raw,
+            power_state->lb_ref_limited,
+            motor_rf->measure.speed_rads,
+            motor_lb->measure.speed_rads,
+            motor_rf->speed_PID.Output,
+            motor_lb->speed_PID.Output,
+            motor_rf->measure.real_current,
+            motor_lb->measure.real_current,
+        };
+        vofa_justfloat_output(vofa_data, sizeof(vofa_data) / sizeof(vofa_data[0]), &huart1);
+    }
+#endif
+#endif
    // 功率限制待添加
    // referee_data->PowerHeatData.chassis_power;
    // referee_data->PowerHeatData.chassis_power_buffer;
@@ -368,8 +531,7 @@ static void EstimateSpeed()
 }

 /* 机器人底盘控制核心任务 */
-void ChassisTask()
-{
+void ChassisTask() {
    // 后续增加没收到消息的处理(双板的情况)
    // 获取新的控制信息
 #ifdef ONE_BOARD
@@ -394,7 +556,6 @@ void ChassisTask()
        DJIMotorEnable(motor_steering_rf);
        DJIMotorEnable(motor_steering_lb);
    }
-
    // 根据控制模式设定旋转速度
    switch (chassis_cmd_recv.chassis_mode)
    {
@@ -402,45 +563,72 @@ void ChassisTask()
            chassis_cmd_recv.wz = 0;
            break;
        case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
-        // float single_round_offset = chassis_cmd_recv.offset_angle;
+            //chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, chassis_cmd_recv.offset_angle, 0);
            float single_round_offset = chassis_cmd_recv.offset_angle;
            if(single_round_offset > 180.0f)
            {
-            single_round_offset -= 360.0f;
+                single_round_offset -= 360.0f;///
            }
            else if(single_round_offset < -180.0f)
            {
                single_round_offset += 360.0f;
            }
-        chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/100;
-        // chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, chassis_cmd_recv.offset_angle, 0)/100;
+            chassis_cmd_recv.wz = -PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/60.0f;
            break;
        case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
-        chassis_cmd_recv.wz = 0.5;
+            chassis_cmd_recv.wz = 1.00;//实验版
            break;
        default:
            break;
    }
+    float sin_theta = 0.0f, cos_theta = 1.0f;
+    // ------------------ 【修改部分：加入相位滞后动态补偿】 ------------------
+    // 提取当前的云台-底盘夹角
+    float real_offset_angle = chassis_cmd_recv.offset_angle;

-    // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
-    // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
-    static float sin_theta, cos_theta;
+    // 如果处于小陀螺模式（或者 wz 不为 0），根据旋转速度进行超前补偿
+    if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE )
+    {
+        // 补偿系数 K 需要实车微调。
+        // 根据你的描述 wz=0.95 时偏差约45度，初始 K 值可以设为 45.0 / 0.95 ≈ 47.3f
+        float lag_compensation_K = 47.3f;//47.3f;45.0f
+
+        // 符号(+或-)取决于你的电机正方向、底盘旋转正方向，如果不正就改成减号
+        real_offset_angle -= (chassis_cmd_recv.wz * lag_compensation_K);
+        cos_theta = arm_cos_f32(real_offset_angle * DEGREE_2_RAD);
+        sin_theta = arm_sin_f32(real_offset_angle * DEGREE_2_RAD);
+
+    }else if (chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW)
+    {
        cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
        sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
-    chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
-    chassis_vy = -chassis_cmd_recv.vx * sin_theta - chassis_cmd_recv.vy * cos_theta;
+
+    }
+    // ----------------------------------------------------------------------
+        chassis_vx = (chassis_cmd_recv.vx * cos_theta + chassis_cmd_recv.vy * sin_theta)/4;
+        chassis_vy = (-chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta)/4;
+
+    // 使用补偿后的角度进行坐标系投影
+
+
+    // static float sin_theta, cos_theta;
+    // cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
+    // sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
+    // chassis_vx = (chassis_cmd_recv.vx * cos_theta + chassis_cmd_recv.vy * sin_theta)/4;
+    // chassis_vy = (-chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta)/4;

    // 根据控制模式进行正运动学解算,计算底盘输出
    SteeringWheelCalculate();
    // MecanumCalculate();

+
    // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
    LimitChassisOutput();

    // 根据电机的反馈速度和IMU(如果有)计算真实速度
    EstimateSpeed();

-    // // 获取裁判系统数据   建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
+    // // 获取裁判系统数据   建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送/
    // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
    // chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0;
    // // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
--- a/application/cmd/robot_cmd.c
+++ b/application/cmd/robot_cmd.c
@@ -8,10 +8,12 @@
 #include "message_center.h"
 #include "general_def.h"
 #include "dji_motor.h"
+#include "referee_task.h"
 // bsp
 #include "bsp_dwt.h"
 #include "bsp_log.h"
 #include "main.h"
+#include "shoot.h"
 // 私有宏,自动将编码器转换成角度值
 #define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360
 #define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI) // pitch水平时电机的角度,0-360
@@ -44,6 +46,7 @@ static Shoot_Ctrl_Cmd_s shoot_cmd_send;      // 传递给发射的控制信息
 static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息

 static Robot_Status_e robot_state; // 机器人整体工作状态
+static uint8_t robot_level = 1; // 机器人等级，范围1-10，上电重置为1

 void RobotCMDInit()
 {
@@ -84,9 +87,17 @@ void RobotCMDInit()
 static void CalcOffsetAngle()
 {
    static float last_offset_angle = 0;
-    const float smooth_factor = 0.3f; // 平滑因子,0~1之间,值越大平滑效果越明显
+    //const float smooth_factor = 0.3f; // 平滑因子,

-    const float deadband = 3.0f;       // 死区值
+     float smooth_factor;
+    // 小陀螺模式下使用更大的平滑因子，让offset_angle更快跟随云台
+    if (chassis_cmd_send.chassis_mode == CHASSIS_ROTATE) {
+        smooth_factor = 0.9f;  // 快速响应
+    } else {
+        smooth_factor = 0.3f;  // 平滑过渡 0~1之间,值越大平滑效果越明显
+    }
+
+    //const float deadband = 3.0f;       // 死区值
    // 新增：跟踪模式变化，用于重置基准角度
    // static chassis_mode_e last_chassis_mode = CHASSIS_NO_FOLLOW;//
    //
@@ -108,14 +119,14 @@ static void CalcOffsetAngle()
    else if (angle_diff < -180.0f)
        new_offset += 360.0f;
    /////
-    // 应用死区
-    if (fabs(new_offset) < deadband) {
-        new_offset = 0.0f;
-    } else if (new_offset > 0) {
-       new_offset -= deadband;
-   } else {
-        new_offset += deadband;
-    }//////
+   //  // 应用死区
+   //  if (fabs(new_offset) < deadband) {
+   //      new_offset = 0.0f;
+   //  } else if (new_offset > 0) {
+   //     new_offset -= deadband;
+   // } else {
+   //      new_offset += deadband;
+   //  }//////
         chassis_cmd_send.offset_angle = last_offset_angle *(1.0f - smooth_factor) + new_offset * smooth_factor;
     last_offset_angle = chassis_cmd_send.offset_angle;
    // // 更新模式状态
@@ -184,12 +195,12 @@ static void RemoteControlSet()
        // around = 0;
        chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
        gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
-        around= chassis_cmd_send.offset_angle/360;
+        around= chassis_cmd_send.offset_angle/(-360);
    }
    else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动
    {
-        //gimbal_fetch_data.yaw_motor_total_angle = gimbal_fetch_data.yaw_motor_total_angle-around*360;
-        chassis_cmd_send.offset_angle =  chassis_cmd_send.offset_angle - 360*around;
+        //gimbal_fetch_data.yaw_motor_total_angle = gimbal_fetch.yaw_motor_total_angle-around*360;
+        chassis_cmd_send.offset_angle =  chassis_cmd_send.offset_angle + 360*around;
        chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
        gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
    }
@@ -197,20 +208,25 @@ static void RemoteControlSet()
    // 云台参数,确定云台控制数据
    if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式
    {
-        // 待添加,视觉会发来和目标的误差,同样将其转化为total angle的增量进行控制
+        // 待添加,视觉会发来和目标的误差,同样将其转化为total angle的增量进行控制/
        // ...
    }
    // 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制
    if (switch_is_down(rc_data[TEMP].rc.switch_left) )
    { // 按照摇杆的输出大小进行角度增量,增益系数需调整
-        gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_r_;
-        gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_r1;
+        gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_l_;
+        gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_l1;
    }
    // 云台软件限位
-    // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
-    chassis_cmd_send.vx = 0.15f * (float)rc_data[TEMP].rc.rocker_l_; // _水平方向
-    chassis_cmd_send.vy = 0.15f * (float)rc_data[TEMP].rc.rocker_l1; // 1数值方向
+    if (gimbal_cmd_send.pitch > PITCH_MAX_ANGLE)
+        gimbal_cmd_send.pitch = PITCH_MAX_ANGLE;
+    else if (gimbal_cmd_send.pitch < PITCH_MIN_ANGLE)
+        gimbal_cmd_send.pitch = PITCH_MIN_ANGLE;

+    // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
+    chassis_cmd_send.vx = (0.25f * (float)rc_data[TEMP].rc.rocker_r1); // 竖直方向
+    chassis_cmd_send.vy = (0.25f * (float)rc_data[TEMP].rc.rocker_r_); // 水平方向
+    //此处如果是右手系，那么x应该是前进方向即竖直方向
    // // 发射参数
    // if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开
    //     ;                                            // 弹舱舵机控制,待添加servo_motor模块,开启
@@ -223,8 +239,8 @@ static void RemoteControlSet()
    else
        shoot_cmd_send.friction_mode = FRICTION_OFF;
    // 拨弹控制,遥控器固定为一种拨弹模式,可自行选择
-    if (rc_data[TEMP].rc.dial < -500)
-        shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
+    if (rc_data[TEMP].rc.dial >500)
+        shoot_cmd_send.load_mode = LOAD_3_BULLET;
    else
        shoot_cmd_send.load_mode = LOAD_STOP;
    // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频,
@@ -237,13 +253,74 @@ static void RemoteControlSet()
 */
 static void MouseKeySet()
 {
-    chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].w * 300 - rc_data[TEMP].key[KEY_PRESS].s * 300; // 系数待测
-    chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].s * 300 - rc_data[TEMP].key[KEY_PRESS].d * 300;
+
+    // // G键控制机器人等级，每次按G键等级+1，最高10级/
+    // if (rc_data[TEMP].key[KEY_PRESS].g && rc_data[TEMP].key_count[KEY_PRESS][Key_G] > 0) {
+    //     robot_level = (robot_level % 10) + 1; // 等级循环增加，1-10
+    // }
+    // G键控制机器人等级，每次按G键等级+1，最高10级
+    if (rc_data[TEMP].key[KEY_PRESS].g && rc_data[TEMP].key_count[KEY_PRESS][Key_G] > 0) {
+        robot_level = (robot_level % 10) + 1; // 等级循环增加，1-10
+        rc_data[TEMP].key_count[KEY_PRESS][Key_G] = 0; // 清除计数器，防止重复执行
+    }
+
+    // B键刷新裁判系统UI
+    if (rc_data[TEMP].key[KEY_PRESS].b && rc_data[TEMP].key_count[KEY_PRESS][Key_B] > 0) {
+        MyUIInit(); // 重新初始化UI
+        rc_data[TEMP].key_count[KEY_PRESS][Key_B] = 0; // 清除计数器，防止重复执行
+    }
+
+    // 将robot_level传递给底盘控制命令
+    chassis_cmd_send.robot_level = robot_level;
+    chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 42 - rc_data[TEMP].key[KEY_PRESS].d * 42; // w向前，s向后
+    chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 42 - rc_data[TEMP].key[KEY_PRESS].s * 42; // a向左，d向右
+    // switch (robot_level)
+    // {
+    //     case 1:
+    //     case 2:
+    //     case 3:
+    //         chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 20 - rc_data[TEMP].key[KEY_PRESS].d * 20); // w向前，s向后
+    //         chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 20 - rc_data[TEMP].key[KEY_PRESS].s * 20); // a向左，d向右
+    //         break;
+    //     case 4:
+    //     case 5:
+    //     case 6:
+    //     case 7:
+    //         chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 30 - rc_data[TEMP].key[KEY_PRESS].d * 30); // w向前，s向后
+    //         chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 30 - rc_data[TEMP].key[KEY_PRESS].s * 30); // a向左，d向右
+    //         break;
+    //     default:
+    //         chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 40 - rc_data[TEMP].key[KEY_PRESS].d * 40); // w向前，s向后
+    //         chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 40 - rc_data[TEMP].key[KEY_PRESS].s * 40); // a向左，d向右
+    //         break;
+    // }
+    // 根据等级设置底盘速度
+    // if (robot_level >= 1 && robot_level <= 3) {
+    //     chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 20 - rc_data[TEMP].key[KEY_PRESS].d * 20); // w向前，s向后
+    //     chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 20 - rc_data[TEMP].key[KEY_PRESS].s * 20); // a向左，d向右
+    // } else if (robot_level >= 4 && robot_level <= 7) {
+    //     chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 30 - rc_data[TEMP].key[KEY_PRESS].d * 30); // w向前，s向后
+    //     chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 30 - rc_data[TEMP].key[KEY_PRESS].s * 30); // a向左，d向右
+    // } else {
+    //     chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 40 - rc_data[TEMP].key[KEY_PRESS].d * 40); // w向前，s向后
+    //     chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 40 - rc_data[TEMP].key[KEY_PRESS].s * 40); // a向左，d向右
+    // }
+
+    // chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].w * 300 - rc_data[TEMP].key[KEY_PRESS].s * 300; // 系数待测
+    // chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].s * 300 - rc_data[TEMP].key[KEY_PRESS].d * 300;
+    //chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 27 - rc_data[TEMP].key[KEY_PRESS].d * 27); // w向前，s向后
+    //chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 27 - rc_data[TEMP].key[KEY_PRESS].s * 27); // a向左，d向右

    gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 10; // 系数待测
-    gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 10;
+    gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 8;

-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速
+    // 云台软件限位1
+    if (gimbal_cmd_send.pitch > PITCH_MAX_ANGLE)
+        gimbal_cmd_send.pitch = PITCH_MAX_ANGLE;
+    else if (gimbal_cmd_send.pitch < PITCH_MIN_ANGLE)
+        gimbal_cmd_send.pitch = PITCH_MIN_ANGLE;
+
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速/1
    {
    case 0:
        shoot_cmd_send.bullet_speed = 15;
@@ -255,20 +332,45 @@ static void MouseKeySet()
        shoot_cmd_send.bullet_speed = 30;
        break;
    }
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 4) // E键设置发射模式
-    {
-    case 0:
-        shoot_cmd_send.load_mode = LOAD_STOP;
-        break;
-    case 1:
-        shoot_cmd_send.load_mode = LOAD_1_BULLET;
-        break;
-    case 2:
-        shoot_cmd_send.load_mode = LOAD_3_BULLET;
-        break;
-    default:
-        shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
-        break;
+    // switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 4) // E键设置发射模式
+    // {
+    // case 0:
+    //     shoot_cmd_send.load_mode = LOAD_STOP;
+    //     break;
+    // case 1:
+    //     shoot_cmd_send.load_mode = LOAD_1_BULLET;
+    //     break;
+    // case 2:
+    //     shoot_cmd_send.load_mode = LOAD_3_BULLET;
+    //     break;
+    // default:
+    //     shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
+    //     break;
+    // }
+    // switch (rc_data[TEMP].mouse.press_l) // 鼠标左键射击
+    // {
+    //     case 0:
+    //         shoot_cmd_send.load_mode = LOAD_STOP;
+    //         break;
+    //     default:
+    //         shoot_cmd_send.load_mode = LOAD_1_BULLET; // 按鼠标进行单发
+    //         break;
+    // }
+    // switch (rc_data[TEMP].mouse.press_r) // 鼠标右键射击
+    // {
+    //     case 0:
+    //         shoot_cmd_send.load_mode = LOAD_STOP;
+    //         break;
+    //     default:
+    //         shoot_cmd_send.load_mode = LOAD_3_BULLET; // 按鼠标进行单发
+    //         break;
+    // }
+    if (rc_data[TEMP].mouse.press_l) {
+        shoot_cmd_send.load_mode = LOAD_BURSTFIRE; // 左键单发
+    } else if (rc_data[TEMP].mouse.press_r) {
+        shoot_cmd_send.load_mode = LOAD_3_BULLET; // 右键三连发
+    } else {
+        shoot_cmd_send.load_mode = LOAD_STOP; // 不按鼠标不发射
    }
    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键开关弹舱
    {
@@ -303,7 +405,18 @@ static void MouseKeySet()
        chassis_cmd_send.chassis_speed_buff = 100;
        break;
    }
-    switch (rc_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键设置底盘运动模式
+    {
+        case 0:
+            chassis_cmd_send.offset_angle =  chassis_cmd_send.offset_angle - 360*around;
+            chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
+            break;
+        default:
+            chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
+            around= chassis_cmd_send.offset_angle/360;
+            break;
+    }
+    switch (rc_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量。
    {
    case 1:

@@ -316,6 +429,9 @@ static void MouseKeySet()
 }

 /**
+ *
+ *
+ *
 * @brief  紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等
 *         停止的阈值'300'待修改成合适的值,或改为开关控制.
 *
@@ -329,6 +445,8 @@ static void EmergencyHandler()
    {
        robot_state = ROBOT_STOP;
        gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
+        gimbal_cmd_send.yaw = -gimbal_fetch_data.gimbal_imu_data.YawTotalAngle;;//0
+        gimbal_cmd_send.pitch = 0;
        chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
        shoot_cmd_send.shoot_mode = SHOOT_OFF;
        shoot_cmd_send.friction_mode = FRICTION_OFF;
@@ -345,8 +463,7 @@ static void EmergencyHandler()
 }

 /* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */
-void RobotCMDTask()
-{
+void RobotCMDTask() {
    // 从其他应用获取回传数据
 #ifdef ONE_BOARD
    SubGetMessage(chassis_feed_sub, (void *)&chassis_fetch_data);
@@ -360,11 +477,25 @@ void RobotCMDTask()
    // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成
    CalcOffsetAngle();
    // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
-    if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制
+    if (switch_is_down(rc_data[TEMP].rc.switch_left)){ // 遥控器左侧开关状态为[下],遥控器控制
        RemoteControlSet();
-    else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
+        shoot_cmd_send.shoot_mode =SHOOT_ON;
+        shoot_cmd_send.friction_mode = FRICTION_OFF;
+    }
+    else if (switch_is_up(rc_data[TEMP].rc.switch_left)) {
+        // 遥控器左侧开关状态为[上],键盘控制
        MouseKeySet();
-
+        shoot_cmd_send.shoot_mode =SHOOT_ON;
+    }
+    else if (switch_is_mid(rc_data[TEMP].rc.switch_left)) {
+        // 遥控器左侧开关状态为[中],视觉模式，使用遥控器控制底盘和云台，同时开启发射系统
+        RemoteControlSet();
+        gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_l_;
+        gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_l1;
+        shoot_cmd_send.shoot_mode =SHOOT_ON;
+        shoot_cmd_send.bullet_speed = 30;
+        shoot_cmd_send.friction_mode = FRICTION_ON;
+    }
    EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况

    // 设置视觉发送数据,还需增加加速度和角速度数据
--- a/application/gimbal/gimbal.c
+++ b/application/gimbal/gimbal.c
@@ -14,7 +14,9 @@ static Publisher_t *gimbal_pub;                   // 云台应用消息发布者
 static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息
-
+static float last_yaw_angle = 0.0f;   // 保护模式前的yaw角度
+// static float last_pitch_angle = 0.0f; // 保护模式前的pitch角度
+uint8_t in_protection_mode = 0; // 是否在保护模式
 void GimbalInit()
 {
    gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
@@ -26,10 +28,10 @@ void GimbalInit()
        },
        .controller_param_init_config = {
            .angle_PID = {
-                //.Kp = 0.785, // 8
-                .Kp = 3, // 8
+                //.Kp = 0.685, // 8
+                .Kp = 0.585, // 8
                //.Ki = 0.315,
-                .Ki = 0.45,
+                .Ki = 0.25,
                .Kd = 0.03,
                //.DeadBand = 0.1
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter | PID_ChangingIntegrationRate,
@@ -40,9 +42,9 @@ void GimbalInit()
                .Derivative_LPF_RC = 0.001
            },
            .speed_PID = {
-               .Kp = 500,  // 50
+               .Kp = 8000,  // 50
                //.Kp = 5000,  // 50
-                .Ki = 2000, // 200
+                .Ki = 5000, // 200
                .Kd = 0,
                .CoefB = 1,
                .CoefA = 10,
@@ -66,24 +68,45 @@ void GimbalInit()
    Motor_Init_Config_s pitch_config = {
        .can_init_config = {
            .can_handle = &hcan2,
-            .tx_id = 4,//2
+            .tx_id = 2,//2
        },
+        // .controller_param_init_config = {
+        //     .angle_PID = {
+        //         .Kp = 40, // 10
+        //         .Ki = 0,
+        //         .Kd = 0,
+        //         .MaxOut = 1000,
+        //     },
+        //     .speed_PID = {
+        //         .Kp = 60,  // 50
+        //         .Ki = 15, // 350
+        //         .Kd = 0.6,   // 0
+        //         .MaxOut = 20000,//20000
+        //     },
        .controller_param_init_config = {
            .angle_PID = {
-                .Kp = 10, // 10
-                .Ki = 0,
-                .Kd = 0,
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
-                .IntegralLimit = 100,
-                .MaxOut = 500,
+                .Kp = 0.53, //0.55
+                .Ki = 0.1,//0.05
+                .Kd = 0.0148,//0.02
+                .CoefA =0.5,//0.5
+                .CoefB = 0.6,//0.6
+                .Output_LPF_RC = 0.0068,
+                .DeadBand = 0.005,//0.02
+                .Derivative_LPF_RC=0.01,//0.008
+                .Improve = PID_Trapezoid_Intergral |PID_ChangingIntegrationRate| PID_Integral_Limit |PID_Derivative_On_Measurement |  PID_OutputFilter |PID_DerivativeFilter,
+                .IntegralLimit = 1,
+                .MaxOut = 600,
            },
            .speed_PID = {
-                .Kp = 50,  // 50
-                .Ki = 350, // 350
-                .Kd = 0,   // 0
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
-                .IntegralLimit = 2500,
-                .MaxOut = 8000,//20000
+                .Kp = 3400,//22000
+                .Ki = 15,//
+                .Kd =1,
+                 .CoefA = 0.8,
+                 .CoefB = 0.1,
+                .Output_LPF_RC = 0.002,//0.002
+                .Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement |  PID_OutputFilter,
+                .IntegralLimit = 6000,
+                .MaxOut = 30000,
            },
            .other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
            // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
@@ -94,9 +117,12 @@ void GimbalInit()
            .speed_feedback_source = OTHER_FEED,
            .outer_loop_type = ANGLE_LOOP,
            .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
+            // .outer_loop_type = SPEED_LOOP,//调试用
+            // .close_loop_type = SPEED_LOOP,
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
        },
-        .motor_type = DM4310,
+
+        .motor_type = GM6020,
    };
    // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
    yaw_motor = DJIMotorInit(&yaw_config);
@@ -119,11 +145,25 @@ void GimbalTask()
    {
    // 停止
    case GIMBAL_ZERO_FORCE:
+        // 进入保护模式时记录当前角度
+        if (!in_protection_mode) {
+            last_yaw_angle = gimba_IMU_data->YawTotalAngle;
+            // last_pitch_angle = gimba_IMU_data->Pitch;
+            in_protection_mode = 1;
+        }
        DJIMotorStop(yaw_motor);
        DJIMotorStop(pitch_motor);
        break;
    // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
    case GIMBAL_GYRO_MODE: // 后续只保留此模式
+        // 从保护模式退出时
+        if (in_protection_mode) {
+            // 计算零飘导致的角度误差
+            float angle_error = gimba_IMU_data->YawTotalAngle - last_yaw_angle;
+            // 补偿目标角度，避免突然旋转
+                   gimbal_cmd_recv.yaw -= angle_error;
+            in_protection_mode = 0;
+        }
        DJIMotorEnable(yaw_motor);
        DJIMotorEnable(pitch_motor);
        DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
@@ -132,17 +172,6 @@ void GimbalTask()
        DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
        DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
        DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
-            // // 根据底盘速度进行补偿，保持云台稳定
-            // // 这里简化处理，实际应用中可能需要更复杂的补偿算法
-            // float yaw_ref = gimbal_cmd_recv.yaw;
-            // float pitch_ref = gimbal_cmd_recv.pitch;
-            //
-            // // 当底盘移动时，云台需要保持相对稳定
-            // // 这里可以根据底盘速度调整云台的控制参考值
-            // // 例如，当底盘向前移动时，云台可能需要稍微向后调整以保持稳定
-            //
-            // DJIMotorSetRef(yaw_motor, yaw_ref); // yaw和pitch会在robot_cmd中处理好多圈和单圈
-            // DJIMotorSetRef(pitch_motor, pitch_ref);
        break;
    // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关
    case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快)
@@ -171,12 +200,3 @@ void GimbalTask()
    // 推送消息
    PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
 }
-// /**
-//  * @brief 获取云台IMU数据指针
-//  *
-//  * @return attitude_t* 云台IMU数据指针
-//  */
-// attitude_t *GetGimbalIMUData()
-// {
-//     return gimba_IMU_data;
-// }
--- a/application/gimbal/gimbal.h
+++ b/application/gimbal/gimbal.h
@@ -1,6 +1,6 @@
 #ifndef GIMBAL_H
 #define GIMBAL_H
-#include "../modules/imu/ins_task.h"
+
 /**
 * @brief 初始化云台,会被RobotInit()调用
 * 
@@ -9,8 +9,8 @@ void GimbalInit();

 /**
 * @brief 云台任务
- * @return attitude_t* 云台IMU数据指针
+ * 
 */
 void GimbalTask();
-//attitude_t *GetGimbalIMUData();
+
 #endif // GIMBAL_H
--- a/application/robot_def.h
+++ b/application/robot_def.h
@@ -26,24 +26,24 @@

 /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
 // 云台参数
-#define YAW_CHASSIS_ALIGN_ECD 1190  // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
+#define YAW_CHASSIS_ALIGN_ECD -3595  // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
 #define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
 #define PITCH_HORIZON_ECD 3412      // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
-#define PITCH_MAX_ANGLE 0           // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
-#define PITCH_MIN_ANGLE 0           // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
+#define PITCH_MAX_ANGLE 30           // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
+#define PITCH_MIN_ANGLE -20           // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
 // 底盘参数
 // #define CHASSIS_OMNI_WHEEL // 是否为全向轮底盘
 // #define CHASSIS_MCNAMEE_WHEEL     // 是否为麦克纳姆轮底盘
 #define CHASSIS_STEERING_WHEEL               // 是否为舵轮
-#define STEERING_CHASSIS_ALIGN_ECD_RF   7367 // 舵电机 A 编码器值，若有机械改动需要修改
-#define STEERING_CHASSIS_ALIGN_ECD_LB  1195 // 舵电机 D 编码器值，若有机械改动需要修改
+#define STEERING_CHASSIS_ALIGN_ECD_RF   7066 // 舵电机 A 编码器值，若有机械改动需要修改
+#define STEERING_CHASSIS_ALIGN_ECD_LB  7138 // 舵电机 D 编码器值，若有机械改动需要修改

 #define STEERING_CHASSIS_ALIGN_ANGLE_RF STEERING_CHASSIS_ALIGN_ECD_RF / 8192.f * 360.f // 舵轮 A 对齐角度
 #define STEERING_CHASSIS_ALIGN_ANGLE_LB STEERING_CHASSIS_ALIGN_ECD_LB / 8192.f * 360.f // 舵轮 D 对齐角度
 // 发射参数
-#define ONE_BULLET_DELTA_ANGLE 36    // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
-#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
-#define NUM_PER_CIRCLE 10            // 拨盘一圈的装载量
+#define ONE_BULLET_DELTA_ANGLE 51.0f    // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
+#define REDUCTION_RATIO_LOADER 36.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
+#define NUM_PER_CIRCLE 7            // 拨盘一圈的装载量
 // 机器人底盘修改的参数,单位为mm(毫米)
 #define WHEEL_BASE 350              // 纵向轴距(前进后退方向)
 #define TRACK_WIDTH 300             // 横向轮距(左右平移方向)
@@ -154,7 +154,7 @@ typedef struct
    chassis_mode_e chassis_mode;
    int chassis_speed_buff;
    // UI部分
-    //  ...
+    uint8_t robot_level; // 机器人等级

 } Chassis_Ctrl_Cmd_s;

@@ -178,6 +178,7 @@ typedef struct
    Bullet_Speed_e bullet_speed; // 弹速枚举
    uint8_t rest_heat;
    float shoot_rate; // 连续发射的射频,unit per s,发/秒
+    float dead_time;  // 每发弹丸的间隔时间,unit ms,毫秒
 } Shoot_Ctrl_Cmd_s;

 /* ----------------gimbal/shoot/chassis发布的反馈数据----------------*/
@@ -207,7 +208,8 @@ typedef struct
 {
    attitude_t gimbal_imu_data;
    uint16_t yaw_motor_single_round_angle;
-    uint16_t yaw_motor_total_angle;
+    //uint16_t yaw_motor_total_angle;
+    float yaw_motor_total_angle;
 } Gimbal_Upload_Data_s;

 typedef struct
--- a/application/shoot/shoot.c
+++ b/application/shoot/shoot.c
@@ -17,7 +17,7 @@ static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信

 // dwt定时,计算冷却用
 static float hibernate_time = 0, dead_time = 0;
-
+//static float target_angle = 0;
 void ShootInit()
 {
    // 左摩擦轮
@@ -27,16 +27,16 @@ void ShootInit()
        },
        .controller_param_init_config = {
            .speed_PID = {
-                .Kp = 0, // 20
-                .Ki = 0, // 1
+                .Kp = 20, // 20
+                .Ki = 1, // 1
                .Kd = 0,
                .Improve = PID_Integral_Limit,
                .IntegralLimit = 10000,
                .MaxOut = 15000,
            },
            .current_PID = {
-                .Kp = 0, // 0.7
-                .Ki = 0, // 0.1
+                .Kp = 0.7, // 0.7
+                .Ki = 0.1, // 0.1
                .Kd = 0,
                .Improve = PID_Integral_Limit,
                .IntegralLimit = 10000,
@@ -52,49 +52,49 @@ void ShootInit()
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
        },
        .motor_type = M3508};
-    friction_config.can_init_config.tx_id = 1,
-    friction_l = DJIMotorInit(&friction_config);
-
-    friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行
-    friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
+    friction_config.can_init_config.tx_id = 5,
    friction_r = DJIMotorInit(&friction_config);

+    friction_config.can_init_config.tx_id = 8; // 左摩擦轮,改txid和方向就行
+    friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
+    friction_l = DJIMotorInit(&friction_config);
+
    // 拨盘电机
    Motor_Init_Config_s loader_config = {
        .can_init_config = {
            .can_handle = &hcan2,
-            .tx_id = 5,
+            .tx_id = 7,
        },
        .controller_param_init_config = {
            .angle_PID = {
                // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
-                .Kp = 0, // 10
-                .Ki = 0,
+                .Kp = 12, // 10
+                .Ki = 5,
                .Kd = 0,
-                .MaxOut = 200,
+                .MaxOut = 10000,
            },
            .speed_PID = {
-                .Kp = 0, // 10
-                .Ki = 0, // 1
+                .Kp = 3, // 10
+                .Ki = 10, // 1
                .Kd = 0,
                .Improve = PID_Integral_Limit,
-                .IntegralLimit = 5000,
-                .MaxOut = 5000,
+                .IntegralLimit = 250,
+                .MaxOut = 10000,
            },
            .current_PID = {
-                .Kp = 0, // 0.7
+                .Kp = 1.5, // 0.7
                .Ki = 0, // 0.1
                .Kd = 0,
                .Improve = PID_Integral_Limit,
                .IntegralLimit = 5000,
-                .MaxOut = 5000,
+                .MaxOut = 10000,
            },
        },
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
-            .close_loop_type = CURRENT_LOOP | SPEED_LOOP,
-            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
+            .close_loop_type = ANGLE_LOOP | SPEED_LOOP|CURRENT_LOOP,
+            .motor_reverse_flag = MOTOR_DIRECTION_REVERSE, // 注意方向设置为拨盘的拨出的击发方向
        },
        .motor_type = M2006 // 英雄使用m3508
    };
@@ -126,8 +126,8 @@ void ShootTask()

    // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可
    // 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发)
-    // if (hibernate_time + dead_time > DWT_GetTimeline_ms())
-    //     return;
+     if (hibernate_time + dead_time > DWT_GetTimeline_ms())
+         return;

    // 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换
    switch (shoot_cmd_recv.load_mode)
@@ -138,18 +138,51 @@ void ShootTask()
        DJIMotorSetRef(loader, 0);             // 同时设定参考值为0,这样停止的速度最快
        break;
    // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
-    case LOAD_1_BULLET:                                                                     // 激活能量机关/干扰对方用,英雄用.
-        DJIMotorOuterLoop(loader, ANGLE_LOOP);                                              // 切换到角度环
-        DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
+        // case LOAD_1_BULLET:
+        //     DJIMotorOuterLoop(loader, ANGLE_LOOP);  // 切换到角度环
+        //     static float last_target_angle = 0; // 存储上次的目标角度
+        //     if (dead_time == 0) { // 第一次触发
+        //         last_target_angle = loader->measure.total_angle;
+        //     }
+        //     last_target_angle += 51*36;//ONE_BULLET_DELTA_ANGLE * REDUCTION_RATIO_LOADER; // 每次都在上次目标角度基础上增加固定角度
+        //     DJIMotorSetRef(loader, last_target_angle);
+        //     hibernate_time = DWT_GetTimeline_ms();     // 记录触发指令的时间
+        //     dead_time      = 450;//shoot_cmd_recv.dead_time; // 完成1发弹丸发射的时间
+        //     break;
+        // case LOAD_1_BULLET:
+            // DJIMotorOuterLoop(loader, ANGLE_LOOP);// 切换到角度环
+            // DJIMotorSetRef(loader, loader->measure.total_angle+51*36);
+            // //DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE);
+            // // 控制量增加一发弹丸的角度，其中90为36减速比*1.5
+            // hibernate_time = DWT_GetTimeline_ms();     // 记录触发指令的时间
+            //     dead_time      = 1000; // 完成1发弹丸发射的时间
+            // // 记录触发指令的时间
+            // //dead_time = 500;
+            // // 完成1发弹丸发射的时间
+            // break;
+        case LOAD_1_BULLET:
+            DJIMotorOuterLoop(loader, ANGLE_LOOP);// 切换到角度环
+            static float target_angle = -1900;
+            target_angle += 51.2*36;
+            //float target_angle = loader->measure.total_angle + 51*36;
+            DJIMotorSetRef(loader, target_angle);
            hibernate_time = DWT_GetTimeline_ms();     // 记录触发指令的时间
-        dead_time = 150;                                                                    // 完成1发弹丸发射的时间
+            dead_time      = 450; // 完成1发弹丸发射的时间
            break;
    // 三连发,如果不需要后续可能删除
    case LOAD_3_BULLET:
-        DJIMotorOuterLoop(loader, ANGLE_LOOP);                                                  // 切换到速度环
-        DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
+        // DJIMotorOuterLoop(loader, ANGLE_LOOP);                                                  // 切换到速度环
+        // DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
+        // hibernate_time = DWT_GetTimeline_ms();                                                  // 记录触发指令的时间
+        // dead_time = 300;                                                                        // 完成3发弹丸发射的时间
+        // break;
+            DJIMotorOuterLoop(loader, ANGLE_LOOP);// 切换到角度环.
+            //static float target_angle = -1872;
+            target_angle += (51.2*36)*3;
+            //float target_angle = loader->measure.total_angle + 51*36;
+            DJIMotorSetRef(loader, target_angle);
            hibernate_time = DWT_GetTimeline_ms();     // 记录触发指令的时间
-        dead_time = 300;                                                                        // 完成3发弹丸发射的时间
+            dead_time      = 700; // 完成3发弹丸发射的时间
            break;
    // 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
    case LOAD_BURSTFIRE:
@@ -175,16 +208,16 @@ void ShootTask()
        switch (shoot_cmd_recv.bullet_speed)
        {
        case SMALL_AMU_15:
-            DJIMotorSetRef(friction_l, 0);
-            DJIMotorSetRef(friction_r, 0);
+            DJIMotorSetRef(friction_l, 30000);
+            DJIMotorSetRef(friction_r, 30000);
            break;
        case SMALL_AMU_18:
-            DJIMotorSetRef(friction_l, 0);
-            DJIMotorSetRef(friction_r, 0);
+            DJIMotorSetRef(friction_l, 30000);
+            DJIMotorSetRef(friction_r, 30000);
            break;
        case SMALL_AMU_30:
-            DJIMotorSetRef(friction_l, 0);
-            DJIMotorSetRef(friction_r, 0);
+            DJIMotorSetRef(friction_l, 50000);
+            DJIMotorSetRef(friction_r, 50000);
            break;
        default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
            DJIMotorSetRef(friction_l, 30000);
--- a/bsp/can/bsp_can.c
+++ b/bsp/can/bsp_can.c
@@ -29,6 +29,7 @@ static uint8_t idx; // 全局CAN实例索引,每次有新的模块注册会自
 static void CANAddFilter(CANInstance *_instance)
 {
    CAN_FilterTypeDef can_filter_conf;
+    memset(&can_filter_conf, 0, sizeof(can_filter_conf)); // 防止未初始化字段(如FilterIdHigh等)成为垃圾匹配ID
    static uint8_t can1_filter_idx = 0, can2_filter_idx = 14; // 0-13给can1用,14-27给can2用

    can_filter_conf.FilterMode = CAN_FILTERMODE_IDLIST;                                                       // 使用id list模式,即只有将rxid添加到过滤器中才会接收到,其他报文会被过滤
@@ -105,24 +106,18 @@ CANInstance *CANRegister(CAN_Init_Config_s *config)
 /* 如果让CANinstance保存txbuff,会增加一次复制的开销 */
 uint8_t CANTransmit(CANInstance *_instance, float timeout)
 {
-    static uint32_t busy_count;
-    static volatile float wait_time __attribute__((unused)); // for cancel warning
-    float dwt_start = DWT_GetTimeline_ms();
-    while (HAL_CAN_GetTxMailboxesFreeLevel(_instance->can_handle) == 0) // 等待邮箱空闲
+    (void)timeout; // 保留接口兼容性；改为非阻塞发送,避免阻塞调用它的任务
+
+    // 邮箱全满说明上一帧还没发出去,或总线上没有节点回应ACK(例如断开某一路CAN)。
+    // 此时直接放弃本帧,保证电机控制等任务的周期稳定,不因一路CAN离线而拖慢另一路CAN。
+    if (HAL_CAN_GetTxMailboxesFreeLevel(_instance->can_handle) == 0)
    {
-        if (DWT_GetTimeline_ms() - dwt_start > timeout) // 超时
-        {
-            LOGWARNING("[bsp_can] CAN MAILbox full! failed to add msg to mailbox. Cnt [%d]", busy_count);
-            busy_count++;
        return 0;
    }
-    }
-    wait_time = DWT_GetTimeline_ms() - dwt_start;
+
    // tx_mailbox会保存实际填入了这一帧消息的邮箱,但是知道是哪个邮箱发的似乎也没啥用
-    if (HAL_CAN_AddTxMessage(_instance->can_handle, &_instance->txconf, _instance->tx_buff, &_instance->tx_mailbox))
+    if (HAL_CAN_AddTxMessage(_instance->can_handle, &_instance->txconf, _instance->tx_buff, &_instance->tx_mailbox) != HAL_OK)
    {
-        LOGWARNING("[bsp_can] CAN bus BUS! cnt:%d", busy_count);
-        busy_count++;
        return 0;
    }
    return 1; // 发送成功
--- a/modules/algorithm/QuaternionEKF.c
+++ b/modules/algorithm/QuaternionEKF.c
@@ -199,8 +199,8 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,

    // 利用四元数反解欧拉角
    QEKF_INS.Yaw = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] + QEKF_INS.q[1] * QEKF_INS.q[2]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f;
-    QEKF_INS.Pitch = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f;
-    QEKF_INS.Roll = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
+    QEKF_INS.Pitch = -asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
+    QEKF_INS.Roll = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f;

    // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
    if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast > 180.0f)
--- a/modules/algorithm/rls.c
+++ b/modules/algorithm/rls.c
@@ -0,0 +1,99 @@
+#include "rls.h"
+
+#include <string.h>
+
+void RLSInit(RLSInstance *rls, uint8_t order, float lambda, float init_covariance, const float *initial_theta)
+{
+    if (rls == 0) {
+        return;
+    }
+
+    memset(rls, 0, sizeof(RLSInstance));
+
+    if (order == 0u) {
+        return;
+    }
+    if (order > RLS_MAX_ORDER) {
+        order = RLS_MAX_ORDER;
+    }
+    if (lambda < 0.01f) {
+        lambda = 0.01f;
+    }
+    if (lambda > 1.0f) {
+        lambda = 1.0f;
+    }
+    if (init_covariance <= 0.0f) {
+        init_covariance = 1000.0f;
+    }
+
+    rls->order = order;
+    rls->lambda = lambda;
+    rls->initialized = 1u;
+
+    for (uint8_t i = 0u; i < order; i++) {
+        rls->theta[i] = initial_theta != 0 ? initial_theta[i] : 0.0f;
+        rls->covariance[i][i] = init_covariance;
+    }
+}
+
+float RLSPredict(const RLSInstance *rls, const float *x)
+{
+    if (rls == 0 || x == 0 || rls->initialized == 0u) {
+        return 0.0f;
+    }
+
+    float prediction = 0.0f;
+    for (uint8_t i = 0u; i < rls->order; i++) {
+        prediction += rls->theta[i] * x[i];
+    }
+    return prediction;
+}
+
+float RLSUpdate(RLSInstance *rls, const float *x, float y)
+{
+    float px[RLS_MAX_ORDER] = {0.0f};
+    float xtp[RLS_MAX_ORDER] = {0.0f};
+
+    if (rls == 0 || x == 0 || rls->initialized == 0u) {
+        return 0.0f;
+    }
+
+    const uint8_t order = rls->order;
+    const float prediction = RLSPredict(rls, x);
+    const float error = y - prediction;
+
+    for (uint8_t i = 0u; i < order; i++) {
+        for (uint8_t j = 0u; j < order; j++) {
+            px[i] += rls->covariance[i][j] * x[j];
+            xtp[i] += x[j] * rls->covariance[j][i];
+        }
+    }
+
+    float denominator = rls->lambda;
+    for (uint8_t i = 0u; i < order; i++) {
+        denominator += x[i] * px[i];
+    }
+    if (denominator <= 1.0e-6f) {
+        denominator = 1.0e-6f;
+    }
+
+    for (uint8_t i = 0u; i < order; i++) {
+        rls->gain[i] = px[i] / denominator;
+        rls->theta[i] += rls->gain[i] * error;
+    }
+
+    for (uint8_t i = 0u; i < order; i++) {
+        for (uint8_t j = 0u; j < order; j++) {
+            rls->covariance[i][j] = (rls->covariance[i][j] - rls->gain[i] * xtp[j]) / rls->lambda;
+        }
+    }
+
+    rls->last_prediction = prediction;
+    rls->last_error = error;
+    return prediction;
+}
+
+const float *RLSGetTheta(const RLSInstance *rls)
+{
+    return rls != 0 ? rls->theta : 0;
+}
--- a/modules/algorithm/rls.h
+++ b/modules/algorithm/rls.h
@@ -0,0 +1,25 @@
+#ifndef RLS_H
+#define RLS_H
+
+#include <stdint.h>
+
+#define RLS_MAX_ORDER 4u
+
+typedef struct
+{
+    uint8_t order;
+    uint8_t initialized;
+    float lambda;
+    float theta[RLS_MAX_ORDER];
+    float covariance[RLS_MAX_ORDER][RLS_MAX_ORDER];
+    float gain[RLS_MAX_ORDER];
+    float last_prediction;
+    float last_error;
+} RLSInstance;
+
+void RLSInit(RLSInstance *rls, uint8_t order, float lambda, float init_covariance, const float *initial_theta);
+float RLSPredict(const RLSInstance *rls, const float *x);
+float RLSUpdate(RLSInstance *rls, const float *x, float y);
+const float *RLSGetTheta(const RLSInstance *rls);
+
+#endif
--- a/modules/chassis_power/chassis_power.c
+++ b/modules/chassis_power/chassis_power.c
@@ -0,0 +1,279 @@
+#include "chassis_power.h"
+
+#include <math.h>
+#include <string.h>
+
+ChassisPower_State_s chassis_power_debug_state;
+
+static float ChassisPowerLimitValue(float value, float min, float max)
+{
+    if (value < min) {
+        return min;
+    }
+    if (value > max) {
+        return max;
+    }
+    return value;
+}
+
+ChassisPowerMotorModel_s ChassisPowerGetDefault3508Model(void)
+{
+    const float it_to_ireal_m3508 = 20.0f / 16384.0f;
+    const float i_to_t_m3508 = 0.3f / (3591.0f / 187.0f);
+    const float it_to_t_m3508 = it_to_ireal_m3508 * i_to_t_m3508;
+
+    ChassisPowerMotorModel_s model = {
+        .k_current_speed = it_to_t_m3508 / 9.55f,
+        .k_speed2 = 1.453e-7f,
+        .k_current2 = 2.53e-7f,
+        .k0 = 4.081f / 4.0f,
+    };
+    return model;
+}
+
+void ChassisPowerInit(ChassisPowerInstance *instance, const ChassisPower_Init_Config_s *config)
+{
+    if (instance == 0) {
+        return;
+    }
+
+    memset(instance, 0, sizeof(ChassisPowerInstance));
+
+    instance->wheel_model = ChassisPowerGetDefault3508Model();
+    instance->default_power_limit = 60.0f;
+    instance->min_power_limit = 20.0f;
+    instance->buffer_low = 30.0f;
+    instance->buffer_high = 50.0f;
+    instance->buffer_gain = 0.25f;
+    instance->steering_power_reserve = 6.0f;
+    instance->min_scale = 0.20f;
+    instance->rls_enable = 0u;
+
+    float lambda = 0.995f;
+    float init_covariance = 1000.0f;
+
+    if (config != 0) {
+        instance->wheel_model = config->wheel_model;
+        if (config->default_power_limit > 0.0f) {
+            instance->default_power_limit = config->default_power_limit;
+        }
+        if (config->min_power_limit > 0.0f) {
+            instance->min_power_limit = config->min_power_limit;
+        }
+        if (config->buffer_low > 0.0f) {
+            instance->buffer_low = config->buffer_low;
+        }
+        if (config->buffer_high > instance->buffer_low) {
+            instance->buffer_high = config->buffer_high;
+        }
+        if (config->buffer_gain > 0.0f) {
+            instance->buffer_gain = config->buffer_gain;
+        }
+        if (config->steering_power_reserve >= 0.0f) {
+            instance->steering_power_reserve = config->steering_power_reserve;
+        }
+        if (config->min_scale > 0.0f && config->min_scale <= 1.0f) {
+            instance->min_scale = config->min_scale;
+        }
+        if (config->rls_lambda > 0.0f) {
+            lambda = config->rls_lambda;
+        }
+        if (config->rls_init_covariance > 0.0f) {
+            init_covariance = config->rls_init_covariance;
+        }
+        instance->rls_enable = config->rls_enable;
+    }
+
+    float theta[CHASSIS_POWER_RLS_ORDER] = {
+        instance->wheel_model.k_current_speed,
+        instance->wheel_model.k_speed2,
+        instance->wheel_model.k_current2,
+        instance->wheel_model.k0 * 2.0f,
+    };
+    RLSInit(&instance->rls, CHASSIS_POWER_RLS_ORDER, lambda, init_covariance, theta);
+    memcpy(instance->state.rls_theta, theta, sizeof(theta));
+}
+
+float ChassisPowerEstimateMotor(const ChassisPowerMotorModel_s *model, float speed, float current_cmd)
+{
+    if (model == 0) {
+        return 0.0f;
+    }
+
+    float power = model->k_current_speed * speed * current_cmd +
+                  model->k_speed2 * speed * speed +
+                  model->k_current2 * current_cmd * current_cmd +
+                  model->k0;
+
+    if (power < 0.0f) {
+        power = 0.0f;
+    }
+    return power;
+}
+
+static float ChassisPowerGetAllowedPower(ChassisPowerInstance *instance, const ChassisPower_Limit_Input_s *input)
+{
+    float allowed = input->referee_limit > 1.0f ? input->referee_limit : instance->default_power_limit;
+
+    if (input->referee_limit > 1.0f) {
+        if (input->power_buffer < instance->buffer_low) {
+            allowed -= (instance->buffer_low - input->power_buffer) * instance->buffer_gain;
+        } else if (input->power_buffer > instance->buffer_high) {
+            allowed += (input->power_buffer - instance->buffer_high) * instance->buffer_gain;
+        }
+    }
+
+    return ChassisPowerLimitValue(allowed, instance->min_power_limit, instance->default_power_limit + 20.0f);
+}
+
+void ChassisPowerUpdateMeasureState(ChassisPowerInstance *instance, const ChassisPower_Limit_Input_s *input)
+{
+    if (instance == 0 || input == 0) {
+        return;
+    }
+
+    ChassisPower_State_s *state = &instance->state;
+    const float rf_power = ChassisPowerEstimateMotor(&instance->wheel_model, input->rf_speed, input->rf_current_cmd);
+    const float lb_power = ChassisPowerEstimateMotor(&instance->wheel_model, input->lb_speed, input->lb_current_cmd);
+
+    state->rf_ref_raw = input->rf_speed_ref;
+    state->lb_ref_raw = input->lb_speed_ref;
+    state->rf_ref_limited = input->rf_speed_ref;
+    state->lb_ref_limited = input->lb_speed_ref;
+    state->rf_power = rf_power;
+    state->lb_power = lb_power;
+    state->estimated_wheel_power = rf_power + lb_power;
+    state->estimated_total_power = state->estimated_wheel_power;
+    state->allowed_power = ChassisPowerGetAllowedPower(instance, input);
+    state->wheel_budget = state->allowed_power;
+    state->scale = 1.0f;
+    state->measured_power = input->measured_power;
+    state->referee_power = input->referee_power;
+    state->power_buffer = input->power_buffer;
+    state->rf_speed = input->rf_speed;
+    state->lb_speed = input->lb_speed;
+    state->rf_current_cmd = input->rf_current_cmd;
+    state->lb_current_cmd = input->lb_current_cmd;
+    state->rf_output_cmd = input->rf_output_cmd;
+    state->lb_output_cmd = input->lb_output_cmd;
+    state->rf_feedback_current = input->rf_feedback_current;
+    state->lb_feedback_current = input->lb_feedback_current;
+    state->power_meter_online = input->power_meter_online;
+    state->limited = 0u;
+
+    state->features[0] = input->rf_speed * input->rf_current_cmd + input->lb_speed * input->lb_current_cmd;
+    state->features[1] = input->rf_speed * input->rf_speed + input->lb_speed * input->lb_speed;
+    state->features[2] = input->rf_current_cmd * input->rf_current_cmd + input->lb_current_cmd * input->lb_current_cmd;
+    state->features[3] = 1.0f;
+    chassis_power_debug_state = *state;
+}
+
+float ChassisPowerLimitWheelRefs(ChassisPowerInstance *instance, const ChassisPower_Limit_Input_s *input, float *rf_ref, float *lb_ref)
+{
+    if (instance == 0 || input == 0 || rf_ref == 0 || lb_ref == 0) {
+        return 1.0f;
+    }
+
+    ChassisPower_State_s *state = &instance->state;
+    const float rf_raw = *rf_ref;
+    const float lb_raw = *lb_ref;
+    const float allowed = ChassisPowerGetAllowedPower(instance, input);
+    float steering_power = input->steering_power_estimate;
+
+    if (steering_power <= 0.0f) {
+        steering_power = instance->steering_power_reserve;
+    }
+
+    const float rf_power = ChassisPowerEstimateMotor(&instance->wheel_model, input->rf_speed, input->rf_current_cmd);
+    const float lb_power = ChassisPowerEstimateMotor(&instance->wheel_model, input->lb_speed, input->lb_current_cmd);
+    const float wheel_power = rf_power + lb_power;
+    const float total_power = wheel_power + steering_power;
+    float wheel_budget = allowed - steering_power;
+    float scale = 1.0f;
+
+    if (wheel_budget < 1.0f) {
+        wheel_budget = 1.0f;
+    }
+
+    if (wheel_power > wheel_budget && wheel_power > 1.0e-3f) {
+        scale = sqrtf(wheel_budget / wheel_power);
+        scale = ChassisPowerLimitValue(scale, instance->min_scale, 1.0f);
+        *rf_ref = rf_raw * scale;
+        *lb_ref = lb_raw * scale;
+        state->limited = 1u;
+    } else {
+        state->limited = 0u;
+    }
+
+    state->rf_ref_raw = rf_raw;
+    state->lb_ref_raw = lb_raw;
+    state->rf_ref_limited = *rf_ref;
+    state->lb_ref_limited = *lb_ref;
+    state->rf_power = rf_power;
+    state->lb_power = lb_power;
+    state->estimated_wheel_power = wheel_power;
+    state->estimated_total_power = total_power;
+    state->allowed_power = allowed;
+    state->wheel_budget = wheel_budget;
+    state->scale = scale;
+    state->measured_power = input->measured_power;
+    state->referee_power = input->referee_power;
+    state->power_buffer = input->power_buffer;
+    state->rf_speed = input->rf_speed;
+    state->lb_speed = input->lb_speed;
+    state->rf_current_cmd = input->rf_current_cmd;
+    state->lb_current_cmd = input->lb_current_cmd;
+    state->rf_output_cmd = input->rf_output_cmd;
+    state->lb_output_cmd = input->lb_output_cmd;
+    state->rf_feedback_current = input->rf_feedback_current;
+    state->lb_feedback_current = input->lb_feedback_current;
+    state->power_meter_online = input->power_meter_online;
+
+    state->features[0] = input->rf_speed * input->rf_current_cmd + input->lb_speed * input->lb_current_cmd;
+    state->features[1] = input->rf_speed * input->rf_speed + input->lb_speed * input->lb_speed;
+    state->features[2] = input->rf_current_cmd * input->rf_current_cmd + input->lb_current_cmd * input->lb_current_cmd;
+    state->features[3] = 1.0f;
+    chassis_power_debug_state = *state;
+
+    return scale;
+}
+
+void ChassisPowerUpdateRLS(ChassisPowerInstance *instance, const ChassisPower_Limit_Input_s *input, float measured_wheel_power)
+{
+    if (instance == 0 || input == 0 || instance->rls_enable == 0u || measured_wheel_power <= 0.0f) {
+        return;
+    }
+
+    float features[CHASSIS_POWER_RLS_ORDER] = {
+        input->rf_speed * input->rf_current_cmd + input->lb_speed * input->lb_current_cmd,
+        input->rf_speed * input->rf_speed + input->lb_speed * input->lb_speed,
+        input->rf_current_cmd * input->rf_current_cmd + input->lb_current_cmd * input->lb_current_cmd,
+        1.0f,
+    };
+
+    RLSUpdate(&instance->rls, features, measured_wheel_power);
+    const float *theta = RLSGetTheta(&instance->rls);
+    if (theta == 0) {
+        return;
+    }
+
+    instance->wheel_model.k_current_speed = theta[0];
+    instance->wheel_model.k_speed2 = theta[1] > 0.0f ? theta[1] : 0.0f;
+    instance->wheel_model.k_current2 = theta[2] > 0.0f ? theta[2] : 0.0f;
+    instance->wheel_model.k0 = theta[3] > 0.0f ? theta[3] * 0.5f : 0.0f;
+    memcpy(instance->state.rls_theta, theta, sizeof(instance->state.rls_theta));
+    chassis_power_debug_state = instance->state;
+}
+
+void ChassisPowerSetWheelModel(ChassisPowerInstance *instance, const ChassisPowerMotorModel_s *model)
+{
+    if (instance == 0 || model == 0) {
+        return;
+    }
+    instance->wheel_model = *model;
+}
+
+const ChassisPower_State_s *ChassisPowerGetState(const ChassisPowerInstance *instance)
+{
+    return instance != 0 ? &instance->state : 0;
+}
--- a/modules/chassis_power/chassis_power.h
+++ b/modules/chassis_power/chassis_power.h
@@ -0,0 +1,109 @@
+#ifndef CHASSIS_POWER_H
+#define CHASSIS_POWER_H
+
+#include <stdint.h>
+
+#include "rls.h"
+
+#define CHASSIS_POWER_RLS_ORDER 4u
+
+typedef struct
+{
+    float k_current_speed;
+    float k_speed2;
+    float k_current2;
+    float k0;
+} ChassisPowerMotorModel_s;
+
+typedef struct
+{
+    ChassisPowerMotorModel_s wheel_model;
+    float default_power_limit;
+    float min_power_limit;
+    float buffer_low;
+    float buffer_high;
+    float buffer_gain;
+    float steering_power_reserve;
+    float min_scale;
+    float rls_lambda;
+    float rls_init_covariance;
+    uint8_t rls_enable;
+} ChassisPower_Init_Config_s;
+
+typedef struct
+{
+    float rf_speed_ref;
+    float lb_speed_ref;
+    float rf_speed;
+    float lb_speed;
+    float rf_current_cmd;
+    float lb_current_cmd;
+    float rf_output_cmd;
+    float lb_output_cmd;
+    float rf_feedback_current;
+    float lb_feedback_current;
+    float referee_limit;
+    float referee_power;
+    float power_buffer;
+    float measured_power;
+    float steering_power_estimate;
+    uint8_t power_meter_online;
+} ChassisPower_Limit_Input_s;
+
+typedef struct
+{
+    float rf_ref_raw;
+    float lb_ref_raw;
+    float rf_ref_limited;
+    float lb_ref_limited;
+    float rf_power;
+    float lb_power;
+    float estimated_wheel_power;
+    float estimated_total_power;
+    float allowed_power;
+    float wheel_budget;
+    float scale;
+    float measured_power;
+    float referee_power;
+    float power_buffer;
+    float rf_speed;
+    float lb_speed;
+    float rf_current_cmd;
+    float lb_current_cmd;
+    float rf_output_cmd;
+    float lb_output_cmd;
+    float rf_feedback_current;
+    float lb_feedback_current;
+    float features[CHASSIS_POWER_RLS_ORDER];
+    float rls_theta[CHASSIS_POWER_RLS_ORDER];
+    uint8_t limited;
+    uint8_t power_meter_online;
+} ChassisPower_State_s;
+
+typedef struct
+{
+    ChassisPowerMotorModel_s wheel_model;
+    RLSInstance rls;
+    ChassisPower_State_s state;
+    float default_power_limit;
+    float min_power_limit;
+    float buffer_low;
+    float buffer_high;
+    float buffer_gain;
+    float steering_power_reserve;
+    float min_scale;
+    uint8_t rls_enable;
+} ChassisPowerInstance;
+
+extern ChassisPower_State_s chassis_power_debug_state;
+
+ChassisPowerMotorModel_s ChassisPowerGetDefault3508Model(void);
+void ChassisPowerInit(ChassisPowerInstance *instance, const ChassisPower_Init_Config_s *config);
+float ChassisPowerEstimateMotor(const ChassisPowerMotorModel_s *model, float speed, float current_cmd);
+void ChassisPowerUpdateMeasureState(ChassisPowerInstance *instance, const ChassisPower_Limit_Input_s *input);
+float ChassisPowerLimitWheelRefs(ChassisPowerInstance *instance, const ChassisPower_Limit_Input_s *input, float *rf_ref, float *lb_ref);
+void ChassisPowerUpdateRLS(ChassisPowerInstance *instance, const ChassisPower_Limit_Input_s *input, float measured_wheel_power);
+void ChassisPowerSetWheelModel(ChassisPowerInstance *instance, const ChassisPowerMotorModel_s *model);
+const ChassisPower_State_s *ChassisPowerGetState(const ChassisPowerInstance *instance);
+
+#endif
--- a/modules/imu/ins_task.c
+++ b/modules/imu/ins_task.c
@@ -135,7 +135,6 @@ void INS_Task(void)
        INS.Gyro[X] = BMI088.Gyro[X];
        INS.Gyro[Y] = BMI088.Gyro[Y];
        INS.Gyro[Z] = BMI088.Gyro[Z];
-
        // demo function,用于修正安装误差,可以不管,本demo暂时没用
        IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);

--- a/modules/motor/DJImotor/dji_motor.c
+++ b/modules/motor/DJImotor/dji_motor.c
@@ -27,9 +27,7 @@ static CANInstance sender_assignment[6] = {
    [4] = {.can_handle = &hcan2, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
    [5] = {.can_handle = &hcan2, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
 };
-static CANInstance DM_sender_assignment[1] = {
-    [0] = {.can_handle = &hcan2, .txconf.StdId = 0x3FE, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}}
-};
+
 /**
 * @brief 6个用于确认是否有电机注册到sender_assignment中的标志位,防止发送空帧,此变量将在DJIMotorControl()使用
 *        flag的初始化在 MotorSenderGrouping()中进行
@@ -323,25 +321,15 @@ void DJIMotorControl()
        // 获取最终输出
        set = (int16_t)pid_ref;

-
        // 分组填入发送数据
        group = motor->sender_group;
        num = motor->message_num;
-
-        if (i == 1)//pitch轴电机对应i值为1,需要特殊处理,因为它的发送顺序和其他电机不同,在同一帧中与其他电机的高八位和低八位相反
-        {
-            DM_sender_assignment[0].tx_buff[6] = (uint8_t)(set & 0x00ff); // 高八位
-            DM_sender_assignment[0].tx_buff[7] = (uint8_t)(set >> 8); // 低八位
-        }
-        else
-        {
        sender_assignment[group].tx_buff[2 * num] = (uint8_t)(set >> 8);         // 低八位
        sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff); // 高八位
-        }

        // 若该电机处于停止状态,直接将buff置零
        if (motor->stop_flag == MOTOR_STOP)
-            memset(sender_assignment[group].tx_buff + 2 * num, 0, 2u);
+            memset(sender_assignment[group].tx_buff + 2 * num, 0, sizeof(uint16_t));
    }

    // 遍历flag,检查是否要发送这一帧报文
@@ -350,7 +338,6 @@ void DJIMotorControl()
        if (sender_enable_flag[i])
        {
            CANTransmit(&sender_assignment[i], 1);
-            CANTransmit(&DM_sender_assignment[0], 1);//达妙电机单独一帧发送
        }
    }
 }
--- a/modules/power_meter/power_meter.c
+++ b/modules/power_meter/power_meter.c
@@ -0,0 +1,105 @@
+#include "power_meter.h"
+
+#include <stdlib.h>
+#include <string.h>
+
+#include "bsp_dwt.h"
+
+static void PowerMeterLostCallback(void *meter_ptr)
+{
+    PowerMeterInstance *meter = (PowerMeterInstance *)meter_ptr;
+    if (meter != 0) {
+        meter->online = 0u;
+    }
+}
+
+static void PowerMeterRxCallback(CANInstance *can)
+{
+    if (can == 0 || can->id == 0 || can->rx_len < 4u) {
+        return;
+    }
+
+    PowerMeterInstance *meter = (PowerMeterInstance *)can->id;
+    const uint8_t *data = can->rx_buff;
+    const float scale = meter->scale > 0.0f ? meter->scale : POWER_METER_DEFAULT_SCALE;
+
+    meter->raw_current = (uint16_t)(((uint16_t)data[1] << 8) | data[0]);
+    meter->raw_voltage = (uint16_t)(((uint16_t)data[3] << 8) | data[2]);
+    meter->current = (float)meter->raw_current / scale;
+    meter->voltage = (float)meter->raw_voltage / scale;
+    meter->power = meter->voltage * meter->current;
+    meter->update_count++;
+    meter->last_update_time_ms = DWT_GetTimeline_ms();
+    meter->online = 1u;
+
+    if (meter->daemon != 0) {
+        DaemonReload(meter->daemon);
+    }
+}
+
+PowerMeterInstance *PowerMeterInit(PowerMeter_Init_Config_s *config)
+{
+    if (config == 0 || config->can_config.can_handle == 0) {
+        return 0;
+    }
+
+    PowerMeterInstance *meter = (PowerMeterInstance *)malloc(sizeof(PowerMeterInstance));
+    if (meter == 0) {
+        return 0;
+    }
+    memset(meter, 0, sizeof(PowerMeterInstance));
+
+    meter->scale = config->scale > 0.0f ? config->scale : POWER_METER_DEFAULT_SCALE;
+    if (config->can_config.rx_id == 0u) {
+        config->can_config.rx_id = POWER_METER_DEFAULT_CAN_ID;
+    }
+    if (config->can_config.tx_id == 0u) {
+        config->can_config.tx_id = config->can_config.rx_id;
+    }
+
+    config->can_config.id = meter;
+    config->can_config.can_module_callback = PowerMeterRxCallback;
+    meter->can_ins = CANRegister(&config->can_config);
+
+    Daemon_Init_Config_s daemon_config = {
+        .callback = PowerMeterLostCallback,
+        .owner_id = meter,
+        .reload_count = 5u,
+        .init_count = 100u,
+    };
+    meter->daemon = DaemonRegister(&daemon_config);
+
+    return meter;
+}
+
+PowerMeterInstance *PowerMeterInitCan(CAN_HandleTypeDef *can_handle, uint32_t rx_id)
+{
+    PowerMeter_Init_Config_s config = {
+        .can_config = {
+            .can_handle = can_handle,
+            .tx_id = rx_id,
+            .rx_id = rx_id,
+        },
+        .scale = POWER_METER_DEFAULT_SCALE,
+    };
+
+    return PowerMeterInit(&config);
+}
+
+const PowerMeterInstance *PowerMeterGetData(PowerMeterInstance *meter)
+{
+    if (meter == 0) {
+        return 0;
+    }
+
+    meter->online = PowerMeterIsOnline(meter);
+    return meter;
+}
+
+uint8_t PowerMeterIsOnline(PowerMeterInstance *meter)
+{
+    if (meter == 0 || meter->daemon == 0) {
+        return 0u;
+    }
+    return DaemonIsOnline(meter->daemon);
+}
--- a/modules/power_meter/power_meter.h
+++ b/modules/power_meter/power_meter.h
@@ -0,0 +1,38 @@
+#ifndef POWER_METER_H
+#define POWER_METER_H
+
+#include <stdint.h>
+
+#include "bsp_can.h"
+#include "daemon.h"
+
+#define POWER_METER_DEFAULT_CAN_ID 0x213u
+#define POWER_METER_DEFAULT_SCALE 100.0f
+
+typedef struct
+{
+    CAN_Init_Config_s can_config;
+    float scale;
+} PowerMeter_Init_Config_s;
+
+typedef struct
+{
+    CANInstance *can_ins;
+    DaemonInstance *daemon;
+    float voltage;
+    float current;
+    float power;
+    float scale;
+    uint16_t raw_voltage;
+    uint16_t raw_current;
+    uint32_t update_count;
+    float last_update_time_ms;
+    uint8_t online;
+} PowerMeterInstance;
+
+PowerMeterInstance *PowerMeterInit(PowerMeter_Init_Config_s *config);
+PowerMeterInstance *PowerMeterInitCan(CAN_HandleTypeDef *can_handle, uint32_t rx_id);
+const PowerMeterInstance *PowerMeterGetData(PowerMeterInstance *meter);
+uint8_t PowerMeterIsOnline(PowerMeterInstance *meter);
+
+#endif
--- a/modules/referee/referee_task.c
+++ b/modules/referee/referee_task.c
@@ -57,7 +57,7 @@ static Graph_Data_t UI_shoot_line[10]; // 射击准线
 static Graph_Data_t UI_Energy[3];      // 电容能量条
 static String_Data_t UI_State_sta[6];  // 机器人状态,静态只需画一次
 static String_Data_t UI_State_dyn[6];  // 机器人状态,动态先add才能change
-static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565};
+static uint32_t shoot_line_location[10] = {525, 950, 490, 515, 565};

 void MyUIInit()
 {
@@ -66,19 +66,19 @@ void MyUIInit()
    while (referee_recv_info->GameRobotState.robot_id == 0)
        osDelay(100); // 若还未收到裁判系统数据,等待一段时间后再检查

-    DeterminRobotID();                                            // 确定ui要发送到的目标客户端
+    DeterminRobotID();                                            // 确定ui要发送到的目标客户端.
    UIDelete(&referee_recv_info->referee_id, UI_Data_Del_ALL, 0); // 清空UI

    // 绘制发射基准线
-    UILineDraw(&UI_shoot_line[0], "sl0", UI_Graph_ADD, 7, UI_Color_White, 3, 710, shoot_line_location[0], 1210, shoot_line_location[0]);
+    //UILineDraw(&UI_shoot_line[0], "sl0", UI_Graph_ADD, 7, UI_Color_White, 3, 710, shoot_line_location[0], 1210, shoot_line_location[0]);
    UILineDraw(&UI_shoot_line[1], "sl1", UI_Graph_ADD, 7, UI_Color_White, 3, shoot_line_location[1], 340, shoot_line_location[1], 740);
    UILineDraw(&UI_shoot_line[2], "sl2", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[2], 1110, shoot_line_location[2]);
-    UILineDraw(&UI_shoot_line[3], "sl3", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[3], 1110, shoot_line_location[3]);
-    UILineDraw(&UI_shoot_line[4], "sl4", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[4], 1110, shoot_line_location[4]);
+    UILineDraw(&UI_shoot_line[3], "sl3", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 860, 475, 1060, 475);
+    //UILineDraw(&UI_shoot_line[4], "sl4", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[4], 1110, shoot_line_location[4]);
    UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_shoot_line[0], UI_shoot_line[1], UI_shoot_line[2], UI_shoot_line[3], UI_shoot_line[4]);

    // 绘制车辆状态标志指示
-    UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 150, 750, "chassis:");
+    UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 150, 750, "level:");
    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[0]);
    UICharDraw(&UI_State_sta[1], "ss1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 150, 700, "gimbal:");
    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[1]);
@@ -91,7 +91,7 @@ void MyUIInit()

    // 绘制车辆状态标志，动态
    // 由于初始化时xxx_last_mode默认为0，所以此处对应UI也应该设为0时对应的UI，防止模式不变的情况下无法置位flag，导致UI无法刷新
-    UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce");
+    UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 270, 750, "1");
    UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]);
    UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce");
    UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
@@ -116,7 +116,7 @@ void MyUIInit()
    UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
 }

-// 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
+// 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常.
 static uint8_t count = 0;
 static uint16_t count1 = 0;
 static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测试用函数，实现模式自动变化
@@ -176,27 +176,15 @@ static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测
 static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data)
 {
    UIChangeCheck(_Interactive_data);
-    // chassis
-    if (_Interactive_data->Referee_Interactive_Flag.chassis_flag == 1)
+    // robot level
+    if (_Interactive_data->Referee_Interactive_Flag.robot_level_flag == 1)
    {
-        switch (_Interactive_data->chassis_mode)
-        {
-        case CHASSIS_ZERO_FORCE:
-            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce");
-            break;
-        case CHASSIS_ROTATE:
-            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "rotate   ");
-            // 此处注意字数对齐问题，字数相同才能覆盖掉
-            break;
-        case CHASSIS_NO_FOLLOW:
-            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "nofollow ");
-            break;
-        case CHASSIS_FOLLOW_GIMBAL_YAW:
-            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "follow   ");
-            break;
-        }
+        char level_str[2];
+        level_str[0] = '0' + _Interactive_data->robot_level;
+        level_str[1] = '\0';
+        UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, level_str);
        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]);
-        _Interactive_data->Referee_Interactive_Flag.chassis_flag = 0;
+        _Interactive_data->Referee_Interactive_Flag.robot_level_flag = 0;
    }
    // gimbal
    if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1)
@@ -261,6 +249,12 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
 */
 static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
 {
+    if (_Interactive_data->robot_level != _Interactive_data->robot_last_level)
+    {
+        _Interactive_data->Referee_Interactive_Flag.robot_level_flag = 1;
+        _Interactive_data->robot_last_level = _Interactive_data->robot_level;
+    }
+
    if (_Interactive_data->chassis_mode != _Interactive_data->chassis_last_mode)
    {
        _Interactive_data->Referee_Interactive_Flag.chassis_flag = 1;
--- a/modules/referee/rm_referee.h
+++ b/modules/referee/rm_referee.h
@@ -54,6 +54,7 @@ typedef struct
 	uint32_t lid_flag : 1;
 	uint32_t friction_flag : 1;
 	uint32_t Power_flag : 1;
+	uint32_t robot_level_flag : 1;
 } Referee_Interactive_Flag_t;

 // 此结构体包含UI绘制与机器人车间通信的需要的其他非裁判系统数据
@@ -61,6 +62,7 @@ typedef struct
 {
 	Referee_Interactive_Flag_t Referee_Interactive_Flag;
 	// 为UI绘制以及交互数据所用
+	uint8_t robot_level;					 // 机器人等级
 	chassis_mode_e chassis_mode;			 // 底盘模式
 	gimbal_mode_e gimbal_mode;				 // 云台模式
 	shoot_mode_e shoot_mode;				 // 发射模式设置
@@ -69,6 +71,7 @@ typedef struct
 	Chassis_Power_Data_s Chassis_Power_Data; // 功率控制

 	// 上一次的模式，用于flag判断
+	uint8_t robot_last_level;
 	chassis_mode_e chassis_last_mode;
 	gimbal_mode_e gimbal_last_mode;
 	shoot_mode_e shoot_last_mode;
--- a/modules/remote/remote_control.c
+++ b/modules/remote/remote_control.c
@@ -20,32 +20,13 @@ static DaemonInstance *rc_daemon_instance;
 * @brief 矫正遥控器摇杆的值,超过660或者小于-660的值都认为是无效值,置0
 *
 */
-// static void RectifyRCjoystick()
-// {
-//     for (uint8_t i = 0; i < 5; ++i)
-//         if (abs(*(&rc_ctrl[TEMP].rc.rocker_l_ + i)) > 660)
-//             *(&rc_ctrl[TEMP].rc.rocker_l_ + i) = 0;
-// }
-static void RectifyRCjoystick()//左遥杆水平向右限幅不够，因为下面的+3，将+3删除就用上面的
+static void RectifyRCjoystick()
 {
-    int16_t *rocker_ptr = &rc_ctrl[TEMP].rc.rocker_l_;
    for (uint8_t i = 0; i < 5; ++i)
-    {
-        if (i == 0) // 左遥杆水平方向
-        {
-            // 增加右向最大值到700，左向保持660不变
-            if (rocker_ptr[i] > 700)
-                rocker_ptr[i] = 700;
-            else if (rocker_ptr[i] < -660)
-                rocker_ptr[i] = -660;
-        }
-        else // 其他通道保持原限制
-        {
-            if (abs(rocker_ptr[i]) > 660)
-                rocker_ptr[i] = 0;
-        }
-    }
+        if (abs(*(&rc_ctrl[TEMP].rc.rocker_l_ + i)) > 660)
+            *(&rc_ctrl[TEMP].rc.rocker_l_ + i) = 0;
 }
+
 /**
 * @brief 遥控器数据解析
 *
@@ -59,7 +40,7 @@ static void sbus_to_rc(const uint8_t *sbus_buf)
    rc_ctrl[TEMP].rc.rocker_l_ = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 2
    rc_ctrl[TEMP].rc.rocker_l1 = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff) - RC_CH_VALUE_OFFSET;                       //!< Channel 3
    rc_ctrl[TEMP].rc.dial = ((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF) - RC_CH_VALUE_OFFSET;
-    rc_ctrl[TEMP].rc.rocker_l_= rc_ctrl[TEMP].rc.rocker_l_+3;
+    rc_ctrl[TEMP].rc.rocker_l_= rc_ctrl[TEMP].rc.rocker_l_+0.5;
    // 左侧拨轮
    RectifyRCjoystick();
    // 开关,0左1右
Author	SHA1	Message	Date
guoxy06	a9fe5069f4	Merge remote-tracking branch 'origin/codex/chassis-power-control' into codex/chassis-power-control	2026-08-29 19:58:09 +08:00
guoxy06	c40525234f	半舵·正式发售	2026-08-29 19:55:45 +08:00
guoxy06	63d4b7ae7d	半舵旗舰版（功率控制有更改）	2026-07-26 11:27:00 +08:00
guoxy06	2b18d60476	半舵旗舰版	2026-05-26 19:43:23 +08:00
guoxy06	72df1a2654	小陀螺旗舰版	2026-04-29 19:43:54 +08:00
guoxy06	428956e3bd	小陀螺家庭专业版	2026-04-24 21:16:42 +08:00
guoxy06	214971c58b	小陀螺前瞻版	2026-04-22 20:45:40 +08:00
guoxy06	194047605d	小陀螺前瞻版	2026-04-22 20:45:40 +08:00
guoxy06	dc1152e186	底盘正常移动，小陀螺降速方便调	2026-04-22 20:45:40 +08:00
guoxy06	90bdda348e	底盘正常移动，小陀螺降速方便调	2026-04-22 20:45:40 +08:00
guoxy06	e8f4bf3cf2	改底盘（疯2）	2026-04-22 20:45:40 +08:00
guoxy06	e783abf8d6	改底盘（疯2）	2026-04-22 20:45:40 +08:00
guoxy06	579b507e3b	改底盘（疯）	2026-04-22 20:45:40 +08:00
guoxy06	f8282c80b9	改底盘1	2026-04-22 20:45:40 +08:00
guoxy06	47aba369d8	改底盘	2026-04-22 20:45:40 +08:00
fcl	be8f3f38c8	no.second	2026-04-22 20:45:40 +08:00
fcl	8e1e2455e0	no.firsr	2026-04-22 20:45:39 +08:00
Zorua	e66dbb8052	变量名更正	2026-04-22 20:45:39 +08:00
guoxy06	a6c7ccd2a1	1V1用	2026-04-22 20:45:39 +08:00
guoxy06	3d7a4592c4	赛场_v1.2	2026-04-22 20:45:39 +08:00
guoxy06	99bfe7cec7	ui init	2026-04-22 20:45:39 +08:00
guoxy06	69b407db26	方案——赛场_1.1	2026-04-22 20:45:39 +08:00
guoxy06	d1d5698c13	临时方案——赛场	2026-04-22 20:45:39 +08:00
Zorua	31ffd2bde4	救火版本	2026-04-22 20:45:39 +08:00
Zorua	b660cbb9f9	救火版本	2026-04-22 20:45:39 +08:00
Zorua	772ce14ae6	救火版本	2026-04-22 20:45:39 +08:00
guoxy06	0eae0777ca	功率，热量	2026-04-22 20:45:39 +08:00
guoxy06	300048362f	解决键鼠发射，现在键鼠模式小陀螺移动有点问题	2026-04-22 20:45:39 +08:00
Zorua	1f4e8d85b9	键鼠模式底盘方向修正，需优化底盘运动	2026-04-22 20:45:39 +08:00
guoxy06	b355992730	单发模式前几发会连发，后续正常，先急停再启动也会出现相同问题	2026-04-22 20:45:39 +08:00
fcl	e8dde8b825	累计问题解决，拨盘电机仍有问题	2026-04-22 20:45:39 +08:00
fcl	df0e1f4b1f	零点感觉还可以...	2026-04-22 20:45:39 +08:00
fcl	277b3a11c0	all ok but loader reversed	2026-04-22 20:45:39 +08:00
fcl	9e7faaca8b	fricok. loader failure	2026-04-22 20:45:39 +08:00
fcl	43b724d7b7	fricok butcant stop instantly.loader failure	2026-04-22 20:45:39 +08:00
guoxy06	a61ed6ff25	摩擦轮，拨盘 driver	2026-04-22 20:45:39 +08:00
Zorua	31eef65a6d	6020pitch driver	2026-04-22 20:45:39 +08:00
Zorua	0235c428c5	damiao motor driver init_1.1	2026-04-22 20:45:39 +08:00
Zorua	4b17e1e193	damiao motor driver init_1	2026-04-22 20:45:39 +08:00
Zorua	9d4e103a01	damiao motor driver init	2026-04-22 20:45:38 +08:00
fcl	082bf3199a	changed pid param for steer motor	2026-04-22 20:45:38 +08:00
guoxy06	3add7c5eb2	4310能转，但不受遥控控制	2026-04-22 20:45:38 +08:00
fcl	bd9314aa7a	reast	2026-04-22 20:45:38 +08:00
fcl	dc0d98d0a6	Revert "supercap can1" This reverts commit `69e97cbefa`.	2026-04-22 20:45:38 +08:00
fcl	d3c9c04c2a	待测版本（ID修改完成）	2026-04-22 20:45:38 +08:00
fcl	933e86e1bb	待测版本（ID修改完成）	2026-04-22 20:45:38 +08:00
fcl	1a537d7f79	待测版本（ID修改完成）	2026-04-22 20:45:38 +08:00
TuxMonkey	42c26fa8ac	supercap can1	2026-04-22 20:45:38 +08:00
TuxMonkey	d6c91d7b6f	supercap can1	2026-04-22 20:45:38 +08:00
TuxMonkey	24d718a755	supercap can1	2026-04-22 20:45:38 +08:00
TuxMonkey	a541614346	supercap	2026-04-22 20:45:38 +08:00
TuxMonkey	351dfc6b71	!2 delete +3 Merge pull request !2 from TuxMonkey/dev	2026-03-09 18:25:42 +00:00