From e791efd1448a5e5f4ff47788036db419fe6b21b0 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Sat, 29 Jun 2024 19:27:47 -0700 Subject: [PATCH] Controls - Quality of Life - Force Standstill State Keeps openpilot in the 'standstill' state until the gas pedal is pressed. --- selfdrive/frogpilot/controls/frogpilot_planner.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index f7e7ebc14..1659d2a59 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -42,6 +42,7 @@ class FrogPilotPlanner: self.lead_one = Lead() self.mtsc = MapTurnSpeedController() + self.override_force_stop = False self.slower_lead = False self.tracking_lead = False @@ -87,6 +88,7 @@ class FrogPilotPlanner: self.road_curvature = abs(float(calculate_road_curvature(modelData, v_ego))) if v_ego > CRUISING_SPEED: + self.override_force_stop = False self.tracking_lead = self.lead_one.status else: self.tracking_lead &= self.lead_one.status @@ -190,8 +192,15 @@ class FrogPilotPlanner: else: self.mtsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0 - targets = [self.mtsc_target] - self.v_cruise = min([target if target > CRUISING_SPEED else v_cruise for target in targets]) + if frogpilot_toggles.force_standstill and v_ego < 1 and not self.override_force_stop: + if carState.gasPressed: + self.override_force_stop = True + else: + self.v_cruise = -1 + + else: + targets = [self.mtsc_target] + self.v_cruise = 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')