Fix height key handlers to start from planner default when unset

Keys 5/6 now initialize from 0.789m (planner default) instead of
operating on -1.0, which caused key 5 to jump to 0.55m and key 6
to have no effect.

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

@@ -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;