修改部分文件

application/chassis/balance.c

@@ -1,404 +0,0 @@
-// app
-#include "balance.h"
-#include "robot_def.h"
-#include "general_def.h"
-#include "ins_task.h"
-#include "HT04.h"
-#include "LK9025.h"
-#include "controller.h"
-#include "can_comm.h"
-#include "super_cap.h"
-#include "user_lib.h"
-#include "remote_control.h"
-#include "referee_task.h"
-#include "stdint.h"
-#include "arm_math.h" // 需要用到较多三角函数
-#include "bsp_dwt.h"
-#include "bsp_log.h"
-#include "linkNleg.h"
-#include "speed_estimation.h"
-#include "lqr_calc.h"
-
-
-static uint32_t balance_dwt_cnt;
-static float del_t;
-
-/* 底盘拥有的模块实例 */
-static attitude_t *imu_data;
-static RC_ctrl_t *rc_data; // 底盘单独调试用
-static Referee_Interactive_info_t my_ui;
-static referee_info_t *referee_data;
-static Chassis_Ctrl_Cmd_s chassis_cmd_recv; 
-static Chassis_Upload_Data_s chassis_feed;
-static CANCommInstance *ci;
-static SuperCapInstance *cap; 
-
-// 四个关节电机和两个驱动轮电机
-static HTMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试
-static LKMotorInstance *l_driven, *r_driven, *driven[2];
-
-// 两个腿的参数,0为左腿,1为右腿
-static LinkNPodParam l_side, r_side; 
-static ChassisParam chassis;
-
-// 综合运动补偿的PID控制器
-static PIDInstance steer_p_pid, steer_v_pid;   // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
-static PIDInstance anti_crash_pid, phi5_pid;   // 抗劈叉,将输出以相反的方向叠加到左右腿的上
-static PIDInstance leglen_pid_l, leglen_pid_r; // 用PD模拟弹簧,不要积分(弹簧是无积分二阶系统),增益不可过大否则抗外界冲击响应时太"硬"
-static PIDInstance roll_compensate_pid, rolldot_pid; // roll轴补偿,用于保持机体水平
-
-static Robot_Status_e chassis_status;
-
-void BalanceInit()
-{
-    rc_data = RemoteControlInit(&huart3);
-    imu_data = INS_Init();
-
-    // 双板通信
-    CANComm_Init_Config_s commconf = {
-        .can_config = {
-            .can_handle = &hcan2,
-            .tx_id = 0x40,
-            .rx_id = 0x41},
-        .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s),
-        .send_data_len = sizeof(Chassis_Upload_Data_s)};
-    ci = CANCommInit(&commconf);
-
-    // 超级电容
-    SuperCap_Init_Config_s cap_conf = {
-        .can_config = {
-            .can_handle = &hcan2,
-            .tx_id = 0x302, // todo 电容id
-            .rx_id = 0x301}};
-    cap = SuperCapInit(&cap_conf);
-
-    // 关节电机
-    Motor_Init_Config_s joint_conf = {
-        // 写一个,剩下的修改方向和id即可
-        .can_init_config = {
-            .can_handle = &hcan1},
-        .controller_param_init_config = {
-            .angle_PID = {
-                .Kp = 0.3,
-                .Kd = 0.1,
-                .Ki = 0,
-                .DeadBand = 0.0001,
-                .Improve = PID_DerivativeFilter,
-                .MaxOut = 4,
-                .Derivative_LPF_RC = 0.05,
-            }, // 仅用于复位腿
-        },
-        .controller_setting_init_config = {
-            .close_loop_type = ANGLE_LOOP,
-            .outer_loop_type = OPEN_LOOP,
-            .motor_reverse_flag = FEEDBACK_DIRECTION_NORMAL,
-            .angle_feedback_source = MOTOR_FEED,
-            .speed_feedback_source = MOTOR_FEED,
-        },
-        .motor_type = HT04};
-    joint_conf.can_init_config.tx_id = 4;
-    joint_conf.can_init_config.rx_id = 14;
-    joint[LF] = lf = HTMotorInit(&joint_conf);
-    joint_conf.can_init_config.tx_id = 3;
-    joint_conf.can_init_config.rx_id = 13;
-    joint[LB] = lb = HTMotorInit(&joint_conf);
-    joint_conf.can_init_config.tx_id = 2;
-    joint_conf.can_init_config.rx_id = 12;
-    joint[RF] = rf = HTMotorInit(&joint_conf);
-    joint_conf.can_init_config.tx_id = 1;
-    joint_conf.can_init_config.rx_id = 11;
-    joint[RB] = rb = HTMotorInit(&joint_conf);
-
-    // 驱动轮电机
-    Motor_Init_Config_s driven_conf = {
-        // 写一个,剩下的修改方向和id即可
-        .can_init_config.can_handle = &hcan2,
-        .controller_setting_init_config = {
-            .angle_feedback_source = MOTOR_FEED,
-            .speed_feedback_source = MOTOR_FEED,
-            .outer_loop_type = OPEN_LOOP,
-            .close_loop_type = OPEN_LOOP,
-            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
-        },
-        .motor_type = LK9025,
-    };
-    driven_conf.can_init_config.tx_id = 1;
-    driven[LD] = l_driven = LKMotorInit(&driven_conf);
-    driven_conf.can_init_config.tx_id = 2;
-    driven[RD] = r_driven = LKMotorInit(&driven_conf);
-
-    // 转向PID
-    PID_Init_Config_s steer_p_pid_conf = {
-        .Kp = 2,
-        .Kd = 1,
-        .Ki = 0.0f,
-        .MaxOut = 4,
-        .DeadBand = 0.01f,
-        .Improve = PID_DerivativeFilter,
-        .Derivative_LPF_RC = 0.05,
-    };
-    PIDInit(&steer_p_pid, &steer_p_pid_conf);
-    PID_Init_Config_s steer_v_pid_conf = {
-        .Kp = 2,
-        .Kd = 0.0f,
-        .Ki = 0.0f,
-        .MaxOut = 100,
-        .DeadBand = 0.0f,
-        .Improve = PID_DerivativeFilter | PID_Integral_Limit,
-        .Derivative_LPF_RC = 0.05,
-        .IntegralLimit = 2,
-    };
-    PIDInit(&steer_v_pid, &steer_v_pid_conf);
-
-    // 抗劈叉
-    PID_Init_Config_s anti_crash_pid_conf = {
-        .Kp = 8,
-        .Kd = 2.5,
-        .Ki = 0.4,
-        .MaxOut = 45,
-        .DeadBand = 0.01f,
-        .Improve = PID_DerivativeFilter | PID_ChangingIntegrationRate | PID_Integral_Limit,
-        .Derivative_LPF_RC = 0.05,
-        .CoefA = 0.05,
-        .CoefB = 0.05,
-        .IntegralLimit = 2,
-    };
-    PIDInit(&anti_crash_pid, &anti_crash_pid_conf);
-
-    // 腿长控制
-    PID_Init_Config_s leg_length_pid_conf = {
-        .Kp = 450,
-        .Kd = 150,
-        .Ki = 5,
-        .MaxOut = 60,
-        .DeadBand = 0.0001f,
-        .Improve = PID_ChangingIntegrationRate | PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_Derivative_On_Measurement,
-        .CoefA = 0.01,
-        .CoefB = 0.02,
-        .Derivative_LPF_RC = 0.08,
-    };
-    PIDInit(&leglen_pid_l, &leg_length_pid_conf);
-    PIDInit(&leglen_pid_r, &leg_length_pid_conf);
-
-    // 横滚角补偿
-    PID_Init_Config_s roll_compensate_pid_conf = {
-        .Kp = 0.0008f,
-        .Kd = 0.00065f,
-        .Ki = 0.0f,
-        .MaxOut = 0.04,
-        .DeadBand = 0.005f,
-        .Improve = PID_DerivativeFilter,
-        .Derivative_LPF_RC = 0.05,
-    };
-    PIDInit(&roll_compensate_pid, &roll_compensate_pid_conf);
-
-    l_side.target_len = r_side.target_len = 0.23;   // 初始腿长
-    chassis.vel_cov = 1000; // 初始化速度协方差
-    chassis_status = ROBOT_READY;
-    DWT_GetDeltaT(&balance_dwt_cnt);
-}
-
-static void EnableAllMotor() /* 打开所有电机 */
-{
-    for (uint8_t i = 0; i < JOINT_CNT; i++) // 打开关节电机
-        HTMotorEnable(joint[i]);
-    for (uint8_t i = 0; i < DRIVEN_CNT; i++) // 打开驱动电机
-        LKMotorEnable(driven[i]);
-}
-
-/* 切换底盘遥控器控制和云台双板控制 */
-static void ControlSwitch()
-{ 
-    // 右侧拨杆向下,进入遥控器底盘控制,此时不响应云台控制指令
-    if (switch_is_down(rc_data->rc.switch_right) && RemoteControlIsOnline())
-    {
-        if (rc_data->rc.rocker_l1 < -600)
-        {
-            chassis_cmd_recv.chassis_mode = CHASSIS_RESET;
-            chassis_cmd_recv.vx = 0.5 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
-        }
-        else // 设定值覆盖双板
-        {
-            chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG;                        // 自由转动&前后
-            chassis_cmd_recv.vx = 0.02 * (float)rc_data[TEMP].rc.rocker_r1;           // speed x, unit m/s
-            chassis_cmd_recv.offset_angle = 0.001 * (float)rc_data[TEMP].rc.rocker_r_; // rotate? follow.
-            chassis_cmd_recv.delta_leglen = -0.0000015f * (float)rc_data[TEMP].rc.dial;
-        }
-    }
-    else if (CANCommIsOnline(ci) && !switch_is_down(rc_data->rc.switch_right))
-        chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(ci); // 获取云台板指令
-    else
-        chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; // 皆离线,急停
-}
-
-
-/* 腿缩回复位,只允许驱动轮电机移动 */
-static void ResetChassis()
-{
-    EnableAllMotor(); // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身
-
-    // 复位时清空距离和腿长积累量,保证顺利站起
-    chassis.dist = chassis.target_dist = 0;
-    l_side.target_len = r_side.target_len = 0.24;
-    // 撞墙时前后移动保证能重新站立,执行速度输入
-    LKMotorSetRef(l_driven, chassis_cmd_recv.vx * 2);
-    LKMotorSetRef(r_driven, -chassis_cmd_recv.vx * 2);
-
-    // 若关节完成复位,进入ready态
-    if (abs(lf->measure.total_angle) < 0.05 && abs(lf->measure.total_angle) > 0.02 &&
-        abs(lb->measure.total_angle) < 0.05 && abs(lb->measure.total_angle) > 0.02 &&
-        abs(rf->measure.total_angle) < 0.05 && abs(rf->measure.total_angle) > 0.02 &&
-        abs(rb->measure.total_angle) < 0.05 && abs(rb->measure.total_angle) > 0.02)
-    {
-        chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
-    }
-    else if (abs(lf->measure.total_angle) <= 0.02 &&
-             abs(lb->measure.total_angle) <= 0.02 &&
-             abs(rf->measure.total_angle) <= 0.02 &&
-             abs(rb->measure.total_angle) <= 0.02)
-    {                                 // 双阈值保证关节能够复位而不会进入死区
-        chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
-        for (uint8_t i = 0; i < JOINT_CNT; i++)
-            HTMotorOuterLoop(joint[i], OPEN_LOOP); // 改回直接开环扭矩输入,让电调对扭矩闭环
-        return;                                    // 退出函数不再执行关节指令
-    }
-    else
-        chassis_status = ROBOT_STOP;
-
-    // 还在复位中,关节改为位置环,执行复位
-    for (uint8_t i = 0; i < JOINT_CNT; i++)
-    {
-        HTMotorOuterLoop(joint[i], ANGLE_LOOP);
-        HTMotorSetRef(joint[i], 0);
-    }
-}
-
-
-/* 工作状态设定 */
-static void WokingStateSet()
-{
-    if (chassis_cmd_recv.chassis_mode == CHASSIS_RESET) // 复位模式
-    {
-        ResetChassis();
-        return;
-    }
-    else if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) // 未收到遥控器和云台指令底盘进入急停
-    {
-        for (uint8_t i = 0; i < JOINT_CNT; i++)
-            HTMotorStop(joint[i]);
-        for (uint8_t i = 0; i < DRIVEN_CNT; i++)
-            LKMotorStop(driven[i]);
-        return; // 关闭所有电机,发送的指令为零
-    }
-
-    // 运动模式
-    EnableAllMotor();
-    // 设置目标速度/腿长/距离
-    l_side.target_len += chassis_cmd_recv.delta_leglen;
-    r_side.target_len += chassis_cmd_recv.delta_leglen;
-    VAL_LIMIT(l_side.target_len, 0.13, 0.3); // 腿长限幅
-    VAL_LIMIT(r_side.target_len, 0.13, 0.3);
-    // 加速度限幅,防止键盘控制摔倒
-    if (abs(chassis_cmd_recv.vx - chassis.target_v) / del_t < MAX_ACC_REF)
-        chassis.target_v = chassis_cmd_recv.vx;
-    else
-        chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t;
-    // 模型距离参考输入
-    chassis.target_dist += chassis.target_v * del_t;
-    chassis.target_yaw = chassis_cmd_recv.offset_angle; // 云台和底盘对齐时电机编码器的单圈反馈角度
-}
-
-
-/**
- * @brief 将电机和imu的数据组装为LinkNPodParam结构体和chassisParam结构体
- *
- * @note HT04电机上电的编码器位置为零(校准过),请看Link2Pod()的note,以及HT04.c中的电机解码部分
- * @note 海泰04电机顺时针旋转为正; LK9025电机逆时针旋转为正,此处皆需要转换为模型中给定的正方向
- *
- */
-static void ParamAssemble()
-{   
-    // 机体参数,视为平面刚体
-    chassis.pitch = (-imu_data->Pitch + BALANCE_GRAVITY_BIAS) * DEGREE_2_RAD;
-    chassis.pitch_w = -imu_data->Gyro[0];
-    chassis.yaw = imu_data->YawTotalAngle * DEGREE_2_RAD;
-    chassis.wz = imu_data->Gyro[2];
-    chassis.roll = imu_data->Roll * DEGREE_2_RAD + ROLL_GRAVITY_BIAS;
-    chassis.roll_w = imu_data->Gyro[1];
-
-    // HT04电机的角度是顺时针为正,LK9025电机的角度是逆时针为正
-    l_side.phi1 = PI + LIMIT_LINK_RAD - lb->measure.total_angle;
-    l_side.phi4 = -lf->measure.total_angle - LIMIT_LINK_RAD;
-    l_side.phi1_w = -lb->measure.speed_rads;
-    l_side.phi4_w = -lf->measure.speed_rads;
-    l_side.w_ecd = l_driven->measure.speed_rads;
-    r_side.phi1 = PI + LIMIT_LINK_RAD + rb->measure.total_angle;
-    r_side.phi4 = rf->measure.total_angle - LIMIT_LINK_RAD;
-    r_side.phi1_w = rb->measure.speed_rads;
-    r_side.phi4_w = rf->measure.speed_rads;
-    r_side.w_ecd = -r_driven->measure.speed_rads;
-}
-
-
-/* 腿部控制:抗劈叉; 轮子控制:转向 */
-static void SynthesizeMotion() 
-{
-    // 跟随云台yaw
-    if (chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW ||
-        chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG) // 角度环
-    {
-        float p_ref = PIDCalculate(&steer_p_pid, chassis_cmd_recv.offset_angle, 0);
-        PIDCalculate(&steer_v_pid, chassis.wz, p_ref); // 双环
-    }
-    else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) // 速度环
-        PIDCalculate(&steer_v_pid, chassis.wz, 4);
-
-    l_side.T_wheel -= steer_v_pid.Output;
-    r_side.T_wheel += steer_v_pid.Output;
-
-    // 抗劈叉
-    volatile static float swerving_speed_ff, ff_coef = 3;
-    swerving_speed_ff = ff_coef * steer_v_pid.Output; // 用于抗劈叉的前馈
-    PIDCalculate(&anti_crash_pid, l_side.phi5 - r_side.phi5, 0);
-    l_side.T_hip += anti_crash_pid.Output - swerving_speed_ff;
-    r_side.T_hip -= anti_crash_pid.Output - swerving_speed_ff;
-}
-
-
-/* 腿长控制和Roll补偿 */
-static void LegControl() 
-{
-    PIDCalculate(&roll_compensate_pid, chassis.roll, 0);
-    l_side.target_len -= roll_compensate_pid.Output;
-    r_side.target_len += roll_compensate_pid.Output;
-
-    static float gravity_comp = 0;
-    static float roll_extra_comp_p = 0;
-    float roll_comp = roll_extra_comp_p * chassis.roll;
-    l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + gravity_comp + roll_comp;
-    r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + gravity_comp - roll_comp;
-
-    // @todo: 还需要加和roll的纯Kp项
-}
-
-
-/* 设定运动模态的输出 */
-static void WattLimitSet() 
-{
-    HTMotorSetRef(lf, 0.2857 * -l_side.T_front); // 根据扭矩常数计算得到的系数
-    HTMotorSetRef(lb, 0.2857 * -l_side.T_back);
-    HTMotorSetRef(rf, 0.2857 * r_side.T_front);
-    HTMotorSetRef(rb, 0.2857 * r_side.T_back);
-    LKMotorSetRef(l_driven, 195.3125 * l_side.T_wheel);
-    LKMotorSetRef(r_driven, 195.3125 * -r_side.T_wheel);
-}
-
-
-void BalanceTask()
-{
-    del_t = DWT_GetDeltaT(&balance_dwt_cnt);
-
-    // 切换遥控器控制or云台板控制
-    ControlSwitch();
-    
-}

application/chassis/balance.h

@@ -42,14 +42,17 @@ typedef struct
     // joint
     float phi1_w, phi4_w, phi2_w, phi5_w; // phi2_w used for calc real wheel speed
     float T_back, T_front;
+
     // link angle, phi1-ph5, phi5 is pod angle
     float phi1, phi2, phi3, phi4, phi5;
+
     // wheel
     float w_ecd;      // 电机编码器速度
     float wheel_dist; // 单侧轮子的位移
     float wheel_w;    // 单侧轮子的速度
     float body_v;     // 髋关节速度
     float T_wheel;
+
     // pod
     float theta, theta_w; // 杆和垂直方向的夹角,为控制状态之一
     float leg_len, legd;
@@ -59,22 +62,25 @@ typedef struct
 
     float coord[6]; // xb yb xc yc xd yd
 
-    float wheel_out[7];
-    float hip_out[7];
 } LinkNPodParam;
 
 typedef struct
 {
+    // 速度
     float vel, target_v;        // 底盘速度
     float vel_m;                // 底盘速度测量值
     float vel_predict;          // 底盘速度预测值
     float vel_cov;              // 速度方差
-    float acc, acc_m, acc_last; // 水平方向加速度,用于计算速度预测值
+    float acc_m, acc_last;      // 水平方向加速度,用于计算速度预测值
 
+    // 位移
     float dist, target_dist;   // 底盘位移距离
+
+    // IMU
     float yaw, wz, target_yaw; // yaw角度和底盘角速度
     float pitch, pitch_w;      // 底盘俯仰角度和角速度
     float roll, roll_w;        // 底盘横滚角度和角速度
+    
 } ChassisParam;
 
 /**

application/chassis/linkNleg.h

@@ -74,6 +74,6 @@ void Link2Leg(LinkNPodParam *p, ChassisParam *chassis)
     p->phi2_w = (phi2_pred - p->phi2) / predict_dt; // 稍后用于修正轮速
     p->phi5_w = (phi5_pred - p->phi5) / predict_dt;
     p->legd = (Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2)) - p->leg_len) / predict_dt;
-    p->theta_w = ((phi5_pred - 0.5 * PI - (chassis->pitch + chassis->pitch_w) - p->theta) / predict_dt); // 可以不考虑机体? -predict_dt*chassis.pitch_w
+    p->theta_w = ((phi5_pred - 0.5 * PI - (chassis->pitch + chassis->pitch_w * predict_dt) - p->theta) / predict_dt); // 可以不考虑机体? -predict_dt*chassis.pitch_w
     p->height_v = p->legd * mcos(p->theta) - p->leg_len * msin(p->theta) * p->theta_w;
 }

application/chassis/lqr_calc.h

@@ -9,8 +9,8 @@
  */
 static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
 {
-    static float k[12][3] = {};
-    float T[2] = {0}; // 0 T_wheel  1 T_hip
+    float k[12][3] = {0};
+    float T[2] = {0};       // 0 T_wheel  1 T_hip
     float l = p->leg_len;
     float lsqr = l * l;
 

application/chassis/speed_estimation.h

@@ -1,100 +0,0 @@
-#include "balance.h"
-#include "user_lib.h"
-#include "ins_task.h"
-#include "general_def.h"
-
-#define EST_FINAL_LPF 0.005f // 最终速度的低通滤波系数
-static KalmanFilter_t kf;
-
-/**
- * @brief 底盘为右手系
- *
- *    ^ y            左轮     右轮
- *    |               |      |  前
- *    |_____ >  x     |------|
- *    z 轴从屏幕向外   |      |  后
- */
-
-void SpeedEstInit()
-{   
-    // 使用kf同时估计速度和加速度
-    // Kalman_Filter_Init(&kf, 2, 0, 2);
-    // float F[4] = {1, 0.001, 0, 1};
-    // float Q[4] = {VEL_PROCESS_NOISE, 0, 0, ACC_PROCESS_NOISE};
-    // float R[4] = {VEL_MEASURE_NOISE, 0, 0, ACC_MEASURE_NOISE};
-    // float P[4] = {100000, 0, 0, 100000};
-    // float H[4] = {1, 0, 0, 1};
-    // memcpy(kf.F_data, F, sizeof(F));
-    // memcpy(kf.Q_data, Q, sizeof(Q));
-    // memcpy(kf.R_data, R, sizeof(R));
-    // memcpy(kf.P_data, P, sizeof(P));
-    // memcpy(kf.H_data, H, sizeof(H));
-}
-
-/**
- * @brief 使用卡尔曼滤波估计底盘速度
- * @todo 增加w和dw的滤波,当w和dw均小于一定值时,不考虑dw导致的角加速度
- *
- * @param lp 左侧腿
- * @param rp 右侧腿
- * @param cp 底盘
- * @param imu imu数据
- * @param delta_t 更新间隔
- */
-void SpeedEstimation(LinkNPodParam *lp, LinkNPodParam *rp, ChassisParam *cp, INS_t *imu, float delta_t)
-{
-    // 修正轮速和距离
-    lp->wheel_w = lp->w_ecd + lp->phi2_w - cp->pitch_w; // 减去和定子固连的phi2_w
-    rp->wheel_w = rp->w_ecd + rp->phi2_w - cp->pitch_w;
-
-    // 直接使用轮速反馈,不进行速度融合
-    // cp->vel = (lp->wheel_w + rp->wheel_w) * WHEEL_RADIUS / 2;
-    // cp->dist = cp->dist + cp->vel * delta_t;
-
-    // 以轮子为基点,计算机体两侧髋关节处的速度
-    lp->body_v = lp->wheel_w * WHEEL_RADIUS + lp->leg_len * lp->theta_w + lp->legd * msin(lp->theta);
-    rp->body_v = rp->wheel_w * WHEEL_RADIUS + rp->leg_len * rp->theta_w + rp->legd * msin(rp->theta);
-    cp->vel_m = (lp->body_v + rp->body_v) / 2; // 机体速度(平动)为两侧速度的平均值
-
-    // 扣除旋转导致的向心加速度和角加速度*R
-    float *gyro = imu->Gyro, *dgyro = imu->dgyro;
-    static float yaw_ddwrNwwr, yaw_p_ddwrNwwr, pitch_ddwrNwwr;
-    static float macc_y, macc_z;                                // 补偿后的实际平动加速度,机体系前进方向和竖直方向
-    yaw_ddwrNwwr = powf(gyro[Z], 2) * CENTER_IMU_W - dgyro[Z] * CENTER_IMU_L;   // yaw旋转导致motion_acc[1]的额外加速度(机体前后方向)
-    yaw_p_ddwrNwwr = powf(gyro[X], 2) * CENTER_IMU_W + dgyro[X] * CENTER_IMU_H; // pitch旋转导致motion_acc[1]的额外加速度(机体前后方向)
-    pitch_ddwrNwwr = powf(gyro[X], 2) * CENTER_IMU_H - dgyro[X] * CENTER_IMU_W; // pitch旋转导致motion_acc[2]的额外加速度(机体竖直方向)
-    macc_y = -imu->MotionAccel_b[Y] - yaw_ddwrNwwr - yaw_p_ddwrNwwr;
-    macc_z = imu->MotionAccel_b[Z] - pitch_ddwrNwwr;
-
-    float pitch = imu->Pitch * DEGREE_2_RAD;
-    cp->acc_last = cp->acc_m; 
-    cp->acc_m = macc_y * mcos(pitch) - macc_z * msin(pitch); // 绝对系下的平动加速度,即机体系下的加速度投影到绝对系
-
-    // for debug 对比修正前后的加速度
-    static float ry, rz, rawaa; 
-    ry = -imu->MotionAccel_b[Y];
-    rz = imu->MotionAccel_b[Z];
-    rawaa = ry * mcos(pitch) - rz * msin(pitch);
-
-    // 使用kf同时估计加速度和速度,滤波更新
-    // kf.MeasuredVector[0] = cp->vel_m;
-    // kf.MeasuredVector[1] = cp->acc_m;
-    // kf.F_data[1] = delta_t; // 更新F矩阵
-    // Kalman_Filter_Update(&kf);
-    // cp->vel = kf.xhat_data[0];
-    // cp->acc = kf.xhat_data[1];
-
-    // 融合加速度计的数据和机体速度
-    static float f, k, prior, measure, cov;
-    f = (cp->acc_m + cp->acc_last) / 2;    // 速度梯形积分
-    prior = cp->vel + f * delta_t; // x' = Fx,先验估计
-    cp->vel_predict = prior;
-    measure = cp->vel_m;    // 测量值
-    cov = cp->vel_cov + VEL_PROCESS_NOISE * delta_t; // P' = P + Q ,先验协方差
-    cp->vel_cov = cov;
-    k = cov / (cov + VEL_MEASURE_NOISE);     // K = P'/(P'+R),卡尔曼增益
-    cp->vel = prior + k * (measure - prior); // x^ = x'+K(z-x'),后验估计
-    cp->vel_cov *= (1 - k);                  // P^ = (1-K)P',后验协方差
-    VAL_LIMIT(cp->vel_cov, 0.01, 100);       // 协方差限幅
-    cp->dist = cp->dist + cp->vel * delta_t;
-}