diff --git a/frogpilot/controls/frogpilot_planner.py b/frogpilot/controls/frogpilot_planner.py index d14c6f53c..3392b9bc4 100644 --- a/frogpilot/controls/frogpilot_planner.py +++ b/frogpilot/controls/frogpilot_planner.py @@ -123,6 +123,9 @@ class FrogPilotPlanner: frogpilotPlan.experimentalMode = self.frogpilot_cem.experimental_mode + frogpilotPlan.forcingStop = self.frogpilot_vcruise.forcing_stop + frogpilotPlan.forcingStopLength = self.frogpilot_vcruise.tracked_model_length + frogpilotPlan.frogpilotEvents = self.frogpilot_events.events.to_msg() frogpilotPlan.frogpilotToggles = json.dumps(vars(frogpilot_toggles)) diff --git a/frogpilot/controls/lib/frogpilot_events.py b/frogpilot/controls/lib/frogpilot_events.py index 757173309..94f016cd8 100644 --- a/frogpilot/controls/lib/frogpilot_events.py +++ b/frogpilot/controls/lib/frogpilot_events.py @@ -30,6 +30,9 @@ class FrogPilotEvents: else: self.max_acceleration = 0 + if self.frogpilot_planner.frogpilot_vcruise.forcing_stop: + self.events.add(FrogPilotEventName.forcingStop) + if self.error_log.is_file(): self.events.add(FrogPilotEventName.openpilotCrashed) diff --git a/frogpilot/controls/lib/frogpilot_vcruise.py b/frogpilot/controls/lib/frogpilot_vcruise.py index e18be27ea..e876eeec9 100644 --- a/frogpilot/controls/lib/frogpilot_vcruise.py +++ b/frogpilot/controls/lib/frogpilot_vcruise.py @@ -1,16 +1,41 @@ #!/usr/bin/env python3 from openpilot.common.constants import CV +from openpilot.common.realtime import DT_MDL -from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED +from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, PLANNER_TIME from openpilot.frogpilot.controls.lib.curve_speed_controller import CurveSpeedController +OVERRIDE_FORCE_STOP_TIMER = 10 + class FrogPilotVCruise: def __init__(self, FrogPilotPlanner): self.frogpilot_planner = FrogPilotPlanner self.csc = CurveSpeedController(self) + self.forcing_stop = False + self.override_force_stop = False + + self.override_force_stop_timer = 0 + def update(self, long_control_active, now, time_validated, v_cruise, v_ego, sm, frogpilot_toggles): + force_stop = self.frogpilot_planner.frogpilot_cem.stop_light_detected and long_control_active and frogpilot_toggles.force_stops + force_stop &= self.frogpilot_planner.model_stopped + force_stop &= self.override_force_stop_timer <= 0 + + self.force_stop_timer = self.force_stop_timer + DT_MDL if force_stop else 0 + + force_stop_enabled = self.force_stop_timer >= 1 + + self.override_force_stop |= sm["carState"].gasPressed + self.override_force_stop |= sm["frogpilotCarState"].accelPressed + self.override_force_stop &= force_stop_enabled + + if self.override_force_stop: + self.override_force_stop_timer = OVERRIDE_FORCE_STOP_TIMER + elif self.override_force_stop_timer > 0: + self.override_force_stop_timer -= DT_MDL + v_cruise_cluster = max(sm["carState"].vCruiseCluster * CV.KPH_TO_MS, v_cruise) v_cruise_diff = v_cruise_cluster - v_cruise @@ -32,7 +57,18 @@ class FrogPilotVCruise: self.csc_target = v_cruise - targets = [self.csc_target, v_cruise] - v_cruise = min([target if target >= CRUISING_SPEED else v_cruise for target in targets]) + if force_stop_enabled and not self.override_force_stop: + self.forcing_stop |= not sm["carState"].standstill + + self.tracked_model_length = max(self.tracked_model_length - (v_ego * DT_MDL), 0) + v_cruise = min((self.tracked_model_length // PLANNER_TIME), v_cruise) + + else: + self.forcing_stop = False + + self.tracked_model_length = self.frogpilot_planner.model_length + + targets = [self.csc_target, v_cruise] + v_cruise = min([target if target >= CRUISING_SPEED else v_cruise for target in targets]) return v_cruise diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 2749d80cc..f99f2a27d 100644 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -410,6 +410,17 @@ def custom_startup_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM return StartupAlert(frogpilot_toggles.startup_alert_top, frogpilot_toggles.startup_alert_bottom, alert_status=FrogPilotAlertStatus.frogpilot) +def forcing_stop_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality, frogpilot_toggles: SimpleNamespace) -> Alert: + model_length = sm["frogpilotPlan"].forcingStopLength + model_length_msg = f"{model_length:.1f} meters" if metric else f"{model_length * CV.METER_TO_FOOT:.1f} feet" + + return Alert( + f"Forcing the car to stop in {model_length_msg}", + "Press the gas pedal or 'Resume' button to override", + FrogPilotAlertStatus.frogpilot, AlertSize.mid, + Priority.MID, VisualAlert.none, AudibleAlert.prompt, 1.) + + EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { # ********** events with no alerts ********** @@ -1055,6 +1066,10 @@ FROGPILOT_EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { ET.PERMANENT: custom_startup_alert, }, + FrogPilotEventName.forcingStop: { + ET.WARNING: forcing_stop_alert, + }, + FrogPilotEventName.openpilotCrashed: { ET.IMMEDIATE_DISABLE: Alert( "openpilot crashed",