From e15a0b1f5872046ad8792fae7a7816e77d7318cf Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Tue, 28 Jul 2026 14:53:45 -0700 Subject: [PATCH] feat: accel controller --- cereal/custom.capnp | 30 + common/params_keys.h | 4 + common/tests/test_params.py | 4 + .../lib/longitudinal_mpc_lib/long_mpc.py | 9 +- .../controls/lib/longitudinal_planner.py | 18 +- .../test/longitudinal_maneuvers/plant.py | 12 +- selfdrive/ui/layouts/settings/toggles.py | 44 +- selfdrive/ui/mici/layouts/settings/toggles.py | 13 + selfdrive/ui/mici/widgets/button.py | 7 +- .../controls/lib/accel_controller/__init__.py | 0 .../lib/accel_controller/accel_controller.py | 384 +++++ .../lib/accel_controller/constants.py | 73 + .../controls/lib/accel_controller/helpers.py | 22 + .../controls/lib/accel_controller/lead.py | 147 ++ .../controls/lib/accel_controller/state.py | 139 ++ .../lib/accel_controller/tests/__init__.py | 0 .../tests/test_accel_controller.py | 882 +++++++++++ .../tests/test_accel_controller_interfaces.py | 515 +++++++ .../test_longcontrol_vehicle_interfaces.py | 83 ++ .../lib/longitudinal_mpc_lib/__init__.py | 0 .../lib/longitudinal_mpc_lib/long_mpc.py | 38 + .../controls/lib/longitudinal_planner.py | 64 +- .../test_vision_controller_closed_loop.py | 82 ++ .../test_accel_controller_closed_loop.py | 1301 +++++++++++++++++ sunnypilot/selfdrive/test/__init__.py | 0 .../test/longitudinal_maneuvers/__init__.py | 0 .../test/longitudinal_maneuvers/plant.py | 402 +++++ .../longitudinal_maneuvers/tests/__init__.py | 0 .../tests/test_plant_sp.py | 154 ++ sunnypilot/sunnylink/params_metadata.json | 22 + sunnypilot/sunnylink/settings_ui.json | 76 +- .../settings_ui_src/pages/cruise.yaml | 19 +- .../sunnylink/tests/test_settings_schema.py | 16 + 33 files changed, 4526 insertions(+), 34 deletions(-) create mode 100644 sunnypilot/selfdrive/controls/lib/accel_controller/__init__.py create mode 100644 sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py create mode 100644 sunnypilot/selfdrive/controls/lib/accel_controller/constants.py create mode 100644 sunnypilot/selfdrive/controls/lib/accel_controller/helpers.py create mode 100644 sunnypilot/selfdrive/controls/lib/accel_controller/lead.py create mode 100644 sunnypilot/selfdrive/controls/lib/accel_controller/state.py create mode 100644 sunnypilot/selfdrive/controls/lib/accel_controller/tests/__init__.py create mode 100644 sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py create mode 100644 sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py create mode 100644 sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_longcontrol_vehicle_interfaces.py create mode 100644 sunnypilot/selfdrive/controls/lib/longitudinal_mpc_lib/__init__.py create mode 100644 sunnypilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py create mode 100644 sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller_closed_loop.py create mode 100644 sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py create mode 100644 sunnypilot/selfdrive/test/__init__.py create mode 100644 sunnypilot/selfdrive/test/longitudinal_maneuvers/__init__.py create mode 100644 sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py create mode 100644 sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/__init__.py create mode 100644 sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 237ec79e64..c002540437 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -194,6 +194,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { aTarget @5 :Float32; events @6 :List(OnroadEventSP.Event); e2eAlerts @7 :E2eAlerts; + accelController @8 :AccelController; struct DynamicExperimentalControl { state @0 :DynamicExperimentalControlState; @@ -296,6 +297,35 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { greenLightAlert @0 :Bool; leadDepartAlert @1 :Bool; } + + struct AccelController { + enabled @0 :Bool; + active @1 :Bool; + shadowOnlyDEPRECATED @2 :Bool; + profile @3 :Profile; + state @4 :State; + + enum Profile { + eco @0; + normal @1; + sport @2; + } + + enum State { + inactive @0; + free @1; + restrict @2; + hold @3; + release @4; + stopHold @5; + } + } + + enum AccelerationPersonality { + eco @0; + normal @1; + sport @2; + } } struct OnroadEventSP @0xda96579883444c35 { diff --git a/common/params_keys.h b/common/params_keys.h index 84f484057a..c8cd1cf3ee 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -235,6 +235,10 @@ inline static std::unordered_map keys = { {"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}}, {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}}, + // Accel Controller profiles (Eco / Normal / Sport) + {"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"AccelPersonality", {PERSISTENT | BACKUP, INT, "1"}}, + // sunnypilot model params {"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}}, {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}}, diff --git a/common/tests/test_params.py b/common/tests/test_params.py index fcb8e5a185..e1771364a0 100644 --- a/common/tests/test_params.py +++ b/common/tests/test_params.py @@ -112,12 +112,16 @@ class TestParams: def test_params_default_value(self): self.params.remove("LanguageSetting") self.params.remove("LongitudinalPersonality") + self.params.remove("AccelPersonalityEnabled") + self.params.remove("AccelPersonality") self.params.remove("LiveParameters") assert self.params.get("LanguageSetting") is None assert self.params.get("LanguageSetting", return_default=False) is None assert isinstance(self.params.get("LanguageSetting", return_default=True), str) assert isinstance(self.params.get("LongitudinalPersonality", return_default=True), int) + assert self.params.get("AccelPersonalityEnabled", return_default=True) is False + assert self.params.get("AccelPersonality", return_default=True) == 1 assert self.params.get("LiveParameters") is None assert self.params.get("LiveParameters", return_default=True) is None diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index deae416489..f9a5342ade 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -9,6 +9,7 @@ from openpilot.common.swaglog import cloudlog # WARNING: imports outside of constants will not trigger a rebuild from openpilot.selfdrive.modeld.constants import index_function from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU +from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpcSP if __name__ == '__main__': # generating code from acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver @@ -213,8 +214,9 @@ def gen_long_ocp(): return ocp -class LongitudinalMpc: +class LongitudinalMpc(LongitudinalMpcSP): def __init__(self, dt=DT_MDL): + LongitudinalMpcSP.__init__(self) self.dt = dt self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.reset() @@ -270,7 +272,8 @@ class LongitudinalMpc: def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard): jerk_factor = get_jerk_factor(personality) a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 - cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST] + cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, + LongitudinalMpcSP.scale_jerk_cost(self, jerk_factor * J_EGO_COST)] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] self.set_cost_weights(cost_weights, constraint_cost_weights) @@ -345,6 +348,7 @@ class LongitudinalMpc: self.params[:,0] = ACCEL_MIN self.params[:,1] = ACCEL_MAX + LongitudinalMpcSP.apply_accel_limits(self) self.params[:,2] = np.min(x_obstacles, axis=1) self.params[:,3] = np.copy(self.a_prev) self.params[:,4] = t_follow @@ -364,6 +368,7 @@ class LongitudinalMpc: self.solver.constraints_set(0, "ubx", self.x0) self.solution_status = self.solver.solve() + LongitudinalMpcSP.save_solution_status(self) self.solve_time = float(self.solver.get_stats('time_tot')[0]) self.time_qp_solution = float(self.solver.get_stats('time_qp')[0]) self.time_linearization = float(self.solver.get_stats('time_lin')[0]) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index e02b02d2e0..4c7b53abf7 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -51,7 +51,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP): def __init__(self, CP, CP_SP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP self.mpc = LongitudinalMpc(dt=dt) - LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc) + LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc, dt=dt) self.fcw = False self.dt = dt self.allow_throttle = True @@ -129,16 +129,12 @@ class LongitudinalPlanner(LongitudinalPlannerSP): clipped_accel_coast = max(accel_coast, accel_clip[0]) clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast]) accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp) - # Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise) - if force_slow_decel: v_cruise = 0.0 - self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality) - self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) - self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality) + is_e2e = LongitudinalPlannerSP.update_mpc(self, sm, v_cruise, prev_accel_constraint, accel_clip[1], reset_state) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) @@ -154,13 +150,14 @@ class LongitudinalPlanner(LongitudinalPlannerSP): self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory)) self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 - action_t = self.CP.longitudinalActuatorDelay + DT_MDL - output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, - action_t=action_t, vEgoStopping=self.CP.vEgoStopping) + action_t = self.CP.longitudinalActuatorDelay + DT_MDL + output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan( + self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, action_t=action_t, vEgoStopping=self.CP.vEgoStopping, + ) output_a_target_e2e = sm['modelV2'].action.desiredAcceleration output_should_stop_e2e = sm['modelV2'].action.shouldStop - if self.is_e2e(sm): + if is_e2e: output_a_target = min(output_a_target_e2e, output_a_target_mpc) self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc if output_a_target < output_a_target_mpc: @@ -168,6 +165,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP): else: output_a_target = output_a_target_mpc self.output_should_stop = output_should_stop_mpc + self.output_should_stop = LongitudinalPlannerSP.update_should_stop(self, self.output_should_stop) for idx in range(2): accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 1431e60019..06c1c7d859 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -11,6 +11,14 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU +class PlannerSM(dict): + def __init__(self, radar_frame: int, services: dict): + super().__init__(services) + self.logMonoTime = {"radarState": radar_frame} + self.valid = {"radarState": True} + self.alive = {"radarState": True} + + class Plant: messaging_initialized = False @@ -132,7 +140,7 @@ class Plant: car_control.carControl.orientationNED = [0., float(pitch), 0.] # ******** get controlsState messages for plotting *** - sm = {'radarState': radar.radarState, + sm = PlannerSM(self.rk.frame, {'radarState': radar.radarState, 'carState': car_state.carState, 'carControl': car_control.carControl, 'controlsState': control.controlsState, @@ -141,7 +149,7 @@ class Plant: 'modelV2': model.modelV2, 'carStateSP': car_state_sp.carStateSP, 'liveMapDataSP': live_map_data_sp.liveMapDataSP, - 'gpsLocation': gps_data.gpsLocation} + 'gpsLocation': gps_data.gpsLocation}) self.planner.update(sm) self.acceleration = self.planner.output_a_target if self.planner.output_should_stop: diff --git a/selfdrive/ui/layouts/settings/toggles.py b/selfdrive/ui/layouts/settings/toggles.py index 4c83584ad5..c86be13f74 100644 --- a/selfdrive/ui/layouts/settings/toggles.py +++ b/selfdrive/ui/layouts/settings/toggles.py @@ -27,6 +27,12 @@ DESCRIPTIONS = { "In relaxed mode sunnypilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with " + "your steering wheel distance button." ), + "AccelPersonalityEnabled": tr_noop( + "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority." + ), + "AccelPersonality": tr_noop( + "Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly." + ), "IsLdwEnabled": tr_noop( "Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line " + "without a turn signal activated while driving over 31 mph (50 km/h)." @@ -106,6 +112,24 @@ class TogglesLayout(Widget): icon="speed_limit.png" ) + self._accel_personality_enabled = toggle_item( + lambda: tr("Enable Accel Controller"), + lambda: tr(DESCRIPTIONS["AccelPersonalityEnabled"]), + self._params.get_bool("AccelPersonalityEnabled"), + callback=self._set_accel_personality_enabled, + icon="speed_limit.png", + ) + + self._accel_personality_setting = multiple_button_item( + lambda: tr("Acceleration Profile"), + lambda: tr(DESCRIPTIONS["AccelPersonality"]), + buttons=[lambda: tr("Eco"), lambda: tr("Normal"), lambda: tr("Sport")], + button_width=300, + callback=self._set_accel_personality, + selected_index=self._params.get("AccelPersonality", return_default=True), + icon="speed_limit.png" + ) + self._toggles = {} self._locked_toggles = set() for param, (title, desc, icon, needs_restart) in self._toggle_defs.items(): @@ -135,9 +159,11 @@ class TogglesLayout(Widget): self._toggles[param] = toggle - # insert longitudinal personality after NDOG toggle + # insert longitudinal personality and Accel Controller settings after NDOG toggle if param == "DisengageOnAccelerator": self._toggles["LongitudinalPersonality"] = self._long_personality_setting + self._toggles["AccelPersonalityEnabled"] = self._accel_personality_enabled + self._toggles["AccelPersonality"] = self._accel_personality_setting self._update_experimental_mode_icon() self._scroller = Scroller(list(self._toggles.values()), line_separator=True, spacing=0) @@ -158,6 +184,7 @@ class TogglesLayout(Widget): def _update_toggles(self): ui_state.update_params() + accel_personality_enabled = self._params.get_bool("AccelPersonalityEnabled") e2e_description = tr( "sunnypilot defaults to driving in chill mode. Experimental mode enables alpha-level features that aren't ready for chill mode. " + @@ -176,11 +203,15 @@ class TogglesLayout(Widget): self._toggles["ExperimentalMode"].action_item.set_enabled(True) self._toggles["ExperimentalMode"].set_description(e2e_description) self._long_personality_setting.action_item.set_enabled(True) + self._accel_personality_enabled.action_item.set_enabled(True) + self._accel_personality_setting.action_item.set_enabled(accel_personality_enabled) else: # no long for now self._toggles["ExperimentalMode"].action_item.set_enabled(False) self._toggles["ExperimentalMode"].action_item.set_state(False) self._long_personality_setting.action_item.set_enabled(False) + self._accel_personality_enabled.action_item.set_enabled(False) + self._accel_personality_setting.action_item.set_enabled(False) self._params.remove("ExperimentalMode") unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control.") @@ -203,6 +234,10 @@ class TogglesLayout(Widget): # refresh toggles from params to mirror external changes for param in self._toggle_defs: self._toggles[param].action_item.set_state(self._params.get_bool(param)) + self._accel_personality_enabled.action_item.set_state(accel_personality_enabled) + self._accel_personality_setting.action_item.set_selected_button( + self._params.get("AccelPersonality", return_default=True) + ) # these toggles need restart, block while engaged for toggle_def in self._toggle_defs: @@ -247,3 +282,10 @@ class TogglesLayout(Widget): def _set_longitudinal_personality(self, button_index: int): self._params.put("LongitudinalPersonality", button_index, block=True) + + def _set_accel_personality(self, button_index: int): + self._params.put("AccelPersonality", button_index, block=True) + + def _set_accel_personality_enabled(self, state: bool): + self._params.put_bool("AccelPersonalityEnabled", state, block=True) + self._accel_personality_setting.action_item.set_enabled(state and ui_state.has_longitudinal_control) diff --git a/selfdrive/ui/mici/layouts/settings/toggles.py b/selfdrive/ui/mici/layouts/settings/toggles.py index 8635336f97..92ece4a312 100644 --- a/selfdrive/ui/mici/layouts/settings/toggles.py +++ b/selfdrive/ui/mici/layouts/settings/toggles.py @@ -14,6 +14,8 @@ class TogglesLayoutMici(NavScroller): super().__init__() self._personality_toggle = BigMultiParamToggle("driving personality", "LongitudinalPersonality", ["aggressive", "standard", "relaxed"]) + self._accel_personality_enabled = BigParamControl("enable accel controller", "AccelPersonalityEnabled") + self._accel_personality_toggle = BigMultiParamToggle("acceleration profile", "AccelPersonality", ["eco", "normal", "sport"]) self._experimental_btn = BigParamControl("experimental mode", "ExperimentalMode") is_metric_toggle = BigParamControl("use metric units", "IsMetric") ldw_toggle = BigParamControl("lane departure warnings", "IsLdwEnabled") @@ -24,6 +26,8 @@ class TogglesLayoutMici(NavScroller): self._scroller.add_widgets([ self._personality_toggle, + self._accel_personality_enabled, + self._accel_personality_toggle, self._experimental_btn, is_metric_toggle, ldw_toggle, @@ -36,6 +40,7 @@ class TogglesLayoutMici(NavScroller): # Toggle lists self._refresh_toggles = ( ("ExperimentalMode", self._experimental_btn), + ("AccelPersonalityEnabled", self._accel_personality_enabled), ("IsMetric", is_metric_toggle), ("IsLdwEnabled", ldw_toggle), ("AlwaysOnDM", always_on_dm_toggle), @@ -45,6 +50,9 @@ class TogglesLayoutMici(NavScroller): ) enable_openpilot.set_enabled(lambda: not ui_state.engaged) + self._accel_personality_toggle.set_enabled( + lambda: ui_state.has_longitudinal_control and ui_state.params.get_bool("AccelPersonalityEnabled") + ) record_front.set_enabled(False if ui_state.params.get_bool("RecordFrontLock") else (lambda: not ui_state.engaged)) record_mic.set_enabled(lambda: not ui_state.engaged) @@ -75,13 +83,18 @@ class TogglesLayoutMici(NavScroller): if ui_state.has_longitudinal_control: self._experimental_btn.set_visible(True) self._personality_toggle.set_visible(True) + self._accel_personality_enabled.set_visible(True) + self._accel_personality_toggle.set_visible(True) else: # no long for now self._experimental_btn.set_visible(False) self._experimental_btn.set_checked(False) self._personality_toggle.set_visible(False) + self._accel_personality_enabled.set_visible(False) + self._accel_personality_toggle.set_visible(False) ui_state.params.remove("ExperimentalMode") # Refresh toggles from params to mirror external changes for key, item in self._refresh_toggles: item.set_checked(ui_state.params.get_bool(key)) + self._accel_personality_toggle.refresh() diff --git a/selfdrive/ui/mici/widgets/button.py b/selfdrive/ui/mici/widgets/button.py index 1dceb79691..0302765b2d 100644 --- a/selfdrive/ui/mici/widgets/button.py +++ b/selfdrive/ui/mici/widgets/button.py @@ -382,13 +382,18 @@ class BigMultiParamToggle(BigMultiToggle): self._load_value() def _load_value(self): - self.set_value(self._options[self._params.get(self._param) or 0]) + value = self._params.get(self._param, return_default=True) + index = value if isinstance(value, int) else 0 + self.set_value(self._options[max(0, min(index, len(self._options) - 1))]) def _handle_mouse_release(self, mouse_pos: MousePos): super()._handle_mouse_release(mouse_pos) new_idx = self._options.index(self.value) self._params.put(self._param, new_idx) + def refresh(self): + self._load_value() + class BigParamControl(BigToggle): def __init__(self, text: str, param: str, toggle_callback: Callable | None = None): diff --git a/sunnypilot/selfdrive/controls/lib/accel_controller/__init__.py b/sunnypilot/selfdrive/controls/lib/accel_controller/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py new file mode 100644 index 0000000000..ca36aeb2ee --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py @@ -0,0 +1,384 @@ +import math + +import numpy as np + +from opendbc.car.interfaces import ACCEL_MAX +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource +from openpilot.sunnypilot import get_sanitize_int_param +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ( + CAP_FILTER_FRAMES, COMFORT_DECEL, DEPARTURE_MOTION_NOISE_FLOOR, LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, + LEAD_BRAKING_ACCEL_THRESHOLD, LEAD_LOSS_HOLD_TIME, LEAD_MATCH_ACCEL_SLEW, LEAD_MATCH_GAP_GAIN, LEAD_MATCH_SPEED_HEADROOM, + MATCHED_SPEED_DECEL_RATE, MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE, + MPC_DECEL_JERK_MAX_TARGET_REDUCTION, MPC_DECEL_TREND_FRAMES, SPEED_RELIEF_DEADBAND, SPEED_RESTRICT_DEADBAND, TARGET_SPEED_ARM_MARGIN, + TARGET_SPEED_RESERVE, PLANNER_BRAKING_ACCEL_THRESHOLD, RADAR_STALE_TIMEOUT, STOP_HOLD_CREEP_DISTANCE, STOP_HOLD_EGO_SPEED, + STOP_HOLD_EXIT_FRAMES, STOP_HOLD_EXIT_SPEED, STOP_HOLD_MAX_LEAD_DISTANCE, VEGO_NOISE_TOLERANCE, PARAM_READ_INTERVAL, AccelProfile, + profile_accel_max, sanitize_profile, +) +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.helpers import build_accel_ceiling, is_valid_context +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan, calculate_lead_plan, has_radar_lead, is_lead_source +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.state import AccelControllerState, TargetState + + +class AccelController: + def __init__(self, CP, dt: float = DT_MDL): + if not math.isfinite(dt) or dt <= 0.0: + raise ValueError("dt must be finite and positive") + + self.dt = dt + self.delay = float(CP.longitudinalActuatorDelay) + DT_MDL + self.lead_loss_hold_frames = max(CAP_FILTER_FRAMES, math.ceil(LEAD_LOSS_HOLD_TIME / dt)) + self.radar_stale_frames = max(1, math.ceil(RADAR_STALE_TIMEOUT / dt)) + self.params = Params() + self.available = bool(CP.openpilotLongitudinalControl) + self.enabled = False + self.profile = AccelProfile.normal + self._param_read_frames = max(1, int(round(PARAM_READ_INTERVAL / dt))) + self._param_frame = 0 + self._jerk_smoothing_blocked = False + self._required_decel_samples: list[float] = [] + self._required_decel_lead = -1 + self._required_decel_lead_track_id = -1 + self._lead_trend_warmup = False + self.target_state = TargetState() + self._held_lead_plan: LeadPlan | None = None + self.is_active = self.launching = self.departure_launching = False + self.output_v_target = 0.0 + self.mpc_accel_max: tuple[float, ...] | None = None + self.state = AccelControllerState.inactive + self.selected_lead = -1 + self.selected_lead_track_id = -1 + self.required_decel = 0.0 + + @property + def is_enabled(self) -> bool: + return self.available and self.enabled + + def update_params(self) -> None: + if self._param_frame % self._param_read_frames == 0: + self.enabled = self.params.get_bool("AccelPersonalityEnabled") + self.profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params) + self._param_frame += 1 + + @staticmethod + def _profile(profile: int) -> int: + return sanitize_profile(profile) + + @staticmethod + def get_profile_accel_max(profile: int, v_ego: float) -> float: + return profile_accel_max(profile, v_ego) + + def _update_target(self, lead_plan: LeadPlan, base_speed: float, v_ego: float, profile: int, profile_max_accel: float, + previous_should_stop: bool, previous_mpc_source, planner_speed: float, planner_accel: float) -> float: + state = self.target_state + lead_filter_ready = state.update_samples(lead_plan, self.dt) + state.active_frames += 1 + has_lead = lead_plan.selected_lead >= 0 + filtered_cap = state.filtered_cap + slot_changed = has_lead and state.selected_lead >= 0 and lead_plan.selected_lead != state.selected_lead + track_changed = (has_lead and state.selected_lead >= 0 and lead_plan.selected_lead == state.selected_lead + and lead_plan.selected_lead_track_id != state.selected_lead_track_id + and (state.selected_lead_track_id >= 0 or lead_plan.selected_lead_track_id >= 0)) + false_relief = has_lead and math.isfinite(filtered_cap) and lead_plan.cap >= filtered_cap + SPEED_RELIEF_DEADBAND + if (slot_changed or track_changed) and false_relief and state.lead_switch_guard_frames == 0 and planner_accel <= PLANNER_BRAKING_ACCEL_THRESHOLD: + state.lead_switch_guard_frames = self.lead_loss_hold_frames + elif state.lead_switch_guard_frames > 0: + state.lead_switch_guard_frames -= 1 + if slot_changed or track_changed: + state.matched_lead = False + state.matched_accel_limit = None + if has_lead: + state.selected_lead = lead_plan.selected_lead + state.selected_lead_track_id = lead_plan.selected_lead_track_id + elif state.lead_loss_frames >= self.lead_loss_hold_frames: + state.lead_switch_guard_frames = 0 + state.selected_lead = state.selected_lead_track_id = -1 + departure_separation = (lead_plan.departure_lead_separations[lead_plan.departure_lead_index] + if lead_plan.departure_lead_index >= 0 else math.inf) + stopped_lead_hold = (has_lead and lead_plan.has_nearly_stopped_lead + and (lead_plan.departure_cap < 0.50 or (state.lead_braking and departure_separation <= STOP_HOLD_MAX_LEAD_DISTANCE))) + invalid_lead = lead_plan.lead_status and not has_lead + prior_lead_context = is_lead_source(previous_mpc_source) or math.isfinite(filtered_cap) or state.lead_braking + previous_stop = previous_should_stop and prior_lead_context and (not has_lead or lead_plan.departure_lead_speed < STOP_HOLD_EXIT_SPEED) + stop_evidence = stopped_lead_hold or lead_plan.cap < 0.50 or filtered_cap < 0.50 or (previous_stop and not state.launching) or invalid_lead + departure_motion_confirmed = (state.launching and state.departure_launch and has_lead + and (state.departure.progress(lead_plan, DEPARTURE_MOTION_NOISE_FLOOR) or state.departure.recent_motion())) + if state.active_frames >= self.lead_loss_hold_frames and math.isfinite(filtered_cap) and has_lead and planner_accel <= PLANNER_BRAKING_ACCEL_THRESHOLD: + state.lead_braking = True + elif not has_lead and state.lead_loss_frames >= self.lead_loss_hold_frames: + state.lead_braking = False + + if state.target_speed is None: + e2e_handoff = previous_mpc_source == LongitudinalPlanSource.e2e + seed_from_ego = has_lead and planner_accel > PLANNER_BRAKING_ACCEL_THRESHOLD and not e2e_handoff + state.target_speed = min(base_speed, v_ego) if seed_from_ego else base_speed + state.e2e_braking_handoff = e2e_handoff and planner_accel < 0.0 + state.state = AccelControllerState.free + if v_ego < STOP_HOLD_EGO_SPEED and not stop_evidence: + state.target_speed = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM) + state.state = AccelControllerState.release + state.launching = True + state.departure_launch = False + elif state.e2e_braking_handoff and planner_accel >= 0.0: + state.e2e_braking_handoff = False + + state.target_speed = min(state.target_speed, base_speed) + if v_ego < STOP_HOLD_EGO_SPEED and stop_evidence and not departure_motion_confirmed and state.state != AccelControllerState.stopHold: + state.enter_stop_hold(lead_plan) + return state.target_speed + + if state.state == AccelControllerState.stopHold: + state.departure.backfill_references() + fast_departure = (has_lead and min(lead_plan.selected_lead_speed, lead_plan.departure_lead_speed) > STOP_HOLD_EXIT_SPEED + and lead_plan.departure_cap > STOP_HOLD_EXIT_SPEED) + raw_departure = fast_departure or not lead_plan.lead_status and state.lead_loss_frames >= self.lead_loss_hold_frames + departed = state.departure.progress(lead_plan, STOP_HOLD_CREEP_DISTANCE) or raw_departure + if fast_departure and state.departure_frames == 0: + state.departure.keep_latest_motion_sample() + state.departure_frames = state.departure_frames + 1 if departed else 0 + state.target_speed = 0.0 + fast_departure_confirmed = fast_departure and state.departure.recent_motion() + if state.departure_frames < STOP_HOLD_EXIT_FRAMES or fast_departure and not fast_departure_confirmed: + return state.target_speed + state.target_speed = base_speed + state.state = AccelControllerState.release + state.departure_frames = 0 + state.launching = True + state.departure_launch = has_lead + return state.target_speed + + if state.launching: + renewed_stop = (has_lead and not departure_motion_confirmed + and (lead_plan.cap < STOP_HOLD_EXIT_SPEED + or (lead_plan.has_nearly_stopped_lead and lead_plan.departure_cap < STOP_HOLD_EXIT_SPEED))) + guarded_departure_loss = state.departure_launch and not lead_plan.lead_status and state.lead_loss_frames < self.lead_loss_hold_frames + if invalid_lead: + state.launching = state.departure_launch = False + if v_ego < STOP_HOLD_EGO_SPEED: + state.enter_stop_hold(lead_plan) + return state.target_speed + state.state = AccelControllerState.hold + return state.target_speed + if guarded_departure_loss: + state.state = AccelControllerState.hold + return state.target_speed + if state.departure_launch and not has_lead: + state.departure_launch = False + if renewed_stop: + state.launching = state.departure_launch = False + if v_ego < STOP_HOLD_EGO_SPEED: + state.enter_stop_hold(lead_plan) + return state.target_speed + if state.launching: + if state.departure_launch: + state.target_speed = base_speed + else: + launch_target = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM) + state.target_speed = min(base_speed, max(state.target_speed, launch_target) + LAUNCH_TARGET_SLEW * self.dt) + if v_ego >= LAUNCH_END_SPEED: + state.launching = state.departure_launch = False + + comfort_decel = COMFORT_DECEL[profile] + if (has_lead and not state.launching and state.state == AccelControllerState.restrict + and lead_plan.closing_speed <= 0.0 and v_ego >= state.filtered_lead_speed - VEGO_NOISE_TOLERANCE): + state.matched_lead = True + elif not has_lead and state.lead_loss_frames >= self.lead_loss_hold_frames: + state.matched_lead = False + + lost_lead_source = is_lead_source(previous_mpc_source) and not has_lead and planner_speed < state.target_speed + if not has_lead and (state.matched_lead or lost_lead_source): + if lost_lead_source: + state.target_speed = max(planner_speed, state.target_speed - MATCHED_SPEED_DECEL_RATE * self.dt) + state.state = AccelControllerState.hold + return state.target_speed + + if state.matched_lead: + if math.isfinite(state.filtered_lead_speed): + recovery_speed = min(base_speed, state.filtered_lead_speed + min(LEAD_MATCH_SPEED_HEADROOM, LEAD_MATCH_GAP_GAIN * lead_plan.usable_gap)) + desired_accel_limit = min(profile_max_accel, max(recovery_speed - v_ego, 0.0)) + else: + desired_accel_limit = 0.0 + if state.filtered_lead_accel < LEAD_BRAKING_ACCEL_THRESHOLD: + desired_accel_limit = profile_max_accel + if state.matched_accel_limit is None: + state.matched_accel_limit = profile_max_accel + if state.lead_switch_guard_frames > 0: + desired_accel_limit = min(desired_accel_limit, state.matched_accel_limit) + state.matched_accel_limit = min(profile_max_accel, float(np.clip( + desired_accel_limit, state.matched_accel_limit - LEAD_MATCH_ACCEL_SLEW * self.dt, + state.matched_accel_limit + LEAD_MATCH_ACCEL_SLEW * self.dt, + ))) + matched_ceiling = min(base_speed, filtered_cap) + if matched_ceiling <= state.target_speed - SPEED_RESTRICT_DEADBAND: + state.target_speed = max(matched_ceiling, state.target_speed - MATCHED_SPEED_DECEL_RATE * self.dt) + state.state = AccelControllerState.restrict + elif state.lead_switch_guard_frames == 0 and matched_ceiling >= state.target_speed + SPEED_RELIEF_DEADBAND: + state.target_speed = min(matched_ceiling, state.target_speed + profile_max_accel * self.dt) + state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.release + else: + state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.hold + return state.target_speed + state.matched_accel_limit = None + + ceiling = min(base_speed, filtered_cap) + if lead_filter_ready and state.active_frames == CAP_FILTER_FRAMES // 2 + 1 and not state.launching and planner_speed < state.target_speed: + state.target_speed = max(planner_speed, state.target_speed - comfort_decel * self.dt) + + if ceiling <= state.target_speed - SPEED_RESTRICT_DEADBAND or (state.state == AccelControllerState.restrict and ceiling < state.target_speed): + state.target_speed = max(ceiling, state.target_speed - comfort_decel * self.dt) + state.state = AccelControllerState.restrict + return state.target_speed + + filter_warmup = has_lead and not math.isfinite(filtered_cap) + guarded_lead_loss = not has_lead and state.lead_loss_frames < self.lead_loss_hold_frames + if (filter_warmup or guarded_lead_loss) and state.target_speed < base_speed - SPEED_RESTRICT_DEADBAND: + state.state = AccelControllerState.hold + return state.target_speed + + confirmed_clear_road = not math.isfinite(filtered_cap) and not guarded_lead_loss + relief = not has_lead or lead_plan.closing_speed <= 0.0 + if relief and (ceiling >= state.target_speed + SPEED_RELIEF_DEADBAND or (confirmed_clear_road and ceiling > state.target_speed)): + if state.lead_switch_guard_frames == 0: + state.target_speed = ceiling + state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.release + else: + state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.hold + return state.target_speed + + def _update_freshness(self, radar_fresh: bool) -> None: + self.target_state.stale_frames = 0 if radar_fresh else self.target_state.stale_frames + 1 + if self.target_state.stale_frames >= self.radar_stale_frames: + self.target_state = TargetState() + + def reset(self) -> None: + self.target_state = TargetState() + self._held_lead_plan = None + self._jerk_smoothing_blocked = False + self._required_decel_samples.clear() + self._required_decel_lead = self._required_decel_lead_track_id = -1 + self._lead_trend_warmup = False + self.is_active = self.launching = self.departure_launching = False + self.output_v_target = 0.0 + self.mpc_accel_max = None + self.state = AccelControllerState.inactive + self.selected_lead = -1 + self.selected_lead_track_id = -1 + self.required_decel = 0.0 + + def update(self, radar_state, *, base_speed: float, v_ego: float, a_ego: float, follow_personality, acc_selected: bool, + engaged: bool, cruise_initialized: bool, stock_accel_max: float, previous_should_stop: bool, radar_fresh: bool = True, + previous_mpc_source=None, planner_speed: float | None = None, planner_accel: float = 0.0) -> None: + self.profile = self._profile(self.profile) + sanitized_v_ego = max(v_ego, 0.0) if math.isfinite(v_ego) and v_ego >= -VEGO_NOISE_TOLERANCE else v_ego + profile_max_accel = self.get_profile_accel_max(self.profile, sanitized_v_ego) + stock_accel_max = float(stock_accel_max) + positive_accel_max = (max(0.0, min(profile_max_accel, stock_accel_max, ACCEL_MAX)) + if math.isfinite(profile_max_accel) and math.isfinite(stock_accel_max) else math.nan) + planner_speed = sanitized_v_ego if planner_speed is None else planner_speed + valid_context = is_valid_context(base_speed, sanitized_v_ego, a_ego, planner_speed, planner_accel, stock_accel_max, self.delay, + engaged, cruise_initialized) + enabled_context = valid_context and self.is_enabled and bool(acc_selected) + if enabled_context and radar_fresh: + lead_plan = calculate_lead_plan(radar_state, sanitized_v_ego, a_ego, self.delay, self.profile, follow_personality) + self._held_lead_plan = lead_plan + elif enabled_context and self._held_lead_plan is not None: + lead_plan = self._held_lead_plan + else: + lead_plan = LeadPlan(lead_status=has_radar_lead(radar_state)) + self._held_lead_plan = None + + if enabled_context: + self._update_freshness(radar_fresh) + active = enabled_context and (radar_fresh or self.target_state.target_speed is not None) + if active and radar_fresh: + target_speed = self._update_target( + lead_plan, base_speed, sanitized_v_ego, self.profile, profile_max_accel, previous_should_stop, + previous_mpc_source, planner_speed, planner_accel, + ) + elif active: + target_speed = self.target_state.target_speed + else: + self.target_state = TargetState() + target_speed = base_speed + + if not radar_fresh and not active: + self._held_lead_plan = None + lead_plan = LeadPlan(lead_status=has_radar_lead(radar_state)) + + state = self.target_state + stop_hold_active = active and state.state == AccelControllerState.stopHold + matched_limit_active = active and state.matched_lead and state.matched_accel_limit is not None and not state.e2e_braking_handoff + lead_accel_request = active and lead_plan.selected_lead >= 0 and lead_plan.closing_speed <= 0.0 and planner_accel >= 0.0 + profile_limit_active = active and not stop_hold_active and (state.launching or not lead_plan.lead_status or lead_accel_request) + if matched_limit_active: + effective_accel_max = min(positive_accel_max, state.matched_accel_limit) + elif profile_limit_active: + effective_accel_max = positive_accel_max + else: + effective_accel_max = math.inf + mpc_accel_max = build_accel_ceiling(effective_accel_max, planner_accel) if matched_limit_active or profile_limit_active else None + guarded_lead_loss = not lead_plan.lead_status and state.selected_lead >= 0 and state.lead_loss_frames < self.lead_loss_hold_frames + lead_context = lead_plan.lead_status or math.isfinite(state.filtered_cap) or guarded_lead_loss + reserve_eligible = (active and lead_context and not stop_hold_active and state.lead_switch_guard_frames == 0 and not state.launching + and not state.e2e_braking_handoff) + if not lead_context: + state.speed_reserve_armed = False + elif (reserve_eligible and not state.speed_reserve_armed and math.isfinite(state.filtered_cap) + and state.filtered_cap <= target_speed + TARGET_SPEED_ARM_MARGIN): + state.speed_reserve_armed = True + + output_target = 0.0 if stop_hold_active else target_speed + if reserve_eligible and state.speed_reserve_armed: + output_target = max(0.0, output_target - TARGET_SPEED_RESERVE) + + self.is_active = active + self.launching = active and state.launching + self.departure_launching = self.launching and state.departure_launch + self.output_v_target = output_target + self.mpc_accel_max = mpc_accel_max + self.state = state.state + self.selected_lead = lead_plan.selected_lead + self.selected_lead_track_id = lead_plan.selected_lead_track_id + self.required_decel = lead_plan.required_decel + + def get_jerk_cost_multiplier(self, actuating: bool, prev_accel_constraint: bool, target_reduction: float, previous_mpc_failed: bool) -> float: + lead_restriction = (actuating and prev_accel_constraint and self.state == AccelControllerState.restrict and self.selected_lead >= 0 + and not self.launching and target_reduction > 1e-6) + same_lead = self.selected_lead == self._required_decel_lead and self.selected_lead_track_id == self._required_decel_lead_track_id + lead_changed = lead_restriction and self._required_decel_lead >= 0 and not same_lead + if lead_changed: + self._lead_trend_warmup = True + elif not lead_restriction: + self._lead_trend_warmup = False + if not lead_restriction or not same_lead or not math.isfinite(self.required_decel): + self._required_decel_samples.clear() + if lead_restriction and math.isfinite(self.required_decel): + self._required_decel_samples.append(self.required_decel) + if len(self._required_decel_samples) > MPC_DECEL_TREND_FRAMES: + self._required_decel_samples.pop(0) + self._required_decel_lead = self.selected_lead if lead_restriction else -1 + self._required_decel_lead_track_id = self.selected_lead_track_id if lead_restriction else -1 + + history = self._required_decel_samples + history_ready = len(history) == MPC_DECEL_TREND_FRAMES + tightening_lead = (history_ready + and (history[-1] - history[0]) / (self.dt * (len(history) - 1)) > MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE + and sum(after > before for before, after in zip(history[:-1], history[1:], strict=True)) >= 2) + modest_decel = (lead_restriction and target_reduction < MPC_DECEL_JERK_MAX_TARGET_REDUCTION + and 0.0 < self.required_decel < MPC_DECEL_JERK_MAX_REQUIRED_DECEL) + smoothing_eligible = modest_decel and (not self._lead_trend_warmup or history_ready) and not tightening_lead + if history_ready: + self._lead_trend_warmup = False + if previous_mpc_failed or (lead_restriction and not self._jerk_smoothing_blocked and (not modest_decel or tightening_lead)): + self._jerk_smoothing_blocked = True + elif not lead_restriction: + self._jerk_smoothing_blocked = False + return MPC_DECEL_JERK_COST_MULTIPLIER if smoothing_eligible and not self._jerk_smoothing_blocked else 1.0 + + def update_should_stop(self, should_stop: bool) -> bool: + if not self.is_active: + return should_stop + if self.departure_launching: + return False + return should_stop or self.state == AccelControllerState.stopHold diff --git a/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py b/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py new file mode 100644 index 0000000000..689fabb410 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py @@ -0,0 +1,73 @@ +import math + +import numpy as np + +from cereal import custom + + +AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile +ACCEL_PROFILES = tuple(AccelProfile.schema.enumerants.values()) + +COMFORT_DECEL = { + AccelProfile.eco: 0.25, + AccelProfile.normal: 0.30, + AccelProfile.sport: 0.35, +} + +ACCEL_PROFILE_MAX_BP = [0.0, 3.0, 10.0, 25.0, 40.0] +ACCEL_PROFILE_MAX_V = { + AccelProfile.eco: [1.65, 1.30, 0.72, 0.32, 0.16], + AccelProfile.normal: [1.80, 1.50, 0.97, 0.48, 0.30], + AccelProfile.sport: [2.00, 1.90, 1.15, 0.68, 0.42], +} + +CAP_FILTER_FRAMES = 5 +LEAD_LOSS_HOLD_TIME = 0.50 +SPEED_RESTRICT_DEADBAND = 0.15 +SPEED_RELIEF_DEADBAND = 0.35 +TARGET_SPEED_ARM_MARGIN = 1.0 +TARGET_SPEED_RESERVE = 0.10 +LAUNCH_TARGET_HEADROOM = 3.0 +LAUNCH_TARGET_SLEW = 8.75 +LAUNCH_END_SPEED = 3.0 +ACCEL_LIMIT_HORIZON_JERK = 1.0 +LEAD_MATCH_GAP_GAIN = 0.04 +LEAD_MATCH_SPEED_HEADROOM = 1.25 +LEAD_MATCH_ACCEL_SLEW = 0.25 +MATCHED_SPEED_DECEL_RATE = 0.50 +PLANNER_BRAKING_ACCEL_THRESHOLD = -0.11 +LEAD_BRAKING_ACCEL_THRESHOLD = -0.11 +MPC_DECEL_JERK_COST_MULTIPLIER = 1.05 +MPC_DECEL_JERK_MAX_REQUIRED_DECEL = 0.80 +MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE = 0.35 +MPC_DECEL_JERK_MAX_TARGET_REDUCTION = 9.0 +MPC_DECEL_TREND_FRAMES = 4 + +STOP_HOLD_EGO_SPEED = 0.30 +STOPPED_LEAD_SPEED = 0.30 +STOP_HOLD_EXIT_SPEED = 0.80 +STOP_HOLD_EXIT_FRAMES = 4 +STOP_HOLD_CREEP_SPEED = 0.15 +STOP_HOLD_CREEP_DISTANCE = 0.30 +DEPARTURE_MOTION_NOISE_FLOOR = 0.03 +DEPARTURE_MOTION_STEP_MIN = 0.005 +STOP_HOLD_MAX_LEAD_DISTANCE = 30.0 +STOP_GAP_RESERVE = 0.75 +STOP_GAP_RESERVE_LEAD_SPEED = 2.0 +STOP_GAP_RESERVE_DECEL_BP = (0.30, 0.80) + +RADAR_STALE_TIMEOUT = 0.50 +MAX_LEAD_ACCEL_TAU = 10.0 +MIN_LEAD_SPEED = -1.0 +VEGO_NOISE_TOLERANCE = 0.10 +PARAM_READ_INTERVAL = 0.25 + + +def sanitize_profile(profile: int) -> int: + return profile if profile in ACCEL_PROFILES else AccelProfile.normal + + +def profile_accel_max(profile: int, v_ego: float) -> float: + if not math.isfinite(v_ego): + return math.nan + return float(np.interp(max(v_ego, 0.0), ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[sanitize_profile(profile)])) diff --git a/sunnypilot/selfdrive/controls/lib/accel_controller/helpers.py b/sunnypilot/selfdrive/controls/lib/accel_controller/helpers.py new file mode 100644 index 0000000000..8339bf1826 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/accel_controller/helpers.py @@ -0,0 +1,22 @@ +import math + +import numpy as np + +from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ACCEL_LIMIT_HORIZON_JERK, VEGO_NOISE_TOLERANCE + + +def is_valid_context(base_speed: float, v_ego: float, a_ego: float, planner_speed: float, planner_accel: float, stock_accel_max: float, + delay: float, engaged: bool, cruise_initialized: bool) -> bool: + values = (base_speed, v_ego, a_ego, planner_speed, planner_accel, stock_accel_max, delay) + return (engaged and cruise_initialized and base_speed >= 0.0 and v_ego >= -VEGO_NOISE_TOLERANCE + and planner_speed >= 0.0 and stock_accel_max >= 0.0 and delay >= 0.0 and all(math.isfinite(value) for value in values)) + + +def build_accel_ceiling(limit: float, planner_accel: float) -> tuple[float, ...] | None: + if limit >= ACCEL_MAX - 1e-9: + return None + a0 = float(np.clip(planner_accel, ACCEL_MIN, ACCEL_MAX)) + ceiling = np.clip(np.maximum(limit, a0 - ACCEL_LIMIT_HORIZON_JERK * T_IDXS), 0.0, ACCEL_MAX) + return tuple(float(value) for value in ceiling) diff --git a/sunnypilot/selfdrive/controls/lib/accel_controller/lead.py b/sunnypilot/selfdrive/controls/lib/accel_controller/lead.py new file mode 100644 index 0000000000..0ae3f44867 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/accel_controller/lead.py @@ -0,0 +1,147 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +import math +from typing import NamedTuple + +import numpy as np + +from cereal import log +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import ( + LongitudinalMpc, LongitudinalPlanSource, STOP_DISTANCE, T_IDXS, get_T_FOLLOW, get_stopped_equivalence_factor, +) +from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ( + COMFORT_DECEL, MAX_LEAD_ACCEL_TAU, MIN_LEAD_SPEED, STOP_GAP_RESERVE, STOP_GAP_RESERVE_DECEL_BP, + STOP_GAP_RESERVE_LEAD_SPEED, STOPPED_LEAD_SPEED, sanitize_profile, +) + + +class LeadPlan(NamedTuple): + cap: float = math.inf + selected_lead: int = -1 + selected_lead_track_id: int = -1 + selected_lead_speed: float = math.inf + selected_lead_accel: float = 0.0 + departure_lead_index: int = -1 + departure_lead_speed: float = math.inf + departure_cap: float = math.inf + departure_lead_speeds: tuple[float, float] = (math.inf, math.inf) + departure_lead_distances: tuple[float, float] = (-math.inf, -math.inf) + departure_lead_track_ids: tuple[int, int] = (-1, -1) + departure_lead_separations: tuple[float, float] = (-math.inf, -math.inf) + usable_gap: float = math.inf + closing_speed: float = 0.0 + required_decel: float = 0.0 + has_nearly_stopped_lead: bool = False + lead_status: bool = False + + +def is_lead_source(source) -> bool: + return source in (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1) + + +def has_radar_lead(radar_state) -> bool: + return bool(radar_state.leadOne.status or radar_state.leadTwo.status) + + +def _project_ego(v_ego: float, a_ego: float, delay: float) -> tuple[float, float]: + if a_ego < 0.0: + stop_time = -v_ego / a_ego if v_ego > 0.0 else 0.0 + if stop_time <= delay: + distance = -v_ego**2 / (2.0 * a_ego) if v_ego > 0.0 else 0.0 + return distance, 0.0 + return max(v_ego * delay + 0.5 * a_ego * delay**2, 0.0), max(v_ego + a_ego * delay, 0.0) + + +def _lead_values(lead) -> tuple[float, float, float, float] | None: + if not lead.status: + return None + d_rel, v_lead = float(lead.dRel), float(lead.vLeadK) + if not math.isfinite(d_rel) or d_rel < 0.0 or not math.isfinite(v_lead) or v_lead < MIN_LEAD_SPEED: + return None + + a_lead = float(lead.aLeadK) + if not math.isfinite(a_lead): + a_lead = 0.0 + a_lead_tau = float(lead.aLeadTau) + if not math.isfinite(a_lead_tau) or not 0.0 < a_lead_tau <= MAX_LEAD_ACCEL_TAU: + a_lead_tau = _LEAD_ACCEL_TAU + return d_rel, max(v_lead, 0.0), float(np.clip(a_lead, -10.0, 5.0)), a_lead_tau + + +def calculate_lead_plan(radar_state, v_ego: float, a_ego: float, delay: float, profile: int, + follow_personality=log.LongitudinalPersonality.standard) -> LeadPlan: + if not all(math.isfinite(value) for value in (v_ego, a_ego, delay)) or v_ego < 0.0 or delay < 0.0: + return LeadPlan() + + leads = (radar_state.leadOne, radar_state.leadTwo) + lead_status = any(lead.status for lead in leads) + t_follow = get_T_FOLLOW(follow_personality) + if not math.isfinite(t_follow) or t_follow < 0.0: + return LeadPlan(lead_status=lead_status) + + profile = sanitize_profile(profile) + x_ego, v_ego_delay = _project_ego(v_ego, a_ego, delay) + comfort_decel = COMFORT_DECEL[profile] + candidates: list[LeadPlan] = [] + departure_candidates: list[tuple[float, int]] = [] + departure_speeds = [math.inf, math.inf] + departure_distances = [-math.inf, -math.inf] + departure_track_ids = [-1, -1] + departure_separations = [-math.inf, -math.inf] + departure_caps = [math.inf, math.inf] + + for lead_index, lead in enumerate(leads): + values = _lead_values(lead) + if values is None: + continue + + d_rel, v_lead, a_lead, a_lead_tau = values + lead_xv = LongitudinalMpc.extrapolate_lead(d_rel, v_lead, a_lead, a_lead_tau) + x_lead = float(np.interp(delay, T_IDXS, lead_xv[:, 0])) + v_lead_delay = float(np.interp(delay, T_IDXS, lead_xv[:, 1])) + safety_gap = max(x_lead - x_ego - STOP_DISTANCE - t_follow * v_lead_delay, 0.0) + closing_speed = max(v_ego_delay - v_lead_delay, 0.0) + required_decel = 0.0 if closing_speed == 0.0 else math.inf if safety_gap == 0.0 else closing_speed**2 / (2.0 * safety_gap) + reserve = float(np.interp(v_lead_delay, (0.0, STOP_GAP_RESERVE_LEAD_SPEED), (STOP_GAP_RESERVE, 0.0))) + reserve_scale = float(np.interp(required_decel, STOP_GAP_RESERVE_DECEL_BP, (1.0, 0.0))) + usable_gap = max(safety_gap - reserve * reserve_scale, 0.0) + cap = v_lead_delay + math.sqrt(2.0 * comfort_decel * usable_gap) + departure_cap = v_lead_delay + math.sqrt(2.0 * comfort_decel * safety_gap) + separation = x_lead - x_ego + departure_distance = x_lead + float(get_stopped_equivalence_factor(v_lead_delay)) + + finite_values = (x_lead, v_lead_delay, safety_gap, usable_gap, closing_speed, cap, departure_cap, departure_distance) + if (not all(math.isfinite(value) and value >= 0.0 for value in finite_values) or math.isnan(required_decel) + or required_decel < 0.0 or not math.isfinite(separation)): + continue + + track_id = max(int(lead.radarTrackId), -1) if math.isfinite(lead.radarTrackId) else -1 + candidates.append(LeadPlan( + cap=cap, selected_lead=lead_index, selected_lead_track_id=track_id, selected_lead_speed=v_lead_delay, selected_lead_accel=a_lead, + usable_gap=usable_gap, closing_speed=closing_speed, required_decel=required_decel, lead_status=lead_status, + )) + departure_candidates.append((departure_distance, lead_index)) + departure_speeds[lead_index] = v_lead_delay + departure_distances[lead_index] = d_rel + departure_track_ids[lead_index] = track_id + departure_separations[lead_index] = separation + departure_caps[lead_index] = departure_cap + + if not candidates: + return LeadPlan(lead_status=lead_status) + + selected = min(candidates, key=lambda candidate: candidate.cap) + departure_lead_index = min(departure_candidates, key=lambda candidate: candidate[0])[1] + departure_lead_speed = departure_speeds[departure_lead_index] + return selected._replace( + departure_lead_index=departure_lead_index, departure_lead_speed=departure_lead_speed, + departure_cap=departure_caps[departure_lead_index], departure_lead_speeds=tuple(departure_speeds), + departure_lead_distances=tuple(departure_distances), departure_lead_track_ids=tuple(departure_track_ids), + departure_lead_separations=tuple(departure_separations), has_nearly_stopped_lead=departure_lead_speed < STOPPED_LEAD_SPEED, + ) diff --git a/sunnypilot/selfdrive/controls/lib/accel_controller/state.py b/sunnypilot/selfdrive/controls/lib/accel_controller/state.py new file mode 100644 index 0000000000..92d3ec8736 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/accel_controller/state.py @@ -0,0 +1,139 @@ +import math +from statistics import median + +import numpy as np + +from cereal import custom +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ( + CAP_FILTER_FRAMES, DEPARTURE_MOTION_NOISE_FLOOR, DEPARTURE_MOTION_STEP_MIN, STOP_HOLD_CREEP_DISTANCE, STOP_HOLD_CREEP_SPEED, STOP_HOLD_EXIT_FRAMES, +) +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan + + +AccelControllerState = custom.LongitudinalPlanSP.AccelController.State + + +class DepartureTracker: + def __init__(self) -> None: + self.samples: list[list[float]] = [[], []] + self.motion_samples: list[float] = [] + self.references: list[float | None] = [None, None] + self.track_ids = [-1, -1] + + def separation(self, lead_index: int) -> float: + samples = self.samples[lead_index] + return float(median(samples)) if samples else -math.inf + + def update(self, lead_plan: LeadPlan, dt: float) -> None: + for lead_index, distance in enumerate(lead_plan.departure_lead_distances): + if not math.isfinite(distance): + continue + samples = self.samples[lead_index] + track_id = lead_plan.departure_lead_track_ids[lead_index] + identity_changed = bool(samples) and track_id != self.track_ids[lead_index] and (track_id >= 0 or self.track_ids[lead_index] >= 0) + max_distance_step = max(STOP_HOLD_CREEP_DISTANCE / 2.0, 3.0 * lead_plan.departure_lead_speeds[lead_index] * dt) + geometry_jump = bool(samples) and abs(distance - samples[-1]) > max_distance_step + if identity_changed or geometry_jump: + samples.clear() + self.references[lead_index] = distance + samples.append(distance) + if len(samples) > CAP_FILTER_FRAMES: + samples.pop(0) + self.track_ids[lead_index] = track_id + lead_index = lead_plan.departure_lead_index + if lead_index >= 0: + distance = lead_plan.departure_lead_distances[lead_index] + samples = self.motion_samples + max_distance_step = max(STOP_HOLD_CREEP_DISTANCE / 2.0, 3.0 * lead_plan.departure_lead_speed * dt) + if samples and abs(distance - samples[-1]) > max_distance_step: + samples.clear() + samples.append(distance) + if len(samples) > CAP_FILTER_FRAMES: + samples.pop(0) + + def seed(self, lead_plan: LeadPlan) -> None: + self.samples = [[], []] + self.motion_samples = [] + self.references = [None, None] + self.track_ids = list(lead_plan.departure_lead_track_ids) + for lead_index, distance in enumerate(lead_plan.departure_lead_distances): + if math.isfinite(distance): + self.samples[lead_index].append(distance) + self.references[lead_index] = distance + if lead_plan.departure_lead_index >= 0: + self.motion_samples.append(lead_plan.departure_lead_distances[lead_plan.departure_lead_index]) + + def progress(self, lead_plan: LeadPlan, minimum_distance: float) -> bool: + lead_index = lead_plan.departure_lead_index + if lead_index < 0 or lead_plan.departure_lead_speed <= STOP_HOLD_CREEP_SPEED: + return False + reference = self.references[lead_index] + distance = self.separation(lead_index) + return reference is not None and distance - reference >= minimum_distance + + def recent_motion(self) -> bool: + samples = self.motion_samples[-STOP_HOLD_EXIT_FRAMES:] + if len(samples) < STOP_HOLD_EXIT_FRAMES: + return False + deltas = np.diff(samples) + return bool(samples[-1] - samples[0] >= DEPARTURE_MOTION_NOISE_FLOOR and np.count_nonzero(deltas > DEPARTURE_MOTION_STEP_MIN) >= 2) + + def backfill_references(self) -> None: + for lead_index in range(len(self.references)): + separation = self.separation(lead_index) + if math.isfinite(separation) and self.references[lead_index] is None: + self.references[lead_index] = separation + + def keep_latest_motion_sample(self) -> None: + if self.motion_samples: + self.motion_samples = self.motion_samples[-1:] + + +class TargetState: + def __init__(self) -> None: + self.cap_samples = [math.inf] * CAP_FILTER_FRAMES + self.lead_speed_samples = [math.inf] * CAP_FILTER_FRAMES + self.lead_accel_samples = [0.0] * CAP_FILTER_FRAMES + self.departure = DepartureTracker() + self.target_speed: float | None = None + self.state = AccelControllerState.inactive + self.departure_frames = self.active_frames = self.lead_loss_frames = 0 + self.lead_switch_guard_frames = self.stale_frames = 0 + self.selected_lead = self.selected_lead_track_id = -1 + self.launching = self.departure_launch = self.matched_lead = False + self.lead_braking = self.e2e_braking_handoff = self.speed_reserve_armed = False + self.matched_accel_limit: float | None = None + + @property + def filtered_cap(self) -> float: + return sorted(self.cap_samples)[CAP_FILTER_FRAMES // 2] + + @property + def filtered_lead_speed(self) -> float: + return sorted(self.lead_speed_samples)[CAP_FILTER_FRAMES // 2] + + @property + def filtered_lead_accel(self) -> float: + return sorted(self.lead_accel_samples)[CAP_FILTER_FRAMES // 2] + + def update_samples(self, lead_plan: LeadPlan, dt: float) -> bool: + had_filtered_lead = math.isfinite(self.filtered_cap) + has_lead = lead_plan.selected_lead >= 0 + self.cap_samples.append(lead_plan.cap if has_lead else math.inf) + self.lead_speed_samples.append(lead_plan.selected_lead_speed if has_lead else math.inf) + self.lead_accel_samples.append(lead_plan.selected_lead_accel if has_lead else 0.0) + self.cap_samples.pop(0) + self.lead_speed_samples.pop(0) + self.lead_accel_samples.pop(0) + self.lead_loss_frames = 0 if has_lead else self.lead_loss_frames + 1 + self.departure.update(lead_plan, dt) + return not had_filtered_lead and math.isfinite(self.filtered_cap) + + def enter_stop_hold(self, lead_plan: LeadPlan) -> None: + self.departure.seed(lead_plan) + self.target_speed = 0.0 + self.state = AccelControllerState.stopHold + self.departure_frames = 0 + self.launching = self.departure_launch = False + self.matched_lead = self.speed_reserve_armed = False + self.matched_accel_limit = None diff --git a/sunnypilot/selfdrive/controls/lib/accel_controller/tests/__init__.py b/sunnypilot/selfdrive/controls/lib/accel_controller/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py new file mode 100644 index 0000000000..c4b166b5e3 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py @@ -0,0 +1,882 @@ +import math +from types import SimpleNamespace + +import numpy as np +import pytest + +from cereal import log +from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import ( + STOP_DISTANCE, T_IDXS, LongitudinalMpc, LongitudinalPlanSource, get_T_FOLLOW, +) +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ( + ACCEL_LIMIT_HORIZON_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, ACCEL_PROFILES, CAP_FILTER_FRAMES, LAUNCH_END_SPEED, + COMFORT_DECEL, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_MATCH_ACCEL_SLEW, MATCHED_SPEED_DECEL_RATE, + MPC_DECEL_JERK_COST_MULTIPLIER, TARGET_SPEED_RESERVE, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_HOLD_EXIT_FRAMES, AccelProfile, +) +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.helpers import build_accel_ceiling +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import _project_ego, calculate_lead_plan + + +def make_lead(*, status=False, d_rel=0.0, v_lead_k=0.0, a_lead_k=0.0, a_lead_tau=1.5, radar_track_id=-1): + return SimpleNamespace(status=status, dRel=d_rel, vLeadK=v_lead_k, aLeadK=a_lead_k, aLeadTau=a_lead_tau, + radarTrackId=radar_track_id) + + +def make_radar(lead_one=None, lead_two=None): + return SimpleNamespace(leadOne=lead_one or make_lead(), leadTwo=lead_two or make_lead()) + + +def make_controller(delay=0.10): + return AccelController(SimpleNamespace(longitudinalActuatorDelay=delay, openpilotLongitudinalControl=True)) + + +def get_lead_plan(controller, radar_state, v_ego: float, a_ego: float, profile: int): + return calculate_lead_plan(radar_state, v_ego, a_ego, controller.delay, profile) + + +def update(controller, radar_state=None, **overrides): + args = { + "base_speed": 25.0, + "v_ego": 10.0, + "a_ego": 0.0, + "profile": AccelProfile.normal, + "follow_personality": log.LongitudinalPersonality.standard, + "enabled": True, + "acc_selected": True, + "engaged": True, + "cruise_initialized": True, + "stock_accel_max": ACCEL_MAX, + "previous_should_stop": False, + } + args.update(overrides) + controller.profile = args.pop("profile") + controller.enabled = args.pop("enabled") + controller.update(radar_state or make_radar(), **args) + return SimpleNamespace( + target_speed=controller.output_v_target, active=controller.is_active, launching=controller.launching, + departure_launching=controller.departure_launching, mpc_accel_max=controller.mpc_accel_max, state=controller.state, + selected_lead=controller.selected_lead, required_decel=controller.required_decel, + ) + + +def effective_accel_max(result): + return math.inf if result.mpc_accel_max is None else min(result.mpc_accel_max) + + +def restrictive_radar(): + return make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, a_lead_k=-0.5)) + + +def enter_stop_hold(controller, *, base_speed=8.0, v_ego=0.1): + stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) + return update(controller, stopped, base_speed=base_speed, v_ego=v_ego, previous_should_stop=True) + + +class TestProfiles: + def test_lookup_table_is_explicit_and_tunable(self): + assert ACCEL_PROFILE_MAX_BP == [0.0, 3.0, 10.0, 25.0, 40.0] + assert ACCEL_PROFILE_MAX_V == { + AccelProfile.eco: [1.65, 1.30, 0.72, 0.32, 0.16], + AccelProfile.normal: [1.80, 1.50, 0.97, 0.48, 0.30], + AccelProfile.sport: [2.00, 1.90, 1.15, 0.68, 0.42], + } + + @pytest.mark.parametrize("profile", ACCEL_PROFILES) + def test_lookup_interpolates_and_stays_inside_global_limit(self, profile): + for speed, expected in zip(ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[profile], strict=True): + assert AccelController.get_profile_accel_max(profile, speed) == expected + + limits = [AccelController.get_profile_accel_max(profile, speed) for speed in np.linspace(-1.0, 50.0, 201)] + assert all(0.0 <= limit <= ACCEL_MAX for limit in limits) + assert np.all(np.diff(limits) <= 0.0) + + @pytest.mark.parametrize("speed", ACCEL_PROFILE_MAX_BP) + def test_profile_order_is_distinct(self, speed): + eco, normal, sport = [AccelController.get_profile_accel_max(profile, speed) for profile in ACCEL_PROFILES] + assert eco < normal < sport + + def test_invalid_profile_defaults_to_normal(self): + assert AccelController._profile(999) == AccelProfile.normal + + def test_stock_limit_intersects_profile_before_mpc(self): + controller = make_controller() + results = [update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=0.30) + for _ in range(controller.lead_loss_hold_frames)] + result = results[-1] + assert AccelController.get_profile_accel_max(AccelProfile.sport, 10.0) == pytest.approx(1.15) + assert effective_accel_max(result) == pytest.approx(0.30) + assert all(sample.mpc_accel_max is not None for sample in results) + assert all(max(sample.mpc_accel_max) <= 0.30 + 1e-9 for sample in results) + + def test_runtime_profile_switch_applies_the_lookup_value_directly(self): + controller = make_controller() + sport = [update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=1.20) + for _ in range(controller.lead_loss_hold_frames)][-1] + eco = update(controller, v_ego=10.0, profile=AccelProfile.eco, stock_accel_max=1.20) + + assert effective_accel_max(sport) == pytest.approx(1.15) + assert effective_accel_max(eco) == pytest.approx(0.72) + + def test_matched_lead_waits_until_ego_catches_the_lead(self): + radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0)) + slow_controller, caught_controller = make_controller(), make_controller() + for controller in (slow_controller, caught_controller): + for _ in range(CAP_FILTER_FRAMES + 10): + update(controller, radar, v_ego=10.0, planner_accel=-0.2) + + update(slow_controller, radar, v_ego=3.0, planner_accel=-0.2) + update(caught_controller, radar, v_ego=8.0, planner_accel=-0.2) + + assert not slow_controller.target_state.matched_lead + assert caught_controller.target_state.matched_lead + + def test_stock_limit_reduction_applies_immediately(self): + controller = make_controller() + for _ in range(controller.lead_loss_hold_frames): + update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=1.20) + + reduced = update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=0.30) + assert effective_accel_max(reduced) == pytest.approx(0.30) + assert reduced.mpc_accel_max is not None + assert max(reduced.mpc_accel_max) <= 0.30 + 1e-9 + + def test_one_frame_stock_zero_does_not_poison_profile_recovery(self): + clean_controller, glitch_controller = make_controller(), make_controller() + for _ in range(clean_controller.lead_loss_hold_frames + 10): + clean = update(clean_controller, v_ego=10.0, stock_accel_max=1.5) + recovered = update(glitch_controller, v_ego=10.0, stock_accel_max=1.5) + + limited = update(glitch_controller, v_ego=10.0, stock_accel_max=0.0) + clean = update(clean_controller, v_ego=10.0, stock_accel_max=1.5) + recovered = update(glitch_controller, v_ego=10.0, stock_accel_max=1.5) + + assert effective_accel_max(limited) == 0.0 + assert effective_accel_max(recovered) == pytest.approx(effective_accel_max(clean)) + + @pytest.mark.parametrize("radar_fresh", (True, False), ids=("dropout", "stale")) + def test_matched_lead_ceiling_obeys_current_stock_limit(self, radar_fresh): + controller = make_controller() + radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0)) + for _ in range(CAP_FILTER_FRAMES + 10): + update(controller, radar, v_ego=10.0, planner_accel=-0.2) + for _ in range(20): + update(controller, radar, v_ego=8.0, planner_accel=-0.2) + assert controller.target_state.matched_lead + + limited = update(controller, stock_accel_max=0.0, radar_fresh=radar_fresh) + assert effective_accel_max(limited) == 0.0 + assert limited.mpc_accel_max is not None + assert max(limited.mpc_accel_max) == 0.0 + + def test_exact_global_max_uses_stock_ceiling(self): + result = update(make_controller(), base_speed=8.0, v_ego=0.0, profile=AccelProfile.sport) + assert AccelController.get_profile_accel_max(AccelProfile.sport, 0.0) == ACCEL_MAX + assert result.mpc_accel_max is None + + +class TestMpcCeiling: + @pytest.mark.parametrize("planner_accel", (-1.0, 0.0, 1.2, ACCEL_MAX)) + def test_ceiling_is_finite_feasible_and_jerk_bounded(self, planner_accel): + limit = 0.50 + ceiling = np.asarray(build_accel_ceiling(limit, planner_accel)) + a0 = float(np.clip(planner_accel, ACCEL_MIN, ACCEL_MAX)) + + assert ceiling.shape == T_IDXS.shape + assert np.all(np.isfinite(ceiling)) + assert np.all((0.0 <= ceiling) & (ceiling <= ACCEL_MAX)) + assert ceiling[0] + 1e-9 >= a0 + assert np.all(ceiling + 1e-9 >= limit) + assert np.all(np.diff(ceiling) <= 1e-9) + assert np.all(-np.diff(ceiling) <= ACCEL_LIMIT_HORIZON_JERK * np.diff(T_IDXS) + 1e-9) + + def test_zero_limit_remains_feasible_for_positive_x0(self): + ceiling = np.asarray(build_accel_ceiling(0.0, 0.8)) + assert ceiling[0] == pytest.approx(0.8) + assert ceiling[-1] == pytest.approx(0.0) + assert np.all(ceiling >= 0.0) + + def test_inactive_controller_has_no_custom_ceiling(self): + controller = make_controller() + result = update(controller, enabled=False) + assert not result.active + assert result.mpc_accel_max is None + assert math.isinf(effective_accel_max(result)) + assert controller.target_state.target_speed is None + + def test_profile_ceiling_does_not_interfere_while_planner_is_braking(self): + controller = make_controller() + radar = restrictive_radar() + warmup = [update(controller, radar, planner_accel=-0.2) for _ in range(controller.lead_loss_hold_frames)] + + assert all(sample.mpc_accel_max is None for sample in warmup) + assert controller.target_state.lead_braking + + bypassed = update(controller, radar, planner_accel=-0.2, acc_selected=False) + assert not bypassed.active and bypassed.mpc_accel_max is None + assert not controller.target_state.lead_braking + + def test_profile_ceiling_stays_continuous_while_a_lead_begins_pulling_away(self): + controller = make_controller() + for _ in range(CAP_FILTER_FRAMES + 5): + update(controller, restrictive_radar(), v_ego=10.0, planner_accel=-0.2) + + pulling_away = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=12.0)) + result = update(controller, pulling_away, v_ego=10.0, planner_accel=0.2) + + assert result.state == AccelControllerState.restrict + assert effective_accel_max(result) == pytest.approx(AccelController.get_profile_accel_max(AccelProfile.normal, 10.0)) + assert result.mpc_accel_max is not None + + def test_matched_lead_terminal_taper_changes_smoothly(self): + controller = make_controller() + radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0)) + for _ in range(CAP_FILTER_FRAMES + 5): + update(controller, radar, v_ego=10.0, planner_accel=-0.2) + + braking = update(controller, radar, v_ego=8.0, planner_accel=-0.2) + braking_limit = controller.target_state.matched_accel_limit + accelerating = update(controller, radar, v_ego=8.0, planner_accel=0.2) + + assert controller.target_state.matched_lead + assert braking.mpc_accel_max is not None and accelerating.mpc_accel_max is not None + assert braking_limit is not None + assert abs(controller.target_state.matched_accel_limit - braking_limit) <= LEAD_MATCH_ACCEL_SLEW * DT_MDL + 1e-9 + profile_accel_max = AccelController.get_profile_accel_max(AccelProfile.normal, 8.0) + assert effective_accel_max(braking) <= profile_accel_max + assert effective_accel_max(accelerating) <= profile_accel_max + + def test_matched_lead_ignores_two_frame_speed_jump(self): + clean_controller, noisy_controller = make_controller(), make_controller() + radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0)) + for controller in (clean_controller, noisy_controller): + for _ in range(CAP_FILTER_FRAMES + 10): + update(controller, radar, v_ego=10.0, planner_accel=-0.2) + for _ in range(20): + update(controller, radar, v_ego=8.0, planner_accel=-0.2) + + speed_jump = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=16.0)) + for _ in range(2): + clean = update(clean_controller, radar, v_ego=8.0) + noisy = update(noisy_controller, speed_jump, v_ego=8.0) + assert effective_accel_max(noisy) == pytest.approx(effective_accel_max(clean)) + assert noisy.target_speed == pytest.approx(clean.target_speed) + + def test_matched_lead_ignores_two_frame_acceleration_jump(self): + clean_controller, noisy_controller = make_controller(), make_controller() + steady = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0)) + for controller in (clean_controller, noisy_controller): + for _ in range(CAP_FILTER_FRAMES + 10): + update(controller, steady, v_ego=10.0, planner_accel=-0.2) + for _ in range(20): + update(controller, steady, v_ego=8.0, planner_accel=-0.2) + + braking_jump = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, a_lead_k=-1.0)) + for _ in range(2): + clean = update(clean_controller, steady, v_ego=8.0) + noisy = update(noisy_controller, braking_jump, v_ego=8.0) + assert effective_accel_max(noisy) == pytest.approx(effective_accel_max(clean)) + assert noisy.target_speed == pytest.approx(clean.target_speed) + + +class TestLead: + def test_cap_matches_stopping_energy_formula(self): + controller = make_controller() + lead = make_lead(status=True, d_rel=50.0, v_lead_k=8.0) + result = get_lead_plan(controller, make_radar(lead), 10.0, 0.0, AccelProfile.normal) + delay = controller.delay + lead_xv = LongitudinalMpc.extrapolate_lead(lead.dRel, lead.vLeadK, lead.aLeadK, lead.aLeadTau) + x_lead = float(np.interp(delay, T_IDXS, lead_xv[:, 0])) + v_lead = float(np.interp(delay, T_IDXS, lead_xv[:, 1])) + x_ego, _ = _project_ego(10.0, 0.0, delay) + safety_gap = max(x_lead - x_ego - STOP_DISTANCE - get_T_FOLLOW(log.LongitudinalPersonality.standard) * v_lead, 0.0) + expected = v_lead + math.sqrt(2.0 * COMFORT_DECEL[AccelProfile.normal] * safety_gap) + + assert result.cap == pytest.approx(expected) + assert result.cap != pytest.approx(math.sqrt(v_lead**2 + 2.0 * COMFORT_DECEL[AccelProfile.normal] * safety_gap)) + + def test_profile_order_controls_approach_timing(self): + radar = make_radar(make_lead(status=True, d_rel=50.0, v_lead_k=8.0)) + caps = [get_lead_plan(make_controller(), radar, 10.0, 0.0, profile).cap for profile in ACCEL_PROFILES] + assert caps[0] < caps[1] < caps[2] + + def test_stopped_lead_reserve_only_reduces_comfort_gap(self): + lead = get_lead_plan(make_controller(), + make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=0.0)), 5.0, 0.0, AccelProfile.normal, + ) + comfort_decel = COMFORT_DECEL[AccelProfile.normal] + safety_gap = (lead.departure_cap - lead.departure_lead_speed) ** 2 / (2.0 * comfort_decel) + assert lead.required_decel < 0.30 + assert safety_gap - lead.usable_gap == pytest.approx(STOP_GAP_RESERVE) + assert lead.departure_cap > lead.cap + + def test_more_restrictive_lead_is_selected(self): + radar = make_radar(make_lead(status=True, d_rel=70.0, v_lead_k=12.0), make_lead(status=True, d_rel=25.0, v_lead_k=8.0)) + assert get_lead_plan(make_controller(), radar, 10.0, 0.0, AccelProfile.normal).selected_lead == 1 + + @pytest.mark.parametrize("field,value", [ + ("aLeadK", math.nan), ("aLeadK", math.inf), ("aLeadTau", math.nan), ("aLeadTau", -1.0), ("radarTrackId", math.nan), + ]) + def test_nonessential_invalid_lead_fields_are_sanitized(self, field, value): + lead = make_lead(status=True, d_rel=30.0, v_lead_k=8.0) + setattr(lead, field, value) + result = get_lead_plan(make_controller(), make_radar(lead), 10.0, 0.0, AccelProfile.normal) + assert result.selected_lead == 0 + assert math.isfinite(result.cap) + + @pytest.mark.parametrize("field,value", [("dRel", math.nan), ("dRel", -1.0), ("vLeadK", math.nan), ("vLeadK", -2.0)]) + def test_invalid_geometry_is_not_used(self, field, value): + lead = make_lead(status=True, d_rel=30.0, v_lead_k=8.0) + setattr(lead, field, value) + result = get_lead_plan(make_controller(), make_radar(lead), 10.0, 0.0, AccelProfile.normal) + assert result.selected_lead == -1 + assert result.lead_status + assert math.isinf(result.cap) + + def test_raw_radar_is_never_mutated(self): + lead = make_lead(status=True, d_rel=30.0, v_lead_k=8.0, a_lead_k=-15.0, a_lead_tau=math.nan) + before = vars(lead).copy() + get_lead_plan(make_controller(), make_radar(lead), 10.0, 0.0, AccelProfile.normal) + assert vars(lead) == before + + +class TestTargetLifecycle: + def test_five_frame_median_needs_three_restrictive_samples(self): + controller = make_controller() + filtered_caps = [] + for _ in range(CAP_FILTER_FRAMES): + update(controller, restrictive_radar()) + filtered_caps.append(controller.target_state.filtered_cap) + assert math.isinf(filtered_caps[1]) + assert math.isfinite(filtered_caps[2]) + + def test_restriction_uses_comfort_rate_with_one_bounded_reserve_step(self): + controller = make_controller() + results = [update(controller, restrictive_radar()) for _ in range(CAP_FILTER_FRAMES + 10)] + targets = np.asarray([result.target_speed for result in results]) + max_step = COMFORT_DECEL[AccelProfile.normal] * DT_MDL + + target_steps = -np.diff(targets) + assert np.count_nonzero(target_steps > max_step + 1e-9) == 1 + assert np.max(target_steps) <= TARGET_SPEED_RESERVE + max_step + 1e-9 + assert results[-1].state == AccelControllerState.restrict + assert results[-1].target_speed < results[0].target_speed + + @pytest.mark.parametrize("clear_frames", (1, 2, CAP_FILTER_FRAMES + 1)) + def test_lead_acquired_after_clear_road_cannot_step_speed_to_planner(self, clear_frames): + controller = make_controller() + for _ in range(clear_frames): + update(controller, base_speed=25.0, v_ego=20.0, planner_speed=25.0) + + results = [update(controller, restrictive_radar(), base_speed=25.0, v_ego=20.0, planner_speed=20.0) + for _ in range(CAP_FILTER_FRAMES)] + targets = np.asarray([25.0, *(result.target_speed for result in results)]) + max_step = COMFORT_DECEL[AccelProfile.normal] * DT_MDL + + target_steps = -np.diff(targets) + assert np.count_nonzero(target_steps > max_step + 1e-9) == 1 + assert np.max(target_steps) <= TARGET_SPEED_RESERVE + max_step + 1e-9 + + def test_lead_slot_is_forgotten_before_reacquisition(self): + controller = make_controller() + lead_one = restrictive_radar() + for _ in range(CAP_FILTER_FRAMES + 10): + update(controller, lead_one, base_speed=25.0, v_ego=20.0, planner_speed=20.0, planner_accel=-0.2) + + for _ in range(controller.lead_loss_hold_frames): + before = update(controller, base_speed=25.0, v_ego=20.0, planner_speed=20.0, planner_accel=-0.2) + assert controller.target_state.selected_lead == -1 + + lead_two = make_radar(lead_two=make_lead(status=True, d_rel=20.0, v_lead_k=8.0, a_lead_k=-0.5)) + results = [update(controller, lead_two, base_speed=25.0, v_ego=20.0, planner_speed=5.0, planner_accel=-0.2) + for _ in range(CAP_FILTER_FRAMES)] + targets = np.asarray([before.target_speed, *(result.target_speed for result in results)]) + max_step = COMFORT_DECEL[AccelProfile.normal] * DT_MDL + + target_steps = -np.diff(targets) + assert np.count_nonzero(target_steps > max_step + 1e-9) == 1 + assert np.max(target_steps) <= TARGET_SPEED_RESERVE + max_step + 1e-9 + + @pytest.mark.parametrize("replacement_track_id", (200, -1), ids=("radar-track", "vision-track")) + def test_false_relief_track_replacement_freezes_bounded_speed_release(self, replacement_track_id): + controller = make_controller() + original = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, radar_track_id=100)) + for _ in range(CAP_FILTER_FRAMES + 10): + update(controller, original, base_speed=25.0, v_ego=10.0, planner_speed=10.0, planner_accel=-0.2) + for _ in range(20): + before = update(controller, original, base_speed=25.0, v_ego=8.0, planner_speed=8.0, planner_accel=-0.2) + assert controller.target_state.matched_lead + + replacement = make_radar(make_lead(status=True, d_rel=40.0, v_lead_k=12.0, radar_track_id=replacement_track_id)) + switched = update(controller, replacement, base_speed=25.0, v_ego=8.0, planner_speed=5.0, planner_accel=-0.2) + + target_drop = before.target_speed - switched.target_speed + assert -TARGET_SPEED_RESERVE - 1e-9 <= target_drop <= MATCHED_SPEED_DECEL_RATE * DT_MDL + 1e-9 + assert effective_accel_max(switched) <= AccelController.get_profile_accel_max(AccelProfile.normal, 8.0) + 1e-9 + assert switched.target_speed < 25.0 + assert controller.target_state.lead_switch_guard_frames == controller.lead_loss_hold_frames + + def test_track_id_churn_without_false_relief_does_not_arm_guard(self): + controller = make_controller() + original = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, radar_track_id=100)) + for _ in range(CAP_FILTER_FRAMES + 10): + update(controller, original, base_speed=25.0, v_ego=10.0, planner_speed=10.0, planner_accel=-0.2) + + replacement = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, radar_track_id=200)) + update(controller, replacement, base_speed=25.0, v_ego=10.0, planner_speed=10.0, planner_accel=-0.2) + + assert controller.target_state.lead_switch_guard_frames == 0 + + def test_short_dropout_holds_then_releases_without_a_second_accel_cap(self): + controller = make_controller() + for _ in range(CAP_FILTER_FRAMES + 20): + restricted = update(controller, restrictive_radar()) + + held = [update(controller) for _ in range(controller.lead_loss_hold_frames - 1)] + assert all(result.target_speed <= restricted.target_speed + 1e-9 for result in held) + + released = update(controller) + assert released.target_speed == 25.0 + + def test_previous_lead_source_synchronizes_down_to_planner(self): + controller = make_controller() + for _ in range(CAP_FILTER_FRAMES + 10): + restricted = update(controller, restrictive_radar()) + planner_speed = restricted.target_speed - 2.0 + synchronized = update(controller, previous_mpc_source=LongitudinalPlanSource.lead0, planner_speed=planner_speed) + assert restricted.target_speed - synchronized.target_speed == pytest.approx(MATCHED_SPEED_DECEL_RATE * DT_MDL) + assert synchronized.state == AccelControllerState.hold + + def test_matched_lead_dropout_synchronizes_down_to_planner(self): + controller = make_controller() + radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0)) + for _ in range(CAP_FILTER_FRAMES + 10): + update(controller, radar, v_ego=10.0, planner_accel=-0.2) + for _ in range(20): + matched = update(controller, radar, v_ego=8.0, planner_accel=-0.2) + assert controller.target_state.matched_lead + + planner_speed = matched.target_speed - 2.0 + synchronized = update(controller, previous_mpc_source=LongitudinalPlanSource.lead0, planner_speed=planner_speed) + assert matched.target_speed - synchronized.target_speed == pytest.approx(MATCHED_SPEED_DECEL_RATE * DT_MDL) + assert synchronized.state == AccelControllerState.hold + + def test_reused_radar_holds_matched_lead_until_a_fresh_dropout(self): + controller = make_controller() + radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0)) + for _ in range(CAP_FILTER_FRAMES + 10): + update(controller, radar, v_ego=10.0, planner_accel=-0.2) + for _ in range(20): + matched = update(controller, radar, v_ego=8.0, planner_accel=-0.2) + assert controller.target_state.matched_lead + + planner_speed = matched.target_speed - 2.0 + held = update(controller, radar, previous_mpc_source=LongitudinalPlanSource.lead0, + planner_speed=planner_speed, radar_fresh=False) + synchronized = update(controller, previous_mpc_source=LongitudinalPlanSource.lead0, planner_speed=planner_speed) + + assert held.target_speed == pytest.approx(matched.target_speed) + assert held.state == matched.state + assert held.target_speed - synchronized.target_speed == pytest.approx(MATCHED_SPEED_DECEL_RATE * DT_MDL) + assert synchronized.state == AccelControllerState.hold + + def test_clear_road_launch_has_immediate_headroom_and_bounded_target_slew(self): + controller = make_controller() + initial = update(controller, base_speed=12.0, v_ego=0.0, profile=AccelProfile.normal) + rolling = update(controller, base_speed=12.0, v_ego=0.31, profile=AccelProfile.normal) + + assert initial.active and initial.launching + assert LAUNCH_TARGET_HEADROOM <= initial.target_speed <= LAUNCH_TARGET_HEADROOM + LAUNCH_TARGET_SLEW * DT_MDL + assert rolling.launching + assert rolling.target_speed >= 0.31 + LAUNCH_TARGET_HEADROOM + assert rolling.target_speed - max(initial.target_speed, 0.31 + LAUNCH_TARGET_HEADROOM) <= LAUNCH_TARGET_SLEW * DT_MDL + 1e-9 + + finished = update(controller, base_speed=12.0, v_ego=LAUNCH_END_SPEED, profile=AccelProfile.normal) + assert not finished.launching + + def test_far_stopped_lead_does_not_create_stop_hold(self): + controller = make_controller() + far_stopped = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=0.0)) + results = [update(controller, far_stopped, base_speed=12.0, v_ego=0.0) for _ in range(4)] + assert all(result.state != AccelControllerState.stopHold for result in results) + + def test_renewed_stop_above_stop_hold_speed_does_not_apply_extra_launch_ramp_step(self): + controller = make_controller() + ramp = [update(controller, base_speed=12.0, v_ego=v_ego, profile=AccelProfile.normal) for v_ego in (0.0, 0.5)] + assert all(result.launching for result in ramp) + + stopped_lead = make_radar(make_lead(status=True, d_rel=3.0, v_lead_k=0.0)) + renewed = update(controller, stopped_lead, base_speed=12.0, v_ego=0.5, profile=AccelProfile.normal) + assert not renewed.launching + assert renewed.target_speed <= ramp[-1].target_speed + 1e-9 + + def test_far_stopped_lead_does_not_use_sticky_braking_history_as_stop_evidence(self): + controller = make_controller() + far_stopped = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=0.0)) + for _ in range(controller.lead_loss_hold_frames): + update(controller, far_stopped, base_speed=12.0, v_ego=10.0, planner_accel=-0.2) + assert controller.target_state.lead_braking + + result = update(controller, far_stopped, base_speed=12.0, v_ego=0.2, planner_accel=-0.2) + assert result.state != AccelControllerState.stopHold + assert result.target_speed > 0.0 + + def test_near_stopped_lead_uses_braking_history_to_hold_completed_stop(self): + controller = make_controller() + stopped = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=0.0)) + for _ in range(controller.lead_loss_hold_frames): + update(controller, stopped, base_speed=12.0, v_ego=10.0, planner_accel=-0.2) + assert controller.target_state.lead_braking + + result = update(controller, stopped, base_speed=12.0, v_ego=0.2, planner_accel=-0.2) + assert result.state == AccelControllerState.stopHold + assert controller.target_state.target_speed == 0.0 + assert result.target_speed == 0.0 + assert math.isinf(effective_accel_max(result)) + assert result.mpc_accel_max is None + + stock_limited = update(controller, stopped, base_speed=12.0, v_ego=0.2, stock_accel_max=0.0) + assert math.isinf(effective_accel_max(stock_limited)) + assert stock_limited.mpc_accel_max is None + + def test_stop_hold_needs_four_confirmed_departure_frames(self): + controller = make_controller() + held = enter_stop_hold(controller) + assert controller.target_state.target_speed == 0.0 + results = [update(controller, make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0)), + base_speed=8.0, v_ego=0.1) for frame in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)] + launch_index = next(index for index, result in enumerate(results) if result.launching) + + assert held.state == AccelControllerState.stopHold + assert held.target_speed == 0.0 and math.isinf(effective_accel_max(held)) + assert held.mpc_accel_max is None + assert all(result.state == AccelControllerState.stopHold and not result.launching for result in results[:launch_index]) + assert launch_index == STOP_HOLD_EXIT_FRAMES - 1 + assert results[launch_index].target_speed >= 0.1 + LAUNCH_TARGET_HEADROOM + assert results[launch_index].departure_launching + assert effective_accel_max(results[launch_index]) == pytest.approx( + AccelController.get_profile_accel_max(AccelProfile.normal, 0.1), + ) + + def test_stopped_governing_lead_rejects_route_51d_radar_speed_pulse_without_delaying_departure(self): + controller = make_controller() + enter_stop_hold(controller, v_ego=0.0) + speed_pulse = (0.1361, 0.1731, 0.2146, 0.2253, 0.2137, 0.1877) + distances = (6.0, 6.0, 6.0, 5.96, 6.04, 6.04) + + for distance, speed in zip(distances, speed_pulse, strict=True): + radar = make_radar(make_lead(status=True, d_rel=distance, v_lead_k=speed, radar_track_id=4887), + make_lead(status=True, d_rel=6.08, v_lead_k=0.0, radar_track_id=4905)) + held = update(controller, radar, base_speed=8.0, v_ego=0.0) + assert held.state == AccelControllerState.stopHold + assert held.target_speed == 0.0 and not held.launching + + results = [ + update(controller, make_radar(make_lead(status=True, d_rel=6.04 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=4887), + make_lead(status=True, d_rel=6.12 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=4905)), + base_speed=8.0, v_ego=0.0) + for frame in range(STOP_HOLD_EXIT_FRAMES) + ] + + assert all(result.state == AccelControllerState.stopHold for result in results[:-1]) + assert results[-1].launching and results[-1].departure_launching + + def test_route_520_slow_lead_pulse_cannot_release_stop_hold_but_real_departure_can(self): + controller = make_controller() + enter_stop_hold(controller, v_ego=0.0) + speeds = (0.01, 0.03, 0.07, 0.10, 0.14, 0.20, 0.26, 0.32, 0.34, 0.33, 0.31, 0.28, 0.24, 0.20, 0.15, 0.09, 0.05, 0.01) + offsets = (0.00, 0.00, 0.00, 0.01, 0.01, 0.02, 0.03, 0.04, 0.06, 0.07, 0.09, 0.11, 0.12, 0.13, 0.14, 0.15, 0.15, 0.16) + + for offset, speed in zip(offsets, speeds, strict=True): + pulse = make_radar(make_lead(status=True, d_rel=6.0 + offset, v_lead_k=speed, radar_track_id=2133)) + held = update(controller, pulse, base_speed=8.0, v_ego=0.0) + assert held.state == AccelControllerState.stopHold + assert held.target_speed == 0.0 and not held.launching + + stopped = make_radar(make_lead(status=True, d_rel=6.2, v_lead_k=0.0, radar_track_id=2133)) + assert update(controller, stopped, base_speed=8.0, v_ego=0.0).state == AccelControllerState.stopHold + results = [update(controller, make_radar(make_lead(status=True, d_rel=6.2 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=2133)), + base_speed=8.0, v_ego=0.0) for frame in range(STOP_HOLD_EXIT_FRAMES)] + + assert all(result.state == AccelControllerState.stopHold for result in results[:-1]) + assert results[-1].launching and results[-1].departure_launching + + def test_fast_speed_signal_that_slows_without_separating_never_releases_stop_hold(self): + controller = make_controller() + enter_stop_hold(controller, v_ego=0.0) + departing = make_radar(make_lead(status=True, d_rel=5.9, v_lead_k=2.0)) + results = [update(controller, departing, base_speed=8.0, v_ego=0.0) for _ in range(STOP_HOLD_EXIT_FRAMES)] + slowed = update(controller, make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.2)), base_speed=8.0, v_ego=0.0) + + assert all(result.state == AccelControllerState.stopHold and not result.launching for result in results) + assert slowed.state == AccelControllerState.stopHold + assert slowed.target_speed == 0.0 and not slowed.launching + + def test_stop_hold_reseeds_departure_distance_when_radar_track_is_replaced(self): + controller = make_controller() + original = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100)) + update(controller, original, base_speed=8.0, v_ego=0.0, previous_should_stop=True) + replacement = make_radar(make_lead(status=True, d_rel=6.4, v_lead_k=0.2, radar_track_id=200)) + results = [update(controller, replacement, base_speed=8.0, v_ego=0.0) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)] + + assert all(result.state == AccelControllerState.stopHold for result in results) + assert all(result.target_speed == 0.0 and not result.launching for result in results) + + def test_stop_hold_rejects_persistent_same_track_distance_step(self): + controller = make_controller() + original = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100)) + update(controller, original, base_speed=8.0, v_ego=0.0, previous_should_stop=True) + stepped = make_radar(make_lead(status=True, d_rel=6.4, v_lead_k=0.2, radar_track_id=100)) + results = [update(controller, stepped, base_speed=8.0, v_ego=0.0) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)] + + assert all(result.state == AccelControllerState.stopHold for result in results) + assert all(result.target_speed == 0.0 and not result.launching for result in results) + + def test_stop_hold_reseeds_non_selected_departure_lead_when_its_track_is_replaced(self): + controller = make_controller() + original = make_radar(make_lead(status=True, d_rel=3.0, v_lead_k=0.2, radar_track_id=100), + make_lead(status=True, d_rel=6.0, v_lead_k=0.1, radar_track_id=200)) + update(controller, original, base_speed=8.0, v_ego=0.0, previous_should_stop=True) + replacement = make_radar(make_lead(status=True, d_rel=3.4, v_lead_k=0.2, radar_track_id=101), + make_lead(status=True, d_rel=6.0, v_lead_k=0.1, radar_track_id=200)) + lead = get_lead_plan(controller, replacement, 0.0, 0.0, AccelProfile.normal) + results = [update(controller, replacement, base_speed=8.0, v_ego=0.0) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)] + + assert lead.selected_lead == 1 and lead.departure_lead_index == 0 + assert all(result.state == AccelControllerState.stopHold for result in results) + assert all(result.target_speed == 0.0 and not result.launching for result in results) + + def test_genuine_departure_survives_lead_slot_and_track_flicker(self): + controller = make_controller() + enter_stop_hold(controller, v_ego=0.0) + results = [] + for frame in range(STOP_HOLD_EXIT_FRAMES): + moving = make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=100) + secondary = make_lead(status=True, d_rel=7.0, v_lead_k=2.0, radar_track_id=200) + results.append(update(controller, make_radar(moving, secondary) if frame % 2 == 0 else make_radar(secondary, moving), + base_speed=8.0, v_ego=0.0)) + + assert all(result.state == AccelControllerState.stopHold for result in results[:-1]) + assert results[-1].launching and results[-1].departure_launching + + def test_fast_speed_glitch_without_distance_progress_stays_in_stop_hold(self): + controller = make_controller() + stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100)) + update(controller, stopped, base_speed=8.0, v_ego=0.0, previous_should_stop=True) + glitch = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.9, radar_track_id=100)) + results = [update(controller, glitch, base_speed=8.0, v_ego=0.0) for _ in range(STOP_HOLD_EXIT_FRAMES)] + results.append(update(controller, stopped, base_speed=8.0, v_ego=0.0)) + + assert all(result.state == AccelControllerState.stopHold for result in results) + assert all(result.target_speed == 0.0 and not result.launching for result in results) + + def test_moving_departure_does_not_reenter_stop_hold_when_speed_crosses_exit_threshold(self): + controller = make_controller() + stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100)) + update(controller, stopped, base_speed=8.0, v_ego=0.0, previous_should_stop=True) + distance = 6.0 + results = [] + for speed in (0.81, 0.82, 0.83, 0.84, 0.79, 0.76, 0.74, 0.72): + distance += speed * DT_MDL + radar = make_radar(make_lead(status=True, d_rel=distance, v_lead_k=speed, radar_track_id=100)) + results.append(update(controller, radar, base_speed=8.0, v_ego=0.0)) + + launch_index = next(index for index, result in enumerate(results) if result.launching) + assert all(result.state != AccelControllerState.stopHold for result in results[launch_index:]) + assert all(result.target_speed > 0.0 and result.departure_launching for result in results[launch_index:]) + + def test_reused_radar_does_not_pulse_stop_hold_or_departure_target(self): + controller = make_controller() + enter_stop_hold(controller) + + for frame in range(STOP_HOLD_EXIT_FRAMES): + departing = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0)) + fresh = update(controller, departing, base_speed=8.0, v_ego=0.1) + held = update(controller, departing, base_speed=8.0, v_ego=0.1, radar_fresh=False, + previous_mpc_source=LongitudinalPlanSource.lead0, planner_speed=0.01) + assert held.target_speed == pytest.approx(fresh.target_speed) + assert held.state == fresh.state + assert held.selected_lead == fresh.selected_lead == 0 + assert effective_accel_max(held) == pytest.approx(effective_accel_max(fresh)) + if frame < STOP_HOLD_EXIT_FRAMES - 1: + assert fresh.state == AccelControllerState.stopHold + assert math.isinf(effective_accel_max(fresh)) + assert fresh.mpc_accel_max is None + + assert fresh.launching and held.launching + assert fresh.departure_launching and held.departure_launching + assert fresh.target_speed == held.target_speed == 8.0 + assert effective_accel_max(fresh) == pytest.approx(AccelController.get_profile_accel_max(AccelProfile.normal, 0.1)) + + def test_single_frame_departure_stays_at_zero_target_without_an_accel_ceiling(self): + controller = make_controller() + enter_stop_hold(controller) + departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0)) + stopped = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=0.0)) + + warm = update(controller, departing, base_speed=8.0, v_ego=0.0) + held = update(controller, stopped, base_speed=8.0, v_ego=0.0) + + assert warm.state == held.state == AccelControllerState.stopHold + assert not warm.launching and not held.launching + assert math.isinf(effective_accel_max(warm)) and warm.mpc_accel_max is None + assert math.isinf(effective_accel_max(held)) and held.mpc_accel_max is None + assert held.target_speed == 0.0 + + def test_previous_stop_without_a_lead_does_not_latch_stop_hold(self): + result = update( + make_controller(), base_speed=8.0, v_ego=0.0, previous_should_stop=True, + previous_mpc_source=LongitudinalPlanSource.cruise, + ) + + assert result.state != AccelControllerState.stopHold + assert result.target_speed >= LAUNCH_TARGET_HEADROOM + + def test_previous_lead_stop_survives_a_fresh_full_field_dropout(self): + result = update( + make_controller(), base_speed=8.0, v_ego=0.0, previous_should_stop=True, + previous_mpc_source=LongitudinalPlanSource.lead0, + ) + + assert result.state == AccelControllerState.stopHold + assert result.target_speed == 0.0 + assert math.isinf(effective_accel_max(result)) + assert result.mpc_accel_max is None + + def test_stop_hold_without_usable_lead_stays_pinned_to_zero(self): + controller = make_controller() + enter_stop_hold(controller) + missing = update(controller, base_speed=8.0, v_ego=0.1) + + assert missing.state == AccelControllerState.stopHold + assert missing.target_speed == 0.0 + assert math.isinf(effective_accel_max(missing)) + assert missing.mpc_accel_max is None + + def test_confirmed_creep_departure_does_not_reenter_stop_hold(self): + controller = make_controller() + enter_stop_hold(controller, v_ego=0.0) + results = [] + for frame in range(60): + creeping = make_radar(make_lead(status=True, d_rel=6.0 + frame * 0.01, v_lead_k=0.2)) + results.append(update(controller, creeping, base_speed=8.0, v_ego=0.0)) + + launch_index = next(index for index, result in enumerate(results) if result.launching) + assert launch_index * DT_MDL <= 2.0 + assert all(result.state != AccelControllerState.stopHold for result in results[launch_index:]) + assert all(result.target_speed > 0.0 for result in results[launch_index:]) + + def test_departure_dropout_holds_without_resurrecting_stop_hold(self): + controller = make_controller() + enter_stop_hold(controller) + results = [update(controller, make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0)), + base_speed=8.0, v_ego=0.1) for frame in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)] + launched = next(result for result in results if result.launching) + before_dropout = results[-1] + dropout = [update(controller, base_speed=8.0, v_ego=0.1) for _ in range(controller.lead_loss_hold_frames + 1)] + + assert launched.launching + assert all(result.state != AccelControllerState.stopHold for result in dropout) + assert all(result.target_speed <= before_dropout.target_speed + 1e-9 for result in dropout[:controller.lead_loss_hold_frames - 1]) + assert dropout[controller.lead_loss_hold_frames - 1].target_speed > before_dropout.target_speed + assert dropout[-1].launching + + def test_invalid_departure_geometry_returns_to_stop_hold(self): + controller = make_controller() + enter_stop_hold(controller) + for frame in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES): + departing = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0)) + launched = update(controller, departing, base_speed=8.0, v_ego=0.1) + invalid = make_radar(make_lead(status=True, d_rel=math.nan, v_lead_k=2.0)) + guarded = update(controller, invalid, base_speed=8.0, v_ego=0.1) + assert launched.launching + assert guarded.state == AccelControllerState.stopHold + assert guarded.target_speed == 0.0 + + def test_stale_timeout_fully_resets_live_state(self): + controller = make_controller() + for _ in range(CAP_FILTER_FRAMES + 10): + restricted = update(controller, restrictive_radar()) + stale_frames = math.ceil(RADAR_STALE_TIMEOUT / DT_MDL) + held = [update(controller, radar_fresh=False) for _ in range(stale_frames - 1)] + timed_out = update(controller, radar_fresh=False) + + assert all(result.active and result.target_speed == pytest.approx(restricted.target_speed) for result in held) + assert not timed_out.active + assert timed_out.target_speed == 25.0 + assert timed_out.mpc_accel_max is None + assert timed_out.selected_lead == -1 and controller._held_lead_plan is None + assert controller.target_state.target_speed is None + + @pytest.mark.parametrize("override", [{"enabled": False}, {"acc_selected": False}, {"engaged": False}, {"cruise_initialized": False}, {"a_ego": math.inf}]) + def test_bypass_or_invalid_context_resets_live_state(self, override): + controller = make_controller() + for _ in range(CAP_FILTER_FRAMES + 10): + update(controller, restrictive_radar()) + result = update(controller, restrictive_radar(), **override) + + assert not result.active + assert result.target_speed == 25.0 + assert result.mpc_accel_max is None + assert controller.target_state.target_speed is None + + def test_acc_bypass_does_not_retain_state_for_live_actuation(self): + controller = make_controller() + for _ in range(CAP_FILTER_FRAMES + 20): + bypassed = update(controller, restrictive_radar(), acc_selected=False) + assert not bypassed.active + assert controller.target_state.target_speed is None + assert controller._held_lead_plan is None + live = update(controller) + + assert live.active and live.target_speed == 25.0 + assert math.isinf(controller.target_state.filtered_cap) + + def test_explicit_reset_clears_target_state(self): + controller = make_controller() + for _ in range(CAP_FILTER_FRAMES + 10): + update(controller, restrictive_radar()) + controller._jerk_smoothing_blocked = True + controller._required_decel_samples = [0.2] + controller._required_decel_lead = controller._required_decel_lead_track_id = 1 + controller._lead_trend_warmup = True + controller.reset() + + assert controller._held_lead_plan is None + assert not controller._jerk_smoothing_blocked + assert controller._required_decel_samples == [] + assert controller._required_decel_lead == controller._required_decel_lead_track_id == -1 + assert not controller._lead_trend_warmup + target_state = controller.target_state + assert target_state.target_speed is None and target_state.matched_accel_limit is None + assert target_state.state == AccelControllerState.inactive + assert target_state.departure_frames == target_state.active_frames == target_state.lead_loss_frames == target_state.stale_frames == 0 + assert target_state.lead_switch_guard_frames == 0 + assert target_state.selected_lead == target_state.selected_lead_track_id == -1 + assert target_state.cap_samples == [math.inf] * CAP_FILTER_FRAMES + assert target_state.lead_speed_samples == [math.inf] * CAP_FILTER_FRAMES + assert target_state.lead_accel_samples == [0.0] * CAP_FILTER_FRAMES + assert target_state.departure.samples == [[], []] and target_state.departure.motion_samples == [] + assert target_state.departure.references == [None, None] and target_state.departure.track_ids == [-1, -1] + assert not target_state.launching and not target_state.departure_launch and not target_state.matched_lead + assert not target_state.lead_braking and not target_state.e2e_braking_handoff and not target_state.speed_reserve_armed + assert math.isinf(target_state.filtered_cap) and math.isinf(target_state.filtered_lead_speed) and target_state.filtered_lead_accel == 0.0 + + @pytest.mark.parametrize("replacement_track_id", (200, -1), ids=("radar-track", "vision-track")) + def test_track_id_change_requires_new_history_before_jerk_smoothing(self, replacement_track_id): + controller = make_controller() + controller.state = AccelControllerState.restrict + controller.launching = False + controller.selected_lead = 0 + controller.selected_lead_track_id = 100 + controller.required_decel = 0.2 + original = [controller.get_jerk_cost_multiplier(True, True, 1.0, False) for _ in range(4)] + + controller.selected_lead_track_id = replacement_track_id + replacement = [controller.get_jerk_cost_multiplier(True, True, 1.0, False) for _ in range(4)] + + assert original == [MPC_DECEL_JERK_COST_MULTIPLIER] * 4 + assert replacement == [1.0, 1.0, 1.0, MPC_DECEL_JERK_COST_MULTIPLIER] + assert controller._required_decel_samples == [0.2] * 4 diff --git a/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py b/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py new file mode 100644 index 0000000000..85462fb8f9 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py @@ -0,0 +1,515 @@ +import inspect +import math +from types import SimpleNamespace + +import numpy as np +import pytest + +from cereal import custom, log, messaging +from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, LongitudinalMpc +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcLongitudinalPlanSource +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ( + MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_TARGET_REDUCTION, AccelProfile, +) +from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpcSP +from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource + + +def radar_state(): + return messaging.new_message("radarState").radarState + + +class PlannerSM(dict): + def __init__(self, radar_log_mono_time: int): + super().__init__( + radarState=radar_state(), + carState=SimpleNamespace(vEgo=10.0, aEgo=0.0, vCruise=20.0), + selfdriveState=SimpleNamespace(personality=0), + controlsState=SimpleNamespace(forceDecel=False), + ) + self.valid = {"radarState": True} + self.alive = {"radarState": True} + self.logMonoTime = {"radarState": radar_log_mono_time} + + +class ControllerStub: + def __init__(self, *, target_speed=15.0, active=True, mpc_accel_max=None, state=AccelControllerState.free, selected_lead=-1, + selected_lead_track_id=-1, launching=False, departure_launching=False, required_decel=0.0): + self.available = self.enabled = True + self.profile = AccelProfile.normal + self.output_v_target = target_speed + self.is_active = active + self.mpc_accel_max = mpc_accel_max + self.state = state + self.selected_lead = selected_lead + self.selected_lead_track_id = selected_lead_track_id + self.launching = launching + self.departure_launching = departure_launching + self.required_decel = required_decel + self.dt = DT_MDL + self._jerk_smoothing_blocked = False + self._required_decel_samples = [] + self._required_decel_lead = -1 + self._required_decel_lead_track_id = -1 + self._lead_trend_warmup = False + self.update_kwargs = None + self.reset_calls = 0 + + def update(self, _radar_state, **kwargs): + self.update_kwargs = kwargs + + @property + def is_enabled(self): + return self.available and self.enabled + + def update_params(self): + pass + + def reset(self): + self.reset_calls += 1 + + def get_jerk_cost_multiplier(self, *args): + return AccelController.get_jerk_cost_multiplier(self, *args) + + def update_should_stop(self, should_stop): + return AccelController.update_should_stop(self, should_stop) + + +def planner_for_mpc_test(*, target_speed=15.0, active=True, is_e2e=False, mpc_accel_max=None, + state=AccelControllerState.free, selected_lead=-1, launching=False, + departure_launching=False, required_decel=0.0, + mpc_source=MpcLongitudinalPlanSource.lead0): + planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP) + is_e2e_calls = [] + planner.is_e2e = lambda _sm: is_e2e_calls.append(True) or is_e2e + planner.output_v_target = 20.0 + planner.output_should_stop = False + planner.allow_throttle = True + planner.a_desired = 0.0 + planner.v_desired_filter = SimpleNamespace(x=10.0) + planner._radar_fresh_this_cycle = True + planner.mpc = SimpleNamespace(source=mpc_source, last_solution_status=0) + planner.accel_controller = ControllerStub( + target_speed=target_speed, active=active, state=state, selected_lead=selected_lead, launching=launching, + departure_launching=departure_launching, required_decel=required_decel, mpc_accel_max=mpc_accel_max, + ) + return planner, is_e2e_calls + + +def run_controller_mpc(planner, *, mpc_v_cruise=20.0, force_decel=False): + calls = [] + planner._run_mpc = lambda _sm, *args, **kwargs: calls.append((({}, *args), kwargs)) + sm = { + "radarState": radar_state(), + "controlsState": SimpleNamespace(forceDecel=force_decel), + "carState": SimpleNamespace(vCruise=20.0, vEgo=10.0, aEgo=0.0), + "selfdriveState": SimpleNamespace(personality=0), + } + is_e2e = planner.update_mpc(sm, mpc_v_cruise, True, ACCEL_MAX, False) + return is_e2e, calls + + +def test_accel_controller_schema_contract(): + expected = {"eco": 0, "normal": 1, "sport": 2} + state = {"inactive": 0, "free": 1, "restrict": 2, "hold": 3, "release": 4, "stopHold": 5} + accel_controller = custom.LongitudinalPlanSP.schema.fields["accelController"] + fields = custom.LongitudinalPlanSP.AccelController.schema.fields + + assert accel_controller.proto.ordinal.explicit == 8 + assert {name: field.proto.ordinal.explicit for name, field in fields.items()} == { + "enabled": 0, "active": 1, "shadowOnlyDEPRECATED": 2, "profile": 3, "state": 4, + } + assert fields["shadowOnlyDEPRECATED"].proto.slot.type.which() == "bool" + assert custom.LongitudinalPlanSP.AccelerationPersonality.schema.enumerants == expected + assert custom.LongitudinalPlanSP.AccelController.Profile.schema.enumerants == expected + assert custom.LongitudinalPlanSP.AccelController.State.schema.enumerants == state + + +def test_accel_controller_schema_round_trip_and_toyota_compatibility(): + message = custom.LongitudinalPlanSP.new_message() + message.accelController.enabled = True + message.accelController.active = True + message.accelController.profile = custom.LongitudinalPlanSP.AccelController.Profile.sport + message.accelController.state = custom.LongitudinalPlanSP.AccelController.State.release + + with custom.LongitudinalPlanSP.from_bytes(message.to_bytes()) as reader: + assert reader.accelController.enabled and reader.accelController.active + assert reader.accelController.profile == custom.LongitudinalPlanSP.AccelController.Profile.sport + assert reader.accelController.state == custom.LongitudinalPlanSP.AccelController.State.release + + from opendbc.car.toyota.carstate import AccelPersonality, CarState + + assert AccelPersonality.schema.enumerants == {"eco": 0, "normal": 1, "sport": 2} + assert CarState.__module__ == "opendbc.car.toyota.carstate" + + +def test_longitudinal_planner_sp_owns_accel_controller_integration(): + assert "update_mpc" in LongitudinalPlannerSP.__dict__ + assert "update_should_stop" in LongitudinalPlannerSP.__dict__ + + +def test_mpc_inherits_accel_controller_extension_without_changing_stock_signature_or_bounds(): + assert LongitudinalMpc.__bases__ == (LongitudinalMpcSP,) + assert tuple(inspect.signature(LongitudinalMpc.update).parameters) == ("self", "radarstate", "v_cruise", "personality") + mpc = LongitudinalMpc() + radar = radar_state() + mpc.run = lambda: None + + mpc.set_cur_state(10.0, 0.8) + mpc.update(radar, 30.0) + np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN) + np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX) + + requested_ceiling = tuple(np.full(N + 1, 0.4)) + mpc.set_accel_controller_params(requested_ceiling, 1.0) + mpc.update(radar, 30.0) + np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN) + assert mpc.params[0, 1] == pytest.approx(0.8) + np.testing.assert_array_equal(mpc.params[1:, 1], requested_ceiling[1:]) + + for malformed_ceiling in ("bad", [0.4] * N, np.full(N + 1, math.nan), [10**10000] * (N + 1)): + mpc.set_accel_controller_params(malformed_ceiling, 1.0) + mpc.update(radar, 30.0) + np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN) + np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX) + + mpc.set_accel_controller_params(None, 1.0) + mpc.update(radar, 30.0) + np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX) + + +def test_mpc_jerk_cost_multiplier_is_backward_compatible_and_does_not_change_other_costs(): + mpc = LongitudinalMpc.__new__(LongitudinalMpc) + LongitudinalMpcSP.__init__(mpc) + captured = [] + mpc.set_cost_weights = lambda costs, constraints: captured.append((np.asarray(costs), np.asarray(constraints))) + + mpc.set_weights(True, personality=log.LongitudinalPersonality.standard) + default_costs, default_constraints = captured[-1] + mpc.set_accel_controller_params(None, 1.0) + mpc.set_weights(True, personality=log.LongitudinalPersonality.standard) + explicit_costs, explicit_constraints = captured[-1] + mpc.set_accel_controller_params(None, 1.2) + mpc.set_weights(True, personality=log.LongitudinalPersonality.standard) + smoothed_costs, smoothed_constraints = captured[-1] + + np.testing.assert_array_equal(explicit_costs, default_costs) + np.testing.assert_array_equal(explicit_constraints, default_constraints) + np.testing.assert_array_equal(smoothed_costs[:-1], default_costs[:-1]) + assert smoothed_costs[-1] == pytest.approx(default_costs[-1] * 1.2) + np.testing.assert_array_equal(smoothed_constraints, default_constraints) + + mpc.set_weights(False, personality=log.LongitudinalPersonality.standard) + assert captured[-1][0][-2] == 0.0 + assert captured[-1][0][-1] == pytest.approx(default_costs[-1] * 1.2) + + +def test_inherited_planner_uses_real_state_raw_radar_and_one_mpc_solve(): + radar = radar_state() + planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP) + planner.a_desired = -0.2 + planner.v_desired_filter = SimpleNamespace(x=12.0) + calls = [] + + def update_mpc(radar_arg, target, *, personality): + calls.append(("update", radar_arg, target, personality)) + + planner.mpc = SimpleNamespace( + set_accel_controller_params=lambda accel_max, multiplier: calls.append(("configure", accel_max, multiplier)), + set_weights=lambda constraint, personality: calls.append(("weights", constraint, personality)), + set_cur_state=lambda speed, accel: calls.append(("state", speed, accel)), + update=update_mpc, + ) + ceiling = tuple(np.linspace(0.8, 0.4, N + 1)) + sm = {"radarState": radar, "selfdriveState": SimpleNamespace(personality=2)} + planner._run_mpc(sm, 17.5, True, ceiling, jerk_cost_multiplier=1.2) + + assert calls == [ + ("configure", ceiling, 1.2), + ("weights", True, 2), + ("state", 12.0, -0.2), + ("update", radar, 17.5, 2), + ] + assert calls[-1][1] is radar + + +def test_active_acc_uses_target_and_ceiling_in_exactly_one_solve(): + ceiling = tuple(np.linspace(0.8, 0.4, N + 1)) + planner, mode_calls = planner_for_mpc_test(mpc_accel_max=ceiling) + is_e2e, calls = run_controller_mpc(planner) + + assert not is_e2e + assert len(mode_calls) == 1 + assert calls == [(({}, 15.0, True, ceiling), {"jerk_cost_multiplier": 1.0})] + + +def test_valid_lead_stop_hold_preplans_from_raw_target_without_an_accel_ceiling(): + planner, _ = planner_for_mpc_test( + target_speed=0.0, mpc_accel_max=None, state=AccelControllerState.stopHold, selected_lead=0, + ) + _, calls = run_controller_mpc(planner) + + assert calls == [(({}, 20.0, True, None), {"jerk_cost_multiplier": 1.0})] + + +def test_missing_lead_stop_hold_keeps_zero_mpc_target_without_an_accel_ceiling(): + planner, _ = planner_for_mpc_test( + target_speed=0.0, mpc_accel_max=None, state=AccelControllerState.stopHold, selected_lead=-1, + ) + _, calls = run_controller_mpc(planner) + + assert calls == [(({}, 0.0, True, None), {"jerk_cost_multiplier": 1.0})] + + +@pytest.mark.parametrize( + ("active", "departure_launching", "expected"), + [ + (True, True, False), + (True, False, True), + (False, True, True), + ], +) +def test_only_confirmed_live_acc_departure_clears_should_stop(active, departure_launching, expected): + planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP) + planner.accel_controller = ControllerStub(active=active, departure_launching=departure_launching, state=AccelControllerState.stopHold) + assert planner.update_should_stop(True) is expected + assert planner.update_should_stop(False) is (active and not departure_launching) + + +@pytest.mark.parametrize(("active", "is_e2e"), [(False, False), (True, True)]) +def test_disabled_or_e2e_is_an_exact_mpc_bypass(active, is_e2e): + ceiling = tuple(np.linspace(0.8, 0.4, N + 1)) + planner, mode_calls = planner_for_mpc_test(active=active, is_e2e=is_e2e, mpc_accel_max=ceiling) + returned_e2e, calls = run_controller_mpc(planner) + + assert returned_e2e is is_e2e + assert len(mode_calls) == 1 + assert calls == [(({}, 20.0, True, None), {"jerk_cost_multiplier": 1.0})] + + +def test_force_decel_target_remains_authoritative_and_disables_ceiling(): + ceiling = tuple(np.linspace(0.8, 0.4, N + 1)) + planner, mode_calls = planner_for_mpc_test(mpc_accel_max=ceiling) + _, calls = run_controller_mpc(planner, mpc_v_cruise=0.0, force_decel=True) + + assert len(mode_calls) == 1 + assert calls == [(({}, 0.0, True, None), {"jerk_cost_multiplier": 1.0})] + + +def test_previous_mpc_failure_gets_one_stock_recovery_cycle(): + ceiling = tuple(np.linspace(0.8, 0.4, N + 1)) + planner, mode_calls = planner_for_mpc_test(mpc_accel_max=ceiling) + controller = planner.accel_controller + planner.mpc.last_solution_status = 4 + + _, failed_recovery_calls = run_controller_mpc(planner) + assert controller.reset_calls == 1 + assert len(mode_calls) == 1 + assert failed_recovery_calls == [(({}, 20.0, True, None), {"jerk_cost_multiplier": 1.0})] + + planner.mpc.last_solution_status = 0 + _, recovered_calls = run_controller_mpc(planner) + assert controller.reset_calls == 1 + assert len(mode_calls) == 2 + assert recovered_calls == [(({}, 15.0, True, ceiling), {"jerk_cost_multiplier": 1.0})] + + +@pytest.mark.parametrize( + "mpc_source", + (MpcLongitudinalPlanSource.cruise, MpcLongitudinalPlanSource.lead0, MpcLongitudinalPlanSource.lead1), +) +def test_routine_governor_restriction_forwards_the_jerk_cost_multiplier(mpc_source): + planner, _ = planner_for_mpc_test( + state=AccelControllerState.restrict, selected_lead=0, required_decel=0.30, + mpc_source=mpc_source, + ) + _, calls = run_controller_mpc(planner) + + assert calls == [(({}, 15.0, True, None), {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER})] + + +def test_ineligible_required_decel_blocks_smoothing_only_until_the_restriction_episode_ends(): + planner, _ = planner_for_mpc_test( + state=AccelControllerState.restrict, selected_lead=0, required_decel=0.30, + ) + _, initial_calls = run_controller_mpc(planner) + controller = planner.accel_controller + assert initial_calls[0][1] == {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER} + + controller.required_decel = MPC_DECEL_JERK_MAX_REQUIRED_DECEL + _, ineligible_calls = run_controller_mpc(planner) + assert ineligible_calls[0][1] == {"jerk_cost_multiplier": 1.0} + + controller.required_decel = 0.30 + _, flicker_calls = run_controller_mpc(planner) + assert flicker_calls[0][1] == {"jerk_cost_multiplier": 1.0} + + controller.state = AccelControllerState.free + controller.output_v_target = 20.0 + run_controller_mpc(planner) + controller.state = AccelControllerState.restrict + controller.output_v_target = 15.0 + _, rearmed_calls = run_controller_mpc(planner) + assert rearmed_calls[0][1] == {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER} + + +def test_consistently_tightening_lead_releases_smoothing_until_the_restriction_ends(): + planner, _ = planner_for_mpc_test( + state=AccelControllerState.restrict, selected_lead=0, required_decel=0.18, + ) + _, calls = run_controller_mpc(planner) + controller = planner.accel_controller + multipliers = [calls[0][1]["jerk_cost_multiplier"]] + for required_decel in (0.20, 0.23, 0.25): + controller.required_decel = required_decel + _, calls = run_controller_mpc(planner) + multipliers.append(calls[0][1]["jerk_cost_multiplier"]) + + assert multipliers == [MPC_DECEL_JERK_COST_MULTIPLIER] * 3 + [1.0] + + controller.required_decel = 0.20 + _, calls = run_controller_mpc(planner) + assert calls[0][1] == {"jerk_cost_multiplier": 1.0} + + controller.state = AccelControllerState.free + controller.output_v_target = 20.0 + run_controller_mpc(planner) + controller.state = AccelControllerState.restrict + controller.output_v_target = 15.0 + controller.required_decel = 0.18 + _, calls = run_controller_mpc(planner) + assert calls[0][1] == {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER} + + +def test_one_frame_required_decel_noise_does_not_disable_routine_smoothing(): + planner, _ = planner_for_mpc_test( + state=AccelControllerState.restrict, selected_lead=0, required_decel=0.18, + ) + _, calls = run_controller_mpc(planner) + controller = planner.accel_controller + multipliers = [calls[0][1]["jerk_cost_multiplier"]] + for required_decel in (0.24, 0.19, 0.22): + controller.required_decel = required_decel + _, calls = run_controller_mpc(planner) + multipliers.append(calls[0][1]["jerk_cost_multiplier"]) + + assert multipliers == [MPC_DECEL_JERK_COST_MULTIPLIER] * 4 + + +@pytest.mark.parametrize( + ("state", "selected_lead", "launching", "required_decel", "target_speed", "mpc_source"), + [ + (AccelControllerState.free, 0, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise), + (AccelControllerState.hold, 0, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise), + (AccelControllerState.stopHold, 0, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise), + (AccelControllerState.restrict, -1, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise), + (AccelControllerState.restrict, 0, True, 0.30, 15.0, MpcLongitudinalPlanSource.cruise), + (AccelControllerState.restrict, 0, False, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, 15.0, MpcLongitudinalPlanSource.cruise), + (AccelControllerState.restrict, 0, False, math.inf, 15.0, MpcLongitudinalPlanSource.cruise), + (AccelControllerState.restrict, 0, False, math.nan, 15.0, MpcLongitudinalPlanSource.cruise), + (AccelControllerState.restrict, 0, False, 0.0, 15.0, MpcLongitudinalPlanSource.cruise), + (AccelControllerState.restrict, 0, False, -0.01, 15.0, MpcLongitudinalPlanSource.cruise), + (AccelControllerState.restrict, 0, False, 0.30, 20.0 - MPC_DECEL_JERK_MAX_TARGET_REDUCTION, MpcLongitudinalPlanSource.cruise), + (AccelControllerState.restrict, 0, False, 0.30, 20.0, MpcLongitudinalPlanSource.cruise), + (AccelControllerState.restrict, 0, False, 0.30, 25.0, MpcLongitudinalPlanSource.cruise), + ], +) +def test_non_routine_or_stock_lead_states_keep_stock_jerk_cost( + state, selected_lead, launching, required_decel, target_speed, mpc_source, +): + planner, _ = planner_for_mpc_test( + state=state, selected_lead=selected_lead, launching=launching, + required_decel=required_decel, target_speed=target_speed, mpc_source=mpc_source, + ) + _, calls = run_controller_mpc(planner) + + assert calls[0][1] == {"jerk_cost_multiplier": 1.0} + + +def test_controller_receives_previous_mpc_state_and_cached_radar_freshness(): + planner, _ = planner_for_mpc_test(mpc_source=log.LongitudinalPlan.LongitudinalPlanSource.lead0) + planner._radar_fresh_this_cycle = True + planner.a_desired = -0.4 + planner.v_desired_filter = SimpleNamespace(x=9.5) + run_controller_mpc(planner) + received = planner.accel_controller.update_kwargs + + assert received["previous_mpc_source"] == log.LongitudinalPlan.LongitudinalPlanSource.lead0 + assert received["planner_speed"] == 9.5 + assert received["planner_accel"] == -0.4 + assert received["radar_fresh"] is True + + +def test_controller_is_disabled_when_openpilot_longitudinal_control_is_unavailable(): + controller = AccelController(SimpleNamespace(longitudinalActuatorDelay=0.1, openpilotLongitudinalControl=False)) + controller.enabled = True + assert not controller.is_enabled + + +def test_radar_freshness_is_computed_once_and_shared_with_dec_and_controller(): + planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP) + planner._radar_log_mono_time = None + planner._radar_fresh_this_cycle = True + planner.events_sp = SimpleNamespace(clear=lambda: None) + dec_freshness = [] + planner.dec = SimpleNamespace(update=lambda _sm, *, radar_fresh, planner_accel: dec_freshness.append(radar_fresh)) + planner.e2e_alerts_helper = SimpleNamespace(update=lambda *_args: None) + planner.output_a_target = 0.0 + planner.output_v_target = 20.0 + planner.output_should_stop = False + planner.allow_throttle = True + planner.a_desired = 0.0 + planner.v_desired_filter = SimpleNamespace(x=10.0) + planner.mpc = SimpleNamespace(source=log.LongitudinalPlan.LongitudinalPlanSource.cruise, last_solution_status=0) + planner.is_e2e = lambda _sm: False + planner._run_mpc = lambda *_args, **_kwargs: None + planner.accel_controller = ControllerStub(target_speed=20.0, active=False) + + sm = PlannerSM(100) + for expected in (True, False): + planner.update(sm) + planner.update_mpc(sm, 20.0, True, ACCEL_MAX, False) + assert dec_freshness[-1] is expected and planner.accel_controller.update_kwargs["radar_fresh"] is expected + + sm.logMonoTime["radarState"] = 101 + planner.update(sm) + planner.update_mpc(sm, 20.0, True, ACCEL_MAX, False) + assert dec_freshness[-1] is True and planner.accel_controller.update_kwargs["radar_fresh"] is True + + +def test_accel_controller_status_publishes_minimal_fields(): + planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP) + planner.source = LongitudinalPlanSource.cruise + planner.output_v_target = 20.0 + planner.output_a_target = 0.0 + planner.events_sp = SimpleNamespace(to_msg=list) + planner.dec = SimpleNamespace(mode=lambda: "acc", enabled=lambda: False, active=lambda: False) + planner.accel_controller = ControllerStub(active=False, state=AccelControllerState.restrict) + planner.scc = SimpleNamespace( + vision=SimpleNamespace(state=0, output_v_target=20.0, output_a_target=0.0, current_lat_acc=0.0, max_pred_lat_acc=0.0, is_enabled=False, is_active=False), + map=SimpleNamespace(state=0, output_v_target=20.0, output_a_target=0.0, is_enabled=False, is_active=False), + ) + planner.resolver = SimpleNamespace( + speed_limit=0.0, speed_limit_last=0.0, speed_limit_final=0.0, speed_limit_final_last=0.0, + speed_limit_valid=False, speed_limit_last_valid=False, speed_limit_offset=0.0, distance=0.0, + source=custom.LongitudinalPlanSP.SpeedLimit.Source.none, + ) + planner.sla = SimpleNamespace( + state=custom.LongitudinalPlanSP.SpeedLimit.AssistState.disabled, is_enabled=False, is_active=False, + output_v_target=20.0, output_a_target=0.0, + ) + planner.e2e_alerts_helper = SimpleNamespace(green_light_alert=False, lead_depart_alert=False) + sent = {} + planner.publish_longitudinal_plan_sp( + SimpleNamespace(all_checks=lambda service_list: True), + SimpleNamespace(send=lambda service, message: sent.update({service: message})), + ) + + telemetry = sent["longitudinalPlanSP"].longitudinalPlanSP.accelController + assert telemetry.enabled and not telemetry.active + assert telemetry.profile == int(AccelProfile.normal) + assert telemetry.state == int(AccelControllerState.restrict) + assert set(custom.LongitudinalPlanSP.AccelController.schema.fields) == {"enabled", "active", "shadowOnlyDEPRECATED", "profile", "state"} diff --git a/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_longcontrol_vehicle_interfaces.py b/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_longcontrol_vehicle_interfaces.py new file mode 100644 index 0000000000..407a15dffe --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_longcontrol_vehicle_interfaces.py @@ -0,0 +1,83 @@ +import pytest + +from opendbc.car import DT_CTRL, gen_empty_fingerprint, structs +from opendbc.car.car_helpers import interfaces +from opendbc.car.ford.values import CAR as FORD +from opendbc.car.gm.values import CAR as GM +from opendbc.car.honda.values import CAR as HONDA +from opendbc.car.hyundai.values import CAR as HYUNDAI +from opendbc.car.toyota.values import CAR as TOYOTA +from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan +from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState, long_control_state_trans + + +VEHICLES = [ + pytest.param(TOYOTA.TOYOTA_RAV4_TSS2, (True, False, 0.0, -2.0, 0.25, 0.25, 0.3), id="toyota-rav4-tss2"), + pytest.param(HONDA.HONDA_ACCORD, (True, False, 0.0, -2.0, 0.5, 0.5, 0.8), id="honda-accord"), + pytest.param(GM.CHEVROLET_BOLT_EUV, (True, False, 0.0, -2.0, 0.25, 0.25, 2.0), id="gm-bolt-euv"), + pytest.param(HYUNDAI.HYUNDAI_SONATA, (True, True, 1.0, -2.0, 0.1, 0.5, 0.8), id="hyundai-sonata"), + pytest.param(FORD.FORD_ESCAPE_MK4, (True, False, 0.0, -2.0, 0.5, 0.5, 0.8), id="ford-escape"), +] + + +def get_car_params(candidate): + fingerprint = gen_empty_fingerprint() + interface = interfaces[candidate] + CP = interface.get_params(candidate, fingerprint, [], True, False, False) + CP_SP = interface.get_params_sp(CP, candidate, fingerprint, [], True, False, False) + return CP, CP_SP + + +@pytest.mark.parametrize(("candidate", "expected"), VEHICLES) +def test_real_vehicle_longcontrol_stop_and_start(candidate, expected): + CP, CP_SP = get_car_params(candidate) + expected_long, expected_starting, *expected_tuning = expected + + assert CP.openpilotLongitudinalControl is expected_long + assert CP.startingState is expected_starting + assert (CP.startAccel, CP.stopAccel, CP.vEgoStarting, CP.vEgoStopping, CP.stoppingDecelRate) == pytest.approx(expected_tuning) + + stop_speeds = [CP.vEgoStopping - 0.01] * 2 + drive_speeds = [CP.vEgoStopping + 0.01] * 2 + _, should_stop = get_accel_from_plan(stop_speeds, [0.0, 0.0], [0.0, 1.0], vEgoStopping=CP.vEgoStopping) + _, should_drive = get_accel_from_plan(drive_speeds, [0.0, 0.0], [0.0, 1.0], vEgoStopping=CP.vEgoStopping) + assert should_stop + assert not should_drive + + departure_state = long_control_state_trans( + CP, + CP_SP, + True, + LongCtrlState.stopping, + CP.vEgoStarting - 0.01, + should_drive, + brake_pressed=False, + cruise_standstill=False, + ) + assert departure_state == (LongCtrlState.starting if CP.startingState else LongCtrlState.pid) + assert ( + long_control_state_trans( + CP, + CP_SP, + True, + departure_state, + CP.vEgoStarting + 0.01, + should_drive, + brake_pressed=False, + cruise_standstill=False, + ) + == LongCtrlState.pid + ) + + CS = structs.CarState() + CS.vEgo = 0.0 + CS.aEgo = 0.0 + control = LongControl(CP, CP_SP) + + stopping_accel = control.update(True, CS, 0.0, should_stop, (-3.0, 2.0)) + assert control.long_control_state == LongCtrlState.stopping + assert stopping_accel == pytest.approx(-CP.stoppingDecelRate * DT_CTRL) + + departure_accel = control.update(True, CS, 0.0, should_drive, (-3.0, 2.0)) + assert control.long_control_state == departure_state + assert departure_accel == pytest.approx(CP.startAccel) diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_mpc_lib/__init__.py b/sunnypilot/selfdrive/controls/lib/longitudinal_mpc_lib/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/sunnypilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py new file mode 100644 index 0000000000..c0cff78b6a --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -0,0 +1,38 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +import numpy as np + +from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX + + +class LongitudinalMpcSP: + def __init__(self) -> None: + self._accel_max_trajectory: tuple[float, ...] | None = None + self._jerk_cost_multiplier = 1.0 + self.last_solution_status = 0 + + def set_accel_controller_params(self, accel_max: tuple[float, ...] | None, jerk_cost_multiplier: float) -> None: + self._accel_max_trajectory = accel_max + self._jerk_cost_multiplier = jerk_cost_multiplier + + def scale_jerk_cost(self, jerk_cost: float) -> float: + return jerk_cost * self._jerk_cost_multiplier + + def apply_accel_limits(self) -> None: + if self._accel_max_trajectory is None: + return + + accel_max = np.asarray(self._accel_max_trajectory) + if accel_max.shape != self.params[:, 1].shape or accel_max.dtype.kind not in "iuf" or not np.all(np.isfinite(accel_max)): + return + + self.params[:, 1] = np.clip(accel_max, 0.0, ACCEL_MAX) + self.params[0, 1] = max(self.params[0, 1], float(np.clip(self.x0[2], ACCEL_MIN, ACCEL_MAX))) + + def save_solution_status(self) -> None: + self.last_solution_status = self.solution_status diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 6efda4585f..df296dda54 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -5,10 +5,12 @@ This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ -from cereal import messaging, custom +from cereal import custom, messaging from opendbc.car import structs from openpilot.common.constants import CV -from openpilot.selfdrive.car.cruise import V_CRUISE_MAX +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl @@ -22,9 +24,10 @@ LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource class LongitudinalPlannerSP: - def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc): + def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc, dt: float = DT_MDL): + self.mpc = mpc + self.accel_controller = AccelController(CP, dt=dt) self.events_sp = EventsSP() - self.resolver = SpeedLimitResolver() self.dec = DynamicExperimentalController(CP, mpc) self.scc = SmartCruiseControl() self.resolver = SpeedLimitResolver() @@ -32,6 +35,8 @@ class LongitudinalPlannerSP: self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None self.source = LongitudinalPlanSource.cruise self.e2e_alerts_helper = E2EAlertsHelper() + self._radar_log_mono_time = None + self._radar_fresh_this_cycle = True self.output_v_target = 0. self.output_a_target = 0. @@ -43,6 +48,41 @@ class LongitudinalPlannerSP: return experimental_mode and self.dec.mode() == "blended" + def _run_mpc(self, sm: messaging.SubMaster, v_cruise: float, prev_accel_constraint: bool, accel_max=None, *, jerk_cost_multiplier: float = 1.0) -> None: + self.mpc.set_accel_controller_params(accel_max, jerk_cost_multiplier) + self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality) + self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) + self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality) + + def update_mpc(self, sm: messaging.SubMaster, v_cruise: float, prev_accel_constraint: bool, stock_accel_max: float, reset_state: bool) -> bool: + is_e2e = self.is_e2e(sm) + force_decel = sm['controlsState'].forceDecel + previous_mpc_failed = self.mpc.last_solution_status != 0 + if previous_mpc_failed: + self.accel_controller.reset() + + self.accel_controller.update( + sm['radarState'], base_speed=self.output_v_target, v_ego=sm['carState'].vEgo, a_ego=sm['carState'].aEgo, + follow_personality=sm['selfdriveState'].personality, acc_selected=not is_e2e and not previous_mpc_failed, + engaged=not reset_state and not force_decel, cruise_initialized=sm['carState'].vCruise != V_CRUISE_UNSET, + stock_accel_max=stock_accel_max if self.allow_throttle else 0.0, previous_should_stop=self.output_should_stop, + radar_fresh=self._radar_fresh_this_cycle, previous_mpc_source=self.mpc.source, planner_speed=self.v_desired_filter.x, + planner_accel=self.a_desired, + ) + controller = self.accel_controller + actuating = controller.is_active and not is_e2e and not force_decel and not previous_mpc_failed + valid_lead_stop_hold = actuating and controller.state == AccelControllerState.stopHold and controller.selected_lead >= 0 + controller_v_cruise = v_cruise if valid_lead_stop_hold else min(v_cruise, controller.output_v_target) if actuating else v_cruise + accel_max = controller.mpc_accel_max if actuating else None + jerk_cost_multiplier = controller.get_jerk_cost_multiplier( + actuating, prev_accel_constraint, v_cruise - controller_v_cruise, previous_mpc_failed, + ) + self._run_mpc(sm, controller_v_cruise, prev_accel_constraint, accel_max, jerk_cost_multiplier=jerk_cost_multiplier) + return is_e2e + + def update_should_stop(self, should_stop: bool) -> bool: + return self.accel_controller.update_should_stop(should_stop) + def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]: CS = sm['carState'] v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX) @@ -73,7 +113,17 @@ class LongitudinalPlannerSP: self.output_v_target, self.output_a_target = targets[self.source] return self.output_v_target, self.output_a_target + def _update_radar_freshness(self, sm: messaging.SubMaster) -> bool: + radar_log_mono_time = sm.logMonoTime['radarState'] + radar_healthy = sm.valid['radarState'] and sm.alive['radarState'] + radar_advanced = self._radar_log_mono_time is None or radar_log_mono_time > self._radar_log_mono_time + if radar_advanced: + self._radar_log_mono_time = radar_log_mono_time + return radar_healthy and radar_advanced + def update(self, sm: messaging.SubMaster) -> None: + self._radar_fresh_this_cycle = self._update_radar_freshness(sm) + self.accel_controller.update_params() self.events_sp.clear() self.dec.update(sm) self.e2e_alerts_helper.update(sm, self.events_sp) @@ -95,6 +145,12 @@ class LongitudinalPlannerSP: dec.enabled = self.dec.enabled() dec.active = self.dec.active() + accelController = longitudinalPlanSP.accelController + accelController.enabled = self.accel_controller.is_enabled + accelController.active = self.accel_controller.is_active + accelController.profile = self.accel_controller.profile + accelController.state = self.accel_controller.state + # Smart Cruise Control smartCruiseControl = longitudinalPlanSP.smartCruiseControl # Vision Control diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller_closed_loop.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller_closed_loop.py new file mode 100644 index 0000000000..979af3ea79 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller_closed_loop.py @@ -0,0 +1,82 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +import gc + +import numpy as np + +from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP as Plant +from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanSource +from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import _A_LAT_REG_MAX + + +def _run_constant_curve(*, scc_enabled: bool, cruise: float, duration: float = 70.) -> dict[str, np.ndarray]: + gc.collect() + curvature = 0.005 + plant = Plant(lead_relevancy=False, speed=30., actuator_delay=0.15, actuator_lag=0.20) + planner = plant.planner + planner.accel_controller.enabled = False + planner.accel_controller.update_params = lambda: None + planner.dec._enabled = False + planner.dec._read_params = lambda: None + planner.scc.map.enabled = False + planner.scc.map.update_params = lambda: None + planner.scc.vision.enabled = scc_enabled + planner.scc.vision._update_params = lambda: None + + if scc_enabled: + original_update_calculations = planner.scc.vision._update_calculations + + def inject_constant_curvature(sm): + velocities = np.asarray(sm['modelV2'].velocity.x, dtype=float) + sm['modelV2'].orientationRate.z = (curvature * velocities).tolist() + sm['controlsState'].curvature = curvature + original_update_calculations(sm) + + planner.scc.vision._update_calculations = inject_constant_curvature + + original_update = planner.update + + def enable_longitudinal(sm): + sm['carControl'].enabled = True + sm['carControl'].longActive = True + original_update(sm) + + planner.update = enable_longitudinal + rows = [] + while plant.current_time < duration: + output = plant.step(v_cruise=cruise) + rows.append(( + plant.current_time, output['speed'], planner.mpc.last_solution_status, output['should_stop'], + planner.scc.vision.is_active, planner.source == LongitudinalPlanSource.sccVision, + planner.scc.vision.output_v_target, + )) + + data = np.asarray(rows, dtype=float) + gc.collect() + return { + 'time': data[:, 0], 'speed': data[:, 1], 'solver_status': data[:, 2], 'should_stop': data[:, 3], + 'active': data[:, 4], 'scc_source': data[:, 5], 'target': data[:, 6], + } + + +def test_constant_curve_recovers_like_stock_speed_cap(): + target = (_A_LAT_REG_MAX / 0.005) ** 0.5 + scc = _run_constant_curve(scc_enabled=True, cruise=30.) + stock = _run_constant_curve(scc_enabled=False, cruise=target) + scc_final = scc['speed'][scc['time'] >= 60.] + stock_final = stock['speed'][stock['time'] >= 60.] + + assert not scc['solver_status'].any() + assert not stock['solver_status'].any() + assert not scc['should_stop'].any() + assert np.all(scc['active'][scc['time'] >= 60.]) + assert np.all(scc['scc_source'][scc['time'] >= 60.]) + assert np.allclose(scc['target'][scc['time'] >= 60.], target) + assert scc_final.min() >= target - 1. + assert abs(scc_final.mean() - stock_final.mean()) < 0.5 + assert abs(scc_final.min() - stock_final.min()) < 1. + assert abs(scc_final.max() - stock_final.max()) < 1. diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py new file mode 100644 index 0000000000..86d90600c5 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py @@ -0,0 +1,1301 @@ +from collections.abc import Callable +from dataclasses import dataclass +import gc +import math + +import numpy as np +import pytest + +from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource, STOP_DISTANCE, get_T_FOLLOW +from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel +from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, LeadObservation, PlantSP as Plant +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller import accel_controller as accel_controller_module +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelControllerState +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ( + MATCHED_SPEED_DECEL_RATE, MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, + MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE, MPC_DECEL_TREND_FRAMES, TARGET_SPEED_RESERVE, STOP_HOLD_EXIT_FRAMES, AccelProfile, +) + +ACTUATOR_DYNAMICS = ( + (0.10, 0.20), + (0.15, 0.25), + (0.20, 0.20), + (0.25, 0.30), + (0.30, 0.35), +) +ACTUATOR_IDS = ("toyota", "honda", "gm", "hyundai", "ford") +ROUTINE_GAP_TOLERANCE = 0.10 +ROUTINE_DECEL_TOLERANCE = 0.10 +DROPOUT_GAP_TOLERANCE = 0.15 +MOVING_LEAD_GAP_TOLERANCE = 0.12 + + +@dataclass +class ClosedLoopTrace: + time: np.ndarray + speed: np.ndarray + distance: np.ndarray + distance_lead: np.ndarray + a_target: np.ndarray + acceleration: np.ndarray + should_stop: np.ndarray + fcw: np.ndarray + source: list + dec_mode: list[str] + active: np.ndarray + launching: np.ndarray + departure_launching: np.ndarray + target_speed: np.ndarray + raw_cap: np.ndarray + selected_lead: np.ndarray + profile_accel_max: np.ndarray + accel_ceiling_active: np.ndarray + state: np.ndarray + required_decel: np.ndarray + planner_seed_accel: np.ndarray + mpc_seed_accel: np.ndarray + mpc_upper_first: np.ndarray + mpc_upper_min: np.ndarray + stock_bounds_valid: np.ndarray + raw_radar_passthrough: np.ndarray + solver_status: np.ndarray + mpc_calls: np.ndarray + solver_failures: int + solver_failure_times: list[float] + + +def _configure_plant(plant: Plant, *, enabled: bool, profile: int = 1, dec_enabled: bool = False) -> None: + plant.planner.accel_controller.enabled = enabled + plant.planner.accel_controller.profile = profile + plant.planner.accel_controller.update_params = lambda: None + plant.planner.dec._enabled = dec_enabled + plant.planner.dec._read_params = lambda: None + + +def _run( + *, + duration: float, + controller_enabled: bool, + profile: int = 1, + v_lead: float | Callable[[float], float] = 0.0, + v_cruise: float = 30.0, + dec_enabled: bool = False, + radar_fresh_fn: Callable[[int], bool] | None = None, + **plant_kwargs, +) -> ClosedLoopTrace: + gc.collect() + plant = Plant(**plant_kwargs) + _configure_plant(plant, enabled=controller_enabled, profile=profile, dec_enabled=dec_enabled) + plant.v_lead_prev = float(v_lead) if isinstance(v_lead, (int, float)) else float(v_lead(0.0)) + if radar_fresh_fn is not None: + radar_frame = 0 + + def patterned_radar_freshness(_sm): + nonlocal radar_frame + fresh = radar_fresh_fn(radar_frame) + radar_frame += 1 + return fresh + + plant.planner._update_radar_freshness = patterned_radar_freshness + + solver_failures = 0 + solver_failure_times = [] + mpc_call_count = 0 + controller_radar = None + radar_passthrough = [] + seed_calls = [] + original_controller_update = plant.planner.accel_controller.update + original_mpc_reset = plant.planner.mpc.reset + original_mpc_set_cur_state = plant.planner.mpc.set_cur_state + original_mpc_update = plant.planner.mpc.update + + def record_controller_radar(radar_state, *args, **kwargs): + nonlocal controller_radar + controller_radar = radar_state + return original_controller_update(radar_state, *args, **kwargs) + + def count_failed_solve(*args, **kwargs) -> None: + nonlocal solver_failures + if plant.planner.mpc.solution_status != 0: + solver_failures += 1 + solver_failure_times.append(plant.current_time) + original_mpc_reset(*args, **kwargs) + + def record_seed(v_ego, a_ego): + seed_calls.append((float(plant.planner.a_desired), float(a_ego))) + return original_mpc_set_cur_state(v_ego, a_ego) + + def count_mpc_call(radar_state, *args, **kwargs): + nonlocal mpc_call_count + mpc_call_count += 1 + radar_passthrough.append(radar_state is controller_radar) + return original_mpc_update(radar_state, *args, **kwargs) + + plant.planner.accel_controller.update = record_controller_radar + plant.planner.mpc.reset = count_failed_solve + plant.planner.mpc.set_cur_state = record_seed + plant.planner.mpc.update = count_mpc_call + rows = [] + sources = [] + dec_modes = [] + try: + while plant.current_time < duration: + lead_speed = float(v_lead) if isinstance(v_lead, (int, float)) else float(v_lead(plant.current_time)) + calls_before = mpc_call_count + radar_checks_before = len(radar_passthrough) + seed_calls_before = len(seed_calls) + result = plant.step(v_lead=lead_speed, v_cruise=v_cruise) + controller = plant.planner.accel_controller + calls_this_frame = mpc_call_count - calls_before + passthrough_this_frame = (len(radar_passthrough) > radar_checks_before + and all(radar_passthrough[radar_checks_before:])) + if len(seed_calls) > seed_calls_before: + planner_seed_accel, mpc_seed_accel = seed_calls[-1] + else: + planner_seed_accel = mpc_seed_accel = np.nan + lower = plant.planner.mpc.params[:, 0] + upper = plant.planner.mpc.params[:, 1] + lead = plant.planner.accel_controller._held_lead_plan + raw_cap = lead.cap if lead is not None else math.inf + profile_accel_max = (plant.planner.accel_controller.get_profile_accel_max( + controller.profile, result["published_v_ego"], + ) if controller.is_active else math.inf) + bounds_valid = (np.allclose(lower, ACCEL_MIN) and np.all(np.isfinite(upper)) + and np.all(upper >= lower) and np.all(upper <= ACCEL_MAX + 1e-9)) + rows.append(( + plant.current_time, result["speed"], result["distance"], result["distance_lead"], result["a_target"], + result["realized_acceleration"], result["should_stop"], result["fcw"], controller.is_active, + controller.launching, controller.departure_launching, controller.output_v_target, raw_cap, controller.selected_lead, + profile_accel_max, controller.mpc_accel_max is not None, controller.state, controller.required_decel, planner_seed_accel, + mpc_seed_accel, upper[0], np.min(upper), bounds_valid, passthrough_this_frame, + plant.planner.mpc.last_solution_status, calls_this_frame, + )) + sources.append(result["mpc_source"]) + dec_modes.append(result["dec_mode"]) + finally: + plant.planner.accel_controller.update = original_controller_update + plant.planner.mpc.reset = original_mpc_reset + plant.planner.mpc.set_cur_state = original_mpc_set_cur_state + plant.planner.mpc.update = original_mpc_update + + data = np.asarray(rows, dtype=float) + trace = ClosedLoopTrace( + time=data[:, 0], speed=data[:, 1], distance=data[:, 2], distance_lead=data[:, 3], a_target=data[:, 4], acceleration=data[:, 5], + should_stop=data[:, 6].astype(bool), fcw=data[:, 7].astype(bool), source=sources, dec_mode=dec_modes, + active=data[:, 8].astype(bool), launching=data[:, 9].astype(bool), departure_launching=data[:, 10].astype(bool), + target_speed=data[:, 11], raw_cap=data[:, 12], selected_lead=data[:, 13].astype(int), profile_accel_max=data[:, 14], + accel_ceiling_active=data[:, 15].astype(bool), state=data[:, 16].astype(int), required_decel=data[:, 17], planner_seed_accel=data[:, 18], + mpc_seed_accel=data[:, 19], mpc_upper_first=data[:, 20], mpc_upper_min=data[:, 21], + stock_bounds_valid=data[:, 22].astype(bool), raw_radar_passthrough=data[:, 23].astype(bool), + solver_status=data[:, 24].astype(int), mpc_calls=data[:, 25].astype(int), solver_failures=solver_failures, + solver_failure_times=solver_failure_times, + ) + gc.collect() + return trace + + +def _first_time_below(trace: ClosedLoopTrace, threshold: float) -> float: + indices = np.flatnonzero(trace.a_target <= threshold) + assert len(indices), f"never reached {threshold} m/s²" + return float(trace.time[indices[0]]) + + +def _sustained_time_below(trace: ClosedLoopTrace, threshold: float, *, after: float = 0.5, duration: float = 0.5) -> float: + required_frames = round(duration / DT_MDL) + below = (trace.time >= after) & (trace.a_target <= threshold) + sustained = np.convolve(below.astype(int), np.ones(required_frames, dtype=int), mode="valid") == required_frames + indices = np.flatnonzero(sustained) + assert len(indices), f"never sustained {threshold} m/s² for {duration} s" + return float(trace.time[indices[0]]) + + +def _command_jerk(trace: ClosedLoopTrace, after: float = 0.0) -> np.ndarray: + indices = np.flatnonzero(trace.time >= after) + assert len(indices) >= 2 + return np.diff(trace.a_target[indices]) / DT_MDL + + +def _filtered_realized_jerk(trace: ClosedLoopTrace, after: float = 1.0) -> np.ndarray: + filtered_acceleration = np.convolve(trace.acceleration, np.ones(3) / 3.0, mode="valid") + samples = trace.time[2:-1] >= after + return (np.diff(filtered_acceleration) / DT_MDL)[samples] + + +def _has_brake_coast_brake(values: np.ndarray, brake: float = -0.8, coast: float = -0.35, frames: int = 2) -> bool: + phase = 0 + for index in range(len(values) - frames + 1): + window = values[index:index + frames] + if np.all(window <= brake): + if phase == 2: + return True + phase = 1 + elif phase == 1 and np.all(window >= coast): + phase = 2 + return False + + +def _has_propulsion_after_braking(values: np.ndarray, propulsion: float = 0.2, brake: float = -0.2, frames: int = 2) -> bool: + braking = False + for index in range(len(values) - frames + 1): + window = values[index:index + frames] + if np.all(window <= brake): + braking = True + elif braking and np.all(window >= propulsion): + return True + return False + + +def _has_propulsion_brake_cycle(values: np.ndarray, propulsion: float = 0.2, brake: float = -0.2, frames: int = 2) -> bool: + phases = [] + for index in range(len(values) - frames + 1): + window = values[index:index + frames] + phase = 1 if np.all(window >= propulsion) else -1 if np.all(window <= brake) else 0 + if phase and (not phases or phase != phases[-1]): + phases.append(phase) + if len(phases) >= 3 and phases[-1] == phases[-3]: + return True + return False + + +def _assert_non_actuating_matches_stock(trace: ClosedLoopTrace, baseline: ClosedLoopTrace) -> None: + np.testing.assert_allclose(trace.a_target, baseline.a_target, atol=1e-6, rtol=0.0) + np.testing.assert_array_equal(trace.should_stop, baseline.should_stop) + np.testing.assert_array_equal(trace.fcw, baseline.fcw) + np.testing.assert_array_equal(trace.solver_status, baseline.solver_status) + assert trace.source == baseline.source + assert trace.solver_failures == baseline.solver_failures + assert trace.solver_failure_times == baseline.solver_failure_times + + +def _assert_no_new_solver_failures(trace: ClosedLoopTrace, baseline: ClosedLoopTrace) -> None: + assert trace.solver_failures <= baseline.solver_failures + + +@pytest.mark.parametrize( + "plant_kwargs", + [ + {"enabled": False, "lead_relevancy": True, "speed": 20.0, "distance_lead": 70.0}, + {"e2e": True, "lead_relevancy": False, "speed": 20.0}, + ], + ids=("disengaged", "e2e"), +) +def test_non_actuating_modes_match_stock(plant_kwargs): + common = dict(duration=2.0, v_lead=14.0, **plant_kwargs) + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + + _assert_non_actuating_matches_stock(trace, baseline) + assert not trace.active.any() + np.testing.assert_allclose(trace.mpc_upper_min, baseline.mpc_upper_min) + assert trace.raw_radar_passthrough.all() + assert np.all(trace.mpc_calls == 1) + + +def test_disabled_profiles_are_identical(): + common = dict(duration=2.0, controller_enabled=False, lead_relevancy=True, speed=20.0, distance_lead=70.0, v_lead=14.0) + traces = [_run(profile=profile, **common) for profile in range(3)] + for trace in traces[1:]: + _assert_non_actuating_matches_stock(trace, traces[0]) + + +@pytest.mark.parametrize("lead_relevancy", (False, True), ids=("clear-road", "lead")) +def test_force_decel_matches_stock(lead_relevancy): + common = dict(duration=2.0, force_decel=True, lead_relevancy=lead_relevancy, speed=20.0, + distance_lead=70.0, v_lead=14.0, profile=0) + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + _assert_non_actuating_matches_stock(trace, baseline) + assert not trace.active.any() + np.testing.assert_allclose(trace.mpc_upper_min, ACCEL_MAX) + + +@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport")) +def test_active_controller_uses_one_raw_mpc_solve_and_feasible_stock_bounds(profile): + trace = _run( + duration=4.0, controller_enabled=True, profile=profile, lead_relevancy=False, speed=0.0, + v_cruise=22.352, actuator_delay=0.15, actuator_lag=0.20, + ) + + assert trace.active.all() + assert np.all(trace.mpc_calls == 1) + assert trace.raw_radar_passthrough.all() + assert trace.stock_bounds_valid.all() + np.testing.assert_allclose(trace.mpc_seed_accel, trace.planner_seed_accel, atol=1e-12, rtol=0.0) + assert np.all(trace.mpc_upper_first + 1e-9 >= trace.mpc_seed_accel) + assert np.all(trace.mpc_upper_min >= 0.0) + assert np.any(trace.mpc_upper_min < ACCEL_MAX - 0.05) + assert trace.solver_failures == 0 + + +def test_e2e_to_radar_acc_handoff_keeps_braking_continuous(): + def run_handoff(controller_enabled: bool): + plant = Plant( + lead_relevancy=True, speed=10.0, distance_lead=30.0, actuator_delay=0.15, actuator_lag=0.20, + model_action_fn=lambda current_time, _v_ego, _a_ego: (-1.0 if current_time < 2.0 else 0.0, False), + ) + _configure_plant(plant, enabled=controller_enabled) + rows = [] + while plant.current_time < 2.4: + plant.e2e = plant.current_time < 2.0 + result = plant.step(v_lead=8.0, v_cruise=20.0) + rows.append((plant.current_time, result["a_target"], plant.planner.mpc.last_solution_status, + plant.planner.accel_controller.is_active)) + return np.asarray(rows, dtype=float).T + + baseline_time, baseline_accel, baseline_status, _ = run_handoff(False) + time_values, acceleration, solver_status, active = run_handoff(True) + np.testing.assert_allclose(time_values, baseline_time, atol=0.0, rtol=0.0) + transition = np.flatnonzero(time_values > 2.0)[0] + baseline_jump = abs(baseline_accel[transition] - baseline_accel[transition - 1]) + controlled_jump = abs(acceleration[transition] - acceleration[transition - 1]) + baseline_jerk = np.max(np.abs(np.diff(baseline_accel[transition:]) / DT_MDL)) + controlled_jerk = np.max(np.abs(np.diff(acceleration[transition:]) / DT_MDL)) + + assert controlled_jump <= baseline_jump + 1e-6 + assert controlled_jerk <= baseline_jerk + 0.10 + assert np.count_nonzero(solver_status[transition:]) <= np.count_nonzero(baseline_status[transition:]) + assert active[transition] + + +def test_clear_road_launch_is_prompt_and_profiles_separate_above_launch_speed(): + traces = [ + _run( + duration=12.0, controller_enabled=True, profile=profile, lead_relevancy=False, speed=0.0, + v_cruise=22.352, actuator_delay=0.15, actuator_lag=0.20, + ) + for profile in range(3) + ] + + for trace in traces: + positive = np.flatnonzero(trace.a_target > 0.05) + moving = np.flatnonzero(trace.speed > 0.01) + assert len(positive) and trace.time[positive[0]] <= 4 * DT_MDL + assert len(moving) and trace.time[moving[0]] <= 1.0 + assert np.interp(1.0, trace.time, trace.speed) >= 0.33 + assert not np.any(trace.a_target < -0.05) + assert trace.solver_failures == 0 + + launch_window = traces[0].time <= 0.5 + np.testing.assert_allclose(traces[1].a_target[launch_window], traces[0].a_target[launch_window], atol=0.10, rtol=0.0) + np.testing.assert_allclose(traces[2].a_target[launch_window], traces[0].a_target[launch_window], atol=0.10, rtol=0.0) + speed_at_eight = [float(np.interp(8.0, trace.time, trace.speed)) for trace in traces] + assert speed_at_eight[0] + 0.75 < speed_at_eight[1] + assert speed_at_eight[1] + 0.30 < speed_at_eight[2] + final_speed = [float(trace.speed[-1]) for trace in traces] + assert final_speed[0] + 1.25 < final_speed[1] + assert final_speed[1] + 0.75 < final_speed[2] + ceiling_at_ten = [float(np.interp(10.0, trace.speed, trace.mpc_upper_min)) for trace in traces] + assert ceiling_at_ten[0] < ceiling_at_ten[1] < ceiling_at_ten[2] + + +@pytest.mark.parametrize( + ("speed", "v_cruise"), + ((0.0, 22.352), (25.0, 30.0), (35.0, 35.0)), +) +def test_decel_smoothing_does_not_change_clear_road_acceleration_at_representative_speeds(monkeypatch, speed, v_cruise): + common = dict( + duration=3.0, controller_enabled=True, profile=AccelProfile.sport, lead_relevancy=False, + speed=speed, v_cruise=v_cruise, actuator_delay=0.15, actuator_lag=0.20, + ) + monkeypatch.setattr(accel_controller_module, "MPC_DECEL_JERK_COST_MULTIPLIER", 1.0) + stock_weight = _run(**common) + monkeypatch.setattr(accel_controller_module, "MPC_DECEL_JERK_COST_MULTIPLIER", MPC_DECEL_JERK_COST_MULTIPLIER) + smoothed = _run(**common) + + np.testing.assert_allclose(smoothed.a_target, stock_weight.a_target, atol=1e-9, rtol=0.0) + np.testing.assert_allclose(smoothed.speed, stock_weight.speed, atol=1e-9, rtol=0.0) + np.testing.assert_allclose(smoothed.mpc_upper_min, stock_weight.mpc_upper_min, atol=1e-9, rtol=0.0) + assert smoothed.solver_failures == stock_weight.solver_failures == 0 + + +def test_lead_bound_routine_decel_uses_smoothing_without_delaying_initial_braking(monkeypatch): + def lead_speed(current_time: float) -> float: + return max(22.5, 25.0 - 0.4 * current_time) + + common = dict( + duration=8.0, controller_enabled=True, profile=AccelProfile.eco, lead_relevancy=True, speed=29.0, + distance_lead=80.0, v_lead=lead_speed, v_cruise=33.528, actuator_delay=0.15, actuator_lag=0.20, + ) + monkeypatch.setattr(accel_controller_module, "MPC_DECEL_JERK_COST_MULTIPLIER", 1.0) + baseline = _run(**common) + monkeypatch.setattr(accel_controller_module, "MPC_DECEL_JERK_COST_MULTIPLIER", MPC_DECEL_JERK_COST_MULTIPLIER) + smoothed = _run(**common) + response = smoothed.time >= 0.5 + baseline_gap = baseline.distance_lead - baseline.distance + gap = smoothed.distance_lead - smoothed.distance + + assert set(np.asarray(smoothed.source)[response]) <= {LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1} + assert np.max(smoothed.required_decel[response]) < 0.80 + assert float(np.percentile(np.abs(_filtered_realized_jerk(smoothed)), 95)) < float(np.percentile(np.abs(_filtered_realized_jerk(baseline)), 95)) + assert float(np.percentile(np.abs(_command_jerk(smoothed, after=0.5)), 95)) < float(np.percentile(np.abs(_command_jerk(baseline, after=0.5)), 95)) + assert _first_time_below(smoothed, -0.2) <= _first_time_below(baseline, -0.2) + 1e-6 + assert _first_time_below(smoothed, -0.5) <= _first_time_below(baseline, -0.5) + 0.25 + 1e-6 + assert np.min(gap) >= np.min(baseline_gap) - 0.25 + assert np.max(np.abs(_command_jerk(smoothed, after=0.5))) < 3.0 + assert not _has_propulsion_brake_cycle(smoothed.a_target[response]) + assert not smoothed.fcw.any() + assert smoothed.solver_failures == 0 + + +def test_tightening_lead_releases_smoothing_before_late_catchup(monkeypatch): + event_time = 3.0 + lead_jerk = 1.02 + max_lead_decel = 2.22 + ramp_time = max_lead_decel / lead_jerk + + def lead_speed(current_time: float) -> float: + braking_time = max(current_time - event_time, 0.0) + ramp = min(braking_time, ramp_time) + return 16.9 - 0.5 * lead_jerk * ramp**2 - max_lead_decel * max(braking_time - ramp_time, 0.0) + + common = dict( + duration=7.0, profile=AccelProfile.eco, lead_relevancy=True, speed=15.9, distance_lead=35.7, + v_lead=lead_speed, v_cruise=17.4, actuator_delay=0.15, actuator_lag=0.20, + ) + stock = _run(controller_enabled=False, **common) + monkeypatch.setattr(accel_controller_module, "MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE", np.inf) + always_smoothed = _run(controller_enabled=True, **common) + monkeypatch.setattr(accel_controller_module, "MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE", MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE) + trace = _run(controller_enabled=True, **common) + response = trace.time >= event_time + response_jerk = trace.time[1:] >= event_time + required_decel_rate = (trace.required_decel[3:] - trace.required_decel[:-3]) / (3 * DT_MDL) + gap = trace.distance_lead - trace.distance + always_smoothed_gap = always_smoothed.distance_lead - always_smoothed.distance + + assert np.max(required_decel_rate[trace.required_decel[3:] >= 0.15]) > MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE + assert _first_time_below(trace, -0.5) <= _first_time_below(stock, -0.5) + 1e-9 + assert _first_time_below(trace, -0.5) <= _first_time_below(always_smoothed, -0.5) - DT_MDL + 1e-9 + assert np.max(np.abs(np.diff(trace.a_target)[response_jerk] / DT_MDL)) < 3.0 + assert not _has_brake_coast_brake(trace.a_target[response]) + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert np.min(gap) >= np.min(always_smoothed_gap) - 1e-6 + assert not stock.fcw.any() and not always_smoothed.fcw.any() and not trace.fcw.any() + assert stock.solver_failures == always_smoothed.solver_failures == trace.solver_failures == 0 + + +def test_same_slot_track_replacement_never_delays_tightening_lead_braking(): + event_time = 3.0 + lead_jerk = 1.02 + max_lead_decel = 2.22 + ramp_time = max_lead_decel / lead_jerk + + def lead_speed(current_time: float) -> float: + braking_time = max(current_time - event_time, 0.0) + ramp = min(braking_time, ramp_time) + return 16.9 - 0.5 * lead_jerk * ramp**2 - max_lead_decel * max(braking_time - ramp_time, 0.0) + + def observe(switch_time: float | None): + def observation(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo": + return None + track_id = 200 if switch_time is not None and current_time >= switch_time else 100 + return truth | {"radar": True, "radarTrackId": track_id} + return observation + + common = dict( + duration=8.0, controller_enabled=True, profile=AccelProfile.eco, lead_relevancy=True, speed=15.9, distance_lead=35.7, + v_lead=lead_speed, v_cruise=17.4, actuator_delay=0.15, actuator_lag=0.20, + ) + baseline = _run(lead_observation_fn=observe(None), **common) + history = MPC_DECEL_TREND_FRAMES - 1 + rate = np.full_like(baseline.required_decel, -math.inf) + rate[history:] = (baseline.required_decel[history:] - baseline.required_decel[:-history]) / (history * DT_MDL) + candidates = np.flatnonzero( + (baseline.time >= event_time) + & (baseline.state == int(AccelControllerState.restrict)) + & (baseline.required_decel > 0.0) + & (baseline.required_decel < MPC_DECEL_JERK_MAX_REQUIRED_DECEL) + & (rate > MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE) + & (baseline.a_target > -1.0) + ) + assert len(candidates) + switch_time = float(baseline.time[candidates[0]]) + switched = _run(lead_observation_fn=observe(switch_time), **common) + response = switched.time >= switch_time + baseline_gap = baseline.distance_lead - baseline.distance + switched_gap = switched.distance_lead - switched.distance + + def minimum_ttc(trace: ClosedLoopTrace, gap: np.ndarray) -> float: + closing_speed = trace.speed - np.asarray([lead_speed(current_time) for current_time in trace.time]) + closing = closing_speed > 0.1 + assert closing.any() + return float(np.min(gap[closing] / closing_speed[closing])) + + assert np.all(baseline.selected_lead[response] == 0) + assert np.all(switched.selected_lead[response] == 0) + for threshold in (-1.0, -2.0): + assert _first_time_below(switched, threshold) <= _first_time_below(baseline, threshold) + 1e-9 + assert np.min(switched_gap) >= np.min(baseline_gap) - 0.02 + assert minimum_ttc(switched, switched_gap) >= minimum_ttc(baseline, baseline_gap) - 0.02 + assert np.min(switched_gap) > 0.0 + assert not switched.fcw.any() + assert baseline.solver_failures == switched.solver_failures == 0 + + +def test_prius_route_model_launches_without_a_dead_pedal(): + trace = _run( + duration=3.0, controller_enabled=True, profile=1, lead_relevancy=False, speed=0.0, + v_cruise=22.352, actuator_model=PRIUS_TSS2_ROUTE_MODEL, + ) + positive = np.flatnonzero(trace.a_target > 0.05) + moving = np.flatnonzero(trace.speed > 0.05) + assert len(positive) and trace.time[positive[0]] <= 4 * DT_MDL + assert len(moving) and trace.time[moving[0]] <= 1.0 + assert trace.solver_failures == 0 + + +def test_stop_hold_survives_short_full_field_dropout(): + def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation | None: + return None if 1.0 <= current_time < 1.1 else truth + + common = dict( + duration=2.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=0.0, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, + ) + baseline = _run(**(common | {"controller_enabled": False})) + trace = _run(**common) + assert np.max(trace.speed) < 1e-3 + assert np.all(trace.target_speed == 0.0) + assert np.all(trace.state == int(AccelControllerState.stopHold)) + assert not trace.fcw.any() + _assert_no_new_solver_failures(trace, baseline) + + +@pytest.mark.parametrize("replacement_track_id", (100, 200), ids=("same-track", "replacement")) +def test_stop_hold_rejects_persistent_same_slot_range_step(replacement_track_id): + step_time = 1.0 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo": + return None + stepped = current_time >= step_time + speed = 0.2 if stepped else 0.0 + return truth | {"dRel": truth["dRel"] + 0.4 * stepped, "vLead": speed, "vLeadK": speed, "vRel": speed, + "aLeadK": 0.0, "radarTrackId": replacement_track_id if stepped else 100, "radar": True} + + trace = _run( + duration=2.5, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=0.0, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, + ) + + assert np.all(trace.state == int(AccelControllerState.stopHold)) + assert np.all(trace.target_speed == 0.0) + assert trace.should_stop.all() + assert np.max(trace.speed) < 1e-3 + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +def test_moving_departure_crossing_exit_speed_releases_once(): + departure_time = 1.0 + speeds = (0.81, 0.82, 0.83, 0.84, 0.79, 0.76, 0.74, 0.72) + + def lead_speed(current_time: float) -> float: + frame = round((current_time - departure_time) / DT_MDL) + return 0.0 if frame < 0 else speeds[min(frame, len(speeds) - 1)] + + def observe(_current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + return None if lead_name == "leadTwo" else truth | {"aLeadK": 0.0, "radarTrackId": 100, "radar": True} + + trace = _run( + duration=3.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=lead_speed, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, + ) + after_departure = trace.time >= departure_time + stop_hold = int(AccelControllerState.stopHold) + releases = np.flatnonzero((trace.state[:-1] == stop_hold) & (trace.state[1:] != stop_hold)) + 1 + + assert len(releases) == 1 + assert trace.launching[releases[0]] + assert not np.any(trace.state[releases[0]:] == stop_hold) + assert np.count_nonzero(np.diff(trace.should_stop[after_departure].astype(int))) == 1 + assert not _has_propulsion_brake_cycle(trace.a_target[after_departure]) + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +def test_route_51d_duplicate_lead_speed_pulse_cannot_release_stop_hold(): + pulse_start = 1.0 + departure_time = 2.0 + pulse_speeds = (0.1361, 0.1731, 0.2146, 0.2253, 0.2137, 0.1877) + pulse_distances = (6.0, 6.0, 6.0, 5.96, 6.04, 6.04) + + def lead_speed(current_time: float) -> float: + return 0.0 if current_time < departure_time else 2.0 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation: + result = truth | {"radar": True, "radarTrackId": 4887 if lead_name == "leadOne" else 4905} + pulse_frame = round((current_time - pulse_start) / DT_MDL) + if 0 <= pulse_frame < len(pulse_speeds): + speed = pulse_speeds[pulse_frame] if lead_name == "leadOne" else 0.0 + distance = pulse_distances[pulse_frame] if lead_name == "leadOne" else 6.08 + result |= {"dRel": distance, "vLead": speed, "vLeadK": speed, "vRel": speed, "aLeadK": 0.0} + return result + + trace = _run( + duration=3.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=lead_speed, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, + ) + pulse = (trace.time >= pulse_start) & (trace.time < pulse_start + len(pulse_speeds) * DT_MDL) + launched = np.flatnonzero((trace.time >= departure_time) & trace.launching) + + assert np.all(trace.state[pulse] == int(AccelControllerState.stopHold)) + assert np.all(trace.target_speed[pulse] == 0.0) + assert np.max(trace.speed[pulse]) < 0.01 + assert len(launched) and trace.time[launched[0]] <= departure_time + STOP_HOLD_EXIT_FRAMES * DT_MDL + 1e-9 + assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= departure_time]) + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +def test_route_520_slow_lead_pulse_cannot_release_stop_hold_or_dampen_real_departure(): + pulse_start = 1.0 + departure_time = 2.5 + pulse_speeds = (0.01, 0.03, 0.07, 0.10, 0.14, 0.20, 0.26, 0.32, 0.34, 0.33, 0.31, 0.28, 0.24, 0.20, 0.15, 0.09, 0.05, 0.01) + pulse_offsets = (0.00, 0.00, 0.00, 0.01, 0.01, 0.02, 0.03, 0.04, 0.06, 0.07, 0.09, 0.11, 0.12, 0.13, 0.14, 0.15, 0.15, 0.16) + + def lead_speed(current_time: float) -> float: + return 0.0 if current_time < departure_time else 2.0 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo": + return None + pulse_frame = round((current_time - pulse_start) / DT_MDL) + if 0 <= pulse_frame < len(pulse_speeds): + speed = pulse_speeds[pulse_frame] + return truth | {"dRel": 6.0 + pulse_offsets[pulse_frame], "vLead": speed, "vLeadK": speed, "vRel": speed, + "aLeadK": 0.0, "radarTrackId": 2133, "radar": True} + return truth | {"aLeadK": 0.0, "radarTrackId": 2133, "radar": True} + + common = dict( + duration=4.0, lead_relevancy=True, speed=0.0, distance_lead=6.0, v_lead=lead_speed, v_cruise=8.0, + lead_observation_fn=observe, actuator_model=PRIUS_TSS2_ROUTE_MODEL, + ) + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + pulse = (trace.time >= pulse_start) & (trace.time < pulse_start + len(pulse_speeds) * DT_MDL) + release = np.flatnonzero((trace.time >= departure_time) & trace.launching) + + assert np.all(trace.state[pulse] == int(AccelControllerState.stopHold)) + assert np.all(trace.target_speed[pulse] == 0.0) + assert np.max(trace.speed[trace.time < departure_time]) < 0.01 + assert len(release) and trace.time[release[0]] <= departure_time + STOP_HOLD_EXIT_FRAMES * DT_MDL + 1e-9 + assert trace.a_target[release[0]] > 0.05 + assert np.allclose(trace.a_target[release[0]:], baseline.a_target[release[0]:], atol=1e-5, rtol=0.0) + assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= departure_time]) + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +def test_stopped_lead_requires_four_departure_frames_and_launches_within_one_second(actuator_delay, actuator_lag): + departure_time = 1.0 + + def lead_speed(current_time: float) -> float: + return 0.0 if current_time < departure_time else 2.0 + + trace = _run( + duration=2.5, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=lead_speed, v_cruise=8.0, actuator_delay=actuator_delay, actuator_lag=actuator_lag, + ) + first_three = (trace.time > departure_time) & (trace.time <= departure_time + 3 * DT_MDL + 1e-9) + release = np.flatnonzero((trace.time >= departure_time) & trace.launching) + moving = np.flatnonzero((trace.time >= departure_time) & (trace.speed > 0.05)) + + assert not trace.launching[first_three].any() + assert trace.should_stop[first_three].all() + assert len(release) and trace.time[release[0]] >= departure_time + 3 * DT_MDL + assert not trace.should_stop[release[0]] + assert len(moving) and trace.time[moving[0]] <= departure_time + 3 * DT_MDL + 1.0 + assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= departure_time]) + assert trace.solver_failures == 0 + + +def test_stop_hold_departure_survives_radar_staleness(): + departure_time = 1.0 + dropout_start_time = 1.3 + dropout_len_frames = round(0.8 / DT_MDL) + dropout_start_frame = round(dropout_start_time / DT_MDL) + + def lead_speed(current_time: float) -> float: + return 0.0 if current_time < departure_time else 2.0 + + def radar_fresh_fn(frame: int) -> bool: + return not (dropout_start_frame <= frame < dropout_start_frame + dropout_len_frames) + + common = dict( + duration=4.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=lead_speed, v_cruise=8.0, actuator_delay=0.15, actuator_lag=0.25, + ) + baseline = _run(**common) + trace = _run(radar_fresh_fn=radar_fresh_fn, **common) + just_before_dropout = (trace.time >= dropout_start_time - 2 * DT_MDL) & (trace.time < dropout_start_time) + after_recovery = trace.time >= dropout_start_time + dropout_len_frames * DT_MDL + gap = trace.distance_lead - trace.distance + baseline_gap = baseline.distance_lead - baseline.distance + + assert trace.departure_launching[just_before_dropout].all() + assert not _has_propulsion_brake_cycle(trace.a_target) + assert not _has_brake_coast_brake(trace.a_target) + assert np.max(np.abs(np.diff(trace.a_target) / DT_MDL)) < 3.0 + assert np.min(gap) >= np.min(baseline_gap) - DROPOUT_GAP_TOLERANCE + assert not trace.fcw.any() + _assert_no_new_solver_failures(trace, baseline) + assert trace.solver_failures == 0 + assert trace.launching[after_recovery].any() + assert trace.time[np.flatnonzero(after_recovery & trace.launching)[0]] <= dropout_start_time + dropout_len_frames * DT_MDL + 1.0 + + +def test_confirmed_departure_full_field_dropout_does_not_worsen_stock_response(): + departure_time = 1.0 + dropout_start = 1.3 + dropout_end = 1.45 + dropped: list[tuple[int, str]] = [] + + def lead_speed(current_time: float) -> float: + return 0.0 if current_time < departure_time else 2.0 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if dropout_start <= current_time < dropout_end: + dropped.append((round(current_time / DT_MDL), lead_name)) + return None + return truth + + common = dict( + duration=4.0, lead_relevancy=True, speed=0.0, distance_lead=6.0, v_lead=lead_speed, + v_cruise=8.0, actuator_delay=0.15, actuator_lag=0.25, lead_observation_fn=observe, + ) + baseline = _run(controller_enabled=False, **common) + dropped.clear() + trace = _run(controller_enabled=True, **common) + before_dropout = (trace.time >= dropout_start - 2 * DT_MDL) & (trace.time < dropout_start) + dropout = (trace.time > dropout_start) & (trace.time <= dropout_end) + jerk_window = (trace.time[1:] > dropout_start) & (trace.time[1:] <= dropout_end + 0.25) + dropped_by_frame: dict[int, set[str]] = {} + for frame, lead_name in dropped: + dropped_by_frame.setdefault(frame, set()).add(lead_name) + + assert trace.departure_launching[before_dropout].all() + assert len(dropped_by_frame) == round((dropout_end - dropout_start) / DT_MDL) + assert all(lead_names == {"leadOne", "leadTwo"} for lead_names in dropped_by_frame.values()) + assert np.all(trace.selected_lead[dropout] == -1) and np.all(np.isinf(trace.raw_cap[dropout])) + assert not np.any(trace.state[dropout] == int(AccelControllerState.stopHold)) + assert not _has_propulsion_brake_cycle(trace.a_target) and not _has_brake_coast_brake(trace.a_target) + assert np.max(np.abs(np.diff(trace.a_target)[jerk_window] / DT_MDL)) <= np.max(np.abs(np.diff(baseline.a_target)[jerk_window] / DT_MDL)) + 0.01 + assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - DROPOUT_GAP_TOLERANCE + assert not trace.fcw.any() + np.testing.assert_array_equal(trace.solver_status, baseline.solver_status) + assert trace.solver_failure_times == baseline.solver_failure_times + + +def test_reused_radar_frames_do_not_pulse_stop_state_during_departure(): + departure_time = 1.0 + trace = _run( + duration=3.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=lambda current_time: 0.0 if current_time < departure_time else 2.0, + v_cruise=8.0, actuator_delay=0.15, actuator_lag=0.25, radar_fresh_fn=lambda frame: frame % 2 == 0, + ) + after_departure = trace.time >= departure_time + should_stop = trace.should_stop[after_departure] + release = np.flatnonzero(after_departure & trace.launching) + moving = np.flatnonzero(after_departure & (trace.speed > 0.05)) + + assert np.count_nonzero(np.diff(should_stop.astype(int))) <= 1 + assert len(release) and len(moving) + assert trace.time[moving[0]] <= trace.time[release[0]] + 1.0 + assert not _has_propulsion_brake_cycle(trace.a_target[after_departure]) + assert trace.solver_failures == 0 + + +@pytest.mark.parametrize("departure_frames", [1, 2, 3]) +def test_short_false_departure_does_not_launch_the_vehicle(departure_frames): + trace = _run( + duration=2.5, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=lambda current_time: 2.0 if 1.0 <= current_time < 1.0 + departure_frames * DT_MDL else 0.0, + v_cruise=8.0, actuator_delay=0.10, actuator_lag=0.20, + ) + + assert np.max(trace.speed) < 0.01 + assert not trace.launching.any() + assert trace.should_stop.all() + assert trace.state[-1] == int(AccelControllerState.stopHold) + assert trace.solver_failures == 0 + + +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +def test_matched_lead_recovery_preserves_profile_ordering(actuator_delay, actuator_lag): + traces = [ + _run( + duration=32.0, controller_enabled=True, profile=profile, lead_relevancy=True, speed=20.0, + distance_lead=100.0, v_lead=10.0, v_cruise=30.0, actuator_delay=actuator_delay, actuator_lag=actuator_lag, + ) + for profile in range(3) + ] + response = (traces[0].time >= 15.0) & (traces[0].time <= 28.5) + mean_accel = [float(np.mean(trace.a_target[response])) for trace in traces] + final_speed = [float(trace.speed[np.flatnonzero(response)[-1]]) for trace in traces] + + assert mean_accel[0] + 0.06 < mean_accel[1] + assert mean_accel[1] + 0.025 < mean_accel[2] + assert final_speed[0] < final_speed[1] < final_speed[2] + assert max(final_speed) < 13.5 + assert all(not _has_propulsion_brake_cycle(trace.a_target[response]) for trace in traces) + assert all(trace.solver_failures == 0 for trace in traces) + + +def test_creeping_lead_departure_is_prompt_and_safe(): + departure_time = 1.0 + + def lead_speed(current_time: float) -> float: + if current_time < departure_time: + return 0.0 + if current_time < departure_time + 0.5: + return 1.6 * (current_time - departure_time) + return min(2.5, 0.8 + 0.7 * (current_time - departure_time - 0.5)) + + def observe(_current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + return None if lead_name == "leadTwo" else truth | {"aLeadK": 0.0, "radarTrackId": 2133, "radar": True} + + common = dict( + duration=6.0, profile=0, lead_relevancy=True, speed=0.0, distance_lead=3.6, v_lead=lead_speed, + v_cruise=22.352, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, + ) + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + after_departure = trace.time >= departure_time + lead_speeds = np.array([lead_speed(max(0.0, current_time - DT_MDL)) for current_time in trace.time]) + baseline_moving = np.flatnonzero((baseline.time >= departure_time) & (baseline.speed > 0.05)) + moving = np.flatnonzero(after_departure & (trace.speed > 0.05)) + + assert len(baseline_moving) and len(moving) + assert trace.time[moving[0]] <= baseline.time[baseline_moving[0]] + assert np.all(trace.speed[after_departure] <= lead_speeds[after_departure] + 0.20) + assert not _has_brake_coast_brake(trace.a_target[after_departure]) + assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - 1e-3 + assert trace.solver_failures == 0 + + +def test_constant_creep_departure_does_not_pulse_between_launch_and_stop_hold(): + departure_time = 1.0 + + def lead_speed(current_time: float) -> float: + return 0.0 if current_time < departure_time else 0.2 + + def observe(_current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + return None if lead_name == "leadTwo" else truth | {"aLeadK": 0.0, "radarTrackId": 2133, "radar": True} + + trace = _run( + duration=8.0, controller_enabled=True, profile=0, lead_relevancy=True, speed=0.0, distance_lead=3.6, + v_lead=lead_speed, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, + ) + launched = np.flatnonzero((trace.time >= departure_time) & trace.launching) + assert len(launched) + assert trace.time[launched[0]] <= departure_time + 2.0 + after_launch = slice(launched[0], None) + + assert not np.any(trace.state[after_launch] == int(AccelControllerState.stopHold)) + assert not _has_propulsion_brake_cycle(trace.a_target[after_launch]) + assert np.max(trace.speed[after_launch]) <= 0.4 + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +def test_invalid_departure_geometry_aborts_launch_until_reconfirmed(): + def lead_speed(current_time: float) -> float: + return 0.0 if current_time < 1.0 else 2.0 + + def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation: + return truth | {"vLeadK": -2.0} if 1.45 <= current_time < 1.70 else truth + + trace = _run( + duration=2.5, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=lead_speed, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, + ) + invalid = (trace.time >= 1.50) & (trace.time < 1.75) + assert invalid.any() + assert not trace.launching[invalid].any() + assert np.max(trace.speed[invalid]) < 0.10 + assert np.max(trace.target_speed[invalid]) == 0.0 + assert np.all(np.isfinite(trace.a_target)) + assert trace.solver_failures == 0 + + +def test_moving_full_field_dropout_never_releases_speed_or_adds_solver_failures(): + dropout_start = 2.0 + dropout_end = 2.15 + + def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation | None: + return None if dropout_start <= current_time < dropout_end else truth + + common = dict( + duration=4.0, lead_relevancy=True, speed=22.0, distance_lead=85.0, v_lead=14.0, v_cruise=30.0, + lead_observation_fn=observe, actuator_delay=0.20, actuator_lag=0.25, + ) + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + before = trace.target_speed[np.flatnonzero(trace.time < dropout_start)[-1]] + response = (trace.time >= dropout_start) & (trace.time <= dropout_end + 0.5) + + assert np.max(trace.target_speed[response]) <= before + 1e-6 + assert not _has_propulsion_after_braking(trace.a_target[response]) + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - DROPOUT_GAP_TOLERANCE + _assert_no_new_solver_failures(trace, baseline) + + +def test_false_range_relief_matches_clean_controller_response(): + glitch_start = 3.0 + glitch_end = 3.15 + + def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation: + return truth | {"dRel": truth["dRel"] + 5.0} if glitch_start <= current_time < glitch_end else truth + + common = dict( + duration=4.0, lead_relevancy=True, speed=22.0, distance_lead=85.0, v_lead=14.0, + v_cruise=30.0, actuator_delay=0.20, actuator_lag=0.25, + ) + baseline = _run(controller_enabled=False, lead_observation_fn=observe, **common) + clean = _run(controller_enabled=True, **common) + trace = _run(controller_enabled=True, lead_observation_fn=observe, **common) + response = (trace.time >= glitch_start) & (trace.time <= glitch_end + 0.5) + jerk_response = (trace.time[1:] >= glitch_start) & (trace.time[1:] <= glitch_end + 0.5) + + assert np.max(np.abs(trace.a_target[response] - clean.a_target[response])) < 0.07 + assert np.max(np.abs(np.diff(trace.a_target)[jerk_response] / DT_MDL)) < 3.0 + assert not _has_propulsion_after_braking(trace.a_target[response]) + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + _assert_no_new_solver_failures(trace, baseline) + + +@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport")) +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +def test_route_507_braking_lead_slot_switch_has_no_false_relief_cycle(profile, actuator_delay, actuator_lag): + glitch_start = 67.0 + glitch_end = 67.5 + + def lead_speed(current_time: float) -> float: + braking_time = np.clip(current_time - 60.0, 0.0, 7.0) + return 10.0 - 0.42 * braking_time + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if glitch_start <= current_time < glitch_end: + if lead_name == "leadOne": + return None + return truth | { + "dRel": truth["dRel"] + 20.0, + "vLead": truth["vLead"] + 4.0, + "vLeadK": truth["vLeadK"] + 4.0, + "vRel": truth["vRel"] + 4.0, + "aLeadK": 0.0, + "radar": True, + "radarTrackId": 200, + } + if lead_name == "leadTwo": + return None + return truth | {"aLeadK": -0.42 if 60.0 <= current_time < glitch_start else 0.0, "radar": True, "radarTrackId": 100} + + common = dict( + duration=73.0, controller_enabled=True, profile=profile, lead_relevancy=True, speed=20.0, + distance_lead=100.0, v_lead=lead_speed, v_cruise=30.0, actuator_delay=actuator_delay, actuator_lag=actuator_lag, + ) + clean = _run(**common) + trace = _run(lead_observation_fn=observe, **common) + response = (trace.time >= 66.0) & (trace.time <= 72.0) + jerk_response = (trace.time[1:] >= 66.0) & (trace.time[1:] <= 72.0) + clean_gap = clean.distance_lead - clean.distance + gap = trace.distance_lead - trace.distance + + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert not _has_brake_coast_brake(trace.a_target[response]) + assert np.max(np.abs(np.diff(trace.a_target)[jerk_response] / DT_MDL)) < 3.0 + assert np.max(-np.diff(trace.target_speed)[jerk_response]) <= max(TARGET_SPEED_RESERVE, MATCHED_SPEED_DECEL_RATE * DT_MDL) + 1e-9 + assert np.min(gap[response]) >= np.min(clean_gap[response]) - DROPOUT_GAP_TOLERANCE + assert not trace.fcw.any() + assert trace.solver_failures == 0 + assert trace.raw_radar_passthrough.all() + assert np.all(trace.mpc_calls == 1) + + +@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport")) +def test_profile_ceiling_and_speed_stay_smooth_through_slot_switch_noise(profile): + glitch_start = 24.0 + glitch_end = 28.0 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if not glitch_start <= current_time < glitch_end: + return truth if lead_name == "leadOne" else None + + selected_slot = "leadOne" if round(current_time / DT_MDL) % 2 == 0 else "leadTwo" + if lead_name != selected_slot: + return None + sign = 1.0 if lead_name == "leadOne" else -1.0 + speed_offset = 0.25 * sign + return truth | { + "dRel": max(0.0, truth["dRel"] + 1.5 * sign), + "vLead": max(0.0, truth["vLead"] + speed_offset), + "vLeadK": max(0.0, truth["vLeadK"] + speed_offset), + "vRel": truth["vRel"] + speed_offset, + "aLeadK": 0.0, + "radar": True, + "radarTrackId": 100 if lead_name == "leadOne" else 200, + } + + common = dict( + duration=32.0, controller_enabled=True, profile=profile, lead_relevancy=True, speed=20.0, + distance_lead=100.0, v_lead=10.0, v_cruise=30.0, actuator_delay=0.15, actuator_lag=0.25, + ) + baseline = _run(**common) + trace = _run(lead_observation_fn=observe, **common) + glitch = (trace.time >= glitch_start) & (trace.time < glitch_end) + response = (trace.time >= glitch_start - 0.5) & (trace.time <= glitch_end + 1.0) + applied_accel_max = trace.mpc_upper_min[response] + selected_leads = trace.selected_lead[glitch] + controller_limited = trace.accel_ceiling_active[response] + stock_accel_max = np.asarray([get_max_accel(speed) for speed in trace.speed[response]]) + + assert set(selected_leads) == {0, 1} + assert np.count_nonzero(np.diff(selected_leads)) > 20 + assert controller_limited.any() + assert np.all(applied_accel_max[controller_limited] <= trace.profile_accel_max[response][controller_limited] + 1e-6) + assert np.all(applied_accel_max[controller_limited] <= stock_accel_max[controller_limited] + 1e-6) + assert np.max(trace.a_target[response]) > 0.2 + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert not _has_brake_coast_brake(trace.a_target[response]) + assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0 + assert trace.raw_radar_passthrough.all() + assert not trace.fcw.any() + _assert_no_new_solver_failures(trace, baseline) + + +@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport")) +def test_matched_lead_dropout_keeps_the_profile_acceleration_ceiling(profile): + dropout_start = 25.0 + dropout_end = 25.15 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo" or dropout_start <= current_time < dropout_end: + return None + return truth + + common = dict( + duration=28.0, controller_enabled=True, profile=profile, lead_relevancy=True, speed=20.0, + distance_lead=100.0, v_lead=10.0, v_cruise=30.0, actuator_delay=0.15, actuator_lag=0.25, + ) + clean = _run(**common) + trace = _run(lead_observation_fn=observe, **common) + dropout = (trace.time >= dropout_start) & (trace.time <= dropout_end) + response = (trace.time >= dropout_start - 0.5) & (trace.time <= dropout_end + 0.75) + + assert trace.accel_ceiling_active[dropout].all() + assert np.max(np.abs(trace.a_target[response] - clean.a_target[response])) < 0.08 + assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0 + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert trace.raw_radar_passthrough.all() + assert np.all(trace.mpc_calls == 1) + assert not trace.fcw.any() + _assert_no_new_solver_failures(trace, clean) + + +def test_low_speed_lead_stop_has_no_release_then_rebrake(): + def lead_speed(current_time: float) -> float: + return max(0.0, 1.9 - 1.16 * current_time) + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo": + return None + moving = lead_speed(current_time) > 0.0 + return truth | {"vLeadK": truth["vLeadK"] if moving else -0.01, "aLeadK": -1.16 if moving else 0.0, + "radarTrackId": 7, "radar": True} + + common = dict( + duration=6.0, profile=0, lead_relevancy=True, speed=4.5, distance_lead=18.0, v_lead=lead_speed, + v_cruise=23.056, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, + ) + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + stop_hold = trace.state == int(AccelControllerState.stopHold) + stopped_response = trace.time >= trace.time[np.flatnonzero(trace.speed < 1e-3)[0]] + + assert stop_hold.any() + assert np.max(trace.speed[stopped_response]) <= np.max(baseline.speed[stopped_response]) + 0.01 + assert np.max(trace.a_target[stopped_response]) <= np.max(baseline.a_target[stopped_response]) + 0.05 + assert not _has_brake_coast_brake(trace.a_target[trace.time >= 1.0]) + assert np.min(trace.a_target) >= np.min(baseline.a_target) - ROUTINE_GAP_TOLERANCE + assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - ROUTINE_GAP_TOLERANCE + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +def test_high_speed_stopped_lead_approach_holds_the_completed_stop(actuator_delay, actuator_lag): + trace = _run( + duration=14.0, controller_enabled=True, profile=1, lead_relevancy=True, speed=20.0, + distance_lead=130.0, v_lead=0.0, v_cruise=30.0, actuator_delay=actuator_delay, actuator_lag=actuator_lag, + ) + stopped = np.flatnonzero(trace.speed < 0.05) + assert len(stopped) + after_stop = slice(stopped[0], None) + gap = trace.distance_lead - trace.distance + + assert STOP_DISTANCE <= np.min(gap[after_stop]) <= 25.0 + assert np.max(trace.speed[after_stop]) < 0.10 + assert not _has_propulsion_brake_cycle(trace.a_target[after_stop]) + assert trace.state[-1] == int(AccelControllerState.stopHold) + assert trace.raw_radar_passthrough.all() + assert np.all(trace.mpc_calls == 1) + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +def test_decelerating_moving_lead_is_stock_safe_without_propulsion_reversal(): + def lead_speed(current_time: float) -> float: + if current_time < 2.0: + return 15.0 + progress = min((current_time - 2.0) / 6.0, 1.0) + return 15.0 - 5.0 * (3.0 * progress**2 - 2.0 * progress**3) + + common = dict( + duration=14.0, profile=1, lead_relevancy=True, speed=20.0, distance_lead=110.0, + v_lead=lead_speed, v_cruise=30.0, actuator_delay=0.20, actuator_lag=0.25, + ) + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + response = trace.time >= 1.0 + baseline_p95 = float(np.percentile(np.abs(_filtered_realized_jerk(baseline)), 95)) + trace_p95 = float(np.percentile(np.abs(_filtered_realized_jerk(trace)), 95)) + + assert not _has_brake_coast_brake(trace.a_target[response]) + assert not _has_propulsion_after_braking(trace.a_target[response]) + assert trace_p95 <= baseline_p95 + 0.02 + assert np.min(trace.acceleration) >= np.min(baseline.acceleration) - ROUTINE_DECEL_TOLERANCE + assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - MOVING_LEAD_GAP_TOLERANCE + assert not trace.fcw.any() + _assert_no_new_solver_failures(trace, baseline) + + +def test_severe_closing_never_delays_stock_braking_or_reduces_clearance(): + common = dict( + duration=12.0, lead_relevancy=True, speed=20.0, distance_lead=160.0, v_lead=3.5, + actuator_delay=0.20, actuator_lag=0.20, + ) + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + for threshold in (-1.0, -2.0): + assert _first_time_below(trace, threshold) <= _first_time_below(baseline, threshold) + 1e-9 + + baseline_gap = baseline.distance_lead - baseline.distance + controlled_gap = trace.distance_lead - trace.distance + baseline_closing = baseline.speed - 3.5 + controlled_closing = trace.speed - 3.5 + baseline_ttc = np.min(baseline_gap[baseline_closing > 0.1] / baseline_closing[baseline_closing > 0.1]) + controlled_ttc = np.min(controlled_gap[controlled_closing > 0.1] / controlled_closing[controlled_closing > 0.1]) + onset = (trace.time[1:] > 0.5) & (trace.time[1:] < 3.0) + + assert np.min(controlled_gap) >= np.min(baseline_gap) - 0.02 + assert controlled_ttc >= baseline_ttc - 0.02 + assert np.min(controlled_gap) > 0.0 + assert np.max(np.abs(np.diff(trace.a_target)[onset] / DT_MDL)) < 4.0 + assert trace.solver_failures == 0 + + +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +def test_far_lead_profiles_start_early_in_order_without_solver_failures(actuator_delay, actuator_lag): + common = dict( + duration=11.0, lead_relevancy=True, speed=25.0, distance_lead=200.0, v_lead=15.0, + actuator_delay=actuator_delay, actuator_lag=actuator_lag, + ) + baseline = _run(controller_enabled=False, **common) + traces = [_run(controller_enabled=True, profile=profile, **common) for profile in range(3)] + baseline_onset = _sustained_time_below(baseline, -0.10) + baseline_jerk_p95 = float(np.percentile(np.abs(_filtered_realized_jerk(baseline)), 95)) + required_improvement = max(0.002, 0.02 * baseline_jerk_p95) + onsets = [_sustained_time_below(trace, -0.10) for trace in traces] + + assert onsets[0] <= baseline_onset - 0.5 + 1e-9 + assert onsets[1] <= baseline_onset + 1e-9 + assert onsets[2] <= baseline_onset + 1e-9 + assert onsets[0] <= onsets[1] + DT_MDL + 1e-9 + assert onsets[1] <= onsets[2] + DT_MDL + 1e-9 + first_finite_caps = [trace.raw_cap[np.flatnonzero(np.isfinite(trace.raw_cap))[0]] for trace in traces] + assert first_finite_caps[0] < first_finite_caps[1] < first_finite_caps[2] + for trace in traces: + assert trace.acceleration.min() >= baseline.acceleration.min() - 0.1 + assert float(np.percentile(np.abs(_filtered_realized_jerk(trace)), 95)) <= baseline_jerk_p95 - required_improvement + assert np.max(np.abs(_command_jerk(trace, after=0.5))) < 1.0 + assert not _has_brake_coast_brake(trace.a_target[trace.time >= 1.0]) + assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= 1.0]) + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +def test_steady_slow_lead_has_no_gas_brake_cycle(): + duration = 60.0 + lead_speed = 10.0 + common = dict( + duration=duration, lead_relevancy=True, speed=20.0, distance_lead=100.0, v_lead=lead_speed, + v_cruise=30.0, actuator_delay=0.15, actuator_lag=0.25, + ) + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + response = trace.time >= 1.0 + settled = trace.time >= duration - 5.0 + desired_gap = STOP_DISTANCE + get_T_FOLLOW() * lead_speed + baseline_gap = baseline.distance_lead - baseline.distance + gap = trace.distance_lead - trace.distance + max_settled_gap = max(np.mean(baseline_gap[settled]) + 10.0, desired_gap + 30.0) + + assert np.mean(trace.speed[settled]) >= lead_speed - 2.0 + assert np.mean(gap[settled]) <= max_settled_gap + assert not _has_brake_coast_brake(trace.a_target[response]) + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert np.min(gap) >= desired_gap - 1.6 + assert np.min(trace.a_target) >= np.min(baseline.a_target) - ROUTINE_GAP_TOLERANCE + _assert_no_new_solver_failures(trace, baseline) + + +def test_matched_lead_slowdown_stays_smooth_without_a_second_braking_stage(): + slowdown_time = 70.0 + settled_lead_speed = 7.0 + + def lead_speed(current_time: float) -> float: + return 10.0 if current_time < slowdown_time else max(settled_lead_speed, 10.0 - 0.5 * (current_time - slowdown_time)) + + trace = _run( + duration=100.0, controller_enabled=True, profile=1, lead_relevancy=True, speed=20.0, + distance_lead=100.0, v_lead=lead_speed, v_cruise=30.0, actuator_delay=0.15, actuator_lag=0.25, + ) + matched = (trace.time >= slowdown_time - 5.0) & (trace.time < slowdown_time) + response = trace.time >= slowdown_time + settled = trace.time >= 95.0 + gap = trace.distance_lead - trace.distance + desired_gap = STOP_DISTANCE + get_T_FOLLOW() * settled_lead_speed + + assert abs(np.mean(trace.speed[matched]) - 10.0) < 0.5 + assert trace.accel_ceiling_active[matched].all() + np.testing.assert_allclose(trace.mpc_upper_min[matched], trace.profile_accel_max[matched], atol=1e-6) + assert not np.any(trace.state[response] == int(AccelControllerState.stopHold)) + assert not trace.launching[response].any() + assert not _has_brake_coast_brake(trace.a_target[response]) + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0 + assert float(np.percentile(np.abs(_filtered_realized_jerk(trace, after=slowdown_time)), 95)) < 0.30 + assert np.min(gap) > STOP_DISTANCE + assert np.mean(trace.speed[settled]) >= settled_lead_speed - 1.5 + assert np.mean(gap[settled]) <= desired_gap + 20.0 + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +def test_acceleration_output_remains_inside_stock_limits(): + trace = _run( + duration=12.0, controller_enabled=True, profile=AccelProfile.sport, lead_relevancy=False, + speed=0.0, v_cruise=30.0, actuator_delay=0.15, actuator_lag=0.20, + ) + stock_max = np.asarray([get_max_accel(speed) for speed in trace.speed]) + assert np.all(trace.a_target >= ACCEL_MIN - 1e-9) + assert np.all(trace.a_target <= stock_max + 0.06) + assert trace.stock_bounds_valid.all() + assert trace.solver_failures == 0 diff --git a/sunnypilot/selfdrive/test/__init__.py b/sunnypilot/selfdrive/test/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/test/longitudinal_maneuvers/__init__.py b/sunnypilot/selfdrive/test/longitudinal_maneuvers/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py b/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py new file mode 100644 index 0000000000..a92c5a2110 --- /dev/null +++ b/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py @@ -0,0 +1,402 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +from collections import deque +from collections.abc import Callable +from dataclasses import dataclass +import math +import time +from typing import Any + +import numpy as np + +from cereal import log +import cereal.messaging as messaging +from openpilot.common.realtime import DT_MDL, Ratekeeper +from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState +from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner +from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU +from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant, PlannerSM + + +LeadObservation = dict[str, Any] +LeadObservationFn = Callable[[float, str, LeadObservation], LeadObservation | None] +ModelActionFn = Callable[[float, float, float], tuple[float, bool]] +EgoObservationFn = Callable[[float, float, float], tuple[float, float]] + + +@dataclass(frozen=True) +class ActuatorModel: + planner_delay: float + transport_delay: float + actuator_lag: float + command_rate_limit: float + stopping_acceleration: float + standstill_breakaway_acceleration: float + standstill_breakaway_time: float + + def __post_init__(self): + nonnegative_fields = { + "planner_delay": self.planner_delay, + "transport_delay": self.transport_delay, + "actuator_lag": self.actuator_lag, + "standstill_breakaway_acceleration": self.standstill_breakaway_acceleration, + "standstill_breakaway_time": self.standstill_breakaway_time, + } + if any(not math.isfinite(value) or value < 0.0 for value in nonnegative_fields.values()): + raise ValueError(f"ActuatorModel fields must be finite and non-negative: {nonnegative_fields}") + if not math.isfinite(self.command_rate_limit) or self.command_rate_limit <= 0.0: + raise ValueError("command_rate_limit must be finite and positive") + if not math.isfinite(self.stopping_acceleration) or self.stopping_acceleration > 0.0: + raise ValueError("stopping_acceleration must be finite and non-positive") + + +# Conservative Prius TSS2 actuator model. +PRIUS_TSS2_ROUTE_MODEL = ActuatorModel( + planner_delay=0.05, + transport_delay=0.0, + actuator_lag=0.20, + command_rate_limit=4.0, + stopping_acceleration=-2.0, + standstill_breakaway_acceleration=1.0, + standstill_breakaway_time=0.05, +) + + +class PlantSP(Plant): + """Closed-loop plant with configurable observations and actuator response.""" + + def __init__( + self, + lead_relevancy=False, + speed=0.0, + distance_lead=2.0, + enabled=True, + only_lead2=False, + only_radar=False, + e2e=False, + personality=0, + force_decel=False, + lead_observation_fn: LeadObservationFn | None = None, + model_action_fn: ModelActionFn | None = None, + ego_observation_fn: EgoObservationFn | None = None, + actuator_delay: float | None = None, + actuator_lag: float = 0.0, + actuator_model: ActuatorModel | None = None, + ): + if actuator_delay is not None and (not math.isfinite(actuator_delay) or actuator_delay < 0.0): + raise ValueError("actuator_delay must be finite and non-negative") + if not math.isfinite(actuator_lag) or actuator_lag < 0.0: + raise ValueError("actuator_lag must be finite and non-negative") + + self.rate = 1.0 / DT_MDL + + if not Plant.messaging_initialized: + Plant.radar = messaging.pub_sock('radarState') + Plant.controls_state = messaging.pub_sock('controlsState') + Plant.selfdrive_state = messaging.pub_sock('selfdriveState') + Plant.car_state = messaging.pub_sock('carState') + Plant.plan = messaging.sub_sock('longitudinalPlan') + Plant.messaging_initialized = True + + self.v_lead_prev = 0.0 + + self.distance = 0.0 + self.speed = speed + self.should_stop = False + self.acceleration = 0.0 + self.a_target = 0.0 + self.actuator_command = 0.0 + self.applied_actuator_command = 0.0 + self.breakaway_confirmed = False + self._breakaway_timer = 0.0 + + # lead car + self.lead_relevancy = lead_relevancy + self.distance_lead = distance_lead + self.enabled = enabled + self.only_lead2 = only_lead2 + self.only_radar = only_radar + self.e2e = e2e + self.personality = personality + self.force_decel = force_decel + self.lead_observation_fn = lead_observation_fn + self.model_action_fn = model_action_fn + self.ego_observation_fn = ego_observation_fn + self.actuator_model = actuator_model + self.actuator_delay = actuator_model.planner_delay if actuator_model is not None else actuator_delay + self.transport_delay = actuator_model.transport_delay if actuator_model is not None else actuator_delay + self.actuator_lag = actuator_model.actuator_lag if actuator_model is not None else actuator_lag + self.publish_realized_a_ego = any((lead_observation_fn is not None, model_action_fn is not None, ego_observation_fn is not None, + actuator_delay is not None, actuator_lag > 0.0, actuator_model is not None)) + + self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0) + self.ts = 1.0 / self.rate + time.sleep(0.1) + self.sm = messaging.SubMaster(['longitudinalPlan']) + + from opendbc.car.honda.values import CAR + from opendbc.car.honda.interface import CarInterface + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + if self.actuator_delay is not None: + CP.longitudinalActuatorDelay = self.actuator_delay + CP_SP = CarInterface.get_non_essential_params_sp(CP, CAR.HONDA_CIVIC) + self.planner = LongitudinalPlanner(CP, CP_SP, init_v=self.speed) + + if self.actuator_model is not None and self.speed >= 0.01: + self.breakaway_confirmed = True + delay_steps = 0 if self.transport_delay is None else round(self.transport_delay / self.ts) + self._actuator_delay_queue = deque([self.acceleration] * delay_steps) + + @staticmethod + def _lead_message(observation: LeadObservation): + lead = log.RadarState.LeadData.new_message() + for field, value in observation.items(): + setattr(lead, field, value) + return lead + + def _observe_lead(self, lead_name: str, truth: LeadObservation, present_by_default: bool) -> LeadObservation | None: + if self.lead_observation_fn is None: + return dict(truth) if present_by_default else None + + observed = self.lead_observation_fn(self.current_time, lead_name, dict(truth)) + if observed is None: + return None + + complete_observation = dict(truth) + complete_observation.update(observed) + return complete_observation + + def _update_actuator(self, command: float) -> tuple[float, float]: + if self._actuator_delay_queue: + self._actuator_delay_queue.append(command) + delayed_command = self._actuator_delay_queue.popleft() + else: + delayed_command = command + + if self.actuator_model is not None: + max_command_delta = self.actuator_model.command_rate_limit * self.ts + self.applied_actuator_command = float(np.clip(delayed_command, + self.applied_actuator_command - max_command_delta, + self.applied_actuator_command + max_command_delta)) + + if self.speed < 0.01: + if self.applied_actuator_command <= 0.0: + self.breakaway_confirmed = False + self._breakaway_timer = 0.0 + elif not self.breakaway_confirmed: + breakaway_ready = self.applied_actuator_command + 1e-9 >= self.actuator_model.standstill_breakaway_acceleration + if breakaway_ready: + self._breakaway_timer += self.ts + else: + self._breakaway_timer = 0.0 + + self.breakaway_confirmed = breakaway_ready and self._breakaway_timer + 1e-9 >= self.actuator_model.standstill_breakaway_time + if not self.breakaway_confirmed: + self.acceleration = 0.0 + return delayed_command, self.acceleration + else: + self.breakaway_confirmed = True + + response_command = self.applied_actuator_command + else: + self.applied_actuator_command = delayed_command + response_command = delayed_command + + if self.actuator_lag > 0.0: + alpha = 1.0 - math.exp(-self.ts / self.actuator_lag) + self.acceleration += alpha * (response_command - self.acceleration) + else: + self.acceleration = response_command + return delayed_command, self.acceleration + + def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50.0, pitch=0.0, prob_throttle=1.0): + # ******** publish a fake model going straight and fake calibration ******** + # note that this is worst case for MPC, since model will delay long mpc by one time step + radar = messaging.new_message('radarState') + control = messaging.new_message('controlsState') + ss = messaging.new_message('selfdriveState') + car_state = messaging.new_message('carState') + lp = messaging.new_message('liveParameters') + car_control = messaging.new_message('carControl') + model = messaging.new_message('modelV2') + car_state_sp = messaging.new_message('carStateSP') + live_map_data_sp = messaging.new_message('liveMapDataSP') + gps_data = messaging.new_message('gpsLocation') + a_lead = (v_lead - self.v_lead_prev) / self.ts + self.v_lead_prev = v_lead + + if self.lead_relevancy: + d_rel = np.maximum(0.0, self.distance_lead - self.distance) + v_rel = v_lead - self.speed + if self.only_radar: + status = True + elif prob_lead > 0.5: + status = True + else: + status = False + else: + d_rel = 200.0 + v_rel = 0.0 + prob_lead = 0.0 + status = False + + truth_lead: LeadObservation = { + "dRel": float(d_rel), + "yRel": 0.0, + "vRel": float(v_rel), + "aRel": float(a_lead - self.acceleration), + "vLead": float(v_lead), + "dPath": 0.0, + "vLat": 0.0, + "vLeadK": float(v_lead), + "aLeadK": float(a_lead), + "fcw": False, + "status": bool(status), + # TODO use real radard logic for this + "aLeadTau": float(_LEAD_ACCEL_TAU), + "modelProb": float(prob_lead), + "radar": bool(self.only_radar), + "radarTrackId": -1, + } + lead_one_observation = self._observe_lead("leadOne", truth_lead, not self.only_lead2) + lead_two_observation = self._observe_lead("leadTwo", truth_lead, True) + if lead_one_observation is not None: + radar.radarState.leadOne = self._lead_message(lead_one_observation) + if lead_two_observation is not None: + radar.radarState.leadTwo = self._lead_message(lead_two_observation) + + # Simulate model predicting slightly faster speed + # this is to ensure lead policy is effective when model + # does not predict slowdown in e2e mode + position = log.XYZTData.new_message() + position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)] + model.modelV2.position = position + if self.model_action_fn is None: + model_acceleration, model_should_stop = self.acceleration + 0.1, False + else: + model_acceleration, model_should_stop = self.model_action_fn(self.current_time, self.speed, self.acceleration) + model.modelV2.action.desiredAcceleration = float(model_acceleration) + model.modelV2.action.shouldStop = bool(model_should_stop) + velocity = log.XYZTData.new_message() + velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)] + velocity.x[0] = float(self.speed) # always start at current speed + model.modelV2.velocity = velocity + acceleration = log.XYZTData.new_message() + acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)] + model.modelV2.acceleration = acceleration + model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)] + + control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off + ss.selfdriveState.experimentalMode = self.e2e + ss.selfdriveState.personality = self.personality + control.controlsState.forceDecel = self.force_decel + true_v_ego = self.speed + true_a_ego = self.acceleration + published_v_ego = true_v_ego + published_a_ego = true_a_ego if self.publish_realized_a_ego else 0.0 + if self.ego_observation_fn is not None: + published_v_ego, published_a_ego = self.ego_observation_fn(self.current_time, true_v_ego, true_a_ego) + car_state.carState.vEgo = float(published_v_ego) + car_state.carState.aEgo = float(published_a_ego) + car_state.carState.standstill = bool(self.speed < 0.01) + car_state.carState.vCruise = float(v_cruise * 3.6) + car_control.carControl.orientationNED = [0.0, float(pitch), 0.0] + + # ******** get controlsState messages for plotting *** + sm = PlannerSM(self.rk.frame, { + 'radarState': radar.radarState, + 'carState': car_state.carState, + 'carControl': car_control.carControl, + 'controlsState': control.controlsState, + 'selfdriveState': ss.selfdriveState, + 'liveParameters': lp.liveParameters, + 'modelV2': model.modelV2, + 'carStateSP': car_state_sp.carStateSP, + 'liveMapDataSP': live_map_data_sp.liveMapDataSP, + 'gpsLocation': gps_data.gpsLocation, + }) + self.planner.update(sm) + self.a_target = self.planner.output_a_target + self.actuator_command = self.a_target + if self.planner.output_should_stop: + stopping_acceleration = -0.5 if self.actuator_model is None else self.actuator_model.stopping_acceleration + self.actuator_command = min(stopping_acceleration, self.actuator_command) + delayed_actuator_command, _ = self._update_actuator(self.actuator_command) + self.speed = self.speed + self.acceleration * self.ts + self.should_stop = self.planner.output_should_stop + fcw = self.planner.fcw + self.distance_lead = self.distance_lead + v_lead * self.ts + + # ******** run the car ******** + # print(self.distance, speed) + if self.speed <= 0: + self.speed = 0 + self.acceleration = 0 + self.distance = self.distance + self.speed * self.ts + + # *** radar model *** + if self.lead_relevancy: + d_rel = np.maximum(0.0, self.distance_lead - self.distance) + v_rel = v_lead - self.speed + else: + d_rel = 200.0 + v_rel = 0.0 + + # print at 5hz + # if (self.rk.frame % (self.rate // 5)) == 0: + # print("%2.2f sec %6.2f m %6.2f m/s %6.2f m/s2 lead_rel: %6.2f m %6.2f m/s" + # % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel)) + + # ******** update prevs ******** + self.rk.monitor_time() + + accel_controller = self.planner.accel_controller + lead_plan = accel_controller._held_lead_plan + target_state = accel_controller.target_state + return { + "distance": self.distance, + "speed": self.speed, + "acceleration": self.acceleration, + "realized_acceleration": self.acceleration, + "a_target": self.a_target, + "planner_acceleration": self.a_target, + "actuator_command": self.actuator_command, + "stop_clamped_actuator_command": self.actuator_command, + "delayed_actuator_command": delayed_actuator_command, + "applied_actuator_command": self.applied_actuator_command, + "vehicle_actuator_command": self.applied_actuator_command, + "true_v_ego": true_v_ego, + "true_a_ego": true_a_ego, + "published_a_ego": published_a_ego, + "published_v_ego": published_v_ego, + "observed_a_ego": published_a_ego, + "observed_v_ego": published_v_ego, + "planner_delay": self.actuator_delay, + "transport_delay": self.transport_delay, + "breakaway_confirmed": self.breakaway_confirmed, + "breakaway_time": self._breakaway_timer, + "should_stop": self.should_stop, + "distance_lead": self.distance_lead, + "fcw": fcw, + "mpc_source": self.planner.mpc.source, + "dec_mode": self.planner.dec.mode(), + "controller_target": accel_controller.output_v_target, + "base_target": self.planner.output_v_target, + "raw_energy_cap": lead_plan.cap if lead_plan is not None else math.inf, + "live_filtered_cap": target_state.filtered_cap, + "accel_controller_selected_lead": accel_controller.selected_lead, + "model_action": { + "desiredAcceleration": float(model_acceleration), + "shouldStop": bool(model_should_stop), + }, + "truth_lead": dict(truth_lead), + "lead_one_observation": None if lead_one_observation is None else dict(lead_one_observation), + "lead_two_observation": None if lead_two_observation is None else dict(lead_two_observation), + } diff --git a/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/__init__.py b/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py b/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py new file mode 100644 index 0000000000..c002dda313 --- /dev/null +++ b/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py @@ -0,0 +1,154 @@ +from collections.abc import Callable +import math + +import pytest + +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant +from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP + +STOCK_STEP_KEYS = ("distance", "speed", "acceleration", "should_stop", "distance_lead", "fcw") + + +def departing_lead(current_time: float) -> float: + return 0.0 if current_time < 1.0 else min(2.0, 2.0 * (current_time - 1.0)) + + +PARITY_SCENARIOS = { + "approach_stopped_lead": dict(lead_relevancy=True, speed=15.0, distance_lead=60.0, v_cruise=20.0, v_lead=0.0, steps=80), + "stop_then_depart": dict(lead_relevancy=True, speed=0.0, distance_lead=6.0, v_cruise=8.0, v_lead=departing_lead, steps=120), +} + + +def _drive(cls, *, v_cruise: float, v_lead: float | Callable[[float], float], steps: int, **kwargs): + plant = cls(**kwargs) + plant.v_lead_prev = float(v_lead(0.0)) if callable(v_lead) else float(v_lead) + solver_failures = 0 + original_reset = plant.planner.mpc.reset + + def counting_reset(*args, **kw): + nonlocal solver_failures + if plant.planner.mpc.solution_status != 0: + solver_failures += 1 + return original_reset(*args, **kw) + + plant.planner.mpc.reset = counting_reset + results = [] + for _ in range(steps): + lead_speed = float(v_lead(plant.current_time)) if callable(v_lead) else v_lead + result = plant.step(v_lead=lead_speed, v_cruise=v_cruise) + results.append((result, plant.planner.mpc.source, plant.planner.output_a_target)) + return results, solver_failures + + +@pytest.mark.parametrize("scenario", PARITY_SCENARIOS, ids=list(PARITY_SCENARIOS)) +def test_plant_sp_matches_stock_plant_on_shared_kwargs(scenario): + kwargs = dict(PARITY_SCENARIOS[scenario]) + v_cruise, v_lead, steps = kwargs.pop("v_cruise"), kwargs.pop("v_lead"), kwargs.pop("steps") + + stock_results, stock_failures = _drive(Plant, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs) + sp_results, sp_failures = _drive(PlantSP, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs) + + assert stock_failures == 0, f"stock Plant solver failed {stock_failures} times in {scenario!r}" + assert sp_failures == 0, f"PlantSP solver failed {sp_failures} times in {scenario!r}" + + for frame, ((stock_result, stock_source, stock_a_target), (sp_result, sp_source, sp_a_target)) in enumerate( + zip(stock_results, sp_results, strict=True), + ): + for key in STOCK_STEP_KEYS: + if isinstance(stock_result[key], float): + assert sp_result[key] == pytest.approx(stock_result[key]), f"{scenario} frame {frame} key {key}" + else: + assert sp_result[key] == stock_result[key], f"{scenario} frame {frame} key {key}" + assert sp_source == stock_source, f"{scenario} frame {frame} mpc.source" + assert sp_a_target == pytest.approx(stock_a_target), f"{scenario} frame {frame} output_a_target" + + if scenario == "stop_then_depart": + departure_frame = round(1.0 / DT_MDL) + for results in (stock_results, sp_results): + assert all(result["speed"] < 0.01 for result, _, _ in results[:departure_frame]) + assert results[departure_frame - 1][0]["should_stop"] + assert any(not result["should_stop"] for result, _, _ in results[departure_frame:]) + assert any(result["speed"] > 0.05 for result, _, _ in results[departure_frame:]) + stock_release = next(frame for frame, (result, _, _) in enumerate(stock_results) if frame >= departure_frame and not result["should_stop"]) + sp_release = next(frame for frame, (result, _, _) in enumerate(sp_results) if frame >= departure_frame and not result["should_stop"]) + stock_motion = next(frame for frame, (result, _, _) in enumerate(stock_results) if frame >= departure_frame and result["speed"] > 0.05) + sp_motion = next(frame for frame, (result, _, _) in enumerate(sp_results) if frame >= departure_frame and result["speed"] > 0.05) + assert sp_release == stock_release + assert sp_motion == stock_motion + + +def test_full_lead_observation_is_independent_from_truth(): + callback_inputs = [] + + def observe_lead(current_time, lead_name, truth): + callback_inputs.append((current_time, lead_name, truth)) + if lead_name == "leadOne": + return { + "dRel": 12.5, + "vRel": -4.0, + "vLead": 6.0, + "vLeadK": 5.5, + "aLeadK": -1.25, + "aLeadTau": 0.7, + "status": True, + "modelProb": 0.9, + "radarTrackId": 42, + } + return None + + plant = PlantSP(lead_relevancy=True, speed=10.0, distance_lead=50.0, lead_observation_fn=observe_lead) + result = plant.step(v_lead=8.0) + + assert [entry[1] for entry in callback_inputs] == ["leadOne", "leadTwo"] + assert callback_inputs[0][2]["dRel"] == pytest.approx(50.0) + assert result["truth_lead"]["dRel"] == pytest.approx(50.0) + assert result["lead_one_observation"]["dRel"] == pytest.approx(12.5) + assert result["lead_one_observation"]["radarTrackId"] == 42 + assert result["lead_two_observation"] is None + assert result["distance_lead"] == pytest.approx(50.0 + 8.0 * DT_MDL) + + +def test_model_action_realized_acceleration_and_source_logging(): + def model_action(current_time, v_ego, a_ego): + return -1.25, True + + plant = PlantSP(speed=10.0, e2e=True, force_decel=True, model_action_fn=model_action, actuator_lag=0.5) + first = plant.step() + second = plant.step() + + assert first["model_action"] == {"desiredAcceleration": -1.25, "shouldStop": True} + assert first["published_a_ego"] == pytest.approx(0.0) + assert second["published_a_ego"] == pytest.approx(first["realized_acceleration"]) + assert first["acceleration"] == first["realized_acceleration"] + assert abs(first["realized_acceleration"]) < abs(first["actuator_command"]) + assert first["mpc_source"] is not None + assert first["dec_mode"] in ("acc", "blended") + assert "controller_target" in first + assert "base_target" in first + assert "raw_energy_cap" in first + assert "live_filtered_cap" in first + assert "shadow_filtered_cap" not in first + assert first["lead_one_observation"] is not None + assert first["truth_lead"] == first["lead_one_observation"] + + +def test_configurable_transport_delay_and_first_order_lag(): + plant = PlantSP(speed=10.0, actuator_delay=2 * DT_MDL, actuator_lag=0.2) + + assert plant.planner.CP.longitudinalActuatorDelay == pytest.approx(2 * DT_MDL) + delayed_commands = [plant._update_actuator(-1.0) for _ in range(3)] + assert [command for command, _ in delayed_commands[:2]] == [0.0, 0.0] + + expected_acceleration = -(1.0 - math.exp(-DT_MDL / 0.2)) + assert delayed_commands[2][0] == -1.0 + assert delayed_commands[2][1] == pytest.approx(expected_acceleration) + + +@pytest.mark.parametrize( + ("delay", "lag"), + [(-0.1, 0.0), (float("nan"), 0.0), (float("inf"), 0.0), (None, -0.1), (None, float("nan")), (None, float("inf"))], +) +def test_invalid_actuator_dynamics(delay, lag): + with pytest.raises(ValueError): + PlantSP(actuator_delay=delay, actuator_lag=lag) diff --git a/sunnypilot/sunnylink/params_metadata.json b/sunnypilot/sunnylink/params_metadata.json index 1e95fd91aa..fed5217300 100644 --- a/sunnypilot/sunnylink/params_metadata.json +++ b/sunnypilot/sunnylink/params_metadata.json @@ -1,4 +1,26 @@ { + "AccelPersonality": { + "title": "Acceleration Profile", + "description": "Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly.", + "options": [ + { + "value": 0, + "label": "Eco" + }, + { + "value": 1, + "label": "Normal" + }, + { + "value": 2, + "label": "Sport" + } + ] + }, + "AccelPersonalityEnabled": { + "title": "Enable Accel Controller", + "description": "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority." + }, "AccessToken": { "title": "AccessTokenIsNice", "description": "" diff --git a/sunnypilot/sunnylink/settings_ui.json b/sunnypilot/sunnylink/settings_ui.json index 1638105651..4205697617 100644 --- a/sunnypilot/sunnylink/settings_ui.json +++ b/sunnypilot/sunnylink/settings_ui.json @@ -519,12 +519,6 @@ } ] }, - { - "key": "RoadEdgeLaneChangeEnabled", - "widget": "toggle", - "title": "Block Lane Change: Road Edge Detection", - "description": "Blocks lane change when the model sees a road edge on the side you signal." - }, { "key": "AutoLaneChangeBsmDelay", "widget": "toggle", @@ -629,8 +623,8 @@ { "key": "AccelPersonalityEnabled", "widget": "toggle", - "title": "Enable Acceleration Profiles", - "description": "Enables acceleration profile selection for longitudinal control.", + "title": "Enable Accel Controller", + "description": "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority.", "visibility": [ { "type": "capability", @@ -650,10 +644,10 @@ "key": "AccelPersonality", "widget": "multiple_button", "title": "Acceleration Profile", - "description": "Controls how quickly sunnypilot accelerates while preserving braking and stop behavior.", + "description": "Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly.", "options": [ { - "value": 2, + "value": 0, "label": "Eco" }, { @@ -661,7 +655,7 @@ "label": "Normal" }, { - "value": 0, + "value": 2, "label": "Sport" } ], @@ -2059,6 +2053,22 @@ "equals": true } ] + }, + { + "key": "PlanplusControl", + "widget": "option", + "title": "Plan Plus Controls", + "description": "Adjust planplus model recentering strength. The higher this number the more aggressively the model will recover to lane center; too high and it will ping-pong.", + "min": 0.0, + "max": 2.0, + "step": 0.1, + "enablement": [ + { + "type": "param", + "key": "ShowAdvancedControls", + "equals": true + } + ] } ] }, @@ -2226,6 +2236,50 @@ "title": "Toyota / Lexus Settings", "description": "", "items": [ + { + "key": "ToyotaAutoHold", + "widget": "toggle", + "needs_onroad_cycle": true, + "title": "Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS", + "enablement": [ + { + "type": "not_engaged" + } + ] + }, + { + "key": "ToyotaEnhancedBsm", + "widget": "toggle", + "needs_onroad_cycle": true, + "title": "Toyota: Prius TSS2 BSM and some tssp", + "enablement": [ + { + "type": "not_engaged" + } + ] + }, + { + "key": "ToyotaTSS2Long", + "widget": "toggle", + "needs_onroad_cycle": true, + "title": "Toyota: custom longitudinal for TSS2", + "enablement": [ + { + "type": "not_engaged" + } + ] + }, + { + "key": "ToyotaDriveMode", + "widget": "toggle", + "needs_onroad_cycle": true, + "title": "Enable drive mode btn link", + "enablement": [ + { + "type": "not_engaged" + } + ] + }, { "key": "ToyotaEnforceStockLongitudinal", "widget": "toggle", diff --git a/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml b/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml index 667cfd1d40..11688c306b 100644 --- a/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml +++ b/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml @@ -43,19 +43,32 @@ sections: label: Relaxed enablement: - $ref: '#/macros/longitudinal' + - key: AccelPersonalityEnabled + widget: toggle + title: Enable Accel Controller + description: Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking + and stopping authority. + visibility: + - $ref: '#/macros/longitudinal' + enablement: + - $ref: '#/macros/longitudinal' - key: AccelPersonality widget: multiple_button title: Acceleration Profile - description: Controls how quickly sunnypilot accelerates while preserving braking and stop behavior. + description: Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts + and recovers more quickly. options: - - value: 2 + - value: 0 label: Eco - value: 1 label: Normal - - value: 0 + - value: 2 label: Sport enablement: - $ref: '#/macros/longitudinal' + - type: param + key: AccelPersonalityEnabled + equals: true - key: IntelligentCruiseButtonManagement widget: toggle title: Intelligent Cruise Button Management (ICBM) (Alpha) diff --git a/sunnypilot/sunnylink/tests/test_settings_schema.py b/sunnypilot/sunnylink/tests/test_settings_schema.py index 61cc0131cf..6f8b381fcc 100644 --- a/sunnypilot/sunnylink/tests/test_settings_schema.py +++ b/sunnypilot/sunnylink/tests/test_settings_schema.py @@ -272,6 +272,22 @@ class TestKnownPanels: nnlc_enable_keys = {r.get("key") for r in nnlc.get("enablement", []) if r.get("type") == "param"} assert "EnforceTorqueControl" in nnlc_enable_keys + def test_accel_controller_profile_mapping_and_enablement(self, schema): + cruise = next(p for p in schema["panels"] if p["id"] == "cruise") + items = {item["key"]: item for item in _iter_panel_items(cruise)} + + assert items["AccelPersonalityEnabled"]["widget"] == "toggle" + assert items["AccelPersonality"]["options"] == [ + {"value": 0, "label": "Eco"}, + {"value": 1, "label": "Normal"}, + {"value": 2, "label": "Sport"}, + ] + assert { + "type": "param", + "key": "AccelPersonalityEnabled", + "equals": True, + } in items["AccelPersonality"]["enablement"] + class TestKnownVehicleSettings: def test_hyundai_has_longitudinal_tuning(self, schema):