From 000293a9b88bcc6264e96339be35bec279e33687 Mon Sep 17 00:00:00 2001
From: Robofish <1683502971@qq.com>
Date: Sun, 5 Oct 2025 13:44:59 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E6=8E=A5=E5=8F=97?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 User/device/virtual_chassis.c | 120 +++++++++++++++++++++++++---------
 1 file changed, 90 insertions(+), 30 deletions(-)

diff --git a/User/device/virtual_chassis.c b/User/device/virtual_chassis.c
index 95a94d2..b2e6f9a 100644
--- a/User/device/virtual_chassis.c
+++ b/User/device/virtual_chassis.c
@@ -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;