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;

opendbc_repo/opendbc/car/hyundai/values.py

@@ -70,6 +70,7 @@ class HyundaiSafetyFlags(IntFlag):
 
 # FrogPilot variables
 class HyundaiFrogPilotSafetyFlags(IntFlag):
+  HAS_LDA_BUTTON = 1024
 
 
 class HyundaiFlags(IntFlag):

opendbc_repo/opendbc/car/interfaces.py

@@ -199,6 +199,9 @@ class CarInterfaceBase(ABC):
 
           fp_ret.isHDA2 = hda2
 
+        if CP.flags & HyundaiFlags.HAS_LDA_BUTTON:
+          fp_ret.safetyConfigs[-1].safetyParam |= HyundaiFrogPilotSafetyFlags.HAS_LDA_BUTTON.value
+
       elif platform in TOYOTA:
         fp_ret.canUsePedal = not CP.autoResumeSng
         fp_ret.canUseSDSU = candidate not in UNSUPPORTED_DSU_CAR and candidate not in TSS2_CAR

opendbc_repo/opendbc/car/rivian/carstate.py

@@ -54,7 +54,7 @@ class CarState(CarStateBase):
     ret.cruiseState.speed = self.last_speed * CV.MPH_TO_MS  # detected speed limit
     if not self.CP.openpilotLongitudinalControl:
       ret.cruiseState.speed = -1
-    ret.cruiseState.available = True  # cp.vl["VDM_AdasSts"]["VDM_AdasInterfaceStatus"] == 1
+    ret.cruiseState.available = cp.vl["VDM_AdasSts"]["VDM_AdasInterfaceStatus"] in (1, 2)
     ret.cruiseState.standstill = cp.vl["VDM_AdasSts"]["VDM_AdasVehicleHoldStatus"] == 1
 
     # ACM_Status->ACM_FaultSupervisorState normally 1, appears to go to 3 when either:

opendbc_repo/opendbc/safety/__init__.py

@@ -10,3 +10,4 @@ class ALTERNATIVE_EXPERIENCE:
   ALLOW_AEB = 16
 
   # FrogPilot variables
+  ALWAYS_ON_LATERAL = 32

opendbc_repo/opendbc/safety/declarations.h

@@ -269,6 +269,10 @@ extern int cruise_button_prev;
 extern bool safety_rx_checks_invalid;
 
 // FrogPilot variables
+extern bool aol_allowed;
+extern bool lkas_button_prev;
+extern bool lkas_on;
+extern bool main_button_prev;
 
 // for safety modes with torque steering control
 extern int desired_torque_last;       // last desired steer torque
@@ -312,6 +316,7 @@ extern bool enable_gas_interceptor;
 #define ALT_EXP_ALLOW_AEB 16
 
 // FrogPilot variables
+#define ALT_EXP_ALWAYS_ON_LATERAL 32
 
 extern int alternative_experience;
 

opendbc_repo/opendbc/safety/lateral.h

@@ -61,7 +61,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee
   bool violation = false;
   uint32_t ts = microsecond_timer_get();
 
-  if (controls_allowed) {
+  if (aol_allowed || controls_allowed) {
     // Some safety models support variable torque limit based on vehicle speed
     int max_torque = limits.max_torque;
     if (limits.dynamic_max_torque) {
@@ -96,7 +96,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee
   }
 
   // no torque if controls is not allowed
-  if (!controls_allowed && (desired_torque != 0)) {
+  if (!(aol_allowed || controls_allowed) && (desired_torque != 0)) {
     violation = true;
   }
 
@@ -138,7 +138,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee
   }
 
   // reset to 0 if either controls is not allowed or there's a violation
-  if (violation || !controls_allowed) {
+  if (violation || !(aol_allowed || controls_allowed)) {
     valid_steer_req_count = 0;
     invalid_steer_req_count = 0;
     desired_torque_last = 0;
@@ -176,7 +176,7 @@ static bool rt_angle_rate_limit_check(AngleSteeringLimits limits) {
 bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const AngleSteeringLimits limits) {
   bool violation = false;
 
-  if (controls_allowed && steer_control_enabled) {
+  if ((aol_allowed || controls_allowed) && steer_control_enabled) {
     // convert floating point angle rate limits to integers in the scale of the desired angle on CAN,
     // add 1 to not false trigger the violation. also fudge the speed by 1 m/s so rate limits are
     // always slightly above openpilot's in case we read an updated speed in between angle commands
@@ -262,12 +262,12 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
   }
 
   // No angle control allowed when controls are not allowed
-  if (!controls_allowed) {
+  if (!(aol_allowed || controls_allowed)) {
     violation |= steer_control_enabled;
   }
 
   // reset to current angle if either controls is not allowed or there's a violation
-  if (violation || !controls_allowed) {
+  if (violation || !(aol_allowed || controls_allowed)) {
     if (limits.inactive_angle_is_zero) {
       desired_angle_last = 0;
     } else {
@@ -304,7 +304,7 @@ bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, co
 
   bool violation = false;
 
-  if (controls_allowed && steer_control_enabled) {
+  if ((aol_allowed || controls_allowed) && steer_control_enabled) {
     // *** ISO lateral jerk limit ***
     // calculate maximum angle rate per second
     const float max_curvature_rate_sec = MAX_LATERAL_JERK / (fudged_speed * fudged_speed);
@@ -340,12 +340,12 @@ bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, co
   }
 
   // No angle control allowed when controls are not allowed
-  if (!controls_allowed) {
+  if (!(aol_allowed || controls_allowed)) {
     violation |= steer_control_enabled;
   }
 
   // reset to current angle if either controls is not allowed or there's a violation
-  if (violation || !controls_allowed) {
+  if (violation || !(aol_allowed || controls_allowed)) {
     desired_angle_last = SAFETY_CLAMP(angle_meas.values[0], -limits.max_angle, limits.max_angle);
   }
 

opendbc_repo/opendbc/safety/modes/chrysler.h

@@ -80,6 +80,7 @@ static void chrysler_rx_hook(const CANPacket_t *msg) {
     pcm_cruise_check(cruise_engaged);
 
     // FrogPilot variables
+    acc_main_on = GET_BIT(msg, 20U);
   }
 
   // TODO: use the same message for both

opendbc_repo/opendbc/safety/modes/ford.h

@@ -160,6 +160,7 @@ static void ford_rx_hook(const CANPacket_t *msg) {
       pcm_cruise_check(cruise_engaged);
 
       // FrogPilot variables
+      acc_main_on = (cruise_state == 3U) || cruise_engaged;
     }
   }
 }

opendbc_repo/opendbc/safety/modes/gm.h

@@ -125,6 +125,9 @@ static void gm_rx_hook(const CANPacket_t *msg) {
   }
 
   // FrogPilot variables
+  if (msg->addr == 0xC9U) {
+    acc_main_on = GET_BIT(msg, 29U);
+  }
 }
 
 static bool gm_tx_hook(const CANPacket_t *msg) {

opendbc_repo/opendbc/safety/modes/honda.h

@@ -238,7 +238,7 @@ static bool honda_tx_hook(const CANPacket_t *msg) {
 
   // STEER: safety check
   if ((msg->addr == 0xE4U) || (msg->addr == 0x194U)) {
-    if (!controls_allowed) {
+    if (!(aol_allowed || controls_allowed)) {
       bool steer_applied = msg->data[0] | msg->data[1];
       if (steer_applied) {
         tx = false;

opendbc_repo/opendbc/safety/modes/hyundai.h

@@ -53,6 +53,8 @@ const LongitudinalLimits HYUNDAI_LONG_LIMITS = {
   {.msg = {{0x91,  0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
 
 // FrogPilot variables
+#define HYUNDAI_LDA_BUTTON_ADDR_CHECK \
+  {.msg = {{0x391, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
 
 static const CanMsg HYUNDAI_TX_MSGS[] = {
   HYUNDAI_COMMON_TX_MSGS(0)
@@ -177,6 +179,9 @@ static void hyundai_rx_hook(const CANPacket_t *msg) {
     }
 
     // FrogPilot variables
+    if (msg->addr == 0x391U) {
+      hyundai_lkas_button_check(GET_BIT(msg, 4U));
+    }
   }
 }
 
@@ -284,11 +289,29 @@ static safety_config hyundai_init(uint16_t param) {
     };
 
     // FrogPilot variables
+    static RxCheck hyundai_long_rx_checks_lda[] = {
+      HYUNDAI_COMMON_RX_CHECKS(false)
+      HYUNDAI_LDA_BUTTON_ADDR_CHECK
+    };
+
+    static RxCheck hyundai_fcev_long_rx_checks_lda[] = {
+      HYUNDAI_COMMON_RX_CHECKS(false)
+      HYUNDAI_FCEV_GAS_ADDR_CHECK
+      HYUNDAI_LDA_BUTTON_ADDR_CHECK
+    };
 
     if (hyundai_fcev_gas_signal) {
-      SET_RX_CHECKS(hyundai_fcev_long_rx_checks, ret);
+      if (hyundai_has_lda_button) {
+        SET_RX_CHECKS(hyundai_fcev_long_rx_checks_lda, ret);
+      } else {
+        SET_RX_CHECKS(hyundai_fcev_long_rx_checks, ret);
+      }
     } else {
-      SET_RX_CHECKS(hyundai_long_rx_checks, ret);
+      if (hyundai_has_lda_button) {
+        SET_RX_CHECKS(hyundai_long_rx_checks_lda, ret);
+      } else {
+        SET_RX_CHECKS(hyundai_long_rx_checks, ret);
+      }
     }
     if (hyundai_camera_scc) {
       SET_TX_MSGS(HYUNDAI_CAMERA_SCC_LONG_TX_MSGS, ret);
@@ -303,8 +326,17 @@ static safety_config hyundai_init(uint16_t param) {
     };
 
     // FrogPilot variables
+    static RxCheck hyundai_cam_scc_rx_checks_lda[] = {
+      HYUNDAI_COMMON_RX_CHECKS(false)
+      HYUNDAI_SCC12_ADDR_CHECK(2)
+      HYUNDAI_LDA_BUTTON_ADDR_CHECK
+    };
 
-    ret = BUILD_SAFETY_CFG(hyundai_cam_scc_rx_checks, HYUNDAI_CAMERA_SCC_TX_MSGS);
+    if (hyundai_has_lda_button) {
+      ret = BUILD_SAFETY_CFG(hyundai_cam_scc_rx_checks_lda, HYUNDAI_CAMERA_SCC_TX_MSGS);
+    } else {
+      ret = BUILD_SAFETY_CFG(hyundai_cam_scc_rx_checks, HYUNDAI_CAMERA_SCC_TX_MSGS);
+    }
   } else {
     static RxCheck hyundai_rx_checks[] = {
        HYUNDAI_COMMON_RX_CHECKS(false)
@@ -318,12 +350,32 @@ static safety_config hyundai_init(uint16_t param) {
     };
 
     // FrogPilot variables
+    static RxCheck hyundai_rx_checks_lda[] = {
+       HYUNDAI_COMMON_RX_CHECKS(false)
+       HYUNDAI_SCC12_ADDR_CHECK(0)
+       HYUNDAI_LDA_BUTTON_ADDR_CHECK
+    };
+
+    static RxCheck hyundai_fcev_rx_checks_lda[] = {
+      HYUNDAI_COMMON_RX_CHECKS(false)
+      HYUNDAI_SCC12_ADDR_CHECK(0)
+      HYUNDAI_FCEV_GAS_ADDR_CHECK
+      HYUNDAI_LDA_BUTTON_ADDR_CHECK
+    };
 
     SET_TX_MSGS(HYUNDAI_TX_MSGS, ret);
     if (hyundai_fcev_gas_signal) {
-      SET_RX_CHECKS(hyundai_fcev_rx_checks, ret);
+      if (hyundai_has_lda_button) {
+        SET_RX_CHECKS(hyundai_fcev_rx_checks_lda, ret);
+      } else {
+        SET_RX_CHECKS(hyundai_fcev_rx_checks, ret);
+      }
     } else {
-      SET_RX_CHECKS(hyundai_rx_checks, ret);
+      if (hyundai_has_lda_button) {
+        SET_RX_CHECKS(hyundai_rx_checks_lda, ret);
+      } else {
+        SET_RX_CHECKS(hyundai_rx_checks, ret);
+      }
     }
   }
   return ret;

opendbc_repo/opendbc/safety/modes/hyundai_canfd.h

@@ -90,11 +90,13 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *msg) {
         main_button = GET_BIT(msg, 19U);
 
         // FrogPilot variables
+        hyundai_lkas_button_check(GET_BIT(msg, 23U));
       } else {
         cruise_button = (msg->data[4] >> 4) & 0x7U;
         main_button = GET_BIT(msg, 34U);
 
         // FrogPilot variables
+        hyundai_lkas_button_check(GET_BIT(msg, 39U));
       }
       hyundai_common_cruise_buttons_check(cruise_button, main_button);
     }

opendbc_repo/opendbc/safety/modes/hyundai_common.h

@@ -43,6 +43,8 @@ extern bool hyundai_alt_limits_2;
 bool hyundai_alt_limits_2 = false;
 
 // FrogPilot variables
+extern bool hyundai_has_lda_button;
+bool hyundai_has_lda_button = false;
 
 static uint8_t hyundai_last_button_interaction;  // button messages since the user pressed an enable button
 
@@ -56,6 +58,7 @@ void hyundai_common_init(uint16_t param) {
   const uint16_t HYUNDAI_PARAM_ALT_LIMITS_2 = 512;
 
   // FrogPilot variables
+  const int HYUNDAI_PARAM_HAS_LDA_BUTTON = 1024;
 
   hyundai_ev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_EV_GAS);
   hyundai_hybrid_gas_signal = !hyundai_ev_gas_signal && GET_FLAG(param, HYUNDAI_PARAM_HYBRID_GAS);
@@ -66,6 +69,7 @@ void hyundai_common_init(uint16_t param) {
   hyundai_alt_limits_2 = GET_FLAG(param, HYUNDAI_PARAM_ALT_LIMITS_2);
 
   // FrogPilot variables
+  hyundai_has_lda_button = GET_FLAG(param, HYUNDAI_PARAM_HAS_LDA_BUTTON);
 
   hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES;
 
@@ -118,6 +122,10 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const bool mai
   }
 
   // FrogPilot variables
+  if (main_button && !main_button_prev) {
+    acc_main_on = !acc_main_on;
+  }
+  main_button_prev = main_button;
 }
 
 #ifdef CANFD
@@ -148,3 +156,9 @@ uint32_t hyundai_common_canfd_compute_checksum(const CANPacket_t *msg) {
 #endif
 
 // FrogPilot variables
+void hyundai_lkas_button_check(const bool lkas_button) {
+  if (lkas_button && !lkas_button_prev) {
+    lkas_on = !lkas_on;
+  }
+  lkas_button_prev = lkas_button;
+}

opendbc_repo/opendbc/safety/modes/mazda.h

@@ -36,6 +36,7 @@ static void mazda_rx_hook(const CANPacket_t *msg) {
       pcm_cruise_check(cruise_engaged);
 
       // FrogPilot variables
+      acc_main_on = GET_BIT(msg, 17U);
     }
 
     if (msg->addr == MAZDA_ENGINE_DATA) {

opendbc_repo/opendbc/safety/modes/nissan.h

@@ -52,6 +52,13 @@ static void nissan_rx_hook(const CANPacket_t *msg) {
   }
 
   // FrogPilot variables
+  if ((msg->addr == 0x1B6U) && (msg->bus == (nissan_alt_eps ? 2U : 1U))) {
+    acc_main_on = GET_BIT(msg, 36U);
+  }
+
+  if ((msg->addr == 0x239U) && (msg->bus == 0U)) {
+    acc_main_on = GET_BIT(msg, 17U);
+  }
 }
 
 

opendbc_repo/opendbc/safety/modes/psa.h

@@ -11,6 +11,7 @@
 #define PSA_LANE_KEEP_ASSIST      1010U // TX from OP,  EPS
 
 // FrogPilot variables
+#define PSA_HS2_DYN1_MDD_ETAT_2B6 694U  // RX from BSI, ACC status
 
 // CAN bus
 #define PSA_MAIN_BUS 0U
@@ -24,6 +25,8 @@ static uint8_t psa_get_counter(const CANPacket_t *msg) {
   } else if (msg->addr == PSA_HS2_DYN_ABR_38D) {
     cnt = (msg->data[5] >> 4) & 0xFU;
   // FrogPilot variables
+  } else if (msg->addr == PSA_HS2_DYN1_MDD_ETAT_2B6) {
+    cnt = (msg->data[7] >> 4) & 0xFU;
   } else {
   }
   return cnt;
@@ -36,6 +39,8 @@ static uint32_t psa_get_checksum(const CANPacket_t *msg) {
   } else if (msg->addr == PSA_HS2_DYN_ABR_38D) {
     chksum = msg->data[5] & 0xFU;
   // FrogPilot variables
+  } else if (msg->addr == PSA_HS2_DYN1_MDD_ETAT_2B6) {
+    chksum = msg->data[7] & 0xFU;
   } else {
   }
   return chksum;
@@ -64,6 +69,8 @@ static uint32_t psa_compute_checksum(const CANPacket_t *msg) {
   } else if (msg->addr == PSA_HS2_DYN_ABR_38D) {
     chk = _psa_compute_checksum(msg, 0x7, 5);
   // FrogPilot variables
+  } else if (msg->addr == PSA_HS2_DYN1_MDD_ETAT_2B6) {
+    chk = _psa_compute_checksum(msg, 0x3, 7);
   } else {
   }
   return chk;
@@ -90,6 +97,9 @@ static void psa_rx_hook(const CANPacket_t *msg) {
       pcm_cruise_check((msg->data[2U] >> 7U) & 1U); // RVV_ACC_ACTIVATION_REQ
     }
     // FrogPilot variables
+    if (msg->addr == PSA_HS2_DYN1_MDD_ETAT_2B6) {
+      acc_main_on = (msg->data[3] & 0x0FU) > 2;
+    }
   }
 
 
@@ -144,6 +154,7 @@ static safety_config psa_init(uint16_t param) {
     {.msg = {{PSA_DAT_BSI, PSA_CAM_BUS, 8, 20U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},        // brake
 
     // FrogPilot variables
+    {.msg = {{PSA_HS2_DYN1_MDD_ETAT_2B6, PSA_ADAS_BUS, 8, 20U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},                      // ACC status
   };
 
   return BUILD_SAFETY_CFG(psa_rx_checks, PSA_TX_MSGS);

opendbc_repo/opendbc/safety/modes/rivian.h

@@ -99,6 +99,7 @@ static void rivian_rx_hook(const CANPacket_t *msg) {
       pcm_cruise_check(feature_status == 1);
 
       // FrogPilot variables
+      acc_main_on = (feature_status == 0) || (feature_status == 1);
     }
   }
 }

opendbc_repo/opendbc/safety/modes/subaru.h

@@ -111,6 +111,7 @@ static void subaru_rx_hook(const CANPacket_t *msg) {
     pcm_cruise_check(cruise_engaged);
 
     // FrogPilot variables
+    acc_main_on = GET_BIT(msg, 40U);
   }
 
   // update vehicle moving with any non-zero wheel speed

opendbc_repo/opendbc/safety/modes/subaru_preglobal.h

@@ -35,6 +35,7 @@ static void subaru_preglobal_rx_hook(const CANPacket_t *msg) {
       pcm_cruise_check(cruise_engaged);
 
       // FrogPilot variables
+      acc_main_on = GET_BIT(msg, 48U);
     }
 
     // update vehicle moving with any non-zero wheel speed

opendbc_repo/opendbc/safety/modes/tesla.h

@@ -161,6 +161,7 @@ static void tesla_rx_hook(const CANPacket_t *msg) {
       pcm_cruise_check(cruise_engaged);
 
       // FrogPilot variables
+      acc_main_on = ((cruise_state == 1) || cruise_engaged) && !tesla_autopark;
     }
 
     if (msg->addr == 0x155U) {

opendbc_repo/opendbc/safety/modes/toyota.h

@@ -40,6 +40,7 @@
   {.msg = {{ 0xaa, 0, 8, 83U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},  \
   {.msg = {{0x260, 0, 8, 50U, .ignore_counter = true, .ignore_quality_flag=!(lta)}, { 0 }, { 0 }}},                           \
   /* FrogPilot Variables */                                                                                                   \
+  {.msg = {{0x1D3, 0, 8, 33U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},  \
 
 #define TOYOTA_RX_CHECKS(lta)                                                                                                               \
   TOYOTA_COMMON_RX_CHECKS(lta)                                                                                                              \
@@ -162,6 +163,13 @@ static void toyota_rx_hook(const CANPacket_t *msg) {
     }
 
     // FrogPilot variables
+    if (msg->addr == 0x1D3U) {
+      acc_main_on = GET_BIT(msg, 15U);
+    }
+
+    if (msg->addr == 0x365U) {
+      acc_main_on = GET_BIT(msg, 0U);
+    }
   }
 }
 

opendbc_repo/opendbc/safety/safety.h

@@ -61,6 +61,10 @@ int cruise_button_prev = 0;
 bool safety_rx_checks_invalid = false;
 
 // FrogPilot variables
+bool aol_allowed = false;
+bool lkas_button_prev = false;
+bool lkas_on = false;
+bool main_button_prev = false;
 
 // for safety modes with torque steering control
 int desired_torque_last = 0;       // last desired steer torque
@@ -370,6 +374,7 @@ static void generic_rx_checks(void) {
   steering_disengage_prev = steering_disengage;
 
   // FrogPilot variables
+  aol_allowed = (acc_main_on || lkas_on) && (alternative_experience & ALT_EXP_ALWAYS_ON_LATERAL);
 }
 
 static void stock_ecu_check(bool stock_ecu_detected) {
@@ -494,6 +499,10 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
   enable_gas_interceptor = false;
 
   // FrogPilot variables
+  aol_allowed = false;
+  lkas_button_prev = false;
+  lkas_on = false;
+  main_button_prev = false;
 
   return set_status;
 }

opendbc_repo/opendbc/safety/tests/common.py

@@ -302,6 +302,40 @@ class TorqueSteeringSafetyTestBase(SafetyTestBase, abc.ABC):
       self.assertFalse(self._tx(self._torque_cmd_msg(self.MAX_TORQUE, 1)))
 
   # FrogPilot variables
+  def _toggle_aol(self, toggle_on):
+    """Toggles "Always On Lateral" On/Off"""
+    pass
+
+  def test_always_on_lateral(self):
+    if self._toggle_aol(True) is None:
+      raise unittest.SkipTest("AOL message not implemented for this safety mode")
+
+    self.safety.set_controls_allowed(False)
+
+    torque_cmd = self.MAX_RATE_UP  # Use the max rate
+
+    # Without alt exp, make sure steering is blocked
+    self.safety.set_alternative_experience(0)
+    self._set_prev_torque(0)
+    self.assertFalse(self._tx(self._torque_cmd_msg(torque_cmd)))
+
+    # With alt exp, but without main on, steering should be blocked
+    self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL)
+    self._rx(self._toggle_aol(False))
+    self._set_prev_torque(0)
+    self.assertFalse(self._tx(self._torque_cmd_msg(torque_cmd)))
+    self.assertFalse(self.safety.get_longitudinal_allowed())
+
+    # With alt exp and main on, steering should be allowed
+    self._rx(self._toggle_aol(True))
+    self._set_prev_torque(0)
+    self.assertTrue(self._tx(self._torque_cmd_msg(torque_cmd)))
+    self.assertFalse(self.safety.get_longitudinal_allowed())
+
+    # Turn off main, steering should be blocked again
+    self._rx(self._toggle_aol(False))
+    self.safety.set_desired_torque_last(torque_cmd)
+    self.assertFalse(self._tx(self._torque_cmd_msg(torque_cmd)))
 
 
 class SteerRequestCutSafetyTest(TorqueSteeringSafetyTestBase, abc.ABC):
@@ -809,6 +843,42 @@ class AngleSteeringSafetyTest(VehicleSpeedSafetyTest):
       self.assertTrue(self._tx(self._angle_cmd_msg(0, True, increment_timer=False)))
 
   # FrogPilot variables
+  def _toggle_aol(self, toggle_on):
+    """Toggles "Always On Lateral" on/off"""
+    pass
+
+  def test_always_on_lateral(self):
+    if self._toggle_aol(True) is None:
+      raise unittest.SkipTest("AOL message not implemented for this safety mode")
+
+    self.safety.set_controls_allowed(False)
+
+    self._reset_angle_measurement(0)
+    self._reset_speed_measurement(1)
+    angle_cmd = self.ANGLE_RATE_UP[0] / 2.0  # Use half of the max angle rate
+
+    # Without alt exp, make sure steering is blocked
+    self.safety.set_alternative_experience(0)
+    self._set_prev_desired_angle(0)
+    self.assertFalse(self._tx(self._angle_cmd_msg(angle=angle_cmd, enabled=True)))
+
+    # With alt exp, but without main on, steering should be blocked
+    self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL)
+    self._rx(self._toggle_aol(False))
+    self._set_prev_desired_angle(0)
+    self.assertFalse(self._tx(self._angle_cmd_msg(angle=angle_cmd, enabled=True)))
+    self.assertFalse(self.safety.get_longitudinal_allowed())
+
+    # With alt exp and main on, steering should be allowed
+    self._rx(self._toggle_aol(True))
+    self._set_prev_desired_angle(0)
+    self.assertTrue(self._tx(self._angle_cmd_msg(angle=angle_cmd, enabled=True)))
+    self.assertFalse(self.safety.get_longitudinal_allowed())
+
+    # Turn off main, steering should be blocked again
+    self._rx(self._toggle_aol(False))
+    self._set_prev_desired_angle(angle_cmd)
+    self.assertFalse(self._tx(self._angle_cmd_msg(angle=angle_cmd, enabled=True)))
 
 
 class SafetyTest(SafetyTestBase):

opendbc_repo/opendbc/safety/tests/hyundai_common.py

@@ -72,6 +72,25 @@ class HyundaiButtonBase:
       self._rx(self._button_msg(Buttons.NONE))
 
   # FrogPilot variables
+  def _toggle_aol(self, toggle_on):
+    """
+      Simulates toggling the main cruise button. The safety model requires a
+      press and release to change the main cruise state. This function
+      resets the safety model to a known state before each call.
+    """
+    if not hasattr(self, "_aol_state"):
+      self._aol_state = False
+
+    # Already in the requested state
+    if toggle_on == self._aol_state:
+      return None
+
+    # Toggle: press + release sequence
+    self._rx(self._button_msg(Buttons.NONE, main_button=1))
+    self._rx(self._button_msg(Buttons.NONE, main_button=0))
+
+    self._aol_state = toggle_on
+    return None  # avoid duplicate message in harness
 
 
 class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest):

opendbc_repo/opendbc/safety/tests/test_chrysler.py

@@ -72,6 +72,10 @@ class TestChryslerSafety(common.CarSafetyTest, common.MotorTorqueSteeringSafetyT
       self.assertFalse(self._tx(self._button_msg(cancel=False, resume=False)))
 
   # FrogPilot variables
+  def _toggle_aol(self, toggle_on):
+    # DAS_3, bit 20 is ACC_AVAILABLE
+    values = {"ACC_AVAILABLE": 1 if toggle_on else 0}
+    return self.packer.make_can_msg_panda("DAS_3", self.DAS_BUS, values)
 
 
 class TestChryslerRamDTSafety(TestChryslerSafety):

opendbc_repo/opendbc/safety/tests/test_ford.py

@@ -379,6 +379,15 @@ class TestFordSafetyBase(common.CarSafetyTest):
         self.assertEqual(enabled, self._tx(self._acc_button_msg(Buttons.CANCEL, bus)))
 
   # FrogPilot variables
+  def _toggle_aol(self, toggle_on):
+    # EngBrakeData, CcStat_D_Actl is the cruise state
+    # 3 is standby (main on), 5 is active (engaged)
+    brake = self.safety.get_brake_pressed_prev()
+    values = {
+      "BpedDrvAppl_D_Actl": 2 if brake else 1,
+      "CcStat_D_Actl": 3 if toggle_on else 0,
+    }
+    return self.packer.make_can_msg_panda("EngBrakeData", 0, values)
 
 
 class TestFordCANFDStockSafety(TestFordSafetyBase):

opendbc_repo/opendbc/safety/tests/test_gm.py

@@ -133,6 +133,10 @@ class TestGmSafetyBase(common.CarSafetyTest, common.DriverTorqueSteeringSafetyTe
     return self.packer.make_can_msg_safety("ASCMSteeringButton", self.BUTTONS_BUS, values)
 
   # FrogPilot variables
+  def _toggle_aol(self, toggle_on):
+    # ECMEngineStatus, bit 29 is CruiseMainOn
+    values = {"CruiseMainOn": 1 if toggle_on else 0}
+    return self.packer.make_can_msg_panda("ECMEngineStatus", 0, values)
 
 
 class TestGmEVSafetyBase(TestGmSafetyBase):

opendbc_repo/opendbc/safety/tests/test_honda.py

@@ -238,6 +238,11 @@ class HondaBase(common.CarSafetyTest):
     self.assertFalse(self._tx(self._send_steer_msg(0x1000)))
 
   # FrogPilot variables
+  def _toggle_aol(self, toggle_on):
+    # SCM_FEEDBACK, bit 28 is MAIN_ON
+    values = {"MAIN_ON": 1 if toggle_on else 0, "COUNTER": self.cnt_acc_state % 4}
+    self.__class__.cnt_acc_state += 1
+    return self.packer.make_can_msg_panda("SCM_FEEDBACK", self.PT_BUS, values)
 
 
 # ********************* Honda Nidec **********************

opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py

@@ -82,6 +82,26 @@ class TestHyundaiCanfdBase(HyundaiButtonBase, common.CarSafetyTest, common.Drive
     return self.packer.make_can_msg_safety("CRUISE_BUTTONS", bus, values)
 
   # FrogPilot variables
+  def _toggle_aol(self, toggle_on):
+    if not hasattr(self, "_aol_state"):
+      self._aol_state = False
+
+    # Already in the requested state
+    if toggle_on == self._aol_state:
+      return None
+
+    # Simulate button press + release
+    values = {
+      "CRUISE_BUTTONS": 0,
+      "ADAPTIVE_CRUISE_MAIN_BTN": 0,
+      "LFA_BTN": 1,
+      "COUNTER": 0,
+    }
+    self._rx(self.packer.make_can_msg_panda("CRUISE_BUTTONS", self.PT_BUS, values))
+    self._rx(self.packer.make_can_msg_panda("CRUISE_BUTTONS", self.PT_BUS, {**values, "LFA_BTN": 0}))
+
+    self._aol_state = toggle_on
+    return None  # avoid duplicate message in harness
 
 
 class TestHyundaiCanfdLFASteeringBase(TestHyundaiCanfdBase):
@@ -153,6 +173,26 @@ class TestHyundaiCanfdLFASteeringAltButtonsBase(TestHyundaiCanfdLFASteeringBase)
       self.assertFalse(self._tx(self._acc_cancel_msg(False)))
 
   # FrogPilot variables
+  def _toggle_aol(self, toggle_on):
+    if not hasattr(self, "_aol_state"):
+      self._aol_state = False
+
+    # Already in the requested state
+    if toggle_on == self._aol_state:
+      return None
+
+    # Simulate button press + release
+    values = {
+      "CRUISE_BUTTONS_ALT": 0,
+      "ADAPTIVE_CRUISE_MAIN_BTN": 0,
+      "LFA_BTN": 1,
+      "COUNTER": 0,
+    }
+    self._rx(self.packer.make_can_msg_panda("CRUISE_BUTTONS_ALT", self.PT_BUS, values))
+    self._rx(self.packer.make_can_msg_panda("CRUISE_BUTTONS_ALT", self.PT_BUS, {**values, "LFA_BTN": 0}))
+
+    self._aol_state = toggle_on
+    return None  # avoid duplicate message in harness
 
 
 @parameterized_class(ALL_GAS_EV_HYBRID_COMBOS)

opendbc_repo/opendbc/safety/tests/test_mazda.py

@@ -81,6 +81,10 @@ class TestMazdaSafety(common.CarSafetyTest, common.DriverTorqueSteeringSafetyTes
     self.assertTrue(self._tx(self._button_msg(resume=True)))
 
   # FrogPilot variables
+  def _toggle_aol(self, toggle_on):
+    # CRZ_CTRL, CRZ_AVAILABLE is the main on button
+    values = {"CRZ_AVAILABLE": 1 if toggle_on else 0}
+    return self.packer.make_can_msg_panda("CRZ_CTRL", 0, values)
 
 
 if __name__ == "__main__":

opendbc_repo/opendbc/safety/tests/test_nissan.py

@@ -80,6 +80,10 @@ class TestNissanSafety(common.CarSafetyTest, common.AngleSteeringSafetyTest):
         self.assertEqual(tx, should_tx)
 
   # FrogPilot variables
+  def _toggle_aol(self, toggle_on):
+    # PRO_PILOT, CRUISE_ON is the main on button for X-Trail/Rogue/Altima
+    values = {"CRUISE_ON": 1 if toggle_on else 0}
+    return self.packer.make_can_msg_panda("PRO_PILOT", 2, values)
 
 
 class TestNissanSafetyAltEpsBus(TestNissanSafety):
@@ -116,6 +120,10 @@ class TestNissanLeafSafety(TestNissanSafety):
     pass
 
   # FrogPilot variables
+  def _toggle_aol(self, toggle_on):
+    # CRUISE_THROTTLE, CRUISE_AVAILABLE is the main on button for Leaf
+    values = {"CRUISE_AVAILABLE": 1 if toggle_on else 0}
+    return self.packer.make_can_msg_panda("CRUISE_THROTTLE", 0, values)
 
 
 if __name__ == "__main__":

opendbc_repo/opendbc/safety/tests/test_subaru.py

@@ -112,6 +112,10 @@ class TestSubaruSafetyBase(common.CarSafetyTest):
     return self.packer.make_can_msg_safety("CruiseControl", self.ALT_MAIN_BUS, values)
 
   # FrogPilot variables
+  def _toggle_aol(self, toggle_on):
+    # CruiseControl, Cruise_On is the main on button
+    values = {"Cruise_On": 1 if toggle_on else 0}
+    return self.packer.make_can_msg_panda("CruiseControl", self.ALT_MAIN_BUS, values)
 
 
 class TestSubaruStockLongitudinalSafetyBase(TestSubaruSafetyBase):

opendbc_repo/opendbc/safety/tests/test_subaru_preglobal.py

@@ -60,6 +60,10 @@ class TestSubaruPreglobalSafety(common.CarSafetyTest, common.DriverTorqueSteerin
     return self.packer.make_can_msg_safety("CruiseControl", 0, values)
 
   # FrogPilot variables
+  def _toggle_aol(self, toggle_on):
+    # CruiseControl, Cruise_On is the main on button
+    values = {"Cruise_On": 1 if toggle_on else 0}
+    return self.packer.make_can_msg_panda("CruiseControl", 0, values)
 
 
 class TestSubaruPreglobalReversedDriverTorqueSafety(TestSubaruPreglobalSafety):

opendbc_repo/opendbc/safety/tests/test_tesla.py

@@ -354,6 +354,10 @@ class TestTeslaSafetyBase(common.CarSafetyTest, common.AngleSteeringSafetyTest,
         self.assertTrue(self._tx(self._angle_cmd_msg(0, True)))
 
   # FrogPilot variables
+  def _toggle_aol(self, toggle_on):
+    # DI_state, DI_cruiseState is the cruise state, 1 is standby
+    values = {"DI_cruiseState": 1 if toggle_on else 0}
+    return self.packer.make_can_msg_panda("DI_state", 0, values)
 
 
 class TestTeslaStockSafety(TestTeslaSafetyBase):

opendbc_repo/opendbc/safety/tests/test_toyota.py

@@ -10,7 +10,7 @@ from opendbc.safety.tests.libsafety import libsafety_py
 import opendbc.safety.tests.common as common
 from opendbc.safety.tests.common import CANPackerSafety
 
-TOYOTA_COMMON_TX_MSGS = [[0x2E4, 0], [0x191, 0], [0x412, 0], [0x343, 0], [0x1D2, 0]]  # LKAS + LTA + ACC & PCM cancel cmds
+TOYOTA_COMMON_TX_MSGS = [[0x2E4, 0], [0x191, 0], [0x412, 0], [0x343, 0], [0x1D2, 0], [0x1D3, 0]]  # LKAS + LTA + ACC & PCM cancel cmds
 TOYOTA_SECOC_TX_MSGS = [[0x131, 0], [0x183, 0]] + TOYOTA_COMMON_TX_MSGS
 TOYOTA_COMMON_LONG_TX_MSGS = [[0x283, 0], [0x2E6, 0], [0x2E7, 0], [0x33E, 0], [0x344, 0], [0x365, 0], [0x366, 0], [0x4CB, 0],  # DSU bus 0
                               [0x128, 1], [0x141, 1], [0x160, 1], [0x161, 1], [0x470, 1],  # DSU bus 1
@@ -123,6 +123,10 @@ class TestToyotaSafetyBase(common.CarSafetyTest, common.LongitudinalAccelSafetyT
       self.assertFalse(self.safety.get_controls_allowed())
 
   # FrogPilot variables
+  def _toggle_aol(self, toggle_on):
+    # pcm_cruise_2, bit 15 is toggle_on
+    values = {"MAIN_ON": 1 if toggle_on else 0}
+    return self.packer.make_can_msg_panda("PCM_CRUISE_2", 0, values)
 
 
 class TestToyotaSafetyTorque(TestToyotaSafetyBase, common.MotorTorqueSteeringSafetyTest, common.SteerRequestCutSafetyTest):

opendbc_repo/opendbc/safety/tests/test_volkswagen_mqb.py

@@ -127,6 +127,9 @@ class TestVolkswagenMqbSafetyBase(common.CarSafetyTest, common.DriverTorqueSteer
     self.assertEqual(0, self.safety.get_torque_driver_min())
 
   # FrogPilot variables
+  def _toggle_aol(self, toggle_on):
+    # TSK_06, TSK_Status is the cruise state, 2 is standby
+    return self._tsk_status_msg(False, main_switch=toggle_on)
 
 
 class TestVolkswagenMqbStockSafety(TestVolkswagenMqbSafetyBase):

opendbc_repo/opendbc/safety/tests/test_volkswagen_pq.py

@@ -109,6 +109,9 @@ class TestVolkswagenPqSafetyBase(common.CarSafetyTest, common.DriverTorqueSteeri
     self.assertEqual(0, self.safety.get_torque_driver_min())
 
   # FrogPilot variables
+  def _toggle_aol(self, toggle_on):
+    # Motor_5, GRA_Hauptschalter is the main cruise switch
+    return self._motor_5_msg(main_switch=toggle_on)
 
 
 class TestVolkswagenPqStockSafety(TestVolkswagenPqSafetyBase):

selfdrive/car/card.py

@@ -17,6 +17,7 @@ from opendbc.car.carlog import carlog
 from opendbc.car.fw_versions import ObdCallback
 from opendbc.car.car_helpers import get_car, interfaces
 from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase
+from opendbc.safety import ALTERNATIVE_EXPERIENCE
 from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
 from openpilot.selfdrive.car.cruise import VCruiseHelper
 from openpilot.selfdrive.car.car_specific import MockCarState
@@ -174,6 +175,9 @@ class Car:
     # FrogPilot variables
     self.frogpilot_toggles = get_frogpilot_toggles()
 
+    if self.frogpilot_toggles.always_on_lateral:
+      self.FPCP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL
+
     fpcp_bytes = self.FPCP.to_bytes()
     self.params.put("FrogPilotCarParams", fpcp_bytes)
     self.params.put_nonblocking("FrogPilotCarParamsPersistent", fpcp_bytes)

selfdrive/controls/controlsd.py

@@ -107,7 +107,7 @@ class Controls:
 
     # Check which actuators can be enabled
     standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, 0.3) or CS.standstill
-    CC.latActive = self.sm['selfdriveState'].active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
+    CC.latActive = (self.sm['selfdriveState'].active or self.sm['frogpilotCarState'].alwaysOnLateralEnabled) and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
                    (not standstill or self.CP.steerAtStandstill) and self.sm['frogpilotPlan'].lateralCheck
     CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and self.CP.openpilotLongitudinalControl
 

selfdrive/monitoring/helpers.py

@@ -436,7 +436,7 @@ class DriverMonitoring:
       rpyCalib = [0., 0., 0.]
     else:
       highway_speed = sm['carState'].vEgo
-      enabled = sm['selfdriveState'].enabled
+      enabled = sm['selfdriveState'].enabled or sm['frogpilotCarState'].alwaysOnLateralEnabled
       wrong_gear = sm['carState'].gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low)
       standstill = sm['carState'].standstill
       driver_engaged = sm['carState'].steeringPressed or sm['carState'].gasPressed

selfdrive/selfdrived/selfdrived.py

@@ -552,7 +552,7 @@ class SelfdriveD:
     CS = self.data_sample()
     self.update_events(CS)
     if not self.CP.passive and self.initialized:
-      self.enabled, self.active = self.state_machine.update(self.events, self.frogpilot_events)
+      self.enabled, self.active = self.state_machine.update(self.events, self.frogpilot_events, self.sm['frogpilotCarState'].alwaysOnLateralEnabled)
     self.update_alerts(CS)
 
     self.publish_selfdriveState(CS)

selfdrive/selfdrived/state.py

@@ -16,7 +16,7 @@ class StateMachine:
     self.state = State.disabled
     self.soft_disable_timer = 0
 
-  def update(self, events: Events, frogpilot_events: Events):
+  def update(self, events: Events, frogpilot_events: Events, alwaysOnLateralEnabled: bool):
     # decrement the soft disable timer at every step, as it's reset on
     # entrance in SOFT_DISABLING state
     self.soft_disable_timer = max(0, self.soft_disable_timer - 1)
@@ -94,7 +94,7 @@ class StateMachine:
     # Check if openpilot is engaged and actuators are enabled
     enabled = self.state in ENABLED_STATES
     active = self.state in ACTIVE_STATES
-    if active:
+    if active or alwaysOnLateralEnabled:
       self.current_alert_types.append(ET.WARNING)
     return enabled, active
 

selfdrive/ui/qt/onroad/buttons.cc

@@ -37,7 +37,7 @@ void ExperimentalButton::changeMode() {
 
 void ExperimentalButton::updateState(const UIState &s, const FrogPilotUIState &fs) {
   const auto cs = (*s.sm)["selfdriveState"].getSelfdriveState();
-  bool eng = cs.getEngageable() || cs.getEnabled();
+  bool eng = cs.getEngageable() || cs.getEnabled() || fs.frogpilot_scene.always_on_lateral_active;
   if ((cs.getExperimentalMode() != experimental_mode) || (eng != engageable)) {
     engageable = eng;
     experimental_mode = cs.getExperimentalMode();

selfdrive/ui/ui.cc

@@ -97,6 +97,8 @@ void UIState::updateStatus(FrogPilotUIState *fs) {
 
     if (state == cereal::SelfdriveState::OpenpilotState::PRE_ENABLED || state == cereal::SelfdriveState::OpenpilotState::OVERRIDING) {
       status = STATUS_OVERRIDE;
+    } else if (frogpilot_scene.always_on_lateral_active) {
+      status = STATUS_ALWAYS_ON_LATERAL_ACTIVE;
     } else {
       status = ss.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED;
     }

selfdrive/ui/ui.h

@@ -46,6 +46,7 @@ typedef enum UIStatus {
   STATUS_ENGAGED,
 
   // FrogPilot variables
+  STATUS_ALWAYS_ON_LATERAL_ACTIVE,
   STATUS_EXPERIMENTAL_MODE_ENABLED,
 } UIStatus;
 
@@ -55,6 +56,7 @@ const QColor bg_colors [] = {
   [STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1),
 
   // FrogPilot variables
+  [STATUS_ALWAYS_ON_LATERAL_ACTIVE] = QColor(0x0a, 0xba, 0xb5, 0xf1),
   [STATUS_EXPERIMENTAL_MODE_ENABLED] = QColor(0xda, 0x6f, 0x25, 0xf1),
 };