/* * @file virtual_chassis.c * @brief 虚拟底盘设备驱动实现 - 控制端 * @details 作为控制端，发送控制命令到底盘，接收底盘反馈数据 */ /* Includes ----------------------------------------------------------------- */ #include "device/virtual_chassis.h" #include "bsp/can.h" #include "bsp/time.h" #include /* 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) { if (chassis == NULL || data == NULL) { return DEVICE_ERR_NULL; } switch (id) { case 150: { // 加速度计数据 - x/y/z 各2字节，精度0.01g int16_t x, y, z; memcpy(&x, &data[0], 2); memcpy(&y, &data[2], 2); memcpy(&z, &data[4], 2); chassis->data.imu.accl.x = x / 100.0f; chassis->data.imu.accl.y = y / 100.0f; chassis->data.imu.accl.z = z / 100.0f; break; } case 151: { // 陀螺仪数据 - x/y/z 各2字节，精度0.01°/s，需转回弧度/s int16_t x, y, z; memcpy(&x, &data[0], 2); memcpy(&y, &data[2], 2); memcpy(&z, &data[4], 2); chassis->data.imu.gyro.x = (x / 100.0f) * 0.0174533f; // °/s -> rad/s chassis->data.imu.gyro.y = (y / 100.0f) * 0.0174533f; chassis->data.imu.gyro.z = (z / 100.0f) * 0.0174533f; break; } case 152: { // 欧拉角数据 - yaw/pitch/roll 各2字节，精度0.01°，需转回弧度 int16_t yaw, pit, rol; memcpy(&yaw, &data[0], 2); memcpy(&pit, &data[2], 2); memcpy(&rol, &data[4], 2); chassis->data.imu.euler.yaw = (yaw / 100.0f) * 0.0174533f; // ° -> rad chassis->data.imu.euler.pit = (pit / 100.0f) * 0.0174533f; chassis->data.imu.euler.rol = (rol / 100.0f) * 0.0174533f; break; } case 153: { // 四元数数据 - q0/q1/q2/q3 各2字节，精度0.0001 int16_t q0, q1, q2, q3; memcpy(&q0, &data[0], 2); memcpy(&q1, &data[2], 2); memcpy(&q2, &data[4], 2); memcpy(&q3, &data[6], 2); chassis->data.imu.quat.q0 = q0 / 10000.0f; chassis->data.imu.quat.q1 = q1 / 10000.0f; chassis->data.imu.quat.q2 = q2 / 10000.0f; chassis->data.imu.quat.q3 = q3 / 10000.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, 150 + i, 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; for (int i = 0; i < 4; i++){ if(BSP_CAN_GetMessage(chassis->param.imu.can, 150 + i, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK){ continue; } else { Virtual_Chassis_DecodeIMU(chassis, 150 + i, msg.data); } } for (int i = 0; i < 4; i++){ if (BSP_CAN_GetMessage(chassis->param.motors.can, 124 + i, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK){ continue; } else { // 解析电机反馈数据 Virtual_Chassis_DecodeMotor(chassis, 124 + i, msg.data); } } 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; } /* Private functions -------------------------------------------------------- */