robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

xiu

Browse Source
This commit is contained in:
Robofish 2025-10-04 23:19:58 +08:00
parent 1a739ab634
commit acb070fce7
1 changed files with 16 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
User/device/virtual_chassis.c
View File

@ -56,8 +56,8 @@ static int8_t Virtual_Chassis_DecodeMotor(Virtual_Chassis_t *chassis, uint8_t id
return DEVICE_OK; return DEVICE_OK;
} }
static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint8_t id, uint8_t *data, uint8_t len) { static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint8_t id, uint8_t *data) {
if (chassis == NULL || data == NULL || len < 8) { if (chassis == NULL || data == NULL) {
return DEVICE_ERR_NULL; return DEVICE_ERR_NULL;
} }
@ -136,7 +136,7 @@ int8_t Virtual_Chassis_Init(Virtual_Chassis_t *chassis, Virtual_Chassis_Param_t
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
BSP_CAN_RegisterId(chassis->param.motors.can, 124 + i, 3); // 电机反馈ID BSP_CAN_RegisterId(chassis->param.motors.can, 124 + i, 3); // 电机反馈ID
BSP_CAN_RegisterId(chassis->param.imu.can, 0x66 + 1, 3); BSP_CAN_RegisterId(chassis->param.imu.can, 0x66 + i, 3);
} }
// 设置设备在线状态 // 设置设备在线状态
@ -158,14 +158,23 @@ int8_t Virtual_Chassis_Update(Virtual_Chassis_t *chassis) {
BSP_CAN_Message_t msg; BSP_CAN_Message_t msg;
for (int i = 0; i < 4; i++){ for (int i = 0; i < 4; i++){
BSP_CAN_GetMessage(chassis->param.motors.can, 124 + i, &msg, BSP_CAN_TIMEOUT_IMMEDIATE); if(BSP_CAN_GetMessage(chassis->param.imu.can, 0x66 + i, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK){
Virtual_Chassis_DecodeMotor(chassis, msg.parsed_id, msg.data); continue;
} else {
Virtual_Chassis_DecodeIMU(chassis, 0x66 + i, msg.data);
}
} }
for (int i = 0; i < 4; i++){ for (int i = 0; i < 4; i++){
BSP_CAN_GetMessage(chassis->param.imu.can, 0x66 + i, &msg, BSP_CAN_TIMEOUT_IMMEDIATE); if (BSP_CAN_GetMessage(chassis->param.motors.can, 124 + i, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK){
Virtual_Chassis_DecodeIMU(chassis, 0x66 + i, msg.data, msg.dlc); continue;
} else {
// 解析电机反馈数据
Virtual_Chassis_DecodeMotor(chassis, 124 + i, msg.data);
}
} }
return DEVICE_OK; return DEVICE_OK;
} }