半舵旗舰版（功率控制有更改）

This reverts commit 69e97cbefa.

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"

application/chassis/chassis.c

@@ -17,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"
@@ -25,24 +28,38 @@
 #include "bsp_dwt.h"
 #include "referee_UI.h"
 #include "arm_math.h"
+#include <math.h>
 
-// =================【功率模型参数】=================
+#ifndef CHASSIS_POWER_LIMIT_ENABLE
-// ⚠️警告：以下参数原本是群星战队为 DJI M3508 电机拟合的。
+#define CHASSIS_POWER_LIMIT_ENABLE 0
-// 你们的驱动电机是 LK9015，它的扭矩常数、内阻、空载损耗和 M3508 不同！
+#endif
-// 建议后期针对 LK9015 重新拟合这四个参数以达到完美的控制效果。
+
-#define TOQUE_COEFF 1.99688994e-6f
+#ifndef CHASSIS_POWER_USE_POWER_METER
-#define K1 1.23e-07f
+#define CHASSIS_POWER_USE_POWER_METER 1
-#define K2 1.453e-07f
+#endif
-#define CONSTANT_POWER 4.081f
+
-#define WARRING_ENERGY_BUFFER 50.0f
+#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
@@ -168,12 +185,12 @@ void ChassisInit()
         .Ki                = 0,
         .Kd                = 0.1f, // 0.5
         .DeadBand          = 0.5,
-        .CoefA             = 0.2,
+        // .CoefA             = 0.2,
-        .CoefB             = 0.3,
+        // .CoefB             = 0.3,
-        .Improve           = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement,
+        // .Improve           = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement,
-        .IntegralLimit     = 50, // 200
+        // .IntegralLimit     = 50, // 200
-        .MaxOut            = 1200,
+        .MaxOut            = 50,
-        .Derivative_LPF_RC = 0.01, // 0.01
+        // .Derivative_LPF_RC = 0.01, // 0.01
     };
     PIDInit(&chassis_follow_pid, &chassis_follow_pid_conf);
 
@@ -190,7 +207,27 @@ void ChassisInit()
     cap = SuperCapInit(&cap_conf); // 超级电容初始化
 
     // （新增）设置超级电容功率补偿，例如补偿5W，可根据实际需求调整
-    // SuperCapSetPowerOffset(cap, 5);
+    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
@@ -295,7 +332,8 @@ static void SteeringWheelCalculate()
         vt_rf = 0, vt_lb = 0;
     } else {
         // 生成预计算变量，减少计算量，空间换时间
-        float w      = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET * SQRT2;
+        //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;与上次相同因此注释
@@ -348,102 +386,136 @@ static void SteeringWheelCalculate()
  * @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值
  *
  */
-/**
- * @brief 根据裁判系统和电容剩余容量对输出进行限制，并应用 WJTLU 功率模型
- */
 static void LimitChassisOutput()
 {
-    float chassis_max_power = 0;
+#if CHASSIS_POWER_LIMIT_ENABLE
-    float chassis_set_power = 90.0f; // 设定的期望功率，可根据需求修改
+    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 = {
-    uint16_t referee_power_limit = referee_data->GameRobotState.chassis_power_limit;
+        .rf_speed_ref = vt_rf,
-    float energy_buffer = referee_data->PowerHeatData.chassis_power_buffer;
+        .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;
 
-    // 1. ============ 超级电容与裁判系统功率统筹 ============
+    ChassisPowerUpdateMeasureState(&chassis_power_control, &power_input);
-    if (SuperCapIsOnline(cap)) {
+    ChassisPowerLimitWheelRefs(&chassis_power_control, &power_input, &limited_rf, &limited_lb);
-        SuperCap_Msg_s cap_msg = SuperCapGet(cap);
+    if (power_input.power_meter_online) {
-        if (cap_msg.ready == READY) {
+        ChassisPowerUpdateRLS(&chassis_power_control, &power_input, power_input.measured_power);
-            // 超电正常可用，动态按比例缩放允许功率
-            if (cap_msg.chassis_power > chassis_set_power) {
-                chassis_max_power = chassis_set_power * (chassis_set_power / (float)cap_msg.chassis_power);
-            } else {
-                chassis_max_power = chassis_set_power;
-            }
-        } else {
-            // 超电保护中，使用裁判系统限制
-            if (cap_msg.chassis_power > referee_power_limit) {
-                chassis_max_power = referee_power_limit * ((float)referee_power_limit / (float)cap_msg.chassis_power);
-            } else {
-                chassis_max_power = referee_power_limit;
-            }
-        }
-
-        // 缓冲能量补偿逻辑
-        uint8_t power_offset = 0;
-        if (energy_buffer < WARRING_ENERGY_BUFFER) {
-            power_offset = (uint8_t)((WARRING_ENERGY_BUFFER - energy_buffer) * 0.2f);
-        }
-        SuperCapSetPowerOffset(cap, power_offset);
-        chassis_max_power -= power_offset;
-    } else {
-        // 超电离线时逻辑
-        chassis_max_power = (energy_buffer < WARRING_ENERGY_BUFFER) ?
-                            (referee_power_limit * energy_buffer * 0.02f) : referee_power_limit;
     }
 
-    // 2. ============ 基于 LK9015 的 WJTLU 二次功率模型限流 ============
+    vt_rf = limited_rf;
-    float initial_total_power = 0;
+    vt_lb = limited_lb;
-    float initial_motor_power[2];
-    LKMotorInstance* motors[2] = {motor_rf, motor_lb}; // 注意舵轮底盘只有两个驱动轮
-    float current_pid_out[2];
 
-    for (int i = 0; i < 2; i++) {
+#if CHASSIS_POWER_VOFA_ENABLE
-        // 修正 1：LK 电机驱动中，speed_PID 是实例的直接成员
+    static uint16_t vofa_divider = 0u;
-        current_pid_out[i] = motors[i]->speed_PID.Output;
+    if (++vofa_divider >= CHASSIS_POWER_VOFA_DIV) {
-
+        vofa_divider = 0u;
-        // 修正 2：LK 的速度单位是 rad/s，WJTLU模型要求 RPM，进行单位转换 (rad/s * 30/pi ≈ RPM)
+        const ChassisPower_State_s *power_state = ChassisPowerGetState(&chassis_power_control);
-        float rpm = motors[i]->measure.speed_rads * 9.549296f;
+        float vofa_data[] = {
-
+            power_state->estimated_total_power,
-        initial_motor_power[i] = current_pid_out[i] * TOQUE_COEFF * rpm +
+            power_state->allowed_power,
-                                 K1 * current_pid_out[i] * current_pid_out[i] +
+            power_state->scale,
-                                 K2 * rpm * rpm + CONSTANT_POWER;
+            power_input.measured_power,
-
+            meter_data != 0 ? meter_data->voltage : 0.0f,
-        if (initial_motor_power[i] > 0) initial_total_power += initial_motor_power[i];
+            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
 
-    // 3. 动态修改电机 PID 的 MaxOut 属性
+    ChassisPower_Limit_Input_s power_input = {
-    if (initial_total_power > chassis_max_power && initial_total_power > 0) {
+        .rf_speed_ref = vt_rf,
-        float scale = chassis_max_power / initial_total_power;
+        .lb_speed_ref = vt_lb,
-        for (int i = 0; i < 2; i++) {
+        .rf_speed = motor_rf->measure.speed_rads,
-            float scaled_p = initial_motor_power[i] * scale;
+        .lb_speed = motor_lb->measure.speed_rads,
-            if (scaled_p <= CONSTANT_POWER) {
+        .rf_current_cmd = motor_rf->speed_PID.Output,
-                motors[i]->speed_PID.MaxOut = 0; // 该轮分配不到功率
+        .lb_current_cmd = motor_lb->speed_PID.Output,
-                continue;
+        .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
-            float rpm = motors[i]->measure.speed_rads * 9.549296f;
+    static uint16_t vofa_divider = 0u;
-            float b = TOQUE_COEFF * rpm;
+    if (++vofa_divider >= CHASSIS_POWER_VOFA_DIV) {
-            float c = K2 * rpm * rpm - scaled_p + CONSTANT_POWER;
+        vofa_divider = 0u;
-            float delta = b * b - 4 * K1 * c;
+        const ChassisPower_State_s *power_state = ChassisPowerGetState(&chassis_power_control);
-
+        float vofa_data[] = {
-            if (delta >= 0) {
+            power_state->estimated_total_power,
-                // 解方程获得当前允许的最大电流
+            power_state->allowed_power,
-                float out_limit = (-b + sqrtf(delta)) / (2.0f * K1);
+            power_state->scale,
-                // 修正 3：动态限制 LK 电机 speed_PID 的最大输出（LK9015 默认最大输出为 12000）
+            power_input.measured_power,
-                motors[i]->speed_PID.MaxOut = fminf(out_limit, 12000.0f);
+            meter_data != 0 ? meter_data->voltage : 0.0f,
-            }
+            meter_data != 0 ? meter_data->current : 0.0f,
-        }
+            power_state->rf_ref_raw,
-    } else {
+            power_state->rf_ref_limited,
-        // 功率充裕时，恢复电机的默认最大输出
+            power_state->lb_ref_raw,
-        motor_rf->speed_PID.MaxOut = 12000.0f;
+            power_state->lb_ref_limited,
-        motor_lb->speed_PID.MaxOut = 12000.0f;
+            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;
 
-    // 4. 下发目标速度（限流操作已通过改变 MaxOut 动态生效）
+    // 完成功率限制后进行电机参考输入设定
-    LKMotorSetRef(motor_rf, vt_rf);
+
-    LKMotorSetRef(motor_lb, vt_lb);
+    // LKMotorSetRef( motor_lf , vt_lf );
+    // LKMotorSetRef( motor_rb , vt_rb );
+
+    LKMotorSetRef( motor_rf , vt_rf );
+    LKMotorSetRef( motor_lb , vt_lb );
 }
 
 /**
@@ -502,53 +574,68 @@ void ChassisTask() {
             {
                 single_round_offset += 360.0f;
             }
-            chassis_cmd_recv.wz = -PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/70.0f;
+            chassis_cmd_recv.wz = -PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/60.0f;
             break;
         case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
-            // chassis_cmd_recv.wz = 0.75;
+            chassis_cmd_recv.wz = 1.00;//实验版
-            chassis_cmd_recv.wz = -0.75;//实验版
             break;
         default:
             break;
     }
+    float sin_theta = 0.0f, cos_theta = 1.0f;
+    // ------------------ 【修改部分：加入相位滞后动态补偿】 ------------------
+    // 提取当前的云台-底盘夹角
+    float real_offset_angle = chassis_cmd_recv.offset_angle;
 
-    // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
+    // 如果处于小陀螺模式（或者 wz 不为 0），根据旋转速度进行超前补偿
-    // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东.)
+    if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE )
-    static float sin_theta, cos_theta;
+    {
-    cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
+        // 补偿系数 K 需要实车微调。
-    sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
+        // 根据你的描述 wz=0.95 时偏差约45度，初始 K 值可以设为 45.0 / 0.95 ≈ 47.3f
-    // chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
+        float lag_compensation_K = 47.3f;//47.3f;45.0f
-    // chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
+
-    chassis_vy = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;//实验版
+        // 符号(+或-)取决于你的电机正方向、底盘旋转正方向，如果不正就改成减号
-     chassis_vx = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
+        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)/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的情况
     // chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
     // chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;
 
-    // ================== 发送超级电容控制指令 ==================
-    // 每帧向超电控制板发送最新状态
-    SuperCap_TxMsg_s cap_tx = {
-        .Enable = 1,
-        .Reserved1 = 0, // 可根据需求动态关断充电
-        .Powerlimit = referee_data->GameRobotState.chassis_power_limit,
-        .Reserved2 = 0 // PLUS版本忽略此位
-    };
-    SuperCapSend(cap, &cap_tx);
-
     // 推送反馈消息
 #ifdef ONE_BOARD
     PubPushMessage(chassis_pub, (void *)&chassis_feedback_data);

application/cmd/robot_cmd.c

@@ -87,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;//
     //
@@ -111,14 +119,14 @@ static void CalcOffsetAngle()
     else if (angle_diff < -180.0f)
         new_offset += 360.0f;
     /////
-    // 应用死区
+   //  // 应用死区
-    if (fabs(new_offset) < deadband) {
+   //  if (fabs(new_offset) < deadband) {
-        new_offset = 0.0f;
+   //      new_offset = 0.0f;
-    } else if (new_offset > 0) {
+   //  } else if (new_offset > 0) {
-       new_offset -= deadband;
+   //     new_offset -= deadband;
-   } else {
+   // } else {
-        new_offset += deadband;
+   //      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;
     // // 更新模式状态
@@ -187,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.yaw_motor_total_angle-around*360;
-        chassis_cmd_send.offset_angle =  chassis_cmd_send.offset_angle - 360*around;
+        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;
     }
@@ -200,7 +208,7 @@ static void RemoteControlSet()
     // 云台参数,确定云台控制数据
     if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式
     {
-        // 待添加,视觉会发来和目标的误差,同样将其转化为total angle的增量进行控制
+        // 待添加,视觉会发来和目标的误差,同样将其转化为total angle的增量进行控制/
         // ...
     }
     // 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制
@@ -216,9 +224,9 @@ static void RemoteControlSet()
         gimbal_cmd_send.pitch = PITCH_MIN_ANGLE;
 
     // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
-    chassis_cmd_send.vx = (0.25f * (float)rc_data[TEMP].rc.rocker_r_); // _水平方向
+    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_r1); // 1数值方向 .
+    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模块,开启

application/gimbal/gimbal.c

@@ -85,12 +85,12 @@ void GimbalInit()
         //     },
         .controller_param_init_config = {
             .angle_PID = {
-                .Kp = 0.75, //0.55
+                .Kp = 0.53, //0.55
-                .Ki = 0.05,//0.05
+                .Ki = 0.1,//0.05
-                .Kd = 0.02,//0.02
+                .Kd = 0.0148,//0.02
                 .CoefA =0.5,//0.5
                 .CoefB = 0.6,//0.6
-                .Output_LPF_RC = 0.01,
+                .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,
@@ -98,15 +98,15 @@ void GimbalInit()
                 .MaxOut = 600,
             },
             .speed_PID = {
-                .Kp = 500,//22000
+                .Kp = 3400,//22000
-                .Ki = 0,//
+                .Ki = 15,//
-                .Kd =0.0005,
+                .Kd =1,
-                // .CoefA = 0.8,
+                 .CoefA = 0.8,
-                // .CoefB = 0.1,
+                 .CoefB = 0.1,
                 .Output_LPF_RC = 0.002,//0.002
                 .Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement |  PID_OutputFilter,
-                .IntegralLimit = 5000,
+                .IntegralLimit = 6000,
-                .MaxOut = 20000,
+                .MaxOut = 30000,
             },
             .other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
             // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明

application/robot_def.h

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

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;
+}

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

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;
+}

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

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);
+}

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

modules/remote/remote_control.c

@@ -40,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_+2.5;
+    rc_ctrl[TEMP].rc.rocker_l_= rc_ctrl[TEMP].rc.rocker_l_+5.5;
     // 左侧拨轮
     RectifyRCjoystick();
     // 开关,0左1右

modules/super_cap/super_cap.c

@@ -5,7 +5,6 @@
 #include "super_cap.h"
 #include "memory.h"
 #include "stdlib.h"
-#include "bsp_dwt.h" // 【新增】引入获取时间的头文件，或者是 "main.h" 用 HAL_GetTick()
 
 static SuperCapInstance *super_cap_instance = NULL;
 
@@ -16,9 +15,6 @@ static void SuperCapRxCallback(CANInstance *_instance)
     rxbuff = _instance->rx_buff;
     Msg = &super_cap_instance->cap_msg;
 
-    // 【新增】更新最后一次收到消息的时间戳
-    super_cap_instance->last_update_tick = DWT_GetTimeline_ms(); // 如果没有DWT，用 HAL_GetTick() 替换
-
     // 按照新协议解析接收数据
     Msg->ready = (SuperCapReadyEnum)rxbuff[0];
     Msg->state = (SuperCapStateEnum)rxbuff[1];
@@ -28,18 +24,6 @@ static void SuperCapRxCallback(CANInstance *_instance)
     Msg->bat_power = rxbuff[5];
 }
 
-// 【新增】在线判断函数
-uint8_t SuperCapIsOnline(SuperCapInstance *instance)
-{
-    if (instance == NULL) return 0;
-
-    // 如果当前时间减去上次接收时间超过 1000ms (1秒)，则认为离线
-    if (DWT_GetTimeline_ms() - instance->last_update_tick > 2000) {
-        return 0; // 离线
-    }
-    return 1; // 在线
-}
-
 SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config)
 {
     super_cap_instance = (SuperCapInstance *)malloc(sizeof(SuperCapInstance));
@@ -56,9 +40,9 @@ void SuperCapSend(SuperCapInstance *instance, SuperCap_TxMsg_s *tx_data)
 
     // 按照新协议装载发送数据
     txbuff[0] = tx_data->Enable;
-    txbuff[1] = tx_data->Reserved1;
+    txbuff[1] = tx_data->Charge;
     txbuff[2] = tx_data->Powerlimit - instance->power_offset; // 计算功率补偿
-    txbuff[3] = tx_data->Reserved2;
+    txbuff[3] = tx_data->ChargePower;
 
     CANTransmit(instance->can_ins, 1);
 }

modules/super_cap/super_cap.h

@@ -44,9 +44,9 @@ typedef struct
 /* 需要发送给超级电容的控制信息结构体 */
 typedef struct {
     uint8_t Enable;      // 超级电容使能。1使能，0失能
-    uint8_t Reserved1;      // Reserved 超级电容充电。1充电，0放电
+    uint8_t Charge;      // 超级电容充电。1充电，0放电
     uint8_t Powerlimit;  // 裁判系统功率限制
-    uint8_t Reserved2; // Reserved 超级电容充电功率
+    uint8_t ChargePower; // 超级电容充电功率
 } SuperCap_TxMsg_s;
 #pragma pack()
 
@@ -56,7 +56,6 @@ typedef struct
     CANInstance *can_ins;    // CAN实例
     SuperCap_Msg_s cap_msg;  // 超级电容接收信息
     uint8_t power_offset;    // 功率补偿值
-    uint32_t last_update_tick; // 【新增】上次收到数据的时间戳
 } SuperCapInstance;
 
 /* 超级电容初始化配置 */
@@ -70,9 +69,6 @@ typedef struct
  */
 SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config);
 
-// 【新增】函数声明
-uint8_t SuperCapIsOnline(SuperCapInstance *instance);
-
 /**
  * @brief 发送超级电容控制信息
  */