IMU pitch correction: Y-axis rotation -6° with runtime tuning

Mathematically verified: gravity[0] = 2*(w*y - x*z) responds to qy,
so Y-axis quaternion [cos(h),0,sin(h),0] is the correct pitch axis.
Left-multiply, default -6°. Keys 9/0 adjust ±1° at runtime.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp

@@ -40,11 +40,15 @@
 #include <thread>
 #include <chrono>
 
+#include <atomic>
 #include "input_interface.hpp"
 #include "zmq_endpoint_interface.hpp"
 #include "gamepad.hpp"
 #include "../localmotion_kplanner.hpp"  // For LocomotionMode enum
 
+/// Runtime-adjustable IMU pitch offset (degrees). Keys 9/0 to adjust via tmux.
+inline std::atomic<double> g_imu_pitch_offset_deg{-6.0};
+
 #if HAS_ROS2
 #include "ros2_input_handler.hpp"
 #endif
@@ -120,6 +124,20 @@ class GamepadManager : public InputInterface {
             is_manager_key = true;
             std::cout << "[GamepadManager] EMERGENCY STOP triggered (O/o key pressed)" << std::endl;
             break;
+          case '9': {
+            double val = g_imu_pitch_offset_deg.load() - 1.0;
+            g_imu_pitch_offset_deg.store(val);
+            std::cout << "[IMU] Pitch offset: " << val << " deg" << std::endl;
+            is_manager_key = true;
+            break;
+          }
+          case '0': {
+            double val = g_imu_pitch_offset_deg.load() + 1.0;
+            g_imu_pitch_offset_deg.store(val);
+            std::cout << "[IMU] Pitch offset: " << val << " deg" << std::endl;
+            is_manager_key = true;
+            break;
+          }
         }
 
         if (!is_manager_key && current_) {

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

@@ -2653,7 +2653,13 @@ class G1Deploy {
         }
       }
 
-      std::array<double, 4> base_quat = float_to_double<4>(ls->imu_state().quaternion()); // qw, qx, qy, qz
+      std::array<double, 4> base_quat_raw = float_to_double<4>(ls->imu_state().quaternion()); // qw, qx, qy, qz
+      // IMU pitch correction: Y-axis rotation, left-multiply. Default -6°.
+      // Adjustable at runtime via keys 9/0 in tmux.
+      double pitch_rad = g_imu_pitch_offset_deg.load() * M_PI / 180.0;
+      double half = pitch_rad / 2.0;
+      std::array<double, 4> pitch_quat = {std::cos(half), 0.0, std::sin(half), 0.0};
+      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_accel = float_to_double<3>(ls->imu_state().accelerometer());