bug fix hem to stop

selfdrive/controls/lib/longitudinal_planner.py

@@ -3046,8 +3046,17 @@ class LongitudinalPlanner:
         getattr(starpilot_toggles, "hybrid_exp_bias", 0.0),
         getattr(starpilot_toggles, "hybrid_vision_brake_sensitivity", 1.0),
       )
-      a_exp_raw = float(sm['modelV2'].action.desiredAcceleration)
       should_stop_exp = bool(sm['modelV2'].action.shouldStop)
+      model_v2 = sm['modelV2']
+      if (len(getattr(model_v2.velocity, 'x', [])) == ModelConstants.IDX_N and
+              len(getattr(model_v2.acceleration, 'x', [])) == ModelConstants.IDX_N):
+        v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_v2.velocity.x)
+        a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_v2.acceleration.x)
+        v_target = float(np.interp(action_t, T_IDXS_MPC, v))
+        a_exp_traj = 2.0 * (v_target - float(v[0])) / max(action_t, 1e-3) - float(a[0])
+        a_exp_raw = float(np.clip(a_exp_traj, -12.0, 3.0))
+      else:
+        a_exp_raw = float(model_v2.action.desiredAcceleration)
 
       # Pass the active lead that MPC is tracking (leadTwo when source == "lead1")
       # so HEM is never blind to the radar lead actually being followed.

starpilot/controls/lib/hybrid_experimental_mode.py

@@ -27,7 +27,7 @@ class HybridExperimentalMode:
     self.prev_a_target = 0.0
     self.last_exp_dominant = False
     self.diag = {}
-    self.record_diag = False
+    self.record_diag = True
 
     # Tunings
     self.HYBRID_EXP_BIAS = 0.0
@@ -107,8 +107,8 @@ class HybridExperimentalMode:
     a_brake_fused = min(a_chill, a_exp)
     a_throttle_fused = a_chill + max(0.0, a_exp - a_chill) * self.HYBRID_EXP_BIAS
 
-    # Output Arbitration
-    is_stopping_event = (self.w_vision > 0.3) or horizon_stopping
+    stop_requested = bool(should_stop_chill or should_stop_exp)
+    is_stopping_event = stop_requested or (self.w_vision > 0.2) or horizon_stopping
     self.last_exp_dominant = bool(is_stopping_event and not is_departing)
 
     if self.last_exp_dominant:

starpilot/controls/tests/test_hybrid_experimental_mode.py

@@ -231,6 +231,15 @@ def test_should_stop_chill_handshake():
   assert should_stop, "should_stop_chill must propagate into the fused stop flag"
 
 
+def test_should_stop_chill_engages_braking_regime_without_vision():
+  controller = make_controller(prev=0.0)
+  model = FakeModel(velocity=[20.0] * 33)
+  a, should_stop = controller.update(20.0, 25.0, FakeLead(), model, 0.0, -1.5, should_stop_chill=True)
+  assert controller.last_exp_dominant, "should_stop_chill must engage the braking regime"
+  assert a == pytest.approx(-1.5, abs=1e-3), f"Must command exp braking on planner stop, got {a}"
+  assert should_stop
+
+
 def test_should_stop_exp_handshake():
   controller = make_controller(prev=0.0)
   model = FakeModel(velocity=[5.0] * 33)