暂存

User/device/virtual_chassis.c

@@ -0,0 +1,417 @@
+/*
+ * @file virtual_chassis.c
+ * @brief 虚拟底盘设备驱动实现 - 控制端
+ * @details 作为控制端，发送控制命令到底盘，接收底盘反馈数据
+ */
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/virtual_chassis.h"
+#include "bsp/can.h"
+#include "bsp/time.h"
+#include <string.h>
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+
+
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* Private function prototypes ---------------------------------------------- */
+static int8_t Virtual_Chassis_DecodeMotor(Virtual_Chassis_t *chassis, uint8_t id, uint8_t *data) {
+    if (chassis == NULL || data == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+
+    // 1. 解析转矩电流 - 前2字节，16位有符号整数 (精度0.01 Nm)
+    int16_t torque_int;
+    memcpy(&torque_int, &data[0], sizeof(int16_t));
+    chassis->data.joint[id-chassis->param.motors.feedback_id].torque_current = (float)torque_int / 100.0f;
+    
+    // 2. 解析位置 - 第3-5字节，24位有符号整数 (精度0.0001 rad)
+    int32_t angle_int = 0;
+    angle_int |= ((int32_t)data[2]) << 16;
+    angle_int |= ((int32_t)data[3]) << 8;
+    angle_int |= ((int32_t)data[4]);
+    
+    // 符号扩展（24位转32位）
+    if (angle_int & 0x800000) {
+        angle_int |= 0xFF000000;
+    }
+    chassis->data.joint[id-chassis->param.motors.feedback_id].rotor_abs_angle = (float)angle_int / 10000.0f;
+    
+    // 3. 解析速度 - 第6-8字节，24位有符号整数 (精度0.001 rad/s)
+    int32_t velocity_int = 0;
+    velocity_int |= ((int32_t)data[5]) << 16;
+    velocity_int |= ((int32_t)data[6]) << 8;
+    velocity_int |= ((int32_t)data[7]);
+    
+    // 符号扩展（24位转32位）
+    if (velocity_int & 0x800000) {
+        velocity_int |= 0xFF000000;
+    }
+    chassis->data.joint[id-chassis->param.motors.feedback_id].rotor_speed = (float)velocity_int / 1000.0f;
+    
+    chassis->data.joint[id-chassis->param.motors.feedback_id].temp = 0.0f;
+    
+    return DEVICE_OK;
+}
+
+static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint8_t id, uint8_t *data, uint8_t len) {
+    if (chassis == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    float *float_data = (float*)data;
+    
+    switch (id) {
+        case 0x66: // 加速度计数据 - 8字节，2个float值 (x, y)
+            chassis->data.imu.accl.x = float_data[0];
+            chassis->data.imu.accl.y = float_data[1];
+            // z轴数据未传输，保持之前的值或设为0
+            // chassis->data.imu.accl.z = 0.0f;
+            break;
+            
+        case 0x67: // 陀螺仪数据 - 8字节，2个float值 (x, y)
+            chassis->data.imu.gyro.x = float_data[0];
+            chassis->data.imu.gyro.y = float_data[1];
+            // z轴数据未传输，保持之前的值或设为0
+            // chassis->data.imu.gyro.z = 0.0f;
+            break;
+            
+        case 0x68: // 欧拉角数据 - 8字节，2个float值 (yaw, pitch)
+            chassis->data.imu.euler.yaw = float_data[0];
+            chassis->data.imu.euler.pit = float_data[1];
+            // roll轴数据未传输，保持之前的值或设为0
+            // chassis->data.imu.euler.rol = 0.0f;
+            break;
+            
+        case 0x69: // 四元数数据 - 8字节，2个float值 (q0, q1)
+            chassis->data.imu.quat.q0 = float_data[0];
+            chassis->data.imu.quat.q1 = float_data[1];
+            // q2, q3数据未传输，保持之前的值或设为0
+            // chassis->data.imu.quat.q2 = 0.0f;
+            // chassis->data.imu.quat.q3 = 0.0f;
+            break;
+            
+        default:
+            return DEVICE_ERR;
+    }
+
+    return DEVICE_OK;
+}
+
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * @brief 初始化虚拟底盘设备
+ */
+int8_t Virtual_Chassis_Init(Virtual_Chassis_t *chassis, Virtual_Chassis_Param_t *param) {
+    if (chassis == NULL || param == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    // 复制参数配置
+    chassis->param = *param;
+    
+    // 初始化设备头部
+    chassis->header.online = false;
+    chassis->header.last_online_time = 0;
+    
+    // 初始化数据
+    memset(&chassis->data, 0, sizeof(Virtual_Chassis_Feedback_t));
+    memset(&chassis->output, 0, sizeof(Virtual_Chassis_Output_t));
+
+    BSP_CAN_Init();
+
+    for (int i = 0; i < 4; i++) {
+        BSP_CAN_RegisterId(chassis->param.motors.can, 124 + i, 3); // 电机反馈ID
+        BSP_CAN_RegisterId(chassis->param.imu.can, 0x66 + 1, 3);
+    }
+    
+    
+    // 设置设备在线状态
+    chassis->header.online = true;
+    chassis->header.last_online_time = BSP_TIME_Get_us();
+    
+    return DEVICE_OK;
+}
+
+
+/**
+ * @brief 更新虚拟底盘数据（接收底盘反馈数据）
+ */
+int8_t Virtual_Chassis_Update(Virtual_Chassis_t *chassis) {
+    if (chassis == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    BSP_CAN_Message_t msg;
+    BSP_CAN_GetMessage(chassis->param.motors.can, 124, &msg, BSP_CAN_TIMEOUT_IMMEDIATE);
+
+    
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 接收底盘反馈数据
+ */
+int8_t Virtual_Chassis_ReceiveFeedback(Virtual_Chassis_t *chassis) {
+    if (chassis == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    // 处理4个电机的反馈数据 (ID 124-127)
+    for (int i = 0; i < VIRTUAL_CHASSIS_MOTOR_COUNT; i++) {
+        Virtual_Chassis_ProcessMotorFeedback(chassis, i);
+    }
+    
+    // 处理IMU反馈数据
+    Virtual_Chassis_ProcessIMUFeedback(chassis);
+    
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 发送电机使能命令
+ */
+int8_t Virtual_Chassis_Enable(Virtual_Chassis_t *chassis) {
+    if (chassis == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    BSP_CAN_StdDataFrame_t enable_frame = {
+        .id = chassis->param.motors.Enable_id,
+    };
+    
+    if (BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &enable_frame) == BSP_OK) {
+        return DEVICE_OK;
+    }
+    
+    return DEVICE_ERR;
+}
+
+/**
+ * @brief 发送电机力矩控制命令
+ */
+int8_t Virtual_Chassis_SendTorqueCommand(Virtual_Chassis_t *chassis, float torques[4]) {
+    if (chassis == NULL || torques == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    BSP_CAN_StdDataFrame_t torque_frame = {
+        .id = chassis->param.motors.MIT_id,
+        .dlc = 8,
+        .data = {0}
+    };
+    
+    // 8字节数据分别是4个电机的力矩 (每个电机2字节，有符号整数，精度0.01 Nm)
+    for (int i = 0; i < 4; i++) {
+        int16_t torque_raw = (int16_t)(torques[i] * 100.0f);
+        memcpy(&torque_frame.data[i * 2], &torque_raw, sizeof(int16_t));
+        chassis->output.joint_torque[i] = torques[i];
+    }
+    
+    return BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &torque_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+/**
+ * @brief 获取电机反馈数据
+ */
+Virtual_Chassis_MotorFeedback_t* Virtual_Chassis_GetMotorFeedback(Virtual_Chassis_t *chassis, uint8_t motor_id) {
+    if (chassis == NULL || motor_id >= VIRTUAL_CHASSIS_MOTOR_COUNT) {
+        return NULL;
+    }
+    
+    return &chassis->data.motors[motor_id];
+}
+
+/**
+ * @brief 获取IMU反馈数据
+ */
+Virtual_Chassis_IMUFeedback_t* Virtual_Chassis_GetIMUFeedback(Virtual_Chassis_t *chassis) {
+    if (chassis == NULL) {
+        return NULL;
+    }
+    
+    return &chassis->data.imu;
+}
+
+/**
+ * @brief 设置目标力矩
+ */
+int8_t Virtual_Chassis_SetTargetTorques(Virtual_Chassis_t *chassis, float torques[VIRTUAL_CHASSIS_MOTOR_COUNT]) {
+    if (chassis == NULL || torques == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    memcpy(chassis->data.target_torques, torques, sizeof(float) * VIRTUAL_CHASSIS_MOTOR_COUNT);
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 使能电机并发送命令
+ */
+int8_t Virtual_Chassis_Enable(Virtual_Chassis_t *chassis) {
+    if (chassis == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    return Virtual_Chassis_SendEnableCommand(chassis);
+}
+
+/**
+ * @brief 检查虚拟底盘是否在线
+ */
+bool Virtual_Chassis_IsOnline(Virtual_Chassis_t *chassis) {
+    if (chassis == NULL) {
+        return false;
+    }
+    
+    uint64_t current_time = BSP_TIME_Get_us();
+    uint64_t timeout_us = VIRTUAL_CHASSIS_TIMEOUT_MS * 1000;
+    
+    return chassis->header.online && 
+           (current_time - chassis->header.last_online_time) < timeout_us;
+}
+
+/* Private functions -------------------------------------------------------- */
+
+/**
+ * @brief 注册CAN接收ID (接收底盘反馈数据)
+ */
+static int8_t Virtual_Chassis_RegisterCANIds(Virtual_Chassis_t *chassis) {
+    if (chassis == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    // 注册电机反馈数据ID
+    for (int i = 0; i < VIRTUAL_CHASSIS_MOTOR_COUNT; i++) {
+        if (BSP_CAN_RegisterId(chassis->param.motors[i].can, chassis->param.motors[i].feedback_id, 0) != BSP_OK) {
+            return DEVICE_ERR;
+        }
+    }
+    
+    // 注册IMU反馈数据ID
+    if (BSP_CAN_RegisterId(chassis->param.imu_can, chassis->param.accl_id, 0) != BSP_OK) {
+        return DEVICE_ERR;
+    }
+    
+    if (BSP_CAN_RegisterId(chassis->param.imu_can, chassis->param.gyro_id, 0) != BSP_OK) {
+        return DEVICE_ERR;
+    }
+    
+    if (BSP_CAN_RegisterId(chassis->param.imu_can, chassis->param.euler_id, 0) != BSP_OK) {
+        return DEVICE_ERR;
+    }
+    
+    if (BSP_CAN_RegisterId(chassis->param.imu_can, chassis->param.quat_id, 0) != BSP_OK) {
+        return DEVICE_ERR;
+    }
+    
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 处理电机反馈数据
+ */
+static int8_t Virtual_Chassis_ProcessMotorFeedback(Virtual_Chassis_t *chassis, uint8_t motor_id) {
+    BSP_CAN_Message_t rx_msg;
+    
+    if (BSP_CAN_GetMessage(chassis->param.motors[motor_id].can, chassis->param.motors[motor_id].feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
+        // 解析数据：转矩电流(2字节) + 位置(3字节) + 速度(3字节) = 8字节
+        
+        // 转矩电流 (精度0.01 Nm)
+        int16_t torque_int;
+        memcpy(&torque_int, &rx_msg.data[0], sizeof(int16_t));
+        chassis->data.motors[motor_id].current_torque = (float)torque_int / 100.0f;
+        
+        // 位置 (精度0.0001 rad)
+        int32_t angle_int = 0;
+        angle_int |= ((int32_t)rx_msg.data[2]) << 16;
+        angle_int |= ((int32_t)rx_msg.data[3]) << 8;
+        angle_int |= ((int32_t)rx_msg.data[4]);
+        if (angle_int & 0x800000) { // 符号扩展
+            angle_int |= 0xFF000000;
+        }
+        chassis->data.motors[motor_id].current_angle = (float)angle_int / 10000.0f;
+        
+        // 速度 (精度0.001 rad/s)
+        int32_t velocity_int = 0;
+        velocity_int |= ((int32_t)rx_msg.data[5]) << 16;
+        velocity_int |= ((int32_t)rx_msg.data[6]) << 8;
+        velocity_int |= ((int32_t)rx_msg.data[7]);
+        if (velocity_int & 0x800000) { // 符号扩展
+            velocity_int |= 0xFF000000;
+        }
+        chassis->data.motors[motor_id].current_velocity = (float)velocity_int / 1000.0f;
+        
+        // 更新在线状态
+        chassis->data.motors[motor_id].online = true;
+        chassis->data.motors[motor_id].last_update_time = BSP_TIME_Get_us();
+        
+        return DEVICE_OK;
+    }
+    
+    return DEVICE_ERR;
+}
+
+/**
+ * @brief 处理IMU反馈数据
+ */
+static int8_t Virtual_Chassis_ProcessIMUFeedback(Virtual_Chassis_t *chassis) {
+    BSP_CAN_Message_t rx_msg;
+    bool imu_updated = false;
+    
+    // 处理加速度计数据
+    if (BSP_CAN_GetMessage(chassis->param.imu_can, chassis->param.accl_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
+        float *accl_data = (float*)rx_msg.data;
+        chassis->data.imu.accl.x = accl_data[0];
+        chassis->data.imu.accl.y = accl_data[1];
+        if (rx_msg.dlc >= 12) { // 如果有第三个数据
+            chassis->data.imu.accl.z = accl_data[2];
+        }
+        imu_updated = true;
+    }
+    
+    // 处理陀螺仪数据
+    if (BSP_CAN_GetMessage(chassis->param.imu_can, chassis->param.gyro_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
+        float *gyro_data = (float*)rx_msg.data;
+        chassis->data.imu.gyro.x = gyro_data[0];
+        chassis->data.imu.gyro.y = gyro_data[1];
+        if (rx_msg.dlc >= 12) { // 如果有第三个数据
+            chassis->data.imu.gyro.z = gyro_data[2];
+        }
+        imu_updated = true;
+    }
+    
+    // 处理欧拉角数据
+    if (BSP_CAN_GetMessage(chassis->param.imu_can, chassis->param.euler_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
+        float *euler_data = (float*)rx_msg.data;
+        chassis->data.imu.euler.yaw = euler_data[0];
+        chassis->data.imu.euler.pit = euler_data[1];
+        if (rx_msg.dlc >= 12) { // 如果有第三个数据
+            chassis->data.imu.euler.rol = euler_data[2];
+        }
+        imu_updated = true;
+    }
+    
+    // 处理四元数数据（可选）
+    if (BSP_CAN_GetMessage(chassis->param.imu_can, chassis->param.quat_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
+        float *quat_data = (float*)rx_msg.data;
+        chassis->data.imu.quat.q0 = quat_data[0];
+        chassis->data.imu.quat.q1 = quat_data[1];
+        if (rx_msg.dlc >= 16) { // 如果有更多数据
+            chassis->data.imu.quat.q2 = quat_data[2];
+            chassis->data.imu.quat.q3 = quat_data[3];
+        }
+        imu_updated = true;
+    }
+    
+    if (imu_updated) {
+        chassis->data.imu.online = true;
+        chassis->data.imu.last_update_time = BSP_TIME_Get_us();
+        return DEVICE_OK;
+    }
+    
+    return DEVICE_ERR;
+}

User/device/virtual_chassis.h

@@ -0,0 +1,151 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/device.h"
+#include "device/motor.h"
+#include "component/ahrs.h"
+#include "bsp/can.h"
+
+/* Exported constants ------------------------------------------------------- */
+
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+/* 电机反馈数据结构体 */
+typedef struct {
+    MOTOR_Feedback_t joint[4]; // 4个电机反馈数据
+    struct {
+        DEVICE_Header_t header;        // 设备通用头部
+        AHRS_Accl_t accl;           // 加速度计数据
+        AHRS_Gyro_t gyro;           // 陀螺仪数据
+        AHRS_Eulr_t euler;          // 欧拉角数据
+        AHRS_Quaternion_t quat;     // 四元数数据
+    } imu;
+} Virtual_Chassis_Feedback_t;
+
+typedef struct {
+    float joint_torque[4]; // 4个电机目标力矩
+} Virtual_Chassis_Output_t;
+
+/* 电机CAN参数结构体 */
+typedef struct {
+    BSP_CAN_t can;              // 电机所在CAN总线
+    uint16_t Enable_id;         // 电机使能命令CAN ID
+    uint16_t MIT_id;        // 电机力矩控制命令CAN ID
+    uint16_t feedback_id;       // 电机反馈数据CAN ID
+} Virtual_Chassis_MotorParam_t;
+
+/* IMU参数结构体 */
+typedef struct {
+    BSP_CAN_t can;              // 电机所在CAN总线
+    uint16_t accl_id;           // 加速度计数据CAN ID
+    uint16_t gyro_id;           // 陀螺仪数据CAN ID
+    uint16_t euler_id;          // 欧拉角数据CAN ID
+    uint16_t quat_id;           // 四元数数据CAN ID
+} Virtual_Chassis_IMUParam_t;
+
+/* 虚拟底盘参数配置结构体 */
+typedef struct {
+    Virtual_Chassis_MotorParam_t motors; // 4个电机CAN参数
+    Virtual_Chassis_IMUParam_t imu;          // IMU CAN参数
+} Virtual_Chassis_Param_t;
+
+/* 虚拟底盘设备结构体 */
+typedef struct {
+    DEVICE_Header_t header;                      // 设备通用头部
+    Virtual_Chassis_Param_t param;               // 参数配置
+    Virtual_Chassis_Feedback_t data;             // 反馈数据
+    Virtual_Chassis_Output_t output;             // 控制输出
+} Virtual_Chassis_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 初始化虚拟底盘设备
+ * @param chassis 虚拟底盘设备结构体指针
+ * @param param 虚拟底盘参数配置指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t Virtual_Chassis_Init(Virtual_Chassis_t *chassis, Virtual_Chassis_Param_t *param);
+
+/**
+ * @brief 反初始化虚拟底盘设备
+ * @param chassis 虚拟底盘设备结构体指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t Virtual_Chassis_DeInit(Virtual_Chassis_t *chassis);
+
+/**
+ * @brief 更新虚拟底盘数据（包括电机和IMU数据）
+ * @param chassis 虚拟底盘设备结构体指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t Virtual_Chassis_Update(Virtual_Chassis_t *chassis);
+
+/**
+ * @brief 接收底盘反馈数据
+ * @param chassis 虚拟底盘设备结构体指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t Virtual_Chassis_ReceiveFeedback(Virtual_Chassis_t *chassis);
+
+/**
+ * @brief 发送电机使能命令
+ * @param chassis 虚拟底盘设备结构体指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t Virtual_Chassis_SendEnableCommand(Virtual_Chassis_t *chassis);
+
+/**
+ * @brief 发送电机力矩控制命令
+ * @param chassis 虚拟底盘设备结构体指针
+ * @param torques 4个电机的目标力矩数组 (-60~60 Nm)
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t Virtual_Chassis_SendTorqueCommand(Virtual_Chassis_t *chassis, float torques[VIRTUAL_CHASSIS_MOTOR_COUNT]);
+
+/**
+ * @brief 获取电机反馈数据
+ * @param chassis 虚拟底盘设备结构体指针
+ * @param motor_id 电机ID (0-3)
+ * @return 电机反馈数据指针，失败返回NULL
+ */
+Virtual_Chassis_MotorFeedback_t* Virtual_Chassis_GetMotorFeedback(Virtual_Chassis_t *chassis, uint8_t motor_id);
+
+/**
+ * @brief 获取IMU反馈数据
+ * @param chassis 虚拟底盘设备结构体指针
+ * @return IMU反馈数据指针，失败返回NULL
+ */
+Virtual_Chassis_IMUFeedback_t* Virtual_Chassis_GetIMUFeedback(Virtual_Chassis_t *chassis);
+
+/**
+ * @brief 设置目标力矩
+ * @param chassis 虚拟底盘设备结构体指针
+ * @param torques 4个电机的目标力矩数组
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t Virtual_Chassis_SetTargetTorques(Virtual_Chassis_t *chassis, float torques[VIRTUAL_CHASSIS_MOTOR_COUNT]);
+
+/**
+ * @brief 使能电机并发送命令
+ * @param chassis 虚拟底盘设备结构体指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t Virtual_Chassis_Enable(Virtual_Chassis_t *chassis);
+
+/**
+ * @brief 检查虚拟底盘是否在线
+ * @param chassis 虚拟底盘设备结构体指针
+ * @return true 在线，false 离线
+ */
+bool Virtual_Chassis_IsOnline(Virtual_Chassis_t *chassis);
+
+#ifdef __cplusplus
+}
+#endif

User/module/balance_chassis.c

@@ -19,7 +19,7 @@ static int8_t Chassis_MotorEnable(Chassis_t *c) {
   if (c == NULL)
     return CHASSIS_ERR_NULL;
   for (int i = 0; i < 4; i++) {
-    MOTOR_LZ_Enable(&c->param->joint_motors[i]);
+    // MOTOR_LZ_Enable(&c->param->joint_motors[i]);
   }
   BSP_TIME_Delay_us(200);
   for (int i = 0; i < 2; i++) {
@@ -32,7 +32,7 @@ static int8_t Chassis_MotorRelax(Chassis_t *c) {
   if (c == NULL)
     return CHASSIS_ERR_NULL;
   for (int i = 0; i < 2; i++) {
-    MOTOR_LK_Relax(&c->param->wheel_motors[i]);
+    // MOTOR_LK_Relax(&c->param->wheel_motors[i]);
   }
   BSP_TIME_Delay_us(200);
   for (int i = 0; i < 4; i++) {
@@ -275,7 +275,7 @@ void Chassis_Output(Chassis_t *c) {
   for (int i = 0; i < 4; i++) {
     MOTOR_LZ_MotionParam_t param = {0};
     param.torque = c->output.joint[i].torque;
-    MOTOR_LZ_MotionControl(&c->param->joint_motors[i], &param);
+    // MOTOR_LZ_MotionControl(&c->param->joint_motors[i], &param);
   }
   BSP_TIME_Delay_us(400); // 等待CAN总线空闲，确保前面的命令发送完成
   for (int i = 0; i < 2; i++) {

User/module/config.c

@@ -183,6 +183,37 @@ Config_RobotParam_t robot_config = {
         .mouse_id = 0x32,
         .keyboard_id = 0x33,
     },
+
+    .virtual_chassis_param = {
+        .control_can = BSP_CAN_1,
+        .enable_id = VIRTUAL_CHASSIS_CAN_ID_ENABLE_DEFAULT,
+        .control_id = VIRTUAL_CHASSIS_CAN_ID_CONTROL_DEFAULT,
+        
+        .motors = {
+            { // 电机0 - 左髋关节反馈
+                .can = BSP_CAN_1,
+                .feedback_id = VIRTUAL_CHASSIS_CAN_ID_FEEDBACK_BASE_DEFAULT + 0, // 124
+            },
+            { // 电机1 - 左膝关节反馈  
+                .can = BSP_CAN_1,
+                .feedback_id = VIRTUAL_CHASSIS_CAN_ID_FEEDBACK_BASE_DEFAULT + 1, // 125
+            },
+            { // 电机2 - 右膝关节反馈
+                .can = BSP_CAN_1,
+                .feedback_id = VIRTUAL_CHASSIS_CAN_ID_FEEDBACK_BASE_DEFAULT + 2, // 126
+            },
+            { // 电机3 - 右髋关节反馈
+                .can = BSP_CAN_1,
+                .feedback_id = VIRTUAL_CHASSIS_CAN_ID_FEEDBACK_BASE_DEFAULT + 3, // 127
+            },
+        },
+        
+        .imu_can = BSP_CAN_1,
+        .accl_id = VIRTUAL_CHASSIS_CAN_ID_ACCL_DEFAULT,
+        .gyro_id = VIRTUAL_CHASSIS_CAN_ID_GYRO_DEFAULT,
+        .euler_id = VIRTUAL_CHASSIS_CAN_ID_EULER_DEFAULT,
+        .quat_id = VIRTUAL_CHASSIS_CAN_ID_QUAT_DEFAULT,
+    },
 };
 
 /* Private function prototypes ---------------------------------------------- */

User/module/config.h

@@ -14,11 +14,13 @@ extern "C" {
 #include "device/motor_lk.h"
 #include "module/balance_chassis.h"
 #include "device/rc_can.h"
+#include "device/virtual_chassis.h"
 
 typedef struct {
     DM_IMU_Param_t imu_param;
     Chassis_Params_t chassis_param;
     RC_CAN_Param_t rc_can_param;
+    Virtual_Chassis_Param_t virtual_chassis_param;  // 虚拟底盘参数
 } Config_RobotParam_t;
 
 /* Exported functions prototypes -------------------------------------------- */

User/task/atti_esti.c

@@ -43,13 +43,13 @@ void Task_atti_esti(void *argument) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
     
-    if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) {
-        osMessageQueueReset(task_runtime.msgq.chassis_imu); // 重置消息队列，防止阻塞
-        chassis_imu_send.accl = dm_imu.data.accl;
-        chassis_imu_send.gyro = dm_imu.data.gyro;
-        chassis_imu_send.euler = dm_imu.data.euler;
-        osMessageQueuePut(task_runtime.msgq.chassis_imu, &chassis_imu_send, 0, 0); // 非阻塞发送IMU数据
-    }
+    // if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) {
+    //     osMessageQueueReset(task_runtime.msgq.chassis_imu); // 重置消息队列，防止阻塞
+    //     chassis_imu_send.accl = dm_imu.data.accl;
+    //     chassis_imu_send.gyro = dm_imu.data.gyro;
+    //     chassis_imu_send.euler = dm_imu.data.euler;
+    //     osMessageQueuePut(task_runtime.msgq.chassis_imu, &chassis_imu_send, 0, 0); // 非阻塞发送IMU数据
+    // }
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }

User/task/ctrl_chassis.c

@@ -9,6 +9,7 @@
 #include "bsp/can.h"
 #include "device/motor_lk.h"
 #include "device/motor_lz.h"
+#include "device/virtual_chassis.h"
 #include "module/config.h"
 #include "module/balance_chassis.h"
 
@@ -30,7 +31,7 @@ Chassis_CMD_t chassis_cmd = {
     .height = 0.0f,
 };
 Chassis_IMU_t chassis_imu;
-Chassis_t chassis1;
+Virtual_Chassis_t virtual_chassis; // 添加虚拟底盘设备
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
@@ -49,15 +50,44 @@ void Task_ctrl_chassis(void *argument) {
 
   Chassis_Init(&chassis, &Config_GetRobotParam()->chassis_param, CTRL_CHASSIS_FREQ);
   
+  // 初始化虚拟底盘设备
+  Virtual_Chassis_Init(&virtual_chassis, &Config_GetRobotParam()->virtual_chassis_param);
+  Virtual_Chassis_Enable(&virtual_chassis); // 使能虚拟底盘
 
   /* USER CODE INIT END */
   
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
+    // 更新虚拟底盘数据
+    Virtual_Chassis_Update(&virtual_chassis);
+    
+    // 从虚拟底盘获取IMU数据
+    Virtual_Chassis_IMUFeedback_t *virtual_imu = Virtual_Chassis_GetIMUFeedback(&virtual_chassis);
+    if (virtual_imu != NULL && virtual_imu->online) {
+        chassis_imu.accl = virtual_imu->accl;
+        chassis_imu.gyro = virtual_imu->gyro; 
+        chassis_imu.euler = virtual_imu->euler;
+        chassis.feedback.imu = chassis_imu;
+    } else {
+        // 如果虚拟底盘IMU离线，从消息队列获取IMU数据作为备用
         if (osMessageQueueGet(task_runtime.msgq.chassis_imu, &chassis_imu, NULL, 0) == osOK) {
             chassis.feedback.imu = chassis_imu;
         }
+    }
+    
+    // 从虚拟底盘获取关节电机数据
+    for (int i = 0; i < 4; i++) {
+        Virtual_Chassis_MotorFeedback_t *virtual_motor = Virtual_Chassis_GetMotorFeedback(&virtual_chassis, i);
+        if (virtual_motor != NULL && virtual_motor->online) {
+            // 将虚拟底盘的电机数据转换为底盘反馈格式
+            chassis.feedback.joint[i].rotor_abs_angle = virtual_motor->current_angle;
+            chassis.feedback.joint[i].rotor_speed = virtual_motor->current_velocity;
+            chassis.feedback.joint[i].torque_current = virtual_motor->current_torque;
+            chassis.feedback.joint[i].temp = 25.0f; // 默认温度
+        }
+    }
+    
     if (osMessageQueueGet(task_runtime.msgq.Chassis_cmd, &chassis_cmd, NULL, 0) == osOK) {
         // 成功接收到命令，更新底盘命令
     }
@@ -65,6 +95,14 @@ void Task_ctrl_chassis(void *argument) {
     Chassis_UpdateFeedback(&chassis);
     Chassis_Control(&chassis, &chassis_cmd);
     
+    // 将控制输出发送给虚拟底盘
+    if (Virtual_Chassis_IsOnline(&virtual_chassis)) {
+        float torques[4];
+        for (int i = 0; i < 4; i++) {
+            torques[i] = chassis.output.joint[i].torque; // 从底盘控制输出获取力矩
+        }
+        // Virtual_Chassis_SendTorqueCommand(&virtual_chassis, torques);
+    }
     
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */

User/task/ctrl_virtual_chassis.c

@@ -0,0 +1,98 @@
+/*
+    ctrl_virtual_chassis Task - 使用虚拟底盘设备的示例任务
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "task/user_task.h"
+/* USER INCLUDE BEGIN */
+#include "device/virtual_chassis.h"
+#include "module/config.h"
+#include "bsp/time.h"
+/* USER INCLUDE END */
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+static Virtual_Chassis_t virtual_chassis;
+static bool chassis_enabled = false;
+static float test_torques[VIRTUAL_CHASSIS_MOTOR_COUNT] = {0.0f, 0.0f, 0.0f, 0.0f};
+/* USER STRUCT END */
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+void Task_ctrl_virtual_chassis(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / 100.0f; // 100Hz频率
+
+  osDelay(500); /* 延时一段时间再开启任务 */
+
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  /* USER CODE INIT BEGIN */
+  
+  // 初始化虚拟底盘
+  if (Virtual_Chassis_Init(&virtual_chassis, &Config_GetRobotParam()->virtual_chassis_param) != DEVICE_OK) {
+      // 初始化失败处理
+      return;
+  }
+  
+  // 使能底盘电机
+  Virtual_Chassis_Enable(&virtual_chassis);
+  chassis_enabled = true;
+  
+  /* USER CODE INIT END */
+  
+  while (1) {
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    /* USER CODE BEGIN */
+    
+    // 更新虚拟底盘数据（接收反馈）
+    Virtual_Chassis_Update(&virtual_chassis);
+    
+    // 检查设备在线状态
+    if (Virtual_Chassis_IsOnline(&virtual_chassis)) {
+        
+        // 发送力矩控制命令（这里可以根据实际需要设置力矩值）
+        if (chassis_enabled) {
+            Virtual_Chassis_SendTorqueCommand(&virtual_chassis, test_torques);
+        }
+        
+        // 获取电机反馈数据
+        for (int i = 0; i < VIRTUAL_CHASSIS_MOTOR_COUNT; i++) {
+            Virtual_Chassis_MotorFeedback_t *motor_fb = Virtual_Chassis_GetMotorFeedback(&virtual_chassis, i);
+            if (motor_fb != NULL && motor_fb->online) {
+                // 这里可以使用电机反馈数据
+                // 例如：发送到其他任务或用于控制算法
+                // motor_fb->current_angle, motor_fb->current_velocity, motor_fb->current_torque
+            }
+        }
+        
+        // 获取IMU反馈数据
+        Virtual_Chassis_IMUFeedback_t *imu_fb = Virtual_Chassis_GetIMUFeedback(&virtual_chassis);
+        if (imu_fb != NULL && imu_fb->online) {
+            // 这里可以使用IMU数据替换原有的陀螺仪数据
+            // 例如：将数据发送给底盘控制任务
+            Chassis_IMU_t chassis_imu = {
+                .accl = imu_fb->accl,
+                .gyro = imu_fb->gyro,
+                .euler = imu_fb->euler,
+            };
+            
+            // 发送IMU数据到底盘控制任务
+            osMessageQueuePut(task_runtime.msgq.chassis_imu, &chassis_imu, 0, 0);
+        }
+    } else {
+        // 设备离线，重新使能
+        if (chassis_enabled) {
+            Virtual_Chassis_Enable(&virtual_chassis);
+        }
+    }
+    
+    /* USER CODE END */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+}