From e0675a8addc6ec0d3eb1e60648385bf4242e069f Mon Sep 17 00:00:00 2001
From: Joe DiPrima <epilectrik@gmail.com>
Date: Sun, 22 Feb 2026 12:23:51 -0600
Subject: [PATCH] =?UTF-8?q?IMU=20pitch=20correction:=20Y-axis=20rotation?=
 =?UTF-8?q?=20-6=C2=B0=20with=20runtime=20tuning?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

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>
---
 .../input_interface/gamepad_manager.hpp        | 18 ++++++++++++++++++
 .../src/g1_deploy_onnx_ref.cpp                 |  8 +++++++-
 2 files changed, 25 insertions(+), 1 deletion(-)

diff --git a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp
index 0cdd965..0d60dd8 100644
--- a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp
+++ b/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_) {
diff --git a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp
index ecd2eef..8808748 100644
--- a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp
+++ b/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());