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

1

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

14
User/device/virtual_chassis.c
View File

@ -62,7 +62,7 @@ static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint8_t id,
}
switch (id) {
case 0x66: { // 加速度计数据 - x/y/z 各2字节，精度0.01g
case 150: { // 加速度计数据 - x/y/z 各2字节，精度0.01g
int16_t x, y, z;
memcpy(&x, &data[0], 2);
memcpy(&y, &data[2], 2);
@ -72,7 +72,7 @@ static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint8_t id,
chassis->data.imu.accl.z = z / 100.0f;
break;
}
case 0x67: { // 陀螺仪数据 - x/y/z 各2字节，精度0.01°/s，需转回弧度/s
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);
@ -82,7 +82,7 @@ static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint8_t id,
chassis->data.imu.gyro.z = (z / 100.0f) * 0.0174533f;
break;
}
case 0x68: { // 欧拉角数据 - yaw/pitch/roll 各2字节，精度0.01°，需转回弧度
case 152: { // 欧拉角数据 - yaw/pitch/roll 各2字节，精度0.01°，需转回弧度
int16_t yaw, pit, rol;
memcpy(&yaw, &data[0], 2);
memcpy(&pit, &data[2], 2);
@ -92,7 +92,7 @@ static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint8_t id,
chassis->data.imu.euler.rol = (rol / 100.0f) * 0.0174533f;
break;
}
case 0x69: { // 四元数数据 - q0/q1/q2/q3 各2字节，精度0.0001
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);
@ -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 + i, 3);
BSP_CAN_RegisterId(chassis->param.imu.can, 150 + i, 3);
}
// 设置设备在线状态
@ -158,10 +158,10 @@ int8_t Virtual_Chassis_Update(Virtual_Chassis_t *chassis) {
BSP_CAN_Message_t msg;
for (int i = 0; i < 4; i++){
if(BSP_CAN_GetMessage(chassis->param.imu.can, 0x66 + i, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK){
if(BSP_CAN_GetMessage(chassis->param.imu.can, 150 + i, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK){
continue;
} else {
Virtual_Chassis_DecodeIMU(chassis, 0x66 + i, msg.data);
Virtual_Chassis_DecodeIMU(chassis, 150 + i, msg.data);
}
}