cem

selfdrive/controls/lib/longitudinal_planner.py

@@ -467,6 +467,7 @@ class LongitudinalPlanner:
 
     classic_model = bool(getattr(starpilot_toggles, "classic_model", False))
     tinygrad_model = bool(getattr(starpilot_toggles, "tinygrad_model", False))
+    experimental_mlsim = bool(tinygrad_model and self.mlsim and self.mode != 'acc')
     action_t = self.CP.longitudinalActuatorDelay + DT_MDL
 
     if classic_model:
@@ -490,10 +491,12 @@ class LongitudinalPlanner:
         self.v_desired_trajectory, self.a_desired_trajectory,
         action_t=action_t, vEgoStopping=starpilot_toggles.vEgoStopping)
 
+    output_accel_min = get_vehicle_min_accel(self.CP, v_ego) if experimental_mlsim else accel_limits_turns[0]
+
     close_lead_caps = []
     if tracking_lead:
       for lead in (self.lead_one, self.lead_two):
-        cap = self.get_close_lead_brake_cap(lead, v_ego, accel_limits_turns[0])
+        cap = self.get_close_lead_brake_cap(lead, v_ego, output_accel_min)
         if cap is not None:
           close_lead_caps.append(cap)
     if close_lead_caps:
@@ -514,7 +517,7 @@ class LongitudinalPlanner:
       output_a_target = min(output_a_target, cruise_accel_cap)
 
     output_accel_max = no_throttle_output_max if not self.allow_throttle else accel_limits_turns[1]
-    output_a_target = float(np.clip(output_a_target, accel_limits_turns[0], output_accel_max))
+    output_a_target = float(np.clip(output_a_target, output_accel_min, output_accel_max))
 
     self.output_a_target = output_a_target
     self.output_should_stop = bool(output_should_stop)

selfdrive/controls/tests/test_conditional_experimental_mode.py

@@ -1,6 +1,9 @@
+from pathlib import Path
 from types import SimpleNamespace
 
 from openpilot.common.constants import CV
+from openpilot.starpilot.controls.starpilot_planner import StarPilotPlanner
+import openpilot.starpilot.controls.starpilot_planner as starpilot_planner_module
 from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
 
 
@@ -39,3 +42,79 @@ def test_predicted_stop_within_threshold_triggers_stop_light():
   cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
 
   assert cem.stop_light_detected
+
+
+class DummyThemeManager:
+  def update_wheel_image(self, *args, **kwargs):
+    pass
+
+
+def test_starpilot_planner_updates_cem_with_current_frame_state(monkeypatch):
+  planner = StarPilotPlanner(Path("/tmp/nonexistent"), DummyThemeManager())
+
+  try:
+    monkeypatch.setattr(starpilot_planner_module, "calculate_road_curvature", lambda model, v_ego: (0.01, 1.0))
+    monkeypatch.setattr(planner.starpilot_acceleration, "update", lambda *args, **kwargs: None)
+    monkeypatch.setattr(planner.starpilot_events, "update", lambda *args, **kwargs: None)
+    monkeypatch.setattr(planner.starpilot_vcruise, "update", lambda *args, **kwargs: 0.0)
+    monkeypatch.setattr(planner.starpilot_weather, "update_weather", lambda *args, **kwargs: None)
+
+    def following_update(*args, **kwargs):
+      planner.starpilot_following.following_lead = planner.tracking_lead
+      planner.starpilot_following.slower_lead = planner.tracking_lead
+
+    monkeypatch.setattr(planner.starpilot_following, "update", following_update)
+
+    seen = {}
+
+    def cem_update(v_ego, sm, starpilot_toggles):
+      seen.update({
+        "tracking_lead": planner.tracking_lead,
+        "following_lead": planner.starpilot_following.following_lead,
+        "slower_lead": planner.starpilot_following.slower_lead,
+        "model_length": planner.model_length,
+        "driving_in_curve": planner.driving_in_curve,
+        "road_curvature_detected": planner.road_curvature_detected,
+      })
+
+    monkeypatch.setattr(planner.starpilot_cem, "update", cem_update)
+
+    planner.model_length = 0.0
+    planner.tracking_lead = False
+    planner.driving_in_curve = False
+    planner.lateral_acceleration = 0.0
+    planner.road_curvature_detected = False
+    planner.starpilot_following.following_lead = False
+    planner.starpilot_following.slower_lead = False
+
+    sm = {
+      "radarState": SimpleNamespace(leadOne=SimpleNamespace(status=True, dRel=25.0, vLead=0.5)),
+      "selfdriveState": SimpleNamespace(enabled=True),
+      "carState": SimpleNamespace(vCruise=50.0, vEgo=20.0, standstill=False, leftBlinker=False, rightBlinker=False),
+      "controlsState": SimpleNamespace(curvature=0.02),
+      "modelV2": SimpleNamespace(position=SimpleNamespace(x=[0.0, 30.0]), laneLines=[None] * 4, roadEdges=[None] * 2),
+      "starpilotCarState": SimpleNamespace(pauseLateral=False),
+      planner.gps_location_service: SimpleNamespace(latitude=1.0, longitude=1.0, bearingDeg=90.0),
+    }
+    starpilot_toggles = SimpleNamespace(
+      set_speed_offset=0,
+      conditional_experimental_mode=True,
+      minimum_lane_change_speed=100.0,
+      pause_lateral_below_speed=0.0,
+      pause_lateral_below_signal=False,
+      stop_distance=6.0,
+      weather_presets=False,
+    )
+
+    planner.update(0.0, False, sm, starpilot_toggles)
+
+    assert seen == {
+      "tracking_lead": True,
+      "following_lead": True,
+      "slower_lead": True,
+      "model_length": 30.0,
+      "driving_in_curve": True,
+      "road_curvature_detected": True,
+    }
+  finally:
+    planner.shutdown()

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -0,0 +1,117 @@
+from types import SimpleNamespace
+
+import pytest
+
+from cereal import log
+from opendbc.car.honda.interface import CarInterface
+from opendbc.car.honda.values import CAR
+from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
+from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_vehicle_min_accel
+from openpilot.selfdrive.modeld.constants import ModelConstants
+
+
+def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0):
+  lead = log.RadarState.LeadData.new_message()
+  lead.status = status
+  lead.dRel = d_rel
+  lead.vLead = v_lead
+  lead.vLeadK = v_lead
+  lead.aLeadK = 0.0
+  lead.vRel = 0.0
+  lead.aRel = 0.0
+  lead.modelProb = 0.0
+  return lead
+
+
+def make_model(v_ego: float, desired_accel: float):
+  model = log.ModelDataV2.new_message()
+  t_idxs = ModelConstants.T_IDXS
+
+  model.position.x = [float(v_ego * t) for t in t_idxs]
+  model.position.y = [0.0] * len(t_idxs)
+  model.position.z = [0.0] * len(t_idxs)
+  model.position.t = [float(t) for t in t_idxs]
+
+  model.velocity.x = [float(v_ego)] * len(t_idxs)
+  model.velocity.y = [0.0] * len(t_idxs)
+  model.velocity.z = [0.0] * len(t_idxs)
+  model.velocity.t = [float(t) for t in t_idxs]
+
+  model.acceleration.x = [0.0] * len(t_idxs)
+  model.acceleration.y = [0.0] * len(t_idxs)
+  model.acceleration.z = [0.0] * len(t_idxs)
+  model.acceleration.t = [float(t) for t in t_idxs]
+
+  model.meta.disengagePredictions.gasPressProbs = [1.0] * 6
+  model.action.desiredAcceleration = desired_accel
+  model.action.shouldStop = False
+  return model
+
+
+def make_sm(v_ego: float, desired_accel: float, min_accel: float):
+  return {
+    "carControl": SimpleNamespace(orientationNED=[0.0, 0.0, 0.0]),
+    "carState": SimpleNamespace(
+      vEgo=v_ego,
+      vEgoCluster=v_ego,
+      aEgo=0.0,
+      vCruise=100.0,
+      standstill=False,
+      steeringAngleDeg=0.0,
+    ),
+    "controlsState": SimpleNamespace(
+      longControlState=LongCtrlState.pid,
+      forceDecel=False,
+    ),
+    "liveParameters": SimpleNamespace(angleOffsetDeg=0.0),
+    "modelV2": make_model(v_ego, desired_accel),
+    "radarState": SimpleNamespace(
+      leadOne=make_lead(status=False),
+      leadTwo=make_lead(status=False),
+    ),
+    "selfdriveState": SimpleNamespace(enabled=True, experimentalMode=True, personality=0),
+    "starpilotPlan": SimpleNamespace(
+      vCruise=v_ego + 5.0,
+      minAcceleration=min_accel,
+      maxAcceleration=2.0,
+      disableThrottle=False,
+      trackingLead=False,
+      accelerationJerk=5.0,
+      dangerJerk=5.0,
+      speedJerk=5.0,
+      dangerFactor=1.0,
+      tFollow=1.45,
+      forcingStopLength=2,
+    ),
+  }
+
+
+def make_toggles():
+  return SimpleNamespace(
+    taco_tune=False,
+    classic_model=False,
+    tinygrad_model=True,
+    model_version="v11",
+    stop_distance=6.0,
+    vEgoStopping=0.5,
+  )
+
+
+def test_experimental_mlsim_uses_vehicle_min_accel_floor():
+  v_ego = 18.0
+  desired_accel = -1.0
+  comfort_min_accel = -0.5
+
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  sm = make_sm(v_ego, desired_accel, comfort_min_accel)
+
+  vehicle_min_accel = get_vehicle_min_accel(CP, v_ego)
+  assert vehicle_min_accel < comfort_min_accel
+
+  planner.update(sm, make_toggles())
+
+  assert planner.mode == "blended"
+  assert planner.mlsim
+  assert planner.output_a_target == pytest.approx(desired_accel, abs=1e-3)
+  assert planner.output_a_target < comfort_min_accel

starpilot/controls/starpilot_planner.py

@@ -86,18 +86,6 @@ class StarPilotPlanner:
       self.starpilot_acceleration.max_accel = 0
       self.starpilot_acceleration.min_accel = 0
 
-    if controls_enabled and starpilot_toggles.conditional_experimental_mode:
-      self.starpilot_cem.update(v_ego, sm, starpilot_toggles)
-    else:
-      self.starpilot_cem.curve_detected = False
-      self.starpilot_cem.stop_sign_and_light(v_ego, sm, PLANNER_TIME - 2)
-
-    self.driving_in_curve = abs(self.lateral_acceleration) >= MINIMUM_LATERAL_ACCELERATION
-
-    self.starpilot_events.update(controls_enabled, v_cruise, sm, starpilot_toggles)
-
-    self.starpilot_following.update(controls_enabled, v_ego, sm, starpilot_toggles)
-
     gps_location = sm[self.gps_location_service]
     self.gps_position = {
       "latitude": gps_location.latitude,
@@ -115,6 +103,7 @@ class StarPilotPlanner:
       self.lane_width_right = 0
 
     self.lateral_acceleration = v_ego**2 * sm["controlsState"].curvature
+    self.driving_in_curve = abs(self.lateral_acceleration) >= MINIMUM_LATERAL_ACCELERATION
 
     self.lateral_check = v_ego >= starpilot_toggles.pause_lateral_below_speed
     self.lateral_check |= not (sm["carState"].leftBlinker or sm["carState"].rightBlinker) and starpilot_toggles.pause_lateral_below_signal
@@ -133,8 +122,18 @@ class StarPilotPlanner:
     if not sm["carState"].standstill:
       self.tracking_lead = self.update_lead_status(starpilot_toggles.stop_distance)
 
+    self.starpilot_following.update(controls_enabled, v_ego, sm, starpilot_toggles)
+
+    if controls_enabled and starpilot_toggles.conditional_experimental_mode:
+      self.starpilot_cem.update(v_ego, sm, starpilot_toggles)
+    else:
+      self.starpilot_cem.curve_detected = False
+      self.starpilot_cem.stop_sign_and_light(v_ego, sm, PLANNER_TIME - 2)
+
     self.v_cruise = self.starpilot_vcruise.update(controls_enabled, now, time_validated, v_cruise, v_ego, sm, starpilot_toggles)
 
+    self.starpilot_events.update(controls_enabled, v_cruise, sm, starpilot_toggles)
+
     if self.gps_valid and time_validated and starpilot_toggles.weather_presets:
       self.starpilot_weather.update_weather(now, starpilot_toggles)
     else: