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

.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`

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"
@@ -112,4 +112,4 @@ add_custom_command(
     COMMENT "Building hex & bin file..."
     COMMENT "EXCUTABLE SIZE:"
     COMMAND ${SIZE} ${PROJECT_NAME}.elf
-  )
+  )

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;

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,6 +28,31 @@
 #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)     // 半轴距
@@ -147,7 +175,7 @@ void ChassisInit()
     };
 
     chassis_motor_steering_config.can_init_config.tx_id = 2;
-    motor_steering_rf                                   = DJIMotorInit(&chassis_motor_steering_config);
+    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);
@@ -164,11 +192,11 @@ void ChassisInit()
         .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 = {
@@ -181,6 +209,26 @@ void ChassisInit()
     // （新增）设置超级电容功率补偿，例如补偿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初始化
@@ -340,6 +388,123 @@ static void SteeringWheelCalculate()
  */
 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;
@@ -391,7 +556,6 @@ void ChassisTask() {
         DJIMotorEnable(motor_steering_rf);
         DJIMotorEnable(motor_steering_lb);
     }
-
     // 根据控制模式设定旋转速度
     switch (chassis_cmd_recv.chassis_mode)
     {
@@ -403,7 +567,7 @@ void ChassisTask() {
             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)
             {
@@ -417,7 +581,7 @@ void ChassisTask() {
         default:
             break;
     }
-    static float sin_theta, cos_theta;
+    float sin_theta = 0.0f, cos_theta = 1.0f;
     // ------------------ 【修改部分：加入相位滞后动态补偿】 ------------------
     // 提取当前的云台-底盘夹角
     float real_offset_angle = chassis_cmd_recv.offset_angle;
@@ -478,4 +642,4 @@ void ChassisTask() {
 #ifdef CHASSIS_BOARD
     CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data);
 #endif // CHASSIS_BOARD
-}
+}

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;
-        }
+        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; // 发送成功

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