diff --git a/User/device/virtual_chassis.c b/User/device/virtual_chassis.c index 496c8cc..9269a46 100644 --- a/User/device/virtual_chassis.c +++ b/User/device/virtual_chassis.c @@ -56,8 +56,8 @@ static int8_t Virtual_Chassis_DecodeMotor(Virtual_Chassis_t *chassis, uint8_t id 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) { +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; } @@ -136,7 +136,7 @@ int8_t Virtual_Chassis_Init(Virtual_Chassis_t *chassis, Virtual_Chassis_Param_t 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); + 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; 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); + if(BSP_CAN_GetMessage(chassis->param.imu.can, 0x66 + i, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK){ + continue; + } else { + Virtual_Chassis_DecodeIMU(chassis, 0x66 + i, 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, 0x66 + i, msg.data, msg.dlc); + 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; }