diff --git a/selfdrive/controls/tests/test_starpilot_acceleration.py b/selfdrive/controls/tests/test_starpilot_acceleration.py index 09e58c479..15b399f9a 100644 --- a/selfdrive/controls/tests/test_starpilot_acceleration.py +++ b/selfdrive/controls/tests/test_starpilot_acceleration.py @@ -13,9 +13,12 @@ from openpilot.starpilot.controls.lib.starpilot_acceleration import ( class FakePlanner: - def __init__(self): - self.v_cruise = 0.0 + def __init__(self, *, v_cruise=0.0, slc_target=0.0, red_light=False, forcing_stop=False, disable_throttle=False): + self.v_cruise = v_cruise self.starpilot_weather = SimpleNamespace(weather_id=0, reduce_acceleration=0.0) + self.starpilot_vcruise = SimpleNamespace(slc_target=slc_target, forcing_stop=forcing_stop) + self.starpilot_cem = SimpleNamespace(stop_light_detected=red_light) + self.starpilot_following = SimpleNamespace(disable_throttle=disable_throttle) def make_toggles(**overrides): @@ -40,10 +43,8 @@ def make_lead(status=False, d_rel=150.0, v_lead=0.0, a_lead_k=0.0): return SimpleNamespace(status=status, dRel=d_rel, vLead=v_lead, aLeadK=a_lead_k) -def make_sm(*, set_speed_kph=100.0, v_target=0.0, slc_target=0.0, lead_one=None, lead_two=None, - standstill=False, force_decel=False, red_light=False, forcing_stop=False, - disable_throttle=False, eco_gear=False, sport_gear=False, force_coast=False, - traffic_mode=False): +def make_sm(*, set_speed_kph=100.0, lead_one=None, lead_two=None, standstill=False, force_decel=False, + eco_gear=False, sport_gear=False, force_coast=False, traffic_mode=False): return { "carState": SimpleNamespace(vCruise=set_speed_kph, standstill=standstill), "controlsState": SimpleNamespace(forceDecel=force_decel), @@ -57,20 +58,23 @@ def make_sm(*, set_speed_kph=100.0, v_target=0.0, slc_target=0.0, lead_one=None, forceCoast=force_coast, trafficModeEnabled=traffic_mode, ), - "starpilotPlan": SimpleNamespace( - vCruise=v_target, - slcSpeedLimit=slc_target, - redLight=red_light, - forcingStop=forcing_stop, - disableThrottle=disable_throttle, - ), } def test_slc_coast_window_prefers_coast_for_small_overspeed(): - accel = StarPilotAcceleration(FakePlanner()) target = 56.0 * CV.MPH_TO_MS - sm = make_sm(set_speed_kph=100.0, v_target=target, slc_target=target) + accel = StarPilotAcceleration(FakePlanner(v_cruise=target, slc_target=target)) + sm = make_sm(set_speed_kph=100.0) + + accel.update(57.0 * CV.MPH_TO_MS, sm, make_toggles(deceleration_profile=DECELERATION_PROFILES["ECO"])) + + assert accel.min_accel == pytest.approx(-0.02, abs=1e-3) + + +def test_slc_coast_window_does_not_require_starpilot_plan_message(): + target = 56.0 * CV.MPH_TO_MS + accel = StarPilotAcceleration(FakePlanner(v_cruise=target, slc_target=target)) + sm = make_sm(set_speed_kph=100.0) accel.update(57.0 * CV.MPH_TO_MS, sm, make_toggles(deceleration_profile=DECELERATION_PROFILES["ECO"])) @@ -89,12 +93,10 @@ def test_slc_coast_window_scales_by_profile_strength(): def test_slc_coast_window_disabled_for_relevant_lead(): - accel = StarPilotAcceleration(FakePlanner()) target = 56.0 * CV.MPH_TO_MS + accel = StarPilotAcceleration(FakePlanner(v_cruise=target, slc_target=target)) sm = make_sm( set_speed_kph=100.0, - v_target=target, - slc_target=target, lead_one=make_lead(status=True, d_rel=20.0, v_lead=45.0 * CV.MPH_TO_MS, a_lead_k=0.0), ) @@ -104,12 +106,10 @@ def test_slc_coast_window_disabled_for_relevant_lead(): def test_slc_coast_window_disabled_when_target_drop_is_not_slc(): - accel = StarPilotAcceleration(FakePlanner()) slc_target = 60.0 * CV.MPH_TO_MS + accel = StarPilotAcceleration(FakePlanner(v_cruise=55.0 * CV.MPH_TO_MS, slc_target=slc_target)) sm = make_sm( set_speed_kph=100.0, - v_target=55.0 * CV.MPH_TO_MS, - slc_target=slc_target, ) accel.update(57.0 * CV.MPH_TO_MS, sm, make_toggles(deceleration_profile=DECELERATION_PROFILES["ECO"])) diff --git a/starpilot/controls/lib/starpilot_acceleration.py b/starpilot/controls/lib/starpilot_acceleration.py index 57f57953c..240c79ed4 100644 --- a/starpilot/controls/lib/starpilot_acceleration.py +++ b/starpilot/controls/lib/starpilot_acceleration.py @@ -196,16 +196,16 @@ class StarPilotAcceleration: raw_v_cruise_kph += starpilot_toggles.set_speed_offset raw_v_cruise = raw_v_cruise_kph * CV.KPH_TO_MS - v_target = float(getattr(sm["starpilotPlan"], "vCruise", raw_v_cruise)) - slc_target = float(getattr(sm["starpilotPlan"], "slcSpeedLimit", 0.0)) + v_target = float(self.starpilot_planner.v_cruise or raw_v_cruise) + slc_target = float(getattr(self.starpilot_planner.starpilot_vcruise, "slc_target", 0.0)) slc_limited = slc_target > 0.0 and abs(v_target - slc_target) <= SLC_TARGET_EPS and v_target < raw_v_cruise - SLC_TARGET_EPS has_relevant_lead = any(lead_is_braking_relevant(lead, v_ego) for lead in (sm["radarState"].leadOne, sm["radarState"].leadTwo)) stop_context = ( sm["carState"].standstill or getattr(sm["controlsState"], "forceDecel", False) or - getattr(sm["starpilotPlan"], "redLight", False) or - getattr(sm["starpilotPlan"], "forcingStop", False) or - getattr(sm["starpilotPlan"], "disableThrottle", False) + getattr(self.starpilot_planner.starpilot_cem, "stop_light_detected", False) or + getattr(self.starpilot_planner.starpilot_vcruise, "forcing_stop", False) or + getattr(self.starpilot_planner.starpilot_following, "disable_throttle", False) ) if (getattr(starpilot_toggles, "speed_limit_controller", False) and v_ego > SLC_COAST_MIN_SPEED and