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

修改接受

Browse Source
This commit is contained in:
Robofish 2025-10-05 13:44:59 +08:00
parent cdd7be6ad7
commit 000293a9b8
1 changed files with 90 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

120
User/device/virtual_chassis.c
View File

@ -126,44 +126,104 @@ static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint16_t id,
return DEVICE_ERR_NULL;
}
if (id == chassis->param.imu.accl_id) { // 加速度计数据 - 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;
if (id == chassis->param.imu.accl_id) {
// 加速度计数据 - x/y轴24位，z轴16位
// x: 24位有符号整数 (字节0-2) - 精度1/1000000
int32_t x_int = 0;
x_int |= ((int32_t)data[0]) << 16;
x_int |= ((int32_t)data[1]) << 8;
x_int |= ((int32_t)data[2]);
// 符号扩展（24位转32位）
if (x_int & 0x800000) {
x_int |= 0xFF000000;
}
chassis->data.imu.accl.x = (float)x_int / 1000000.0f;
// y: 24位有符号整数 (字节3-5) - 精度1/1000000
int32_t y_int = 0;
y_int |= ((int32_t)data[3]) << 16;
y_int |= ((int32_t)data[4]) << 8;
y_int |= ((int32_t)data[5]);
// 符号扩展（24位转32位）
if (y_int & 0x800000) {
y_int |= 0xFF000000;
}
chassis->data.imu.accl.y = (float)y_int / 1000000.0f;
// z: 16位有符号整数 (字节6-7) - 精度1/10000
int16_t z_int;
memcpy(&z_int, &data[6], sizeof(int16_t));
chassis->data.imu.accl.z = (float)z_int / 10000.0f;
}
else if (id == chassis->param.imu.gyro_id) { // 陀螺仪数据 - x/y/z 各2字节，精度0.01°/s
int16_t x, y, z;
memcpy(&x, &data[0], 2);
memcpy(&y, &data[2], 2);
memcpy(&z, &data[4], 2);
// 注意：底盘发送的是精度0.01°/s，而不是0.1°/s，所以除以100而不是10
chassis->data.imu.gyro.x = (x / 100.0f) * M_PI / 180.0f; // °/s -> rad/s
chassis->data.imu.gyro.y = (y / 100.0f) * M_PI / 180.0f;
chassis->data.imu.gyro.z = (z / 100.0f) * M_PI / 180.0f;
else if (id == chassis->param.imu.gyro_id) {
// 陀螺仪数据 - x/y轴24位，z轴16位
// x: 24位有符号整数 (字节0-2) - 精度0.001°/s
int32_t x_int = 0;
x_int |= ((int32_t)data[0]) << 16;
x_int |= ((int32_t)data[1]) << 8;
x_int |= ((int32_t)data[2]);
// 符号扩展（24位转32位）
if (x_int & 0x800000) {
x_int |= 0xFF000000;
}
chassis->data.imu.gyro.x = ((float)x_int / 1000.0f) * M_PI / 180.0f; // °/s -> rad/s
// y: 24位有符号整数 (字节3-5) - 精度0.001°/s
int32_t y_int = 0;
y_int |= ((int32_t)data[3]) << 16;
y_int |= ((int32_t)data[4]) << 8;
y_int |= ((int32_t)data[5]);
// 符号扩展（24位转32位）
if (y_int & 0x800000) {
y_int |= 0xFF000000;
}
chassis->data.imu.gyro.y = ((float)y_int / 1000.0f) * M_PI / 180.0f; // °/s -> rad/s
// z: 16位有符号整数 (字节6-7) - 精度0.01°/s
int16_t z_int;
memcpy(&z_int, &data[6], sizeof(int16_t));
chassis->data.imu.gyro.z = ((float)z_int / 100.0f) * M_PI / 180.0f; // °/s -> rad/s
}
else if (id == chassis->param.imu.euler_id) { // 欧拉角数据 - 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) * M_PI / 180.0f; // ° -> rad
chassis->data.imu.euler.pit = (pit / 100.0f) * M_PI / 180.0f;
chassis->data.imu.euler.rol = (rol / 100.0f) * M_PI / 180.0f;
else if (id == chassis->param.imu.euler_id) {
// 欧拉角数据 - yaw/pitch轴24位，roll轴16位
// yaw: 24位有符号整数 (字节0-2) - 精度0.0001°
int32_t yaw_int = 0;
yaw_int |= ((int32_t)data[0]) << 16;
yaw_int |= ((int32_t)data[1]) << 8;
yaw_int |= ((int32_t)data[2]);
// 符号扩展（24位转32位）
if (yaw_int & 0x800000) {
yaw_int |= 0xFF000000;
}
chassis->data.imu.euler.yaw = ((float)yaw_int / 10000.0f) * M_PI / 180.0f; // ° -> rad
// pitch: 24位有符号整数 (字节3-5) - 精度0.0001°
int32_t pit_int = 0;
pit_int |= ((int32_t)data[3]) << 16;
pit_int |= ((int32_t)data[4]) << 8;
pit_int |= ((int32_t)data[5]);
// 符号扩展（24位转32位）
if (pit_int & 0x800000) {
pit_int |= 0xFF000000;
}
chassis->data.imu.euler.pit = ((float)pit_int / 10000.0f) * M_PI / 180.0f; // ° -> rad
// roll: 16位有符号整数 (字节6-7) - 精度0.001°
int16_t rol_int;
memcpy(&rol_int, &data[6], sizeof(int16_t));
chassis->data.imu.euler.rol = ((float)rol_int / 1000.0f) * M_PI / 180.0f; // ° -> rad
}
else if (id == chassis->param.imu.quat_id) { // 四元数数据 - q0/q1/q2/q3 各2字节，精度1/32000
else if (id == chassis->param.imu.quat_id) {
// 四元数数据 - q0/q1/q2/q3 各2字节，精度1/32767
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 / 32000.0f;
chassis->data.imu.quat.q1 = q1 / 32000.0f;
chassis->data.imu.quat.q2 = q2 / 32000.0f;
chassis->data.imu.quat.q3 = q3 / 32000.0f;
chassis->data.imu.quat.q0 = q0 / 32767.0f;
chassis->data.imu.quat.q1 = q1 / 32767.0f;
chassis->data.imu.quat.q2 = q2 / 32767.0f;
chassis->data.imu.quat.q3 = q3 / 32767.0f;
}
else {
return DEVICE_ERR;