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 be7744f..8512141 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
@@ -179,18 +179,21 @@ class GamepadManager : public InputInterface {
             break;
           }
           case '5': {
-            double val = std::max(g_standing_height_m.load() - 0.01, 0.55);
+            double cur = g_standing_height_m.load();
+            if (cur < 0) cur = 0.789;  // Start from planner default
+            double val = std::max(cur - 0.01, 0.55);
             g_standing_height_m.store(val);
             std::cout << "[HEIGHT] Standing height: " << val << " m" << std::endl;
             is_manager_key = true;
             break;
           }
           case '6': {
-            double val = std::min(g_standing_height_m.load() + 0.01, 0.80);
+            double cur = g_standing_height_m.load();
+            if (cur < 0) cur = 0.789;  // Start from planner default
+            double val = std::min(cur + 0.01, 0.85);
             g_standing_height_m.store(val);
             std::cout << "[HEIGHT] Standing height: " << val << " m" << std::endl;
             is_manager_key = true;
-            break;
           }
           case '3': {
             double val = g_knee_offset_deg.load() - 1.0;