Browse Source

Fix IMU pitch correction to Y-axis rotation, -8° default

X-axis was producing roll (sideways tilt). Y-axis is the correct
pitch axis in SONIC's quaternion frame.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
main
Joe DiPrima 1 month ago
parent
commit
7b6687bcac
  1. 2
      gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp

2
gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp

@ -2659,7 +2659,7 @@ class G1Deploy {
// Runtime-adjustable via keys 9/0 (±1° steps), default -6°. // Runtime-adjustable via keys 9/0 (±1° steps), default -6°.
double pitch_offset_rad = g_imu_pitch_offset_deg.load() * M_PI / 180.0; double pitch_offset_rad = g_imu_pitch_offset_deg.load() * M_PI / 180.0;
double half = pitch_offset_rad / 2.0; double half = pitch_offset_rad / 2.0;
std::array<double, 4> pitch_quat = {std::cos(half), std::sin(half), 0.0, 0.0}; // w,x,y,z
std::array<double, 4> pitch_quat = {std::cos(half), 0.0, std::sin(half), 0.0}; // w,x,y,z — Y-axis rotation
std::array<double, 4> base_quat = quat_mul_d(pitch_quat, base_quat_raw); std::array<double, 4> base_quat = quat_mul_d(pitch_quat, base_quat_raw);
std::array<double, 3> base_ang_vel = float_to_double<3>(ls->imu_state().gyroscope()); std::array<double, 3> base_ang_vel = float_to_double<3>(ls->imu_state().gyroscope());
std::array<double, 3> base_accel = float_to_double<3>(ls->imu_state().accelerometer()); std::array<double, 3> base_accel = float_to_double<3>(ls->imu_state().accelerometer());

Loading…
Cancel
Save