Always On Lateral

Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>

frogpilot/common/frogpilot_variables.py

@@ -14,6 +14,7 @@ from cereal import car, custom, log
 from opendbc.car import gen_empty_fingerprint
 from opendbc.car.car_helpers import interfaces
 from opendbc.car.gm.values import GMFlags
+from opendbc.car.hyundai.values import HyundaiFlags
 from opendbc.car.interfaces import CarInterfaceBase, GearShifter
 from opendbc.car.mock.values import CAR as MOCK
 from opendbc.car.subaru.values import SubaruFlags
@@ -207,9 +208,11 @@ class FrogPilotVariables:
     toggle.has_zss = toggle.car_make == "toyota" and bool(FPCP.flags & ToyotaFrogPilotFlags.ZSS.value)
     is_angle_car = CP.steerControlType == car.CarParams.SteerControlType.angle
     latAccelFactor = CP.lateralTuning.torque.latAccelFactor
+    toggle.lkas_allowed_for_aol = toggle.car_make == "hyundai" and bool(CP.flags & HyundaiFlags.CANFD or CP.flags & HyundaiFlags.HAS_LDA_BUTTON)
     longitudinalActuatorDelay = CP.longitudinalActuatorDelay
     toggle.openpilot_longitudinal = CP.openpilotLongitudinalControl and not toggle.disable_openpilot_long
     pcm_cruise = CP.pcmCruise
+    prohibited_main_aol = not toggle.openpilot_longitudinal and toggle.car_make == "hyundai" and bool(CP.flags & HyundaiFlags.CANFD or CP.flags & HyundaiFlags.HAS_LDA_BUTTON)
     startAccel = CP.startAccel
     stopAccel = CP.stopAccel
     steerActuatorDelay = CP.steerActuatorDelay
@@ -265,6 +268,11 @@ class FrogPilotVariables:
     toggle.warningSoft_volume = self.get_value("WarningSoftVolume", cast=float, condition=toggle.alert_volume_controller)
     toggle.warningImmediate_volume = max(self.get_value("WarningImmediateVolume", cast=float, condition=toggle.alert_volume_controller, default=25), 25)
 
+    toggle.always_on_lateral = self.get_value("AlwaysOnLateral")
+    toggle.always_on_lateral_lkas = toggle.always_on_lateral and toggle.lkas_allowed_for_aol and self.get_value("AlwaysOnLateralLKAS")
+    toggle.always_on_lateral_main = toggle.always_on_lateral and not prohibited_main_aol and not toggle.always_on_lateral_lkas
+    toggle.always_on_lateral_pause_speed = self.get_value("PauseAOLOnBrake", cast=float, condition=toggle.always_on_lateral)
+
     toggle.automatic_updates = self.get_value("AutomaticUpdates", condition=(self.release_branch or self.vetting_branch), default=True) and not BACKUP_PATH.is_file()
 
     car_model = self.params.get("CarModel")

frogpilot/controls/frogpilot_card.py

@@ -1,6 +1,10 @@
 #!/usr/bin/env python3
+from opendbc.safety import ALTERNATIVE_EXPERIENCE
 from openpilot.common.params import Params
 from openpilot.selfdrive.car.cruise import ButtonType
+from openpilot.selfdrive.selfdrived.events import ET
+
+from openpilot.frogpilot.common.frogpilot_variables import NON_DRIVING_GEARS
 
 class FrogPilotCard:
   def __init__(self, CP, FPCP):
@@ -10,9 +14,28 @@ class FrogPilotCard:
     self.params_memory = Params(memory=True)
 
     self.accel_pressed = False
+    self.always_on_lateral_allowed = False
     self.decel_pressed = False
 
+    self.always_on_lateral_set = bool(FPCP.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL)
+
   def update(self, carState, frogpilotCarState, sm, frogpilot_toggles):
+    if self.CP.brand == "hyundai":
+      for be in carState.buttonEvents:
+        if be.type == ButtonType.lkas and be.pressed and frogpilot_toggles.always_on_lateral_lkas:
+          self.always_on_lateral_allowed = not self.always_on_lateral_allowed
+        elif be.type == ButtonType.mainCruise and be.pressed and frogpilot_toggles.always_on_lateral_main:
+          self.always_on_lateral_allowed = not self.always_on_lateral_allowed
+    elif frogpilot_toggles.always_on_lateral_main:
+      self.always_on_lateral_allowed = carState.cruiseState.available
+
+    self.always_on_lateral_enabled = self.always_on_lateral_allowed and self.always_on_lateral_set
+    self.always_on_lateral_enabled &= carState.gearShifter not in NON_DRIVING_GEARS
+    self.always_on_lateral_enabled &= sm["frogpilotPlan"].lateralCheck
+    self.always_on_lateral_enabled &= sm["liveCalibration"].calPerc >= 1
+    self.always_on_lateral_enabled &= (ET.IMMEDIATE_DISABLE not in sm["selfdriveState"].alertType + sm["frogpilotSelfdriveState"].alertType)
+    self.always_on_lateral_enabled &= not (carState.brakePressed and carState.vEgo < frogpilot_toggles.always_on_lateral_pause_speed) or carState.standstill
+
     if sm.updated["frogpilotPlan"] or any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in carState.buttonEvents):
       self.accel_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in carState.buttonEvents)
 
@@ -20,6 +43,7 @@ class FrogPilotCard:
       self.decel_pressed = any(be.type == ButtonType.decelCruise for be in carState.buttonEvents)
 
     frogpilotCarState.accelPressed = self.accel_pressed
+    frogpilotCarState.alwaysOnLateralEnabled = self.always_on_lateral_enabled
     frogpilotCarState.decelPressed = self.decel_pressed
 
     return frogpilotCarState

frogpilot/ui/frogpilot_ui.cc

@@ -12,6 +12,7 @@ static void update_state(FrogPilotUIState *fs) {
   }
   if (fpsm.updated("frogpilotCarState")) {
     const cereal::FrogPilotCarState::Reader &frogpilotCarState = fpsm["frogpilotCarState"].getFrogpilotCarState();
+    frogpilot_scene.always_on_lateral_active = !frogpilot_scene.enabled && frogpilotCarState.getAlwaysOnLateralEnabled();
   }
   if (fpsm.updated("frogpilotPlan")) {
     const cereal::FrogPilotPlan::Reader &frogpilotPlan = fpsm["frogpilotPlan"].getFrogpilotPlan();

frogpilot/ui/frogpilot_ui.h

@@ -6,6 +6,7 @@
 #include "frogpilot/ui/qt/widgets/frogpilot_controls.h"
 
 struct FrogPilotUIScene {
+  bool always_on_lateral_active;
   bool enabled;
   bool frogpilot_panel_active;
   bool online;