From 11aef0acb2fe9aecff06aeb48d77586b4094ade2 Mon Sep 17 00:00:00 2001
From: Robofish <1683502971@qq.com>
Date: Sun, 5 Oct 2025 19:42:19 +0800
Subject: [PATCH] =?UTF-8?q?=E6=94=B9=E8=A7=A3=E6=9E=90?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 User/device/virtual_chassis.c | 24 ++++++++++++------------
 User/module/balance_chassis.c |  4 ++--
 2 files changed, 14 insertions(+), 14 deletions(-)

diff --git a/User/device/virtual_chassis.c b/User/device/virtual_chassis.c
index 4e9dade..7ff105d 100644
--- a/User/device/virtual_chassis.c
+++ b/User/device/virtual_chassis.c
@@ -157,7 +157,7 @@ static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint16_t id,
     }
     else if (id == chassis->param.imu.gyro_id) { 
         // 陀螺仪数据 - x/y轴24位，z轴16位
-        // x: 24位有符号整数 (字节0-2) - 精度0.001°/s
+        // x: 24位有符号整数 (字节0-2) - 精度0.001 rad/s (不再转换单位)
         int32_t x_int = 0;
         x_int |= ((int32_t)data[0]) << 16;
         x_int |= ((int32_t)data[1]) << 8;
@@ -166,9 +166,9 @@ static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint16_t id,
         if (x_int & 0x800000) {
             x_int |= 0xFF000000;
         }
-        chassis->data.imu.gyro.x = ((float)x_int / 1000.0f) * M_PI / 180.0f; // °/s -> rad/s
+        chassis->data.imu.gyro.x = (float)x_int / 1000.0f; // 直接除以1000，保持rad/s
         
-        // y: 24位有符号整数 (字节3-5) - 精度0.001°/s
+        // y: 24位有符号整数 (字节3-5) - 精度0.001 rad/s (不再转换单位)
         int32_t y_int = 0;
         y_int |= ((int32_t)data[3]) << 16;
         y_int |= ((int32_t)data[4]) << 8;
@@ -177,16 +177,16 @@ static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint16_t id,
         if (y_int & 0x800000) {
             y_int |= 0xFF000000;
         }
-        chassis->data.imu.gyro.y = ((float)y_int / 1000.0f) * M_PI / 180.0f; // °/s -> rad/s
+        chassis->data.imu.gyro.y = (float)y_int / 1000.0f; // 直接除以1000，保持rad/s
         
-        // z: 16位有符号整数 (字节6-7) - 精度0.01°/s
+        // z: 16位有符号整数 (字节6-7) - 精度0.01 rad/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
+        chassis->data.imu.gyro.z = (float)z_int / 100.0f; // 直接除以100，保持rad/s
     }
     else if (id == chassis->param.imu.euler_id) { 
         // 欧拉角数据 - yaw/pitch轴24位，roll轴16位
-        // yaw: 24位有符号整数 (字节0-2) - 精度0.0001°
+        // yaw: 24位有符号整数 (字节0-2) - 精度0.0001 rad (不再转换单位)
         int32_t yaw_int = 0;
         yaw_int |= ((int32_t)data[0]) << 16;
         yaw_int |= ((int32_t)data[1]) << 8;
@@ -195,9 +195,9 @@ static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint16_t id,
         if (yaw_int & 0x800000) {
             yaw_int |= 0xFF000000;
         }
-        chassis->data.imu.euler.yaw = ((float)yaw_int / 10000.0f) * M_PI / 180.0f; // ° -> rad
+        chassis->data.imu.euler.yaw = (float)yaw_int / 10000.0f; // 直接除以10000，保持rad
         
-        // pitch: 24位有符号整数 (字节3-5) - 精度0.0001°
+        // pitch: 24位有符号整数 (字节3-5) - 精度0.0001 rad (不再转换单位)
         int32_t pit_int = 0;
         pit_int |= ((int32_t)data[3]) << 16;
         pit_int |= ((int32_t)data[4]) << 8;
@@ -206,12 +206,12 @@ static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint16_t id,
         if (pit_int & 0x800000) {
             pit_int |= 0xFF000000;
         }
-        chassis->data.imu.euler.pit = ((float)pit_int / 10000.0f) * M_PI / 180.0f; // ° -> rad
+        chassis->data.imu.euler.pit = (float)pit_int / 10000.0f; // 直接除以10000，保持rad
         
-        // roll: 16位有符号整数 (字节6-7) - 精度0.001°
+        // roll: 16位有符号整数 (字节6-7) - 精度0.001 rad (不再转换单位)
         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
+        chassis->data.imu.euler.rol = (float)rol_int / 1000.0f; // 直接除以1000，保持rad
     }
     else if (id == chassis->param.imu.quat_id) { 
         // 四元数数据 - q0/q1/q2/q3 各2字节，精度1/32767
diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c
index 45c7bed..f4b116c 100644
--- a/User/module/balance_chassis.c
+++ b/User/module/balance_chassis.c
@@ -73,9 +73,9 @@ static void Chassis_UpdateChassisState(Chassis_t *c) {
     return;
 
   // 从轮子编码器估计机体速度 (参考C++代码)
-  float left_wheel_speed_dps = -c->feedback.wheel[0].rotor_speed; // dps (度每秒)
+  float left_wheel_speed_dps = c->feedback.wheel[0].rotor_speed; // dps (度每秒)
   float right_wheel_speed_dps =
-      -c->feedback.wheel[1].rotor_speed; // dps (度每秒)
+      c->feedback.wheel[1].rotor_speed; // dps (度每秒)
 
   // 将dps转换为rad/s
   float left_wheel_speed = left_wheel_speed_dps * M_PI / 180.0f;   // rad/s