From 9580a1efce5454b360d42d3ad1c25f093a32a0af Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Tue, 23 Jul 2024 00:59:44 -0700
Subject: [PATCH] Controls - Quality of Life - Force Standstill State

Keeps openpilot in the 'standstill' state until the gas pedal is pressed.
---
 selfdrive/controls/controlsd.py                 | 17 +++++++++++++----
 selfdrive/controls/lib/events.py                |  8 ++++++++
 .../frogpilot/controls/frogpilot_planner.py     | 17 +++++++++++++++--
 3 files changed, 36 insertions(+), 6 deletions(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 7878db33e..c384621bf 100644
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -196,6 +196,8 @@ class Controls:
     self.onroad_distance_pressed = False
     self.openpilot_crashed_triggered = False
     self.previous_traffic_mode = False
+    self.resume_pressed = False
+    self.resume_previously_pressed = False
     self.update_toggles = False
 
     self.display_timer = 0
@@ -237,8 +239,8 @@ class Controls:
       return
 
     # Block resume if cruise never previously enabled
-    resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents)
-    if not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and resume_pressed:
+    self.resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents)
+    if not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and self.resume_pressed:
       self.events.add(EventName.resumeBlocked)
 
     if not self.CP.notCar:
@@ -424,7 +426,7 @@ class Controls:
         self.events.add(EventName.modeldLagging)
 
     # Update FrogPilot events
-    self.update_frogpilot_events(CS, self.sm['frogpilotCarState'])
+    self.update_frogpilot_events(CS, self.sm['frogpilotCarState'], self.sm['frogpilotPlan'])
 
   def data_sample(self):
     """Receive data from sockets"""
@@ -902,7 +904,10 @@ class Controls:
       e.set()
       t.join()
 
-  def update_frogpilot_events(self, frogpilotCarState, CS):
+  def update_frogpilot_events(self, CS, frogpilotCarState, frogpilotPlan):
+    if frogpilotPlan.forcingStop:
+      self.events.add(EventName.forcingStop)
+
     if not self.openpilot_crashed_triggered and os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')):
       self.events.add(EventName.openpilotCrashed)
       self.openpilot_crashed_triggered = True
@@ -961,8 +966,12 @@ class Controls:
           self.experimental_mode = not self.experimental_mode
           self.params.put_bool_nonblocking("ExperimentalMode", self.experimental_mode)
 
+    if self.sm.frame % 10 == 0 or self.resume_pressed:
+      self.resume_previously_pressed = self.resume_pressed
+
     FPCC = custom.FrogPilotCarControl.new_message()
     FPCC.alwaysOnLateral = self.always_on_lateral_active
+    FPCC.resumePressed = self.resume_pressed or self.resume_previously_pressed
 
     return FPCC
 
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index 83b34c86b..42360da39 100755
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -986,6 +986,14 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
     ET.NO_ENTRY: NoEntryAlert("Please don't use the 'Development' branch!"),
   },
 
+  EventName.forcingStop: {
+    ET.WARNING: Alert(
+      "Forcing the car to stop",
+      "Press the gas pedal or 'Resume' button to override",
+      AlertStatus.frogpilot, AlertSize.mid,
+      Priority.MID, VisualAlert.none, AudibleAlert.prompt, 1.),
+  },
+
   EventName.noLaneAvailable: {
     ET.PERMANENT: no_lane_available_alert,
   },
diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py
index d42292a71..7d7c30727 100644
--- a/selfdrive/frogpilot/controls/frogpilot_planner.py
+++ b/selfdrive/frogpilot/controls/frogpilot_planner.py
@@ -47,7 +47,9 @@ class FrogPilotPlanner:
     self.lead_one = Lead()
     self.mtsc = MapTurnSpeedController()
 
+    self.forcing_stop = False
     self.model_stopped = False
+    self.override_force_stop = False
     self.slower_lead = False
     self.tracking_lead = False
 
@@ -96,6 +98,8 @@ class FrogPilotPlanner:
 
     self.model_length = modelData.position.x[MODEL_LENGTH - 1]
     self.model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME
+    self.override_force_stop |= carState.gasPressed
+    self.override_force_stop |= frogpilotCarControl.resumePressed
     self.road_curvature = calculate_road_curvature(modelData, v_ego) if not carState.standstill and driving_gear else 1
 
     self.set_acceleration(controlsState, frogpilotCarState, v_cruise, v_ego, frogpilot_toggles)
@@ -207,8 +211,15 @@ class FrogPilotPlanner:
     else:
       self.mtsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0
 
-    targets = [self.mtsc_target]
-    self.v_cruise = float(min([target if target > CRUISING_SPEED else v_cruise for target in targets]))
+    if frogpilot_toggles.force_standstill and carState.standstill and not self.override_force_stop and controlsState.enabled:
+      self.forcing_stop = True
+      self.v_cruise = -1
+
+    else:
+      self.forcing_stop = False
+
+      targets = [self.mtsc_target]
+      self.v_cruise = float(min([target if target > CRUISING_SPEED else v_cruise for target in targets]))
 
   def publish(self, sm, pm, frogpilot_toggles):
     frogpilot_plan_send = messaging.new_message('frogpilotPlan')
@@ -226,6 +237,8 @@ class FrogPilotPlanner:
 
     frogpilotPlan.conditionalExperimentalActive = self.cem.experimental_mode
 
+    frogpilotPlan.forcingStop = self.forcing_stop
+
     frogpilotPlan.laneWidthLeft = self.lane_width_left
     frogpilotPlan.laneWidthRight = self.lane_width_right