From bdff3330469acca8eb4f53a4a73515859b07413c Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sun, 16 Aug 2026 20:59:10 -0700 Subject: [PATCH] simplify --- .../controls/lib/longitudinal_planner.py | 4 +- .../test/longitudinal_maneuvers/plant.py | 1 + .../selfdrive/ui/layouts/settings/toggles.py | 27 +- .../ui/mici/layouts/settings/toggles.py | 13 +- .../lib/accel_controller/accel_controller.py | 462 +++++++++---- .../lib/accel_controller/constants.py | 79 +-- .../controls/lib/accel_controller/lead.py | 107 --- .../tests/test_accel_controller.py | 646 +++++++++++++----- .../tests/test_accel_controller_interfaces.py | 353 ++++++---- .../controls/lib/longitudinal_planner.py | 87 ++- .../test_accel_controller_closed_loop.py | 211 +++++- .../controls/lib/tests/test_longcontrol_sp.py | 5 +- .../test/longitudinal_maneuvers/plant.py | 2 - .../tests/test_plant_sp.py | 2 - .../sunnypilot/sunnylink/settings_ui.json | 9 +- .../settings_ui_src/pages/cruise.yaml | 11 +- .../sunnylink/tests/test_settings_schema.py | 11 +- 17 files changed, 1365 insertions(+), 665 deletions(-) delete mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead.py diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index c508dd6b8f..3863acd376 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -101,9 +101,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP): throttle_probs = sm['modelV2'].meta.disengagePredictions.gasPressProbs throttle_prob = throttle_probs[1] if len(throttle_probs) > 1 else 1.0 stock_allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED - self.allow_throttle = self.accel_controller.update_allow_throttle( - stock_allow_throttle, throttle_prob, force_allow=v_ego <= MIN_ALLOW_THROTTLE_SPEED, - ) + self.allow_throttle = stock_allow_throttle steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['vehicleParameters'].angleOffsetDeg diff --git a/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py b/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py index e2474767eb..96974efd56 100755 --- a/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py @@ -14,6 +14,7 @@ from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU class PlannerSM(dict): def __init__(self, radar_frame: int, services: dict): super().__init__(services) + self.frame = radar_frame self.logMonoTime = {"radarState": radar_frame} self.valid = {"radarState": True} self.alive = {"radarState": True} diff --git a/openpilot/selfdrive/ui/layouts/settings/toggles.py b/openpilot/selfdrive/ui/layouts/settings/toggles.py index f82d05e0dd..defcb7a3cc 100644 --- a/openpilot/selfdrive/ui/layouts/settings/toggles.py +++ b/openpilot/selfdrive/ui/layouts/settings/toggles.py @@ -28,10 +28,10 @@ DESCRIPTIONS = { "your steering wheel distance button." ), "AccelPersonalityEnabled": tr_noop( - "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority." + "Use the Accel Controller for smooth, early lead following and stop-and-go. Stock emergency braking remains available as a safety backstop." ), "AccelPersonality": tr_noop( - "Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly." + "Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across profiles." ), "IsLdwEnabled": tr_noop( "Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line " + @@ -112,11 +112,11 @@ class TogglesLayout(Widget): icon="speed_limit.png" ) - self._accel_personality_enabled = toggle_item( + self._accel_controller_enabled = toggle_item( lambda: tr("Enable Accel Controller"), lambda: tr(DESCRIPTIONS["AccelPersonalityEnabled"]), self._params.get_bool("AccelPersonalityEnabled"), - callback=self._set_accel_personality_enabled, + callback=self._set_accel_controller_enabled, icon="speed_limit.png", ) @@ -162,7 +162,7 @@ class TogglesLayout(Widget): # 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["AccelPersonalityEnabled"] = self._accel_controller_enabled self._toggles["AccelPersonality"] = self._accel_personality_setting self._update_experimental_mode_icon() @@ -184,7 +184,7 @@ class TogglesLayout(Widget): def _update_toggles(self): ui_state.update_params() - accel_personality_enabled = self._params.get_bool("AccelPersonalityEnabled") + accel_controller_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. " + @@ -203,14 +203,14 @@ 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) + self._accel_controller_enabled.action_item.set_enabled(True) + self._accel_personality_setting.action_item.set_enabled(True) 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_controller_enabled.action_item.set_enabled(False) self._accel_personality_setting.action_item.set_enabled(False) self._params.remove("ExperimentalMode") @@ -234,10 +234,8 @@ 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) - ) + self._accel_controller_enabled.action_item.set_state(accel_controller_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: @@ -286,6 +284,5 @@ class TogglesLayout(Widget): def _set_accel_personality(self, button_index: int): self._params.put("AccelPersonality", button_index, block=True) - def _set_accel_personality_enabled(self, state: bool): + def _set_accel_controller_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/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py b/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py index 40c6bb2d3a..fa10c486f0 100644 --- a/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py +++ b/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py @@ -42,7 +42,7 @@ 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_controller_enabled = BigParamControl("enable accel controller", "AccelPersonalityEnabled") self._accel_personality_toggle = BigMultiParamToggle("acceleration profile", "AccelPersonality", ["eco", "normal", "sport"]) self._experimental_btn = BigToggle("experimental mode", initial_state=ui_state.params.get_bool("ExperimentalMode"), toggle_callback=self._on_experimental_mode) @@ -55,7 +55,7 @@ class TogglesLayoutMici(NavScroller): self._scroller.add_widgets([ self._personality_toggle, - self._accel_personality_enabled, + self._accel_controller_enabled, self._accel_personality_toggle, self._experimental_btn, is_metric_toggle, @@ -69,7 +69,7 @@ class TogglesLayoutMici(NavScroller): # Toggle lists self._refresh_toggles = ( ("ExperimentalMode", self._experimental_btn), - ("AccelPersonalityEnabled", self._accel_personality_enabled), + ("AccelPersonalityEnabled", self._accel_controller_enabled), ("IsMetric", is_metric_toggle), ("IsLdwEnabled", ldw_toggle), ("AlwaysOnDM", always_on_dm_toggle), @@ -79,9 +79,6 @@ 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) @@ -112,14 +109,14 @@ 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_controller_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_controller_enabled.set_visible(False) self._accel_personality_toggle.set_visible(False) ui_state.params.remove("ExperimentalMode") diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py index 4e81f201d8..7f6375aae2 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py @@ -1,16 +1,16 @@ import math from dataclasses import dataclass -from openpilot.cereal import custom -from openpilot.common.params import Params +import numpy as np + +from openpilot.cereal import custom, log from openpilot.common.realtime import DT_MDL -from openpilot.sunnypilot import get_sanitize_int_param from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ( - COMFORT_DECEL, EARLY_DECEL_EPSILON, EARLY_DECEL_RELEASE_RATE, EARLY_DECEL_RESPONSE_TIME, - EARLY_DECEL_SPEED_DEADBAND, EARLY_DECEL_TIGHTEN_RATE, PARAM_READ_INTERVAL, THROTTLE_REENABLE_PROB, - VEGO_NOISE_TOLERANCE, AccelProfile, profile_accel_max, sanitize_profile, + ACCEL_V, BRAKE_BUILD_JERK, BRAKE_ONSET_JERK, DECEL_V, DESIRED_STOP_DISTANCE, FOLLOW_HEADWAY, GAP_DEADBAND_METERS, + GAP_DEADBAND_SECONDS, LAUNCH_JERK, NEUTRAL_ACCEL, PACE_BUFFER_TIME, PACE_GAIN, PACE_MAX_CLOSING_SPEED, PACE_MAX_OPENING_SPEED, + PACE_STABILITY_MARGIN, RELEASE_JERK, ROUTINE_DECEL, SPEED_BP, SPEED_RESPONSE_TIME, STOP_MARGIN_BP, STOP_MARGIN_V, TERMINAL_MAX_DECEL, + TERMINAL_PREVIEW_TIME, TERMINAL_TIME_CONSTANT, URGENT_BRAKE_JERK, AccelProfile, ) -from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan, calculate_lead_plan AccelControllerState = custom.LongitudinalPlanSP.AccelController.State @@ -18,9 +18,29 @@ AccelControllerState = custom.LongitudinalPlanSP.AccelController.State @dataclass(frozen=True) class AccelDecision: - cruise_accel_max: float | None = None - early_decel: float | None = None - active: bool = False + a_target: float | None = None + should_stop: bool = False + stock_safety_required: bool = False + + +@dataclass(frozen=True) +class LeadObservation: + index: int + distance: float + speed: float + accel: float + track_id: int + model_prob: float + + +@dataclass(frozen=True) +class ProjectedLead: + observation: LeadObservation + distance: float + speed: float + relative_speed: float + relative_speed_target: float + accel: float class AccelController: @@ -29,132 +49,342 @@ class AccelController: raise ValueError("dt must be finite and positive") self.dt = float(dt) - self.delay = float(CP.longitudinalActuatorDelay) + DT_MDL - 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 / self.dt))) - self._param_frame = 0 + self.action_time = float(np.clip(float(CP.longitudinalActuatorDelay) + DT_MDL, DT_MDL, 1.0)) + + self._a_command: float | None = None + self._last_primary: LeadObservation | None = None + self._last_secondary: LeadObservation | None = None + self._primary_frames = 0 + self._secondary_frames = 0 + self._lead_clear_frames = 0 + self._braking_latched = False + self._radar_faulted = False + self._radar_recovery_frames = 0 - self._early_decel: float | None = None - self.is_active = False - self.cruise_accel_max: float | None = None - self.early_decel: float | None = None self.state = AccelControllerState.inactive self.selected_lead = -1 - self.selected_lead_track_id = -1 - self.required_decel = 0.0 - self._allow_throttle = True @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 + def is_active(self) -> bool: + return self.state != AccelControllerState.inactive def reset(self) -> None: - self._early_decel = None - self._allow_throttle = True - self.is_active = False - self.cruise_accel_max = None - self.early_decel = None + self._a_command = None + self._last_primary = None + self._last_secondary = None + self._primary_frames = 0 + self._secondary_frames = 0 + self._lead_clear_frames = 0 + self._braking_latched = False + self._radar_faulted = False + self._radar_recovery_frames = 0 self.state = AccelControllerState.inactive self.selected_lead = -1 - self.selected_lead_track_id = -1 - self.required_decel = 0.0 - def update_allow_throttle(self, stock_allowed: bool, throttle_prob: float, *, force_allow: bool = False) -> bool: - if not self.is_enabled or force_allow: - self._allow_throttle = bool(stock_allowed) - elif self._allow_throttle: - self._allow_throttle = bool(stock_allowed) - elif stock_allowed and math.isfinite(throttle_prob) and throttle_prob > THROTTLE_REENABLE_PROB: - self._allow_throttle = True - return self._allow_throttle + @staticmethod + def _read_lead(lead, index: int) -> LeadObservation | None: + try: + if not bool(lead.present): + return None + distance = float(lead.dRel) + speed = float(lead.vLeadK) + if not math.isfinite(distance) or distance < 0.0 or not math.isfinite(speed) or not -1.0 <= speed <= 100.0: + return None - def _valid_context(self, *, v_ego: float, a_ego: float, v_cruise: float, stock_accel_max: float, - engaged: bool, cruise_initialized: bool) -> bool: - values = (v_ego, a_ego, v_cruise, stock_accel_max, self.delay) - return (engaged and cruise_initialized and v_ego >= -VEGO_NOISE_TOLERANCE and v_cruise >= 0.0 - and self.delay >= 0.0 and all(math.isfinite(value) for value in values)) + accel = float(lead.aLeadK) + accel = float(np.clip(accel, -5.0, 3.0)) if math.isfinite(accel) else 0.0 + track_id_value = float(lead.radarTrackId) + track_id = int(track_id_value) if math.isfinite(track_id_value) else -1 + model_prob = float(lead.modelProb) + model_prob = float(np.clip(model_prob, 0.0, 1.0)) if math.isfinite(model_prob) else 0.0 + except (AttributeError, TypeError, ValueError): + return None - def _raw_early_decel(self, lead_plan: LeadPlan) -> float: - speed_error = lead_plan.speed_ceiling - lead_plan.v_ego_projected - if (lead_plan.selected_lead < 0 or lead_plan.closing_speed <= 0.0 - or speed_error >= -EARLY_DECEL_SPEED_DEADBAND): + return LeadObservation(index, distance, max(speed, 0.0), accel, max(track_id, -1), model_prob) + + @staticmethod + def _claims_present(lead) -> bool: + try: + return bool(lead.present) + except (AttributeError, TypeError, ValueError): + return False + + @staticmethod + def _project_lead(lead: LeadObservation, action_time: float) -> tuple[float, float]: + accel = min(lead.accel, 0.0) + travel_time = min(action_time, -lead.speed / accel) if accel < 0.0 else action_time + speed = max(lead.speed + accel * travel_time, 0.0) + travel = (lead.speed + speed) * travel_time * 0.5 + return max(lead.distance + travel, 0.0), speed + + @staticmethod + def _same_track(previous: LeadObservation | None, current: LeadObservation) -> bool: + if previous is None or previous.index != current.index: + return False + if previous.track_id >= 0 and current.track_id >= 0: + return previous.track_id == current.track_id + return abs(current.distance - previous.distance) <= 5.0 + + @staticmethod + def _same_physical_lead(first: ProjectedLead, second: ProjectedLead) -> bool: + first_track = first.observation.track_id + second_track = second.observation.track_id + if first_track >= 0 and second_track >= 0: + return first_track == second_track + return abs(first.distance - second.distance) <= 0.5 and abs(first.speed - second.speed) <= 0.5 + + def _update_lead_continuity(self, primary: LeadObservation | None, secondary: LeadObservation | None) -> None: + if primary is None: + self._primary_frames = 0 + self._lead_clear_frames = self._lead_clear_frames + 1 if self._last_primary is not None else 0 + if self._lead_clear_frames >= 3: + self._last_primary = None + elif self._same_track(self._last_primary, primary): + self._primary_frames += 1 + self._lead_clear_frames = 0 + else: + self._primary_frames = 1 + self._lead_clear_frames = 0 + + if secondary is not None and self._same_track(self._last_secondary, secondary): + self._secondary_frames += 1 + else: + self._secondary_frames = int(secondary is not None) + + @staticmethod + def _pace_target(lead: LeadObservation, ego_distance: float, v_ego: float, headway: float, action_time: float) -> ProjectedLead: + lead_distance, lead_speed = AccelController._project_lead(lead, action_time) + projected_gap = max(lead_distance - ego_distance, 0.0) + desired_gap = DESIRED_STOP_DISTANCE + headway * v_ego + gap_error = projected_gap - desired_gap + gap_deadband = GAP_DEADBAND_METERS + GAP_DEADBAND_SECONDS * v_ego + effective_gap_error = math.copysign(max(abs(gap_error) - gap_deadband, 0.0), gap_error) + relative_speed = lead_speed - v_ego + relative_speed_target = -float(np.clip(effective_gap_error / PACE_BUFFER_TIME, -PACE_MAX_OPENING_SPEED, PACE_MAX_CLOSING_SPEED)) + nominal_denominator = headway ** 2 / PACE_BUFFER_TIME + 2.0 * headway + pace_gain = max(PACE_GAIN, PACE_STABILITY_MARGIN / nominal_denominator) + accel = pace_gain * (relative_speed - relative_speed_target) + return ProjectedLead(lead, projected_gap, lead_speed, relative_speed, relative_speed_target, accel) + + @staticmethod + def _terminal_target(projected: ProjectedLead, v_ego: float) -> float | None: + usable_distance = max(projected.distance - DESIRED_STOP_DISTANCE, 0.0) + if v_ego > 0.0 and usable_distance / v_ego > TERMINAL_PREVIEW_TIME: + return None + brake_tau = ROUTINE_DECEL * TERMINAL_TIME_CONSTANT + stop_speed = math.sqrt(brake_tau ** 2 + 2.0 * ROUTINE_DECEL * usable_distance) - brake_tau + if v_ego <= stop_speed: return 0.0 - comfort_decel = COMFORT_DECEL[self.profile] - return max(speed_error / EARLY_DECEL_RESPONSE_TIME, -comfort_decel) + curve_decel = ROUTINE_DECEL * v_ego / (v_ego + brake_tau) if v_ego > 0.0 else 0.0 + braking_distance = max(usable_distance, 0.1) + required_decel = v_ego ** 2 / (2.0 * braking_distance) * float(np.interp(v_ego, STOP_MARGIN_BP, STOP_MARGIN_V)) + return -min(max(curve_decel, required_decel), TERMINAL_MAX_DECEL) - def _update_early_decel(self, raw_target: float, previous_plan_accel: float) -> None: - raw_target = min(float(raw_target), 0.0) - plan_accel = float(previous_plan_accel) if math.isfinite(previous_plan_accel) else 0.0 - previous = self._early_decel if self._early_decel is not None else max(plan_accel, 0.0) + @staticmethod + def _follow_target(projected: ProjectedLead, max_decel: float) -> float: + demand = max(-projected.accel, 0.0) + limit = min(ROUTINE_DECEL + 0.15 * max(demand - ROUTINE_DECEL, 0.0), max_decel) + return max(projected.accel, -limit) - if raw_target < previous - EARLY_DECEL_EPSILON: - updated = max(raw_target, previous - EARLY_DECEL_TIGHTEN_RATE * self.dt) - state = AccelControllerState.restrict - elif raw_target > previous + EARLY_DECEL_EPSILON: - updated = min(raw_target, previous + EARLY_DECEL_RELEASE_RATE * self.dt) - state = AccelControllerState.release + @staticmethod + def _stopped(speed: float) -> bool: + return speed < 0.3 + + def _departure_confirmed(self, primary: LeadObservation | None, standstill: bool) -> bool: + if not standstill or self._radar_faulted: + return False + if primary is None: + return self._last_primary is None + if self._last_primary is None or not self._same_track(self._last_primary, primary): + return False + range_opening = primary.distance - self._last_primary.distance + return self._primary_frames >= 2 and not self._stopped(primary.speed) and range_opening >= 0.02 + + @staticmethod + def _lead_clear_for_launch(lead: ProjectedLead) -> bool: + distance_clear = lead.distance >= DESIRED_STOP_DISTANCE + 1.0 or not AccelController._stopped(lead.speed) + return lead.accel >= -NEUTRAL_ACCEL and distance_clear + + def _fault_fallback(self) -> AccelDecision: + self.reset() + self._radar_faulted = True + return AccelDecision() + + def _govern_accel(self, raw_target: float, max_accel: float, previous_plan_accel: float, *, launch: bool, terminal: bool) -> float: + if self._a_command is None: + initial = previous_plan_accel if math.isfinite(previous_plan_accel) else 0.0 + self._a_command = 0.0 if launch else min(initial, max_accel) + + previous = self._a_command + if raw_target < previous: + if raw_target < -ROUTINE_DECEL - 0.75: + jerk = URGENT_BRAKE_JERK + elif previous > -0.5: + jerk = BRAKE_ONSET_JERK + else: + jerk = BRAKE_BUILD_JERK + updated = max(raw_target, previous - jerk * self.dt) else: - updated = raw_target - state = AccelControllerState.hold if updated < -EARLY_DECEL_EPSILON else AccelControllerState.free + if launch: + jerk = LAUNCH_JERK + elif terminal: + jerk = min(RELEASE_JERK, 0.6) + else: + jerk = RELEASE_JERK + updated = min(raw_target, previous + jerk * self.dt) - if raw_target >= -EARLY_DECEL_EPSILON and updated >= -EARLY_DECEL_EPSILON: - self._early_decel = None - self.early_decel = None - self.state = AccelControllerState.free - else: - self._early_decel = updated - self.early_decel = updated - self.state = state + self._a_command = float(updated) + return self._a_command def update(self, radar_state, *, v_ego: float, a_ego: float, v_cruise: float, follow_personality, - engaged: bool, cruise_initialized: bool, acc_selected: bool, stock_accel_max: float, - radar_fresh: bool = True, radar_healthy: bool = True, force_decel: bool = False, - previous_plan_accel: float = 0.0) -> AccelDecision: - self.profile = sanitize_profile(self.profile) - valid_context = self._valid_context( - v_ego=v_ego, a_ego=a_ego, v_cruise=v_cruise, stock_accel_max=stock_accel_max, - engaged=engaged, cruise_initialized=cruise_initialized, + radar_valid: bool, stock_cruise_accel: float, stock_mpc_accel: float, + stock_should_stop: bool, fcw: bool, standstill: bool, stock_mpc_lead: int = -1, + previous_plan_accel: float = 0.0, profile: int = AccelProfile.normal) -> AccelDecision: + numeric_context = (v_ego, a_ego, v_cruise, stock_cruise_accel, stock_mpc_accel, self.action_time) + valid_context = (radar_valid and -0.1 <= v_ego <= 100.0 and 0.0 <= v_cruise <= 100.0 + and abs(a_ego) <= 10.0 and abs(stock_cruise_accel) <= 10.0 + and abs(stock_mpc_accel) <= 10.0 and all(math.isfinite(value) for value in numeric_context)) + if not valid_context: + return self._fault_fallback() + if self._a_command is not None and math.isfinite(previous_plan_accel): + self._a_command = min(self._a_command, previous_plan_accel) + + try: + raw_leads = (radar_state.leadOne, radar_state.leadTwo) + leads = [self._read_lead(raw_leads[0], 0), self._read_lead(raw_leads[1], 1)] + except AttributeError: + return self._fault_fallback() + if any(self._claims_present(raw_lead) and lead is None for raw_lead, lead in zip(raw_leads, leads, strict=True)): + return self._fault_fallback() + primary = leads[0] + secondary = leads[1] + if standstill and primary is None and secondary is not None: + return self._fault_fallback() + if self._radar_faulted: + if primary is None: + self._radar_recovery_frames += 1 + self._radar_faulted = self._radar_recovery_frames < 3 + else: + self._radar_faulted = False + self._radar_recovery_frames = 0 + self._update_lead_continuity(primary, secondary) + primary_recently_missing = primary is None and self._last_primary is not None + + current_v_ego = max(v_ego, 0.0) + projected_v_ego = max(current_v_ego + a_ego * self.action_time, 0.0) + ego_distance = (current_v_ego + projected_v_ego) * self.action_time * 0.5 + accel_v = ACCEL_V.get(profile, ACCEL_V[AccelProfile.normal]) + max_accel = float(np.interp(projected_v_ego, SPEED_BP, accel_v)) + max_decel = float(np.interp(projected_v_ego, SPEED_BP, DECEL_V)) + headway = FOLLOW_HEADWAY.get(follow_personality, FOLLOW_HEADWAY[log.LongitudinalPersonality.standard]) + projected = [None if lead is None else self._pace_target(lead, ego_distance, projected_v_ego, headway, self.action_time) for lead in leads] + primary_projected, secondary_projected = projected + observed_leads = [lead for lead in projected if lead is not None] + + free_accel = float(np.clip((v_cruise - projected_v_ego) / SPEED_RESPONSE_TIME, -ROUTINE_DECEL, max_accel)) + raw_target = free_accel + selected_projected = None + secondary_trusted = secondary is not None and self._secondary_frames >= 3 and secondary.model_prob >= 0.5 + trusted_leads = [lead for lead in (primary_projected, secondary_projected if secondary_trusted else None) if lead is not None] + terminal = False + for lead in trusted_leads: + if self._stopped(lead.speed): + lead_target = self._terminal_target(lead, projected_v_ego) + if lead_target is None: + continue + terminal = True + else: + lead_target = self._follow_target(lead, max_decel) + if lead_target < raw_target: + raw_target = lead_target + selected_projected = lead + if secondary_projected is not None and not secondary_trusted and secondary_projected.accel < -NEUTRAL_ACCEL: + raw_target = min(raw_target, 0.0) + + all_leads_clear = all(self._lead_clear_for_launch(lead) for lead in observed_leads) + departure_confirmed = self._departure_confirmed(primary, bool(standstill)) and (primary is not None or secondary is None) + launch = bool(standstill and departure_confirmed and all_leads_clear and v_cruise > 0.3) + if launch: + self._braking_latched = False + raw_target = min(max_accel, max(0.8, free_accel)) + selected_projected = primary_projected + elif standstill: + raw_target = 0.0 + self._a_command = 0.0 + + if primary_projected is not None: + pace_margin = primary_projected.relative_speed - primary_projected.relative_speed_target + if self._braking_latched and raw_target > 0.0 and pace_margin < 0.2 and not launch: + raw_target = 0.0 + elif pace_margin >= 0.2: + self._braking_latched = False + elif not primary_recently_missing: + self._braking_latched = False + + if primary_recently_missing: + raw_target = min(raw_target, 0.0) + + if abs(raw_target) < NEUTRAL_ACCEL and not launch and not terminal: + raw_target = 0.0 + raw_target = float(np.clip(raw_target, -(TERMINAL_MAX_DECEL if terminal else max_decel), max_accel)) + if not standstill or launch: + raw_target = min(raw_target, stock_cruise_accel) + + a_target = self._govern_accel(raw_target, max_accel, previous_plan_accel, launch=launch, terminal=terminal) + if not standstill or launch: + a_target = min(a_target, stock_cruise_accel) + self._a_command = a_target + if a_target <= -0.15 and primary_projected is not None: + self._braking_latched = True + + should_stop = bool(standstill and not launch or + not launch and primary_projected is not None and self._stopped(primary_projected.speed) + and self._stopped(max(v_ego, 0.0)) + and primary_projected.distance <= DESIRED_STOP_DISTANCE + 1.0 + ) + terminal_feasible = False + if terminal and selected_projected is not None and self._stopped(selected_projected.speed): + available_decel = min(max(-a_target, 0.0), max(-a_ego, 0.0)) + usable_distance = max(selected_projected.distance - DESIRED_STOP_DISTANCE, 0.0) + stopping_distance = projected_v_ego ** 2 / (2.0 * max(available_decel, 0.1)) + terminal_feasible = available_decel > NEUTRAL_ACCEL and usable_distance >= stopping_distance + stock_projected = projected[stock_mpc_lead] if 0 <= stock_mpc_lead < len(projected) else None + terminal_source_covered = (terminal_feasible and selected_projected is not None and stock_projected is not None + and self._same_physical_lead(selected_projected, stock_projected)) + urgent_ttc = False + danger_gap = False + current_desired_gap = DESIRED_STOP_DISTANCE + headway * current_v_ego + projected_desired_gap = DESIRED_STOP_DISTANCE + headway * projected_v_ego + for observed_lead in observed_leads: + current_closing_speed = max(current_v_ego - observed_lead.observation.speed, 0.0) + projected_closing_speed = max(-observed_lead.relative_speed, 0.0) + current_ttc = observed_lead.observation.distance / current_closing_speed if current_closing_speed > 1e-3 else math.inf + projected_ttc = observed_lead.distance / projected_closing_speed if projected_closing_speed > 1e-3 else math.inf + lead_covered = terminal_source_covered and selected_projected is not None and self._same_physical_lead(selected_projected, observed_lead) + urgent_ttc |= min(current_ttc, projected_ttc) < 4.0 and not lead_covered + danger_gap |= (observed_lead.observation.distance < 0.75 * current_desired_gap + or observed_lead.distance < 0.75 * projected_desired_gap) + stock_more_urgent = not terminal_source_covered and stock_mpc_accel <= -1.2 and stock_mpc_accel < a_target - 0.3 + stock_source_confirmed = (primary is None and stock_mpc_lead == -1) or (primary is not None and stock_mpc_lead == 0) + stale_stock_stop = bool(stock_should_stop and launch and stock_source_confirmed and not fcw and not danger_gap) + stock_safety_required = bool( + fcw or danger_gap or urgent_ttc or stock_more_urgent or stock_should_stop and not stale_stock_stop ) - if not (self.is_enabled and valid_context and bool(acc_selected) and not force_decel): - self.reset() - return AccelDecision() - sanitized_v_ego = max(float(v_ego), 0.0) - positive_stock_max = max(float(stock_accel_max), 0.0) - self.cruise_accel_max = profile_accel_max(self.profile, sanitized_v_ego, positive_stock_max) - - if radar_healthy and not radar_fresh: - if self._early_decel is not None: - self.state = AccelControllerState.hold + self.selected_lead = -1 if selected_projected is None else selected_projected.observation.index + if should_stop: + self.state = AccelControllerState.stopHold + elif launch or a_target > (previous_plan_accel if math.isfinite(previous_plan_accel) else 0.0) + 1e-6: + self.state = AccelControllerState.release + elif a_target < -NEUTRAL_ACCEL: + self.state = AccelControllerState.restrict + elif primary_projected is not None: + self.state = AccelControllerState.hold else: - lead_plan = LeadPlan(v_ego_projected=sanitized_v_ego) - if radar_healthy and radar_state is not None: - try: - lead_plan = calculate_lead_plan( - radar_state, sanitized_v_ego, float(a_ego), self.delay, self.profile, follow_personality, - ) - except (AttributeError, TypeError, ValueError): - lead_plan = LeadPlan(v_ego_projected=sanitized_v_ego) + self.state = AccelControllerState.free - 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 - self._update_early_decel(self._raw_early_decel(lead_plan), previous_plan_accel) - - profile_binding = self.cruise_accel_max < positive_stock_max - EARLY_DECEL_EPSILON - self.is_active = profile_binding or self.early_decel is not None - - return AccelDecision( - cruise_accel_max=self.cruise_accel_max, - early_decel=self.early_decel, - active=self.is_active, - ) + decision = AccelDecision(a_target, should_stop, stock_safety_required) + if primary is not None: + self._last_primary = primary + self._last_secondary = secondary + return decision diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py index c86f0ae35f..32a14e2718 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py @@ -1,54 +1,43 @@ -import math - -import numpy as np - -from openpilot.cereal import custom +from openpilot.cereal import custom, log AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile -ACCEL_PROFILES = tuple(AccelProfile.schema.enumerants.values()) -# Scale the stock cruise candidate; stock turn and throttle limits stay authoritative. -ACCEL_SCALE_BP = [0.0, 3.0, 10.0, 25.0, 40.0] -ACCEL_SCALE_V = { - AccelProfile.eco: [0.82, 0.76, 0.63, 0.53, 0.42], - AccelProfile.normal: [0.95, 0.90, 0.84, 0.76, 0.63], - AccelProfile.sport: [1.00, 1.00, 1.00, 1.00, 1.00], +SPEED_BP = (0.0, 3.0, 10.0, 20.0, 30.0, 40.0) +ACCEL_V = { + AccelProfile.eco: (0.90, 0.85, 0.78, 0.65, 0.52, 0.40), + AccelProfile.normal: (1.10, 1.05, 0.95, 0.80, 0.65, 0.50), + AccelProfile.sport: (1.20, 1.15, 1.05, 0.90, 0.72, 0.56), +} +DECEL_V = (1.0, 1.2, 2.3, 2.5, 2.5, 2.5) + +FOLLOW_HEADWAY = { + log.LongitudinalPersonality.relaxed: 1.8, + log.LongitudinalPersonality.standard: 1.6, + log.LongitudinalPersonality.aggressive: 1.4, } -# Smaller values start slowing earlier; stock MPC still owns lead braking. -COMFORT_DECEL = { - AccelProfile.eco: 0.25, - AccelProfile.normal: 0.30, - AccelProfile.sport: 0.35, -} +PACE_BUFFER_TIME = 10.0 +PACE_MAX_CLOSING_SPEED = 1.2 +PACE_MAX_OPENING_SPEED = 1.0 +PACE_GAIN = 0.65 +PACE_STABILITY_MARGIN = 2.2 +SPEED_RESPONSE_TIME = 3.0 +ROUTINE_DECEL = DECEL_V[0] -EARLY_DECEL_RESPONSE_TIME = 1.0 -EARLY_DECEL_SPEED_DEADBAND = 0.15 -EARLY_DECEL_TIGHTEN_RATE = 1.0 -EARLY_DECEL_RELEASE_RATE = 0.35 -EARLY_DECEL_EPSILON = 1e-6 +BRAKE_ONSET_JERK = 1.0 +BRAKE_BUILD_JERK = 0.45 +URGENT_BRAKE_JERK = 2.2 +RELEASE_JERK = 0.8 +LAUNCH_JERK = 1.8 -STOP_GAP_RESERVE = 0.75 -MAX_LEAD_ACCEL_TAU = 10.0 -MIN_LEAD_SPEED = -1.0 -VEGO_NOISE_TOLERANCE = 0.10 -PARAM_READ_INTERVAL = 0.25 -THROTTLE_REENABLE_PROB = 0.45 +DESIRED_STOP_DISTANCE = 6.0 +TERMINAL_TIME_CONSTANT = 3.0 +TERMINAL_PREVIEW_TIME = 12.0 +TERMINAL_MAX_DECEL = DECEL_V[-1] +STOP_MARGIN_BP = (0.0, 5.0, 15.0) +STOP_MARGIN_V = (1.0, 1.0, 1.6) - -def sanitize_profile(profile: int) -> int: - return profile if profile in ACCEL_PROFILES else AccelProfile.normal - - -def profile_accel_scale(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_SCALE_BP, ACCEL_SCALE_V[sanitize_profile(profile)])) - - -def profile_accel_max(profile: int, v_ego: float, stock_accel_max: float) -> float: - if not math.isfinite(stock_accel_max): - return math.nan - stock_positive_max = max(float(stock_accel_max), 0.0) - return stock_positive_max * profile_accel_scale(profile, v_ego) +GAP_DEADBAND_METERS = 2.0 +GAP_DEADBAND_SECONDS = 0.15 +NEUTRAL_ACCEL = 0.08 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead.py deleted file mode 100644 index da3ba7c3d8..0000000000 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead.py +++ /dev/null @@ -1,107 +0,0 @@ -""" -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 openpilot.cereal import log -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import ( - LongitudinalMpc, STOP_DISTANCE, T_IDXS, get_T_FOLLOW, -) -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, sanitize_profile, -) - - -class LeadPlan(NamedTuple): - speed_ceiling: float = math.inf - selected_lead: int = -1 - selected_lead_track_id: int = -1 - closing_speed: float = 0.0 - required_decel: float = 0.0 - v_ego_projected: float = 0.0 - - -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, int] | None: - if not lead.present: - return None - - d_rel = float(lead.dRel) - v_lead = 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 - track_id = max(int(lead.radarTrackId), -1) if math.isfinite(lead.radarTrackId) else -1 - return d_rel, max(v_lead, 0.0), float(np.clip(a_lead, -10.0, 5.0)), a_lead_tau, track_id - - -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() - - try: - t_follow = float(get_T_FOLLOW(follow_personality)) - except (NotImplementedError, TypeError, ValueError): - return LeadPlan() - if not math.isfinite(t_follow) or t_follow < 0.0: - return LeadPlan() - - profile = sanitize_profile(profile) - x_ego, v_ego_projected = _project_ego(v_ego, a_ego, delay) - comfort_decel = COMFORT_DECEL[profile] - candidates: list[LeadPlan] = [] - - for lead_index, lead in enumerate((radar_state.leadOne, radar_state.leadTwo)): - values = _lead_values(lead) - if values is None: - continue - - d_rel, v_lead, a_lead, a_lead_tau, track_id = 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_projected = float(np.interp(delay, T_IDXS, lead_xv[:, 1])) - # Match the stock MPC gap convention, including ego-speed time headway. - safety_gap = max(x_lead - x_ego - STOP_DISTANCE - t_follow * v_ego_projected, 0.0) - closing_speed = max(v_ego_projected - v_lead_projected, 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) - usable_gap = max(safety_gap - STOP_GAP_RESERVE, 0.0) - speed_ceiling = v_lead_projected + math.sqrt(2.0 * comfort_decel * usable_gap) - - finite_values = (x_lead, v_lead_projected, safety_gap, usable_gap, closing_speed, speed_ceiling) - 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): - continue - - candidates.append(LeadPlan( - speed_ceiling=speed_ceiling, - selected_lead=lead_index, - selected_lead_track_id=track_id, - closing_speed=closing_speed, - required_decel=required_decel, - v_ego_projected=v_ego_projected, - )) - - return min(candidates, key=lambda candidate: candidate.speed_ceiling) if candidates else LeadPlan(v_ego_projected=v_ego_projected) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py index 8a71e06f7c..2396ee4043 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py @@ -2,23 +2,29 @@ import math from dataclasses import FrozenInstanceError from types import SimpleNamespace +import numpy as np + from openpilot.cereal import log from openpilot.common.realtime import DT_MDL from openpilot.common.test import OpenpilotTestCase -from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import ( - AccelController, AccelControllerState, AccelDecision, -) +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelDecision from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ( - ACCEL_PROFILES, ACCEL_SCALE_BP, ACCEL_SCALE_V, EARLY_DECEL_RELEASE_RATE, EARLY_DECEL_TIGHTEN_RATE, - AccelProfile, profile_accel_max, profile_accel_scale, sanitize_profile, + ACCEL_V, BRAKE_ONSET_JERK, DECEL_V, RELEASE_JERK, SPEED_BP, AccelProfile, ) -from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import calculate_lead_plan -def lead(*, present=False, distance=0.0, speed=0.0, accel=0.0, tau=1.5, track_id=-1): +def lead(*, present=False, distance=0.0, speed=0.0, accel=0.0, tau=1.5, track_id=-1, probability=1.0): return SimpleNamespace( - present=present, dRel=distance, vLeadK=speed, aLeadK=accel, - aLeadTau=tau, radarTrackId=track_id, + present=present, + dRel=distance, + vRel=0.0, + vLead=speed, + vLeadK=speed, + aLeadK=accel, + aLeadTau=tau, + radarTrackId=track_id, + modelProb=probability, + radar=True, ) @@ -26,11 +32,9 @@ def radar(lead_one=None, lead_two=None): return SimpleNamespace(leadOne=lead_one or lead(), leadTwo=lead_two or lead()) -def controller(*, enabled=True, profile=AccelProfile.normal, dt=DT_MDL): - instance = AccelController(SimpleNamespace(longitudinalActuatorDelay=0.10, openpilotLongitudinalControl=True), dt=dt) - instance.enabled = enabled - instance.profile = profile - return instance +def controller(*, delay=0.10, dt=DT_MDL): + CP = SimpleNamespace(longitudinalActuatorDelay=delay, openpilotLongitudinalControl=True) + return AccelController(CP, dt=dt) def update(instance, radar_state=None, **overrides): @@ -39,204 +43,480 @@ def update(instance, radar_state=None, **overrides): "a_ego": 0.0, "v_cruise": 25.0, "follow_personality": log.LongitudinalPersonality.standard, - "engaged": True, - "cruise_initialized": True, - "acc_selected": True, - "stock_accel_max": 1.0, - "radar_fresh": True, - "radar_healthy": True, - "force_decel": False, + "radar_valid": True, + "stock_cruise_accel": 1.0, + "stock_mpc_accel": 1.0, + "stock_should_stop": False, + "stock_mpc_lead": -1, + "fcw": False, + "standstill": False, + "profile": AccelProfile.normal, } arguments.update(overrides) return instance.update(radar() if radar_state is None else radar_state, **arguments) -def restrictive_radar(): - return radar(lead(present=True, distance=25.0, speed=5.0, accel=-0.2, track_id=101)) +def settle(instance, radar_state=None, frames=80, **overrides): + decision = AccelDecision() + for _ in range(frames): + decision = update(instance, radar_state, **overrides) + return decision -class TestProfiles(OpenpilotTestCase): - def test_profiles_are_faster_without_exceeding_stock(self): - self.assertEqual(ACCEL_SCALE_V[AccelProfile.eco], [0.82, 0.76, 0.63, 0.53, 0.42]) - self.assertEqual(ACCEL_SCALE_V[AccelProfile.normal], [0.95, 0.90, 0.84, 0.76, 0.63]) - self.assertEqual(ACCEL_SCALE_V[AccelProfile.sport], [1.0] * len(ACCEL_SCALE_BP)) - - def test_profile_order_and_stock_scaling(self): - for speed in (*ACCEL_SCALE_BP, 17.0, 50.0): - with self.subTest(speed=speed): - scales = [profile_accel_scale(profile, speed) for profile in ACCEL_PROFILES] - self.assertLess(scales[0], scales[1]) - self.assertLess(scales[1], scales[2]) - self.assertEqual(scales[2], 1.0) - for profile, scale in zip(ACCEL_PROFILES, scales, strict=True): - self.assertAlmostEqual(profile_accel_max(profile, speed, 0.73), 0.73 * scale) - - def test_profiles_never_expand_stock_candidate(self): - for profile in ACCEL_PROFILES: - for stock_limit in (-0.5, 0.0, 0.4, 1.6): - with self.subTest(profile=profile, stock_limit=stock_limit): - limit = profile_accel_max(profile, 12.0, stock_limit) - self.assertGreaterEqual(limit, 0.0) - self.assertLessEqual(limit, max(stock_limit, 0.0)) - - def test_invalid_profile_is_normal_and_nonfinite_propagates(self): - self.assertEqual(sanitize_profile(999), AccelProfile.normal) - self.assertTrue(math.isnan(profile_accel_scale(AccelProfile.normal, math.nan))) - self.assertTrue(math.isnan(profile_accel_max(AccelProfile.normal, 10.0, math.inf))) - - -class TestAccelDecision(OpenpilotTestCase): +class TestAccelControllerContract(OpenpilotTestCase): def test_decision_is_a_small_immutable_contract(self): - decision = AccelDecision(cruise_accel_max=0.4, early_decel=-0.2, active=True) - self.assertEqual((decision.cruise_accel_max, decision.early_decel, decision.active), (0.4, -0.2, True)) - field = "active" + decision = AccelDecision(a_target=-0.4, should_stop=True, stock_safety_required=True) + self.assertEqual( + (decision.a_target, decision.should_stop, decision.stock_safety_required), + (-0.4, True, True), + ) with self.assertRaises(FrozenInstanceError): - setattr(decision, field, False) + decision.should_stop = False - def test_context_gates_reset_without_actuating(self): + def test_invalid_dt_is_rejected(self): + for dt in (0.0, -0.1, math.nan, math.inf): + with self.subTest(dt=dt), self.assertRaises(ValueError): + controller(dt=dt) + + def test_inactive_context_resets_without_actuating(self): cases = ( - {"enabled": False}, - {"engaged": False}, - {"cruise_initialized": False}, - {"acc_selected": False}, - {"force_decel": True}, + {"radar_valid": False}, {"v_ego": math.nan}, + {"a_ego": math.inf}, + {"a_ego": -100.0}, + {"v_ego": 1e308}, {"v_cruise": -1.0}, - {"stock_accel_max": math.inf}, + {"stock_cruise_accel": math.nan}, + {"stock_mpc_accel": math.nan}, ) for case in cases: with self.subTest(case=case): - arguments = dict(case) - enabled = arguments.pop("enabled", True) - instance = controller(enabled=enabled) - decision = update(instance, restrictive_radar(), **arguments) - self.assertEqual(decision, AccelDecision()) + instance = controller() + self.assertEqual(update(instance, **case), AccelDecision()) self.assertFalse(instance.is_active) - self.assertEqual(instance.state, AccelControllerState.inactive) - def test_allow_throttle_hysteresis_filters_route_chatter(self): + def test_acceleration_limit_never_exceeds_stock(self): + decision = settle(controller(), v_ego=5.0, stock_cruise_accel=1.1, stock_mpc_accel=1.1) + self.assertIsNotNone(decision.a_target) + self.assertLessEqual(decision.a_target, min(1.1, np.interp(5.0, SPEED_BP, ACCEL_V[AccelProfile.normal]))) + + def test_selected_profile_changes_positive_acceleration_only(self): + outputs = {} + for profile in ACCEL_V: + instance = controller() + previous = 0.0 + for _ in range(80): + decision = update(instance, v_ego=5.0, stock_cruise_accel=2.0, stock_mpc_accel=2.0, previous_plan_accel=previous, profile=profile) + previous = decision.a_target + outputs[profile] = decision.a_target + self.assertLess(outputs[AccelProfile.eco], outputs[AccelProfile.normal]) + self.assertLess(outputs[AccelProfile.normal], outputs[AccelProfile.sport]) + + slower_lead = radar(lead(present=True, distance=55.0, speed=14.0, track_id=7)) + braking = { + profile: settle(controller(), slower_lead, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7, profile=profile).a_target + for profile in ACCEL_V + } + self.assertEqual(len({round(value, 9) for value in braking.values()}), 1) + + def test_acceleration_profiles_are_ordered_and_linearly_interpolated(self): + self.assertEqual(tuple(ACCEL_V), (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)) + self.assertEqual(set(ACCEL_V), {AccelProfile.eco, AccelProfile.normal, AccelProfile.sport}) + for values in ACCEL_V.values(): + self.assertEqual(len(values), len(SPEED_BP)) + self.assertTrue(all(after <= before for before, after in zip(values[:-1], values[1:], strict=True))) + for speed, expected in zip(SPEED_BP, values, strict=True): + self.assertEqual(np.interp(speed, SPEED_BP, values), expected) + for index in range(len(SPEED_BP) - 1): + midpoint = (SPEED_BP[index] + SPEED_BP[index + 1]) / 2.0 + expected = (values[index] + values[index + 1]) / 2.0 + self.assertAlmostEqual(np.interp(midpoint, SPEED_BP, values), expected) + for speed in SPEED_BP: + self.assertLess(np.interp(speed, SPEED_BP, ACCEL_V[AccelProfile.eco]), np.interp(speed, SPEED_BP, ACCEL_V[AccelProfile.normal])) + self.assertLess(np.interp(speed, SPEED_BP, ACCEL_V[AccelProfile.normal]), np.interp(speed, SPEED_BP, ACCEL_V[AccelProfile.sport])) + + def test_deceleration_curve_is_ascending_and_linearly_interpolated(self): + self.assertEqual(len(DECEL_V), len(SPEED_BP)) + self.assertTrue(all(after >= before for before, after in zip(DECEL_V[:-1], DECEL_V[1:], strict=True))) + for speed, expected in zip(SPEED_BP, DECEL_V, strict=True): + self.assertEqual(np.interp(speed, SPEED_BP, DECEL_V), expected) + for index in range(len(SPEED_BP) - 1): + midpoint = (SPEED_BP[index] + SPEED_BP[index + 1]) / 2.0 + expected = (DECEL_V[index] + DECEL_V[index + 1]) / 2.0 + self.assertAlmostEqual(np.interp(midpoint, SPEED_BP, DECEL_V), expected) + + +class TestElasticPace(OpenpilotTestCase): + def test_slower_lead_requests_early_decel_before_ttc_is_urgent(self): + instance = controller() + slower_lead = lead(present=True, distance=55.0, speed=14.0, track_id=7) + decision = settle(instance, radar(slower_lead), v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7) + + self.assertGreater(55.0 / (20.0 - 14.0), 5.0) + self.assertIsNotNone(decision.a_target) + self.assertLess(decision.a_target, 0.0) + self.assertTrue(instance.is_active) + self.assertFalse(decision.stock_safety_required) + + def test_closing_gap_produces_one_monotonic_braking_decision(self): instance = controller() outputs = [] - for probability in (0.32, 0.41, 0.36, 0.42, 0.31, 0.44, 0.35): - stock_allowed = probability > 0.4 - outputs.append(instance.update_allow_throttle(stock_allowed, probability)) + for distance in (65.0, 60.0, 55.0, 50.0, 45.0, 40.0, 35.0): + decision = settle( + instance, + radar(lead(present=True, distance=distance, speed=14.0, track_id=11)), + frames=4, + v_ego=20.0, + v_cruise=30.0, + stock_cruise_accel=0.7, + stock_mpc_accel=0.7, + ) + outputs.append(decision.a_target) - self.assertEqual(outputs, [False] * len(outputs)) - self.assertTrue(instance.update_allow_throttle(True, 0.46)) + self.assertTrue(all(value is not None and math.isfinite(value) for value in outputs)) + self.assertTrue(all(after <= before + 1e-9 for before, after in zip(outputs[:-1], outputs[1:], strict=True))) - def test_allow_throttle_uses_stock_when_disabled_and_low_speed_override(self): - instance = controller(enabled=False) - self.assertFalse(instance.update_allow_throttle(False, 0.39)) - self.assertTrue(instance.update_allow_throttle(True, 0.0, force_allow=True)) + def test_actuator_delay_makes_the_same_closing_lead_no_less_restrictive(self): + radar_state = radar(lead(present=True, distance=45.0, speed=14.0, track_id=12)) + short_delay = settle(controller(delay=0.05), radar_state, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7) + long_delay = settle(controller(delay=0.50), radar_state, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7) - instance.enabled = True - self.assertFalse(instance.update_allow_throttle(False, 0.0)) - self.assertTrue(instance.update_allow_throttle(True, 0.0, force_allow=True)) + self.assertLessEqual(long_delay.a_target, short_delay.a_target + 1e-9) - def test_allow_throttle_hysteresis_resets_with_controller(self): + def test_routine_brake_onset_is_jerk_limited(self): instance = controller() - self.assertFalse(instance.update_allow_throttle(False, 0.39)) + settle(instance, frames=30, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7) + restrictive = radar(lead(present=True, distance=45.0, speed=12.0, track_id=13)) + outputs = [ + update(instance, restrictive, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7).a_target + for _ in range(20) + ] - instance.reset() + self.assertTrue(all(value is not None for value in outputs)) + drops = [before - after for before, after in zip(outputs[:-1], outputs[1:], strict=True)] + self.assertLessEqual(max(drops), BRAKE_ONSET_JERK * DT_MDL + 1e-9) + self.assertLess(outputs[-1], outputs[0]) - self.assertTrue(instance.update_allow_throttle(True, 0.41)) - - def test_profile_limit_is_only_active_when_binding(self): - for profile in ACCEL_PROFILES: - with self.subTest(profile=profile): - instance = controller(profile=profile) - decision = update(instance, v_ego=10.0, stock_accel_max=1.0) - expected = profile_accel_max(profile, 10.0, 1.0) - self.assertAlmostEqual(decision.cruise_accel_max, expected) - self.assertEqual(decision.active, expected < 1.0) - - -class TestEarlyDecel(OpenpilotTestCase): - def test_early_decel_starts_from_the_previous_positive_plan(self): + def test_duplicate_radar_falls_back_to_stock(self): instance = controller() - decision = update(instance, restrictive_radar(), previous_plan_accel=0.48) + tracked = radar(lead(present=True, distance=50.0, speed=14.0, track_id=14)) + settle(instance, tracked, frames=20, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7) + duplicate_with_different_data = radar(lead(present=True, distance=15.0, speed=2.0, track_id=14)) - self.assertIsNotNone(decision.early_decel) - self.assertAlmostEqual(decision.early_decel, 0.48 - EARLY_DECEL_TIGHTEN_RATE * DT_MDL) - - def test_early_decel_is_nonpositive_and_tightens_at_bound(self): - instance = controller() - samples = [0.0] - for _ in range(10): - decision = update(instance, restrictive_radar()) - samples.append(decision.early_decel or 0.0) - - self.assertTrue(all(value <= 0.0 for value in samples)) - self.assertTrue(any(value < 0.0 for value in samples)) - for before, after in zip(samples[:-1], samples[1:], strict=True): - self.assertGreaterEqual(after - before, -EARLY_DECEL_TIGHTEN_RATE * DT_MDL - 1e-9) - - def test_fresh_dropout_and_unhealthy_radar_release_at_bound(self): - releases = ((radar(), True, True), (restrictive_radar(), False, False)) - for radar_state, radar_fresh, radar_healthy in releases: - with self.subTest(radar_fresh=radar_fresh, radar_healthy=radar_healthy): - instance = controller() - for _ in range(10): - update(instance, restrictive_radar()) - previous = instance.early_decel - self.assertIsNotNone(previous) - - for _ in range(30): - decision = update(instance, radar_state, radar_fresh=radar_fresh, radar_healthy=radar_healthy) - current = decision.early_decel or 0.0 - self.assertLessEqual(current, 0.0) - self.assertGreaterEqual(current, previous - 1e-9) - self.assertLessEqual(current - previous, EARLY_DECEL_RELEASE_RATE * DT_MDL + 1e-9) - previous = current - if decision.early_decel is None: - break - self.assertIsNone(instance.early_decel) - - def test_healthy_duplicate_radar_holds_early_decel(self): - instance = controller() - for _ in range(10): - update(instance, restrictive_radar()) - previous = instance.early_decel - - decision = update(instance, restrictive_radar(), radar_fresh=False, radar_healthy=True) - - self.assertEqual(decision.early_decel, previous) - self.assertEqual(instance.state, AccelControllerState.hold) - - def test_early_decel_is_bounded_by_profile_comfort(self): - for profile in ACCEL_PROFILES: - with self.subTest(profile=profile): - instance = controller(profile=profile) - for _ in range(30): - decision = update(instance, restrictive_radar()) - self.assertIsNotNone(decision.early_decel) - self.assertLessEqual(decision.early_decel, 0.0) - self.assertGreaterEqual(decision.early_decel, -0.35) - - -class TestLeadPlan(OpenpilotTestCase): - def test_more_restrictive_of_two_leads_is_selected(self): - radar_state = radar( - lead(present=True, distance=80.0, speed=12.0, track_id=10), - lead(present=True, distance=25.0, speed=6.0, track_id=20), + fallback = update( + instance, + duplicate_with_different_data, + v_ego=20.0, + v_cruise=30.0, + stock_cruise_accel=0.7, + stock_mpc_accel=0.7, + radar_valid=False, ) - plan = calculate_lead_plan(radar_state, 10.0, 0.0, 0.15, AccelProfile.normal) - self.assertEqual((plan.selected_lead, plan.selected_lead_track_id), (1, 20)) - self.assertGreater(plan.closing_speed, 0.0) - self.assertGreater(plan.required_decel, 0.0) + self.assertEqual(fallback, AccelDecision()) - def test_malformed_lead_is_ignored_without_hiding_valid_second_lead(self): - malformed = lead(present=True, distance=math.nan, speed=8.0) - valid = lead(present=True, distance=30.0, speed=7.0, accel=math.nan, tau=math.inf, track_id=math.nan) - plan = calculate_lead_plan(radar(malformed, valid), 10.0, 0.0, 0.15, AccelProfile.normal) - self.assertEqual(plan.selected_lead, 1) - self.assertEqual(plan.selected_lead_track_id, -1) - self.assertTrue(math.isfinite(plan.speed_ceiling)) + def test_equal_speed_lead_does_not_chase_a_small_gap_error(self): + instance = controller() + decision = settle(instance, radar(lead(present=True, distance=24.0, speed=10.0, track_id=15)), v_ego=10.0, v_cruise=25.0) + self.assertAlmostEqual(decision.a_target, 0.0) - def test_malformed_radar_and_scalar_inputs_fail_closed_to_no_extension(self): - for radar_state in (None, SimpleNamespace(), radar(lead(present=True, distance=-1.0, speed=5.0))): - with self.subTest(radar_state=radar_state): - instance = controller(profile=AccelProfile.sport) - decision = update(instance, radar_state) - self.assertIsNone(decision.early_decel) - self.assertFalse(decision.active) + def test_fresh_lead_dropout_does_not_immediately_reaccelerate(self): + instance = controller() + tracked = radar(lead(present=True, distance=35.0, speed=6.0, track_id=16)) + settle(instance, tracked, v_ego=12.0, v_cruise=25.0) + for _ in range(2): + self.assertLessEqual(update(instance, radar(), v_ego=12.0, v_cruise=25.0).a_target, 0.0) + + def test_second_lead_requires_persistence_and_can_only_add_braking(self): + instance = controller() + primary = lead(present=True, distance=80.0, speed=20.0, track_id=21, probability=0.95) + second = lead(present=True, distance=55.0, speed=14.0, track_id=22, probability=0.95) + decisions = [ + update(instance, radar(primary, second), v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7) + for _ in range(3) + ] + + self.assertGreaterEqual(decisions[1].a_target, decisions[0].a_target) + self.assertLess(decisions[2].a_target, decisions[1].a_target) + self.assertLess(decisions[2].a_target, 0.7) + + +class TestStopAndLaunch(OpenpilotTestCase): + def test_distant_stationary_lead_does_not_start_a_long_brake_event(self): + decision = settle(controller(), radar(lead(present=True, distance=300.0, speed=0.0, track_id=29)), v_ego=20.0, v_cruise=25.0, + stock_cruise_accel=0.8, stock_mpc_accel=0.8) + self.assertGreaterEqual(decision.a_target, 0.0) + + def test_stopped_lead_converges_to_stop_hold_without_positive_rebound(self): + instance = controller() + outputs = [] + should_stop = [] + samples = ((2.0, 11.0), (1.2, 8.5), (0.6, 7.0), (0.25, 6.3), (0.05, 6.05), (0.0, 6.0)) + for speed, distance in samples: + decision = settle( + instance, + radar(lead(present=True, distance=distance, speed=0.0, track_id=30)), + frames=10, + v_ego=speed, + v_cruise=8.0, + stock_cruise_accel=0.8, + stock_mpc_accel=-0.5, + stock_should_stop=speed < 0.3, + standstill=speed < 0.01, + ) + outputs.append(decision.a_target) + should_stop.append(decision.should_stop) + + self.assertTrue(all(value is not None and value <= 0.0 for value in outputs)) + self.assertFalse(any(should_stop[:3])) + self.assertTrue(any(should_stop[3:])) + self.assertTrue(should_stop[-1]) + + def test_same_track_departure_has_no_controller_added_dwell(self): + instance = controller() + stopped = radar(lead(present=True, distance=6.0, speed=0.0, track_id=31)) + held = settle( + instance, + stopped, + frames=8, + v_ego=0.0, + v_cruise=8.0, + stock_cruise_accel=0.8, + stock_mpc_accel=-0.5, + stock_should_stop=True, + stock_mpc_lead=0, + standstill=True, + ) + self.assertTrue(held.should_stop) + + departing = radar(lead(present=True, distance=6.1, speed=1.0, accel=1.0, track_id=31)) + released = update( + instance, + departing, + v_ego=0.0, + v_cruise=8.0, + stock_cruise_accel=0.8, + stock_mpc_accel=0.8, + stock_should_stop=False, + standstill=True, + ) + self.assertFalse(released.should_stop) + self.assertGreater(released.a_target, 0.0) + + def test_radar_fault_requires_clear_road_confirmation_before_launch(self): + instance = controller() + stopped = radar(lead(present=True, distance=6.0, speed=0.0, track_id=31)) + settle(instance, stopped, frames=8, v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5, + stock_should_stop=True, stock_mpc_lead=0, standstill=True) + self.assertEqual(update(instance, stopped, v_ego=0.0, v_cruise=8.0, radar_valid=False, standstill=True), AccelDecision()) + + clear_road = radar() + for _ in range(2): + held = update(instance, clear_road, v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5, + stock_should_stop=True, stock_mpc_lead=-1, standstill=True) + self.assertTrue(held.should_stop) + self.assertLessEqual(held.a_target, 0.0) + + released = update(instance, clear_road, v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5, + stock_should_stop=True, stock_mpc_lead=-1, standstill=True) + self.assertFalse(released.should_stop) + self.assertGreater(released.a_target, 0.0) + + def test_new_track_departure_requires_one_confirmation_frame(self): + instance = controller() + stopped = radar(lead(present=True, distance=6.0, speed=0.0, track_id=31)) + settle( + instance, + stopped, + frames=8, + v_ego=0.0, + v_cruise=8.0, + stock_cruise_accel=0.8, + stock_mpc_accel=-0.5, + stock_should_stop=True, + stock_mpc_lead=0, + standstill=True, + ) + + changed_track = radar(lead(present=True, distance=6.1, speed=1.0, accel=1.0, track_id=99)) + unconfirmed = update( + instance, + changed_track, + v_ego=0.0, + v_cruise=8.0, + stock_cruise_accel=0.8, + stock_mpc_accel=-0.5, + stock_should_stop=True, + stock_mpc_lead=0, + standstill=True, + ) + self.assertTrue(unconfirmed.should_stop) + self.assertLessEqual(unconfirmed.a_target, 0.0) + + confirmed_track = radar(lead(present=True, distance=6.2, speed=1.0, accel=1.0, track_id=99)) + confirmed = update( + instance, + confirmed_track, + v_ego=0.0, + v_cruise=8.0, + stock_cruise_accel=0.8, + stock_mpc_accel=-0.5, + stock_should_stop=True, + standstill=True, + ) + self.assertFalse(confirmed.should_stop) + self.assertGreater(confirmed.a_target, 0.0) + + def test_secondary_lead_can_veto_but_not_authorize_launch(self): + def stopped_instance(): + instance = controller() + stopped = radar(lead(present=True, distance=6.0, speed=0.0, track_id=60)) + settle(instance, stopped, frames=8, v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5, + stock_should_stop=True, stock_mpc_lead=0, standstill=True) + return instance + + departing = lead(present=True, distance=6.1, speed=1.0, accel=1.0, track_id=60) + blocked = update(stopped_instance(), radar(departing, lead(present=True, distance=4.0, speed=0.0, track_id=61)), + v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5, stock_should_stop=True, + stock_mpc_lead=0, standstill=True) + self.assertTrue(blocked.should_stop) + self.assertLessEqual(blocked.a_target, 0.0) + + buffered = update(stopped_instance(), radar(departing, lead(present=True, distance=6.1, speed=0.0, track_id=61)), + v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5, stock_should_stop=True, + stock_mpc_lead=0, standstill=True) + self.assertTrue(buffered.should_stop) + self.assertLessEqual(buffered.a_target, 0.0) + + released = update(stopped_instance(), radar(departing, lead(present=True, distance=20.0, speed=2.0, track_id=62)), + v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5, stock_should_stop=True, + stock_mpc_lead=0, standstill=True) + self.assertFalse(released.should_stop) + self.assertGreater(released.a_target, 0.0) + + def test_secondary_only_at_standstill_falls_back_to_stock(self): + decision = update(controller(), radar(lead_two=lead(present=True, distance=20.0, speed=2.0, track_id=63)), + v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=0.8, stock_mpc_lead=1, standstill=True) + self.assertEqual(decision, AccelDecision()) + + +class TestSafetyAndFallback(OpenpilotTestCase): + def test_nonurgent_stock_braking_does_not_own_normal_driving(self): + decision = settle( + controller(), + v_ego=15.0, + v_cruise=25.0, + stock_cruise_accel=1.0, + stock_mpc_accel=-0.6, + stock_should_stop=False, + ) + self.assertFalse(decision.stock_safety_required) + self.assertGreater(decision.a_target, -0.6) + + def test_stock_cruise_candidate_is_always_an_upper_bound(self): + decision = settle(controller(), v_ego=15.0, v_cruise=25.0, stock_cruise_accel=-0.3, stock_mpc_accel=0.8) + self.assertLessEqual(decision.a_target, -0.3) + + def test_fcw_bypasses_comfort_ramp_and_preserves_stock_braking(self): + instance = controller() + settle(instance, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7) + decision = update( + instance, + radar(lead(present=True, distance=15.0, speed=5.0, track_id=40)), + v_ego=20.0, + v_cruise=30.0, + stock_cruise_accel=0.7, + stock_mpc_accel=-2.5, + fcw=True, + ) + self.assertTrue(decision.stock_safety_required) + self.assertTrue(math.isfinite(decision.a_target)) + + def test_urgent_ttc_uses_stock_floor_without_fcw(self): + decision = update( + controller(), + radar(lead(present=True, distance=20.0, speed=10.0, track_id=41)), + v_ego=20.0, + v_cruise=30.0, + stock_cruise_accel=0.7, + stock_mpc_accel=-1.8, + ) + self.assertTrue(decision.stock_safety_required) + self.assertTrue(math.isfinite(decision.a_target)) + + def test_current_ttc_stays_authoritative_during_measured_deceleration(self): + decision = update( + controller(delay=0.95), + radar(lead(present=True, distance=70.0, speed=0.0, track_id=42)), + v_ego=20.0, + a_ego=-10.0, + v_cruise=30.0, + stock_cruise_accel=0.7, + stock_mpc_accel=-1.0, + ) + self.assertTrue(decision.stock_safety_required) + + def test_feasible_terminal_braking_does_not_escalate_to_late_stock_braking(self): + instance = controller(delay=0.20) + stationary = radar(lead(present=True, distance=60.0, speed=0.0, track_id=43)) + decision = settle( + instance, + stationary, + frames=60, + v_ego=15.0, + a_ego=-2.5, + v_cruise=30.0, + stock_cruise_accel=0.7, + stock_mpc_accel=-3.3, + stock_mpc_lead=0, + ) + self.assertFalse(decision.stock_safety_required) + self.assertGreaterEqual(decision.a_target, -2.5) + + unsafe = update(instance, radar(lead(present=True, distance=45.0, speed=0.0, track_id=43)), v_ego=15.0, a_ego=-2.5, + v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=-3.3, stock_mpc_lead=0) + self.assertTrue(unsafe.stock_safety_required) + + def test_terminal_feasibility_only_covers_the_selected_stock_source(self): + instance = controller() + primary = lead(present=True, distance=180.0, speed=0.0, track_id=44) + settle(instance, radar(primary), frames=60, v_ego=20.0, a_ego=-2.0, v_cruise=30.0, + stock_cruise_accel=0.7, stock_mpc_accel=-2.0, stock_mpc_lead=0) + + secondary = lead(present=True, distance=70.0, speed=0.0, track_id=45) + decision = update(instance, radar(primary, secondary), v_ego=20.0, a_ego=-2.0, v_cruise=30.0, + stock_cruise_accel=0.7, stock_mpc_accel=-3.0, stock_mpc_lead=1) + self.assertTrue(decision.stock_safety_required) + + def test_duplicate_radar_leads_share_terminal_feasibility(self): + instance = controller(delay=0.20) + primary = lead(present=True, distance=60.0, speed=0.0, track_id=-1) + settle(instance, radar(primary), frames=60, v_ego=15.0, a_ego=-2.5, v_cruise=30.0, + stock_cruise_accel=0.7, stock_mpc_accel=-3.3, stock_mpc_lead=0) + + duplicate = lead(present=True, distance=60.0, speed=0.0, track_id=-1) + decision = update(instance, radar(primary, duplicate), v_ego=15.0, a_ego=-2.5, v_cruise=30.0, + stock_cruise_accel=0.7, stock_mpc_accel=-3.3, stock_mpc_lead=1) + self.assertFalse(decision.stock_safety_required) + + def test_stock_emergency_releases_through_the_jerk_governor(self): + instance = controller() + settle(instance, stock_cruise_accel=0.8, stock_mpc_accel=0.8) + update(instance, stock_cruise_accel=0.8, stock_mpc_accel=-2.5, fcw=True) + released = update(instance, stock_cruise_accel=0.8, stock_mpc_accel=0.8, previous_plan_accel=-2.5) + self.assertLessEqual(released.a_target - (-2.5), RELEASE_JERK * DT_MDL + 1e-9) + + def test_unhealthy_radar_falls_back_to_stock(self): + decision = update( + controller(), + radar_valid=False, + stock_cruise_accel=0.7, + stock_mpc_accel=-0.4, + stock_should_stop=True, + ) + self.assertEqual(decision, AccelDecision()) + + def test_malformed_lead_never_produces_nonfinite_output(self): + malformed = radar(lead(present=True, distance=math.nan, speed=math.inf, accel=math.nan, track_id=50)) + decision = update(controller(), malformed, stock_cruise_accel=0.7, stock_mpc_accel=-0.3) + + self.assertEqual(decision, AccelDecision()) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py index fd6fffe3fc..49b35f867d 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py @@ -2,22 +2,27 @@ from types import SimpleNamespace from typing import Any from openpilot.cereal import custom, log -from openpilot.common.realtime import DT_MDL +from openpilot.common.params import Params from openpilot.common.test import OpenpilotTestCase from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcSource -from openpilot.selfdrive.controls.lib.longitudinal_planner import get_coast_accel, get_cruise_accel, get_max_accel from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState -from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ( - ACCEL_PROFILES, EARLY_DECEL_TIGHTEN_RATE, AccelProfile, profile_accel_scale, -) +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import AccelProfile from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource -def lead(*, present=False, distance=0.0, speed=0.0): +def lead(*, present=False, distance=0.0, speed=0.0, accel=0.0, track_id=-1, probability=1.0): return SimpleNamespace( - present=present, dRel=distance, vLeadK=speed, aLeadK=0.0, - aLeadTau=1.5, radarTrackId=-1, + present=present, + dRel=distance, + vRel=0.0, + vLead=speed, + vLeadK=speed, + aLeadK=accel, + aLeadTau=1.5, + radarTrackId=track_id, + modelProb=probability, + radar=True, ) @@ -26,74 +31,194 @@ def radar(lead_one=None, lead_two=None): class PlannerSM(dict): - def __init__(self, *, experimental=False, force_decel=False, radar_state=None, radar_time=100): + def __init__(self, *, experimental=False, force_decel=False, radar_state=None, radar_time=100, standstill=False, frame=0): super().__init__( radarState=radar_state or radar(), - carState=SimpleNamespace(vEgo=10.0, aEgo=0.0, vCruise=72.0), + carState=SimpleNamespace(vEgo=10.0, aEgo=0.0, vCruise=72.0, standstill=standstill), selfdriveState=SimpleNamespace( - enabled=True, experimentalMode=experimental, personality=log.LongitudinalPersonality.standard, + enabled=True, + experimentalMode=experimental, + personality=log.LongitudinalPersonality.standard, ), controlsState=SimpleNamespace(forceDecel=force_decel, longControlState=LongCtrlState.pid), ) self.valid = {"radarState": True} self.alive = {"radarState": True} self.logMonoTime = {"radarState": radar_time} + self.frame = frame def all_checks(self, service_list=None): return True -def accel_controller(*, enabled=True, profile=AccelProfile.normal): - cp = SimpleNamespace(longitudinalActuatorDelay=0.1, openpilotLongitudinalControl=True) - instance = AccelController(cp) - instance.enabled = enabled - instance.profile = profile - return instance +def accel_controller(): + CP = SimpleNamespace(longitudinalActuatorDelay=0.1, openpilotLongitudinalControl=True) + return AccelController(CP) -def planner_for_hook(*, enabled=True, profile=AccelProfile.normal): +def planner_for_hook(*, available=True, enabled=True, profile=AccelProfile.normal): planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP) dynamic_planner: Any = planner - dynamic_planner.accel_controller = accel_controller(enabled=enabled, profile=profile) + dynamic_planner.accel_controller = accel_controller() + dynamic_planner.accel_controller_available = available + dynamic_planner.accel_controller_enabled = enabled + dynamic_planner.accel_controller_profile = profile dynamic_planner.dec = SimpleNamespace(active=lambda: False) + dynamic_planner.model_accel_transition = SimpleNamespace(active=False) dynamic_planner.output_v_target = 30.0 dynamic_planner.previous_plan_accel = 0.0 dynamic_planner.a_cruise = 0.0 + dynamic_planner.fcw = False dynamic_planner._radar_fresh_this_cycle = True dynamic_planner._radar_healthy_this_cycle = True return planner -class TestPlannerHook(OpenpilotTestCase): - def test_disabled_e2e_and_force_decel_preserve_exact_candidate_tuple(self): +class TestPlannerOwnership(OpenpilotTestCase): + def test_active_controller_owns_routine_acc_instead_of_stock_mpc(self): + planner = planner_for_hook() + candidates = [(-0.6, MpcSource.lead0, False), (0.7, MpcSource.cruise, False)] + + result = planner.update_accel_controller(PlannerSM(), candidates) + + self.assertEqual(len(result), 1) + self.assertEqual(result[0][1:], (MpcSource.cruise, False)) + self.assertGreater(result[0][0], -0.6) + self.assertLessEqual(result[0][0], 0.7) + self.assertTrue(planner.accel_controller.is_active) + + def test_controller_candidate_uses_the_selected_primary_lead_source(self): + planner = planner_for_hook() + planner.output_v_target = 30.0 + sm = PlannerSM(radar_state=radar(lead(present=True, distance=55.0, speed=14.0, track_id=10))) + sm["carState"].vEgo = 20.0 + candidates = [(-0.6, MpcSource.lead0, False), (0.7, MpcSource.cruise, False)] + + result = planner.update_accel_controller(sm, candidates) + + self.assertEqual(len(result), 1) + self.assertEqual(result[0][1], MpcSource.lead0) + self.assertLess(result[0][0], 0.0) + + def test_only_second_lead_is_published_as_lead_one_source(self): + planner = planner_for_hook() + sm = PlannerSM(radar_state=radar(lead_two=lead(present=True, distance=55.0, speed=14.0, track_id=20))) + sm["carState"].vEgo = 20.0 + candidates = [(-0.6, MpcSource.lead0, False), (0.7, MpcSource.cruise, False)] + + for _ in range(3): + result = planner.update_accel_controller(sm, candidates) + + self.assertEqual(len(result), 1) + self.assertEqual(result[0][1], MpcSource.lead1) + + +class TestPlannerSafetyArbitration(OpenpilotTestCase): + def test_fake_mpc_lead_does_not_block_free_road_launch(self): + planner = planner_for_hook() + sm = PlannerSM(standstill=True) + sm["carState"].vEgo = 0.0 + candidates = [(0.0, MpcSource.lead1, True), (0.8, MpcSource.cruise, False)] + + result = planner.update_accel_controller(sm, candidates) + + self.assertEqual(len(result), 1) + self.assertGreater(result[0][0], 0.0) + self.assertFalse(result[0][2]) + + def test_fcw_keeps_stock_emergency_candidate_in_parallel(self): + planner = planner_for_hook() + planner.fcw = True + candidates = [(-2.5, MpcSource.lead0, False), (0.7, MpcSource.cruise, False)] + + result = planner.update_accel_controller(PlannerSM(), candidates) + + self.assertEqual(len(result), 2) + self.assertTrue(planner.accel_controller.is_active) + self.assertIn((-2.5, MpcSource.lead0, False), result) + self.assertEqual(min(result, key=lambda candidate: candidate[0]), (-2.5, MpcSource.lead0, False)) + + def test_urgent_ttc_keeps_stock_stop_and_braking_authoritative(self): + planner = planner_for_hook() + sm = PlannerSM(radar_state=radar(lead(present=True, distance=20.0, speed=10.0, track_id=30))) + sm["carState"].vEgo = 20.0 + candidates = [(-1.8, MpcSource.lead0, True), (0.7, MpcSource.cruise, False)] + + result = planner.update_accel_controller(sm, candidates) + + self.assertEqual(len(result), 2) + self.assertIn((-1.8, MpcSource.lead0, True), result) + self.assertTrue(any(stop for _, _, stop in result)) + self.assertEqual(min(result, key=lambda candidate: candidate[0]), (-1.8, MpcSource.lead0, True)) + + def test_confirmed_departure_suppresses_stale_stock_stop_in_same_frame(self): + planner = planner_for_hook() + stopped_sm = PlannerSM( + radar_state=radar(lead(present=True, distance=6.0, speed=0.0, track_id=31)), + standstill=True, + ) + stopped_sm["carState"].vEgo = 0.0 + stopped_candidates = [(-0.5, MpcSource.lead0, True), (0.8, MpcSource.cruise, False)] + for _ in range(2): + held = planner.update_accel_controller(stopped_sm, stopped_candidates) + self.assertTrue(any(stop for _, _, stop in held)) + + departure_sm = PlannerSM( + radar_state=radar(lead(present=True, distance=6.1, speed=1.0, accel=1.0, track_id=31)), + standstill=True, + ) + departure_sm["carState"].vEgo = 0.0 + released = planner.update_accel_controller(departure_sm, stopped_candidates) + + self.assertEqual(len(released), 1) + self.assertGreater(released[0][0], 0.0) + self.assertFalse(released[0][2]) + + +class TestPlannerFallbacks(OpenpilotTestCase): + def test_disabled_e2e_force_decel_and_unhealthy_radar_preserve_exact_candidates(self): candidates = ( (-0.4, MpcSource.lead0, True), (0.6, MpcSource.cruise, False), (0.2, MpcSource.e2e, False), ) + disabled = planner_for_hook(enabled=False) + blended = planner_for_hook() + forced = planner_for_hook() + unhealthy = planner_for_hook() + unhealthy._radar_healthy_this_cycle = False cases = ( - (planner_for_hook(enabled=False), PlannerSM()), - (planner_for_hook(profile=AccelProfile.eco), PlannerSM(experimental=True)), - (planner_for_hook(profile=AccelProfile.eco), PlannerSM(force_decel=True)), + (disabled, PlannerSM()), + (blended, PlannerSM(experimental=True)), + (forced, PlannerSM(force_decel=True)), + (unhealthy, PlannerSM()), ) + for planner, sm in cases: - with self.subTest(experimental=sm["selfdriveState"].experimentalMode, - force_decel=sm["controlsState"].forceDecel, - enabled=planner.accel_controller.enabled): + with self.subTest(planner=planner, sm=sm): result = planner.update_accel_controller(sm, candidates) self.assertIs(result, candidates) - self.assertEqual(result, candidates) - def test_missing_cruise_candidate_is_exact_noop(self): - planner = planner_for_hook(profile=AccelProfile.eco) - candidates = ((-0.4, MpcSource.lead0, True), (0.2, MpcSource.e2e, False)) - self.assertIs(planner.update_accel_controller(PlannerSM(), candidates), candidates) + def test_missing_cruise_or_mpc_candidate_is_exact_noop(self): + planner = planner_for_hook() + candidates_without_cruise = ((-0.4, MpcSource.lead0, True), (0.2, MpcSource.e2e, False)) + candidates_without_mpc = ((0.6, MpcSource.cruise, False), (0.2, MpcSource.e2e, False)) - def test_inactive_acc_does_not_publish_a_stale_positive_cruise_candidate(self): - planner = planner_for_hook(profile=AccelProfile.eco) + self.assertIs(planner.update_accel_controller(PlannerSM(), candidates_without_cruise), candidates_without_cruise) + self.assertIs(planner.update_accel_controller(PlannerSM(), candidates_without_mpc), candidates_without_mpc) + + def test_malformed_radar_is_exact_stock_fallback(self): + planner = planner_for_hook() + candidates = ((-0.4, MpcSource.lead0, True), (0.6, MpcSource.cruise, False)) + malformed_sm = PlannerSM(radar_state=SimpleNamespace()) + + self.assertIs(planner.update_accel_controller(malformed_sm, candidates), candidates) + + def test_disengaged_acc_cannot_publish_stale_positive_acceleration(self): + planner = planner_for_hook() sm = PlannerSM() sm["controlsState"].longControlState = LongCtrlState.off - candidates = [(-0.4, MpcSource.lead0, True), (0.6, MpcSource.cruise, False)] + candidates = [(-0.4, MpcSource.lead0, False), (0.6, MpcSource.cruise, False)] result = planner.update_accel_controller(sm, candidates) @@ -101,63 +226,6 @@ class TestPlannerHook(OpenpilotTestCase): self.assertEqual(planner.a_cruise, 0.0) self.assertEqual(planner.accel_controller.state, AccelControllerState.inactive) - stale_positive = [(1.3, MpcSource.lead0, False), (0.9, MpcSource.cruise, False)] - self.assertEqual(min(candidate[0] for candidate in planner.update_accel_controller(sm, stale_positive)), 0.0) - - braking = [(-0.4, MpcSource.lead0, True), (-0.2, MpcSource.cruise, True)] - self.assertIs(planner.update_accel_controller(sm, braking), braking) - - disabled = planner_for_hook(enabled=False) - self.assertIs(disabled.update_accel_controller(sm, candidates), candidates) - - blended = planner_for_hook(profile=AccelProfile.eco) - self.assertIs(blended.update_accel_controller(PlannerSM(experimental=True), candidates), candidates) - - def test_profiles_scale_final_stock_turn_and_coast_candidates(self): - cp = SimpleNamespace(steerRatio=15.0, wheelbase=2.7) - scenarios = ( - (25.0, 9.0, 0.35, 2.0, True), - (4.0, 0.0, 0.40, get_coast_accel(0.0), False), - ) - for v_ego, steering_angle, previous, accel_coast, allow_throttle in scenarios: - stock = get_cruise_accel( - False, 30.0, v_ego, previous, steering_angle, cp, DT_MDL, accel_coast, allow_throttle, - ) - self.assertGreater(stock, 0.0) - self.assertLess(stock, get_max_accel(v_ego)) - for profile in ACCEL_PROFILES: - with self.subTest(v_ego=v_ego, profile=profile): - planner = planner_for_hook(profile=profile) - sm = PlannerSM() - sm["carState"].vEgo = v_ego - candidates = [(-0.5, MpcSource.lead0, True), (stock, MpcSource.cruise, False)] - result = planner.update_accel_controller(sm, candidates) - self.assertEqual(result[0], candidates[0]) - self.assertEqual(result[1][1:], candidates[1][1:]) - self.assertAlmostEqual(result[1][0], stock * profile_accel_scale(profile, v_ego)) - - def test_early_decel_is_an_additive_nonpositive_cruise_candidate(self): - planner = planner_for_hook(profile=AccelProfile.sport) - sm = PlannerSM(radar_state=radar(lead(present=True, distance=25.0, speed=5.0))) - candidates = [(-0.02, MpcSource.lead0, False), (0.6, MpcSource.cruise, False)] - result = planner.update_accel_controller(sm, candidates) - self.assertEqual(result[:2], candidates) - self.assertEqual(len(result), 3) - self.assertEqual(result[-1][1:], (MpcSource.cruise, False)) - self.assertLessEqual(result[-1][0], 0.0) - - def test_early_decel_enters_from_the_previous_positive_command(self): - planner = planner_for_hook(profile=AccelProfile.sport) - planner.previous_plan_accel = 0.48 - sm = PlannerSM(radar_state=radar(lead(present=True, distance=25.0, speed=5.0))) - candidates = [(-0.02, MpcSource.lead0, False), (0.60, MpcSource.cruise, False)] - - result = planner.update_accel_controller(sm, candidates) - - self.assertEqual(result[:2], candidates) - self.assertAlmostEqual(result[-1][0], 0.48 - EARLY_DECEL_TIGHTEN_RATE * DT_MDL) - self.assertEqual(result[-1][1:], (MpcSource.cruise, False)) - def test_radar_freshness_requires_a_healthy_advanced_message(self): planner = planner_for_hook() planner._radar_log_mono_time = None @@ -173,19 +241,41 @@ class TestPlannerHook(OpenpilotTestCase): class TestParamsSchemaAndTelemetry(OpenpilotTestCase): - def test_params_enable_and_sanitize_profile(self): - instance = accel_controller(enabled=False) - instance.params.put_bool("AccelPersonalityEnabled", True, block=True) - instance.params.put("AccelPersonality", 99, block=True) - instance.update_params() - self.assertTrue(instance.is_enabled) - self.assertEqual(instance.profile, AccelProfile.sport) - self.assertEqual(instance.params.get("AccelPersonality"), AccelProfile.sport) + def test_planner_reads_and_sanitizes_controller_params(self): + planner = planner_for_hook(enabled=False) + planner.params = Params() + planner.params.put_bool("AccelPersonalityEnabled", False, block=True) - instance.params.put_bool("AccelPersonalityEnabled", False, block=True) - instance._param_frame = 0 - instance.update_params() - self.assertFalse(instance.is_enabled) + for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport): + planner.params.put("AccelPersonality", profile, block=True) + planner.read_accel_controller_params() + self.assertFalse(planner.accel_controller_enabled) + self.assertEqual(planner.accel_controller_profile, profile) + + planner.params.put_bool("AccelPersonalityEnabled", True, block=True) + for value, expected in ((-99, AccelProfile.eco), (99, AccelProfile.sport)): + planner.params.put("AccelPersonality", value, block=True) + planner.read_accel_controller_params() + self.assertTrue(planner.accel_controller_enabled) + self.assertEqual(planner.accel_controller_profile, expected) + self.assertEqual(planner.params.get("AccelPersonality"), expected) + + def test_planner_refreshes_controller_params_every_five_frames(self): + planner = planner_for_hook(enabled=False) + planner._radar_log_mono_time = None + planner.events_sp = SimpleNamespace(clear=lambda: None) + planner.dec.update = lambda sm: None + planner.e2e_alerts_helper = SimpleNamespace(update=lambda sm, events: None) + calls = [] + sm = PlannerSM() + planner.read_accel_controller_params = lambda: calls.append(sm.frame) + + for frame in range(10): + sm.frame = frame + sm.logMonoTime["radarState"] = frame + planner.update(sm) + + self.assertEqual(calls, [0, 5]) def test_schema_contract_and_round_trip(self): fields = custom.LongitudinalPlanSP.AccelController.schema.fields @@ -201,15 +291,15 @@ class TestParamsSchemaAndTelemetry(OpenpilotTestCase): message = custom.LongitudinalPlanSP.new_message() message.accelController.enabled = True message.accelController.active = True - message.accelController.profile = AccelProfile.sport - message.accelController.state = AccelControllerState.release + message.accelController.profile = custom.LongitudinalPlanSP.AccelController.Profile.sport + message.accelController.state = AccelControllerState.stopHold with custom.LongitudinalPlanSP.from_bytes(message.to_bytes()) as reader: self.assertTrue(reader.accelController.enabled) self.assertTrue(reader.accelController.active) - self.assertEqual(reader.accelController.profile, AccelProfile.sport) - self.assertEqual(reader.accelController.state, AccelControllerState.release) + self.assertEqual(reader.accelController.profile, custom.LongitudinalPlanSP.AccelController.Profile.sport) + self.assertEqual(reader.accelController.state, AccelControllerState.stopHold) - def test_minimal_controller_telemetry_is_published(self): + def test_controller_telemetry_is_published(self): planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP) dynamic_planner: Any = planner dynamic_planner.source = LongitudinalPlanSource.cruise @@ -217,36 +307,55 @@ class TestParamsSchemaAndTelemetry(OpenpilotTestCase): dynamic_planner.output_a_target = -0.1 dynamic_planner.events_sp = SimpleNamespace(to_msg=list) dynamic_planner.dec = SimpleNamespace(mode=lambda: "acc", enabled=lambda: False, active=lambda: False) - dynamic_planner.accel_controller = accel_controller(profile=AccelProfile.eco) - dynamic_planner.accel_controller.is_active = True + dynamic_planner.accel_controller = accel_controller() + dynamic_planner.accel_controller_available = True + dynamic_planner.accel_controller_enabled = True + dynamic_planner.accel_controller_profile = AccelProfile.sport dynamic_planner.accel_controller.state = AccelControllerState.restrict dynamic_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, + 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), ) dynamic_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, + 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, ) dynamic_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, + state=custom.LongitudinalPlanSP.SpeedLimit.AssistState.disabled, + is_enabled=False, + is_active=False, + output_v_target=20.0, + output_a_target=0.0, ) dynamic_planner.e2e_alerts_helper = SimpleNamespace(green_light_alert=False, lead_depart_alert=False) sent = {} + dynamic_planner.publish_longitudinal_plan_sp( - PlannerSM(), SimpleNamespace(send=lambda service, message: sent.update({service: message})), + PlannerSM(), + SimpleNamespace(send=lambda service, message: sent.update({service: message})), ) telemetry = sent["longitudinalPlanSP"].longitudinalPlanSP.accelController self.assertTrue(telemetry.enabled) self.assertTrue(telemetry.active) - self.assertEqual(telemetry.profile, AccelProfile.eco) + self.assertEqual(telemetry.profile, AccelProfile.sport) self.assertEqual(telemetry.state, AccelControllerState.restrict) - self.assertEqual(set(custom.LongitudinalPlanSP.AccelController.schema.fields), { - "enabled", "active", "shadowOnlyDEPRECATED", "profile", "state", - }) + self.assertEqual( + set(custom.LongitudinalPlanSP.AccelController.schema.fields), + {"enabled", "active", "shadowOnlyDEPRECATED", "profile", "state"}, + ) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index e55e4899ce..88b4eb117b 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -8,10 +8,13 @@ See the LICENSE.md file in the root directory for more details. from openpilot.cereal import messaging, custom from opendbc.car import structs from openpilot.common.constants import CV +from openpilot.common.params import Params from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcSource +from openpilot.sunnypilot import get_sanitize_int_param from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import AccelProfile from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, ModelAccelTransition 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 @@ -27,6 +30,11 @@ LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource class LongitudinalPlannerSP: def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc): self.accel_controller = AccelController(CP, dt=mpc.dt) + self.accel_controller_available = bool(CP.openpilotLongitudinalControl) + self.accel_controller_enabled = False + self.accel_controller_profile = AccelProfile.normal + self.params = Params() + self.read_accel_controller_params() self.events_sp = EventsSP() self.dec = DynamicExperimentalController(CP, mpc) self.model_accel_transition = ModelAccelTransition(mpc.dt) @@ -44,6 +52,10 @@ class LongitudinalPlannerSP: self.output_a_target = 0. self.previous_plan_accel = 0. + def read_accel_controller_params(self) -> None: + self.accel_controller_enabled = self.params.get_bool("AccelPersonalityEnabled") + self.accel_controller_profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params) + def is_e2e(self, sm: messaging.SubMaster) -> bool: experimental_mode = sm['selfdriveState'].experimentalMode if not self.dec.active(): @@ -58,42 +70,56 @@ class LongitudinalPlannerSP: ) def update_accel_controller(self, sm: messaging.SubMaster, candidates): - cruise_index = next((i for i, candidate in enumerate(candidates) if candidate[1] == MpcSource.cruise), -1) + if not candidates: + return candidates + cruise_index = next((i for i, candidate in enumerate(candidates[1:], start=1) if candidate[1] == MpcSource.cruise), -1) if cruise_index < 0: return candidates CS = sm['carState'] long_control_off = sm['controlsState'].longControlState == LongCtrlState.off - reset_state = ((long_control_off if self.accel_controller.available else not sm['selfdriveState'].enabled) + reset_state = ((long_control_off if self.accel_controller_available else not sm['selfdriveState'].enabled) or CS.vCruise == V_CRUISE_UNSET) cruise_accel = candidates[cruise_index][0] - acc_selected = not self.is_e2e(sm) - decision = self.accel_controller.update( - sm['radarState'], v_ego=CS.vEgo, a_ego=CS.aEgo, v_cruise=self.output_v_target, - follow_personality=sm['selfdriveState'].personality, engaged=not reset_state, - cruise_initialized=CS.vCruise != V_CRUISE_UNSET, acc_selected=acc_selected, - stock_accel_max=max(cruise_accel, 0.0), radar_fresh=self._radar_fresh_this_cycle, - radar_healthy=self._radar_healthy_this_cycle, - force_decel=sm['controlsState'].forceDecel, previous_plan_accel=self.previous_plan_accel, - ) - if self.accel_controller.is_enabled and acc_selected and reset_state: - # Do not carry an unactuated cruise ramp into engagement. - self.a_cruise = 0.0 - if cruise_accel > 0.0: - candidates = list(candidates) - _, source, stop = candidates[cruise_index] - candidates[cruise_index] = (0.0, source, stop) - return candidates - if decision.cruise_accel_max is None and decision.early_decel is None: + mpc_accel = candidates[0][0] + mpc_source = candidates[0][1] + radar_state = sm['radarState'] + lead_one_present = bool(getattr(getattr(radar_state, 'leadOne', None), 'present', False)) + lead_two_present = bool(getattr(getattr(radar_state, 'leadTwo', None), 'present', False)) + stock_mpc_lead = 0 if mpc_source == MpcSource.lead0 and lead_one_present else 1 if mpc_source == MpcSource.lead1 and lead_two_present else -1 + stock_should_stop = any(candidate[2] for candidate in candidates) + transitioning_from_e2e = self.model_accel_transition.active + acc_selected = not self.is_e2e(sm) and not transitioning_from_e2e + active = not reset_state and acc_selected and not sm['controlsState'].forceDecel + radar_valid = self._radar_fresh_this_cycle and self._radar_healthy_this_cycle + if not self.accel_controller_available or not self.accel_controller_enabled or not active: + self.accel_controller.reset() + if self.accel_controller_available and self.accel_controller_enabled and acc_selected and reset_state: + self.a_cruise = 0.0 + if cruise_accel > 0.0: + candidates = list(candidates) + _, source, stop = candidates[cruise_index] + candidates[cruise_index] = (0.0, source, stop) return candidates - candidates = list(candidates) - if decision.cruise_accel_max is not None: - _, source, stop = candidates[cruise_index] - candidates[cruise_index] = (min(cruise_accel, decision.cruise_accel_max), source, stop) - if decision.early_decel is not None: - candidates.append((decision.early_decel, MpcSource.cruise, False)) - return candidates + decision = self.accel_controller.update( + radar_state, v_ego=CS.vEgo, a_ego=CS.aEgo, v_cruise=self.output_v_target, follow_personality=sm['selfdriveState'].personality, + radar_valid=radar_valid, stock_cruise_accel=cruise_accel, stock_mpc_accel=mpc_accel, stock_should_stop=stock_should_stop, + fcw=self.fcw, standstill=CS.standstill, stock_mpc_lead=stock_mpc_lead, previous_plan_accel=self.previous_plan_accel, + profile=self.accel_controller_profile, + ) + if decision.a_target is None: + return candidates + + controller_source = (MpcSource.lead1 if self.accel_controller.selected_lead == 1 else + MpcSource.lead0 if self.accel_controller.selected_lead == 0 else MpcSource.cruise) + controller_candidate = (decision.a_target, controller_source, decision.should_stop) + if not decision.stock_safety_required: + return [controller_candidate] + + stock_candidate = min(candidates, key=lambda candidate: candidate[0]) + stock_safety_candidate = (stock_candidate[0], stock_candidate[1], stock_should_stop) + return [controller_candidate, stock_safety_candidate] def _update_radar_freshness(self, sm: messaging.SubMaster) -> bool: radar_log_mono_time = sm.logMonoTime['radarState'] @@ -136,7 +162,8 @@ class LongitudinalPlannerSP: def update(self, sm: messaging.SubMaster) -> None: self._radar_fresh_this_cycle = self._update_radar_freshness(sm) - self.accel_controller.update_params() + if sm.frame % 5 == 0: + self.read_accel_controller_params() self.events_sp.clear() self.dec.update(sm) self.e2e_alerts_helper.update(sm, self.events_sp) @@ -159,9 +186,9 @@ class LongitudinalPlannerSP: dec.active = self.dec.active() accel_controller = longitudinalPlanSP.accelController - accel_controller.enabled = self.accel_controller.is_enabled + accel_controller.enabled = self.accel_controller_available and self.accel_controller_enabled accel_controller.active = self.accel_controller.is_active - accel_controller.profile = self.accel_controller.profile + accel_controller.profile = self.accel_controller_profile accel_controller.state = self.accel_controller.state # Smart Cruise Control diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py index f64c11516b..5bab90e94d 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py @@ -1,3 +1,4 @@ +from contextlib import contextmanager import inspect from typing import Any from unittest import mock @@ -6,16 +7,16 @@ import numpy as np from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN from openpilot.common.test import OpenpilotTestCase +from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, LongitudinalMpc, LongitudinalPlanSource -from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import AccelProfile -from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP as Plant +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import NEUTRAL_ACCEL +from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP as Plant -def configure(plant, *, enabled=True, profile=AccelProfile.normal): - controller: Any = plant.planner.accel_controller - controller.enabled = enabled - controller.profile = profile - controller.update_params = lambda: None +def configure(plant, *, enabled=True): + planner: Any = plant.planner + planner.accel_controller_enabled = enabled + planner.read_accel_controller_params = lambda: None dec: Any = plant.planner.dec dec._enabled = False dec._read_params = lambda: None @@ -36,10 +37,32 @@ def record_candidates(plant): return snapshots +@contextmanager +def scripted_stock_candidates(plant, *, mpc_accel, cruise_accel, source=LongitudinalPlanSource.cruise): + values = {"mpc": float(mpc_accel), "cruise": float(cruise_accel)} + + def update_mpc(_radar_state, personality): + plant.planner.mpc.source = source + plant.planner.mpc.crash_cnt = 0 + + with ( + mock.patch.object(plant.planner.mpc, "update", side_effect=update_mpc), + mock.patch( + "openpilot.selfdrive.controls.lib.longitudinal_planner.get_accel_from_plan", + side_effect=lambda *_args, **_kwargs: values["mpc"], + ), + mock.patch( + "openpilot.selfdrive.controls.lib.longitudinal_planner.get_cruise_accel", + side_effect=lambda *_args, **_kwargs: values["cruise"], + ), + ): + yield values + + class TestAccelControllerPlannerIntegration(OpenpilotTestCase): def test_one_stock_mpc_solve_with_unmodified_bounds_and_api(self): plant = Plant(enabled=True, lead_relevancy=True, speed=20.0, distance_lead=70.0) - configure(plant, profile=AccelProfile.eco) + configure(plant) mpc: Any = plant.planner.mpc original_run = mpc.run run_calls = [] @@ -65,7 +88,7 @@ class TestAccelControllerPlannerIntegration(OpenpilotTestCase): def test_stock_lead_mpc_braking_remains_authoritative(self): plant = Plant(enabled=True, lead_relevancy=True, speed=20.0, distance_lead=30.0) - configure(plant, profile=AccelProfile.eco) + configure(plant) snapshots = record_candidates(plant) stock_mpc_won = False @@ -84,7 +107,7 @@ class TestAccelControllerPlannerIntegration(OpenpilotTestCase): def test_stock_should_stop_survives_controller_candidates(self): plant = Plant(enabled=True, lead_relevancy=True, speed=0.2, distance_lead=3.0) - configure(plant, profile=AccelProfile.normal) + configure(plant) snapshots = record_candidates(plant) result = plant.step(v_lead=0.0, v_cruise=8.0) @@ -180,7 +203,6 @@ class TestAccelControllerPlannerIntegration(OpenpilotTestCase): plant.planner.mpc.source = LongitudinalPlanSource.cruise plant.planner.mpc.crash_cnt = 0 - # Exercise final candidate arbitration without depending on the local ACADOS build. with ( mock.patch.object(plant.planner.mpc, "update", side_effect=update_mpc), mock.patch("openpilot.selfdrive.controls.lib.longitudinal_planner.get_accel_from_plan", return_value=1.94), @@ -197,9 +219,9 @@ class TestAccelControllerPlannerIntegration(OpenpilotTestCase): self.assertLessEqual(second - first, 0.15 + 1e-9) self.assertEqual(plant.planner.mpc.source, LongitudinalPlanSource.cruise) - def test_disengaged_cruise_state_cannot_leak_into_first_sport_accel(self): + def test_disengaged_cruise_state_cannot_leak_into_first_accel(self): plant = Plant(enabled=False, speed=10.0) - configure(plant, profile=AccelProfile.sport) + configure(plant) with ( mock.patch.object(plant.planner.mpc, "update", return_value=None), @@ -214,3 +236,166 @@ class TestAccelControllerPlannerIntegration(OpenpilotTestCase): self.assertGreater(first, 0.0) self.assertLessEqual(first, 0.10) + + +class TestAccelControllerClosedLoopAcceptance(OpenpilotTestCase): + def test_confirmed_lead_departure_releases_brake_in_the_same_planner_frame(self): + plant = Plant( + enabled=True, + lead_relevancy=True, + speed=0.0, + distance_lead=6.0, + actuator_model=PRIUS_TSS2_ROUTE_MODEL, + run_long_control=True, + ) + configure(plant) + + with scripted_stock_candidates(plant, mpc_accel=-0.5, cruise_accel=1.1, source=LongitudinalPlanSource.lead0) as stock: + for _ in range(12): + held = plant.step(v_lead=0.0, v_cruise=8.0) + self.assertTrue(held["should_stop"]) + self.assertLess(held["actuator_command"], 0.0) + + stock["mpc"] = 1.1 + unconfirmed = plant.step(v_lead=1.0, v_cruise=8.0) + self.assertTrue(unconfirmed["should_stop"]) + + confirmed = plant.step(v_lead=1.0, v_cruise=8.0) + self.assertFalse(confirmed["should_stop"]) + self.assertGreater(confirmed["a_target"], 0.0) + self.assertGreater(confirmed["actuator_command"], 0.0) + self.assertEqual(confirmed["long_control_state"], LongCtrlState.pid) + + for _ in range(12): + moving = plant.step(v_lead=1.0, v_cruise=8.0) + if moving["speed"] > 0.0: + break + self.assertGreater(moving["speed"], 0.0) + + def test_slower_lead_causes_routine_decel_while_ttc_is_still_long(self): + initial_gap = 55.0 + initial_ego_speed = 20.0 + lead_speed = 14.0 + plant = Plant(enabled=True, lead_relevancy=True, speed=initial_ego_speed, distance_lead=initial_gap) + configure(plant) + + with scripted_stock_candidates(plant, mpc_accel=0.7, cruise_accel=0.7): + result = plant.step(v_lead=lead_speed, v_cruise=30.0) + + self.assertGreater(initial_gap / (initial_ego_speed - lead_speed), 8.0) + self.assertTrue(result["controller_active"]) + self.assertLess(result["a_target"], 0.0) + + def test_monotonically_slowing_lead_does_not_cause_brake_gas_brake(self): + plant = Plant( + enabled=True, + lead_relevancy=True, + speed=18.0, + distance_lead=65.0, + actuator_delay=0.15, + actuator_lag=0.20, + ) + configure(plant) + targets = [] + + with scripted_stock_candidates(plant, mpc_accel=0.8, cruise_accel=0.8): + for frame in range(80): + lead_speed = max(8.0, 20.0 - 0.15 * frame) + targets.append(plant.step(v_lead=lead_speed, v_cruise=30.0)["a_target"]) + + phases = [] + for target in targets: + phase = 1 if target > NEUTRAL_ACCEL else -1 if target < -NEUTRAL_ACCEL else 0 + if phase and (not phases or phase != phases[-1]): + phases.append(phase) + + self.assertIn(-1, phases) + first_brake = phases.index(-1) + self.assertNotIn(1, phases[first_brake + 1:]) + + def test_terminal_stop_is_bounded_and_has_no_positive_rebound(self): + plant = Plant( + enabled=True, + lead_relevancy=True, + speed=5.0, + distance_lead=24.0, + actuator_model=PRIUS_TSS2_ROUTE_MODEL, + run_long_control=True, + ) + configure(plant) + samples = [] + + with scripted_stock_candidates(plant, mpc_accel=0.8, cruise_accel=0.8): + for _ in range(400): + result = plant.step(v_lead=0.0, v_cruise=8.0) + samples.append(result) + if result["speed"] == 0.0: + break + + self.assertEqual(samples[-1]["speed"], 0.0) + self.assertTrue(samples[-1]["should_stop"]) + self.assertGreaterEqual(plant.distance_lead - plant.distance, 5.8) + self.assertLessEqual(plant.distance_lead - plant.distance, 7.1) + + moving_samples = samples[:-1] + terminal_samples = [sample for sample in moving_samples if sample["speed"] <= 0.3] + self.assertTrue(terminal_samples) + self.assertLessEqual(max(sample["a_target"] for sample in terminal_samples), 1e-9) + self.assertLessEqual(max(sample["realized_acceleration"] for sample in terminal_samples), 0.02) + self.assertGreaterEqual(min(sample["realized_acceleration"] for sample in moving_samples), -1.2) + + realized_jerk = [ + abs(current["realized_acceleration"] - previous["realized_acceleration"]) / plant.ts + for previous, current in zip(moving_samples[:-1], moving_samples[1:], strict=True) + ] + self.assertLessEqual(max(realized_jerk), 1.2) + + def test_high_speed_stop_uses_planned_decel_before_stock_emergency(self): + plant = Plant(enabled=True, lead_relevancy=True, speed=20.0, distance_lead=120.0, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True) + configure(plant) + samples = [] + + with scripted_stock_candidates(plant, mpc_accel=0.8, cruise_accel=0.8): + for _ in range(800): + result = plant.step(v_lead=0.0, v_cruise=25.0) + samples.append(result) + if result["speed"] == 0.0 and result["should_stop"]: + break + + self.assertEqual(samples[-1]["speed"], 0.0) + self.assertTrue(samples[-1]["should_stop"]) + self.assertGreaterEqual(plant.distance_lead - plant.distance, 5.8) + self.assertGreaterEqual(min(sample["a_target"] for sample in samples), -2.5) + + def test_terminal_decel_ceiling_does_not_fade_before_highway_stop(self): + plant = Plant(enabled=True, lead_relevancy=True, speed=25.0, distance_lead=150.0, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True) + configure(plant) + samples = [] + + with scripted_stock_candidates(plant, mpc_accel=0.8, cruise_accel=0.8): + for _ in range(1000): + result = plant.step(v_lead=0.0, v_cruise=30.0) + samples.append(result) + if result["speed"] == 0.0 and result["should_stop"]: + break + + self.assertEqual(samples[-1]["speed"], 0.0) + self.assertTrue(samples[-1]["should_stop"]) + self.assertGreaterEqual(plant.distance_lead - plant.distance, 5.8) + self.assertGreaterEqual(min(sample["a_target"] for sample in samples), -2.5) + + def test_feasible_highway_stop_does_not_fall_back_to_late_stock_braking(self): + plant = Plant(enabled=True, lead_relevancy=True, speed=25.0, distance_lead=150.0, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True) + configure(plant) + samples = [] + + for _ in range(500): + result = plant.step(v_lead=0.0, v_cruise=30.0) + samples.append(result) + if result["speed"] == 0.0 and result["should_stop"]: + break + + self.assertEqual(samples[-1]["speed"], 0.0) + self.assertTrue(samples[-1]["should_stop"]) + self.assertGreaterEqual(plant.distance_lead - plant.distance, 5.8) + self.assertGreaterEqual(min(sample["a_target"] for sample in samples), -2.55) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_longcontrol_sp.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_longcontrol_sp.py index c6fd44cb55..ebf0f573c8 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_longcontrol_sp.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_longcontrol_sp.py @@ -613,8 +613,7 @@ class TestLongControlSP(OpenpilotTestCase): run_long_control=True, actuator_model=PRIUS_TSS2_ROUTE_MODEL, ) - plant.planner.accel_controller.enabled = True - plant.planner.accel_controller.profile = 1 + plant.planner.accel_controller_enabled = True plant.planner.dec._enabled = False commands = [] speeds = [] @@ -622,7 +621,7 @@ class TestLongControlSP(OpenpilotTestCase): solver_statuses = [] with ( - mock.patch.object(plant.planner.accel_controller, "update_params", return_value=None), + mock.patch.object(plant.planner, "read_accel_controller_params", return_value=None), mock.patch.object(plant.planner.dec, "_read_params", return_value=None), ): while plant.current_time < 5.0: diff --git a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py index f4c66522c1..b7209e6844 100644 --- a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py @@ -385,8 +385,6 @@ class PlantSP(Plant): "mpc_source": self.planner.mpc.source, "dec_mode": self.planner.dec.mode(), "controller_active": accel_controller.is_active, - "controller_accel_max": accel_controller.cruise_accel_max, - "controller_early_decel": accel_controller.early_decel, "model_action": { "desiredAcceleration": float(model_acceleration), "shouldStop": bool(model_should_stop), diff --git a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py index 74d4ee06bf..e99e578101 100644 --- a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py +++ b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py @@ -135,8 +135,6 @@ class TestPlantSP(OpenpilotTestCase): assert first["mpc_source"] is not None assert first["dec_mode"] in ("acc", "blended") assert "controller_active" in first - assert "controller_accel_max" in first - assert "controller_early_decel" in first assert first["lead_one_observation"] is not None assert first["truth_lead"] == first["lead_one_observation"] diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index 42fc108b05..2114d06d99 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -656,7 +656,7 @@ "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.", + "description": "Use the Accel Controller for smooth, early lead following and stop-and-go. Stock emergency braking remains available as a safety backstop.", "visibility": [ { "type": "capability", @@ -676,7 +676,7 @@ "key": "AccelPersonality", "widget": "multiple_button", "title": "Acceleration Profile", - "description": "Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly.", + "description": "Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across profiles.", "options": [ { "value": 0, @@ -696,11 +696,6 @@ "type": "capability", "field": "has_longitudinal_control", "equals": true - }, - { - "type": "param", - "key": "AccelPersonalityEnabled", - "equals": true } ] }, diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml index feafdd6a01..40a38100af 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml @@ -46,8 +46,8 @@ sections: - 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. + description: Use the Accel Controller for smooth, early lead following and stop-and-go. Stock emergency braking + remains available as a safety backstop. visibility: - $ref: '#/macros/longitudinal' enablement: @@ -55,8 +55,8 @@ sections: - key: AccelPersonality widget: multiple_button title: Acceleration Profile - description: Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts - and recovers more quickly. + description: Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across + profiles. options: - value: 0 label: Eco @@ -66,9 +66,6 @@ sections: 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/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py b/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py index 098580aa3b..ce13f37671 100644 --- a/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py +++ b/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py @@ -287,10 +287,17 @@ class TestKnownPanels(OpenpilotTestCase): {"value": 2, "label": "Sport"}, ] assert { - "type": "param", - "key": "AccelPersonalityEnabled", + "type": "capability", + "field": "has_longitudinal_control", + "equals": True, + } in items["AccelPersonalityEnabled"]["enablement"] + assert { + "type": "capability", + "field": "has_longitudinal_control", "equals": True, } in items["AccelPersonality"]["enablement"] + profile_enable_keys = {rule.get("key") for rule in items["AccelPersonality"]["enablement"] if rule.get("type") == "param"} + assert "AccelPersonalityEnabled" not in profile_enable_keys class TestKnownVehicleSettings(OpenpilotTestCase):