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 19:42:19 +08:00
parent 7492cf8e05
commit 11aef0acb2
2 changed files with 14 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
User/device/virtual_chassis.c
View File

@ -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) { else if (id == chassis->param.imu.gyro_id) {
// 陀螺仪数据 - x/y轴24位，z轴16位 // 陀螺仪数据 - 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; int32_t x_int = 0;
x_int |= ((int32_t)data[0]) << 16; x_int |= ((int32_t)data[0]) << 16;
x_int |= ((int32_t)data[1]) << 8; 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) { if (x_int & 0x800000) {
x_int |= 0xFF000000; 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; int32_t y_int = 0;
y_int |= ((int32_t)data[3]) << 16; y_int |= ((int32_t)data[3]) << 16;
y_int |= ((int32_t)data[4]) << 8; 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) { if (y_int & 0x800000) {
y_int |= 0xFF000000; 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; int16_t z_int;
memcpy(&z_int, &data[6], sizeof(int16_t)); 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) { else if (id == chassis->param.imu.euler_id) {
// 欧拉角数据 - yaw/pitch轴24位，roll轴16位 // 欧拉角数据 - yaw/pitch轴24位，roll轴16位
// yaw: 24位有符号整数 (字节0-2) - 精度0.0001° // yaw: 24位有符号整数 (字节0-2) - 精度0.0001 rad (不再转换单位)
int32_t yaw_int = 0; int32_t yaw_int = 0;
yaw_int |= ((int32_t)data[0]) << 16; yaw_int |= ((int32_t)data[0]) << 16;
yaw_int |= ((int32_t)data[1]) << 8; 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) { if (yaw_int & 0x800000) {
yaw_int |= 0xFF000000; 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; int32_t pit_int = 0;
pit_int |= ((int32_t)data[3]) << 16; pit_int |= ((int32_t)data[3]) << 16;
pit_int |= ((int32_t)data[4]) << 8; 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) { if (pit_int & 0x800000) {
pit_int |= 0xFF000000; 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; int16_t rol_int;
memcpy(&rol_int, &data[6], sizeof(int16_t)); 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) { else if (id == chassis->param.imu.quat_id) {
// 四元数数据 - q0/q1/q2/q3 各2字节，精度1/32767 // 四元数数据 - q0/q1/q2/q3 各2字节，精度1/32767

4
User/module/balance_chassis.c
View File

@ -73,9 +73,9 @@ static void Chassis_UpdateChassisState(Chassis_t *c) {
return; return;
// 从轮子编码器估计机体速度 (参考C++代码) // 从轮子编码器估计机体速度 (参考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 = float right_wheel_speed_dps =
-c->feedback.wheel[1].rotor_speed; // dps (度每秒) c->feedback.wheel[1].rotor_speed; // dps (度每秒)
// 将dps转换为rad/s // 将dps转换为rad/s
float left_wheel_speed = left_wheel_speed_dps * M_PI / 180.0f; // rad/s float left_wheel_speed = left_wheel_speed_dps * M_PI / 180.0f; // rad/s