From 088db5d94ff8a03ccfe85b86332e4f94444a3bac Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sat, 4 Oct 2025 21:49:19 +0800 Subject: [PATCH] 1 --- CMakeLists.txt | 1 + User/device/virtual_chassis.c | 295 ++++++---------------------------- User/device/virtual_chassis.h | 47 +----- User/module/balance_chassis.h | 1 + User/module/config.c | 41 ++--- User/task/ctrl_chassis.c | 47 +----- 6 files changed, 67 insertions(+), 365 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 750eaea..9b8a186 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,6 +76,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE User/device/motor_lz.c User/device/motor_rm.c User/device/rc_can.c + User/device/virtual_chassis.c # User/module sources User/module/balance_chassis.c diff --git a/User/device/virtual_chassis.c b/User/device/virtual_chassis.c index ef9db68..6bd147b 100644 --- a/User/device/virtual_chassis.c +++ b/User/device/virtual_chassis.c @@ -60,39 +60,50 @@ static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint8_t id, 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; + case 0x66: { // 加速度计数据 - 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 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; + } + case 0x67: { // 陀螺仪数据 - 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 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; + } + case 0x68: { // 欧拉角数据 - 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 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; + } + case 0x69: { // 四元数数据 - 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; } @@ -128,7 +139,6 @@ int8_t Virtual_Chassis_Init(Virtual_Chassis_t *chassis, Virtual_Chassis_Param_t BSP_CAN_RegisterId(chassis->param.imu.can, 0x66 + 1, 3); } - // 设置设备在线状态 chassis->header.online = true; chassis->header.last_online_time = BSP_TIME_Get_us(); @@ -146,30 +156,20 @@ int8_t Virtual_Chassis_Update(Virtual_Chassis_t *chassis) { } BSP_CAN_Message_t msg; - BSP_CAN_GetMessage(chassis->param.motors.can, 124, &msg, BSP_CAN_TIMEOUT_IMMEDIATE); + for (int i = 0; i < 4; i++){ + BSP_CAN_GetMessage(chassis->param.motors.can, 124 + i, &msg, BSP_CAN_TIMEOUT_IMMEDIATE); + Virtual_Chassis_DecodeMotor(chassis, msg.parsed_id, msg.data); + } + + for (int i = 0; i < 4; i++){ + BSP_CAN_GetMessage(chassis->param.imu.can, 0x66 + i, &msg, BSP_CAN_TIMEOUT_IMMEDIATE); + Virtual_Chassis_DecodeIMU(chassis, msg.parsed_id, msg.data, msg.dlc); + } 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 发送电机使能命令 @@ -214,204 +214,5 @@ int8_t Virtual_Chassis_SendTorqueCommand(Virtual_Chassis_t *chassis, float torqu 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; -} \ No newline at end of file diff --git a/User/device/virtual_chassis.h b/User/device/virtual_chassis.h index 6195366..ad36e60 100644 --- a/User/device/virtual_chassis.h +++ b/User/device/virtual_chassis.h @@ -73,13 +73,6 @@ typedef struct { */ 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 虚拟底盘设备结构体指针 @@ -87,50 +80,12 @@ int8_t Virtual_Chassis_DeInit(Virtual_Chassis_t *chassis); */ 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]); +int8_t Virtual_Chassis_Enable(Virtual_Chassis_t *chassis); /** * @brief 使能电机并发送命令 diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index a2313be..88bb825 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -25,6 +25,7 @@ extern "C" { #include "device/motor_lk.h" #include "device/motor_lz.h" #include "device/motor_dm.h" +#include "device/virtual_chassis.h" /* Exported constants ------------------------------------------------------- */ #define CHASSIS_OK (0) /* 运行正常 */ #define CHASSIS_ERR (-1) /* 运行时发现了其他错误 */ diff --git a/User/module/config.c b/User/module/config.c index 7789fa1..1e67ff8 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -185,34 +185,19 @@ Config_RobotParam_t robot_config = { }, .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, + .motors = { + .can = BSP_CAN_1, + .Enable_id = 121, + .MIT_id = 0x122, + .feedback_id = 124, + }, + .imu = { + .can = BSP_CAN_1, + .accl_id = 0x66, + .gyro_id = 0x67, + .euler_id = 0x68, + .quat_id = 0x69, + }, }, }; diff --git a/User/task/ctrl_chassis.c b/User/task/ctrl_chassis.c index 199e707..369d0a9 100644 --- a/User/task/ctrl_chassis.c +++ b/User/task/ctrl_chassis.c @@ -49,60 +49,19 @@ void Task_ctrl_chassis(void *argument) { /* USER CODE INIT BEGIN */ 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) { - // 成功接收到命令，更新底盘命令 - } - - Chassis_UpdateFeedback(&chassis); + // 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); /* 运行结束，等待下一次唤醒 */