Always on Lateral

Added toggle for Always On Lateral / No disengage lateral on gas and brake.

Lots of credit goes to "pfeiferj"! Couldn't of done it without him!

https: //github.com/pfeiferj/openpilot
Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>

cereal/custom.capnp

@@ -9,6 +9,7 @@ $Cxx.namespace("cereal");
 
 # you can rename the struct, but don't change the identifier
 struct FrogPilotCarControl @0x81c2f05a394cf4af {
+  alwaysOnLateral @0: Bool;
 }
 
 struct FrogPilotDeviceState @0xaedffd8f31e7b55d {

common/params.cc

@@ -214,6 +214,8 @@ std::unordered_map<std::string, uint32_t> keys = {
     // FrogPilot parameters
     {"AccelerationProfile", PERSISTENT},
     {"AggressiveAcceleration", PERSISTENT},
+    {"AlwaysOnLateral", PERSISTENT},
+    {"AlwaysOnLateralMain", PERSISTENT},
     {"ApiCache_DriveStats", PERSISTENT},
     {"FrogPilotTogglesUpdated", PERSISTENT},
     {"GasRegenCmd", PERSISTENT},

panda/board/safety.h

@@ -556,7 +556,13 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
   bool violation = false;
   uint32_t ts = microsecond_timer_get();
 
+  bool aol_allowed = acc_main_on && (alternative_experience & ALT_EXP_ALWAYS_ON_LATERAL);
   if (controls_allowed) {
+    // acc main must be on if controls are allowed
+    acc_main_on = controls_allowed;
+  }
+
+  if (controls_allowed || aol_allowed) {
     // *** global torque limit check ***
     violation |= max_limit_check(desired_torque, limits.max_steer, -limits.max_steer);
 
@@ -583,7 +589,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
   }
 
   // no torque if controls is not allowed
-  if (!controls_allowed && (desired_torque != 0)) {
+  if (!(controls_allowed || aol_allowed) && (desired_torque != 0)) {
     violation = true;
   }
 
@@ -625,7 +631,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
   }
 
   // reset to 0 if either controls is not allowed or there's a violation
-  if (violation || !controls_allowed) {
+  if (violation || !(controls_allowed || aol_allowed)) {
     valid_steer_req_count = 0;
     invalid_steer_req_count = 0;
     desired_torque_last = 0;
@@ -640,8 +646,13 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
 // Safety checks for angle-based steering commands
 bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const SteeringLimits limits) {
   bool violation = false;
+  bool aol_allowed = acc_main_on && (alternative_experience & ALT_EXP_ALWAYS_ON_LATERAL);
+  if (controls_allowed) {
+    // acc main must be on if controls are allowed
+    acc_main_on = controls_allowed;
+  }
 
-  if (controls_allowed && steer_control_enabled) {
+  if ((controls_allowed || aol_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
@@ -684,7 +695,7 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
   }
 
   // No angle control allowed when controls are not allowed
-  violation |= !controls_allowed && steer_control_enabled;
+  violation |= !(controls_allowed || aol_allowed) && steer_control_enabled;
 
   return violation;
 }

panda/board/safety/safety_chrysler.h

@@ -185,6 +185,7 @@ static void chrysler_rx_hook(CANPacket_t *to_push) {
   // enter controls on rising edge of ACC, exit controls on ACC off
   const int das_3_bus = (chrysler_platform == CHRYSLER_PACIFICA) ? 0 : 2;
   if ((bus == das_3_bus) && (addr == chrysler_addrs->DAS_3)) {
+    acc_main_on = GET_BIT(to_push, 20U) == 1U;
     bool cruise_engaged = GET_BIT(to_push, 21U) == 1U;
     pcm_cruise_check(cruise_engaged);
   }

panda/board/safety/safety_ford.h

@@ -250,6 +250,7 @@ static void ford_rx_hook(CANPacket_t *to_push) {
 
       // Signal: CcStat_D_Actl
       unsigned int cruise_state = GET_BYTE(to_push, 1) & 0x07U;
+      acc_main_on = (cruise_state == 3U) ||(cruise_state == 4U) || (cruise_state == 5U);
       bool cruise_engaged = (cruise_state == 4U) || (cruise_state == 5U);
       pcm_cruise_check(cruise_engaged);
     }

panda/board/safety/safety_gm.h

@@ -127,6 +127,10 @@ static void gm_rx_hook(CANPacket_t *to_push) {
       brake_pressed = GET_BIT(to_push, 40U) != 0U;
     }
 
+    if (addr == 0xC9) {
+      acc_main_on = GET_BIT(to_push, 29U) != 0U;
+    }
+
     if (addr == 0x1C4) {
       gas_pressed = GET_BYTE(to_push, 5) != 0U;
 

panda/board/safety/safety_honda.h

@@ -331,7 +331,8 @@ static bool honda_tx_hook(CANPacket_t *to_send) {
 
   // STEER: safety check
   if ((addr == 0xE4) || (addr == 0x194)) {
-    if (!controls_allowed) {
+    bool aol_allowed = acc_main_on && (alternative_experience & ALT_EXP_DISABLE_DISENGAGE_ON_GAS);
+    if (!(controls_allowed || aol_allowed)) {
       bool steer_applied = GET_BYTE(to_send, 0) | GET_BYTE(to_send, 1);
       if (steer_applied) {
         tx = false;

panda/board/safety/safety_hyundai_common.h

@@ -63,6 +63,10 @@ void hyundai_common_cruise_state_check(const int cruise_engaged) {
 }
 
 void hyundai_common_cruise_buttons_check(const int cruise_button, const int main_button) {
+  if (main_button != 0 && main_button != cruise_main_prev) {
+    acc_main_on = !acc_main_on;
+  }
+  cruise_main_prev = main_button;
   if ((cruise_button == HYUNDAI_BTN_RESUME) || (cruise_button == HYUNDAI_BTN_SET) || (cruise_button == HYUNDAI_BTN_CANCEL) ||
       (main_button != 0)) {
     hyundai_last_button_interaction = 0U;

panda/board/safety/safety_mazda.h

@@ -52,6 +52,7 @@ static void mazda_rx_hook(CANPacket_t *to_push) {
 
     // enter controls on rising edge of ACC, exit controls on ACC off
     if (addr == MAZDA_CRZ_CTRL) {
+      acc_main_on = GET_BIT(to_push, 17U);
       bool cruise_engaged = GET_BYTE(to_push, 0) & 0x8U;
       pcm_cruise_check(cruise_engaged);
     }

panda/board/safety/safety_nissan.h

@@ -21,6 +21,8 @@ const CanMsg NISSAN_TX_MSGS[] = {
 
 // Signals duplicated below due to the fact that these messages can come in on either CAN bus, depending on car model.
 RxCheck nissan_rx_checks[] = {
+  {.msg = {{0x1b6, 0, 8, .frequency = 100U},
+         {0x1b6, 1, 8, .frequency = 100U}, { 0 }}},  // PRO_PILOT (100HZ)
   {.msg = {{0x2, 0, 5, .frequency = 100U},
            {0x2, 1, 5, .frequency = 100U}, { 0 }}},  // STEER_ANGLE_SENSOR
   {.msg = {{0x285, 0, 8, .frequency = 50U},
@@ -64,11 +66,16 @@ static void nissan_rx_hook(CANPacket_t *to_push) {
       UPDATE_VEHICLE_SPEED((right_rear + left_rear) / 2.0 * 0.005 / 3.6);
     }
 
+    if (addr == 0x1b6) {
+      acc_main_on = GET_BIT(to_push, 36U);
+    }
+
     // X-Trail 0x15c, Leaf 0x239
     if ((addr == 0x15c) || (addr == 0x239)) {
       if (addr == 0x15c){
         gas_pressed = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3U)) > 3U;
       } else {
+        acc_main_on = GET_BIT(to_push, 17U);
         gas_pressed = GET_BYTE(to_push, 0) > 3U;
       }
     }

panda/board/safety/safety_subaru.h

@@ -152,6 +152,7 @@ static void subaru_rx_hook(CANPacket_t *to_push) {
 
   // enter controls on rising edge of ACC, exit controls on ACC off
   if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_main_bus)) {
+    acc_main_on = GET_BIT(to_push, 40U) != 0U;
     bool cruise_engaged = GET_BIT(to_push, 41U) != 0U;
     pcm_cruise_check(cruise_engaged);
   }

panda/board/safety/safety_subaru_preglobal.h

@@ -56,6 +56,7 @@ static void subaru_preglobal_rx_hook(CANPacket_t *to_push) {
 
     // enter controls on rising edge of ACC, exit controls on ACC off
     if (addr == MSG_SUBARU_PG_CruiseControl) {
+      acc_main_on = GET_BIT(to_push, 48U) != 0U;
       bool cruise_engaged = GET_BIT(to_push, 49U) != 0U;
       pcm_cruise_check(cruise_engaged);
     }

panda/board/safety/safety_tesla.h

@@ -88,6 +88,14 @@ static void tesla_rx_hook(CANPacket_t *to_push) {
     if(addr == (tesla_powertrain ? 0x256 : 0x368)) {
       // Cruise state
       int cruise_state = (GET_BYTE(to_push, 1) >> 4);
+
+      acc_main_on = (cruise_state == 1) ||  // STANDBY
+        (cruise_state == 2) ||  // ENABLED
+        (cruise_state == 3) ||  // STANDSTILL
+        (cruise_state == 4) ||  // OVERRIDE
+        (cruise_state == 6) ||  // PRE_FAULT
+        (cruise_state == 7);    // PRE_CANCEL
+
       bool cruise_engaged = (cruise_state == 2) ||  // ENABLED
                             (cruise_state == 3) ||  // STANDSTILL
                             (cruise_state == 4) ||  // OVERRIDE

panda/board/safety/safety_toyota.h

@@ -47,6 +47,7 @@ const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 805;
   {0x283, 0, 7}, {0x2E6, 0, 8}, {0x2E7, 0, 8}, {0x33E, 0, 7}, {0x344, 0, 8}, {0x365, 0, 7}, {0x366, 0, 7}, {0x4CB, 0, 8},  /* DSU bus 0 */  \
   {0x128, 1, 6}, {0x141, 1, 4}, {0x160, 1, 8}, {0x161, 1, 7}, {0x470, 1, 4},  /* DSU bus 1 */                                               \
   {0x2E4, 0, 5}, {0x191, 0, 8}, {0x411, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8},  /* LKAS + ACC */                               \
+  {0x1D3, 0, 8},                                                                                                                            \
 
 const CanMsg TOYOTA_TX_MSGS[] = {
   TOYOTA_COMMON_TX_MSGS
@@ -61,6 +62,7 @@ const CanMsg TOYOTA_INTERCEPTOR_TX_MSGS[] = {
   {.msg = {{ 0xaa, 0, 8, .check_checksum = false, .frequency = 83U}, { 0 }, { 0 }}},                        \
   {.msg = {{0x260, 0, 8, .check_checksum = true, .quality_flag = (lta), .frequency = 50U}, { 0 }, { 0 }}},  \
   {.msg = {{0x1D2, 0, 8, .check_checksum = true, .frequency = 33U}, { 0 }, { 0 }}},                         \
+  {.msg = {{0x1D3, 0, 8, .check_checksum = true, .frequency = 33U}, { 0 }, { 0 }}},                         \
   {.msg = {{0x224, 0, 8, .check_checksum = false, .frequency = 40U},                                        \
            {0x226, 0, 8, .check_checksum = false, .frequency = 40U}, { 0 }}},                               \
 
@@ -166,6 +168,9 @@ static void toyota_rx_hook(CANPacket_t *to_push) {
         update_sample(&angle_meas, angle_meas_new);
       }
     }
+    if (addr == 0x1D3) {
+      acc_main_on = GET_BIT(to_push, 15U);
+    }
 
     // enter controls on rising edge of ACC, exit controls on ACC off
     // exit controls on rising edge of gas press

panda/board/safety_declarations.h

@@ -227,6 +227,7 @@ struct sample_t vehicle_speed;
 bool vehicle_moving = false;
 bool acc_main_on = false;  // referred to as "ACC off" in ISO 15622:2018
 int cruise_button_prev = 0;
+int cruise_main_prev = 0;
 bool safety_rx_checks_invalid = false;
 
 // for safety modes with torque steering control
@@ -269,3 +270,6 @@ int alternative_experience = 0;
 uint32_t safety_mode_cnt = 0U;
 // allow 1s of transition timeout after relay changes state before assessing malfunctioning
 const uint32_t RELAY_TRNS_TIMEOUT = 1U;
+
+// Always on Lateral
+#define ALT_EXP_ALWAYS_ON_LATERAL 16

panda/python/__init__.py

@@ -111,6 +111,7 @@ class ALTERNATIVE_EXPERIENCE:
   DISABLE_DISENGAGE_ON_GAS = 1
   DISABLE_STOCK_AEB = 2
   RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX = 8
+  ALWAYS_ON_LATERAL = 16
 
 class Panda:
 

selfdrive/car/hyundai/carcontroller.py

@@ -151,7 +151,7 @@ class CarController:
         use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
         can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2),
                                                         hud_control.leadVisible, set_speed_in_units, stopping,
-                                                        CC.cruiseControl.override, use_fca))
+                                                        CC.cruiseControl.override, use_fca, CS.out.cruiseState.available))
 
       # 20 Hz LFA MFA message
       if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:

selfdrive/car/hyundai/carstate.py

@@ -102,7 +102,7 @@ class CarState(CarStateBase):
     # cruise state
     if self.CP.openpilotLongitudinalControl:
       # These are not used for engage/disengage since openpilot keeps track of state using the buttons
-      ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0
+      ret.cruiseState.available = self.main_enabled
       ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1
       ret.cruiseState.standstill = False
       ret.cruiseState.nonAdaptive = False
@@ -162,7 +162,10 @@ class CarState(CarStateBase):
     self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"]  # 0 NOT ACTIVE, 1 ACTIVE
     self.prev_cruise_buttons = self.cruise_buttons[-1]
     self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"])
+    self.prev_main_buttons = self.main_buttons[-1]
     self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"])
+    if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
+      self.main_enabled = not self.main_enabled
 
     return ret
 
@@ -217,7 +220,7 @@ class CarState(CarStateBase):
 
     # cruise state
     # CAN FD cars enable on main button press, set available if no TCS faults preventing engagement
-    ret.cruiseState.available = cp.vl["TCS"]["ACCEnable"] == 0
+    ret.cruiseState.available = self.main_enabled
     if self.CP.openpilotLongitudinalControl:
       # These are not used for engage/disengage since openpilot keeps track of state using the buttons
       ret.cruiseState.enabled = cp.vl["TCS"]["ACC_REQ"] == 1
@@ -238,7 +241,10 @@ class CarState(CarStateBase):
 
     self.prev_cruise_buttons = self.cruise_buttons[-1]
     self.cruise_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"])
+    self.prev_main_buttons = self.main_buttons[-1]
     self.main_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["ADAPTIVE_CRUISE_MAIN_BTN"])
+    if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
+      self.main_enabled = not self.main_enabled
     self.buttons_counter = cp.vl[self.cruise_btns_msg_canfd]["COUNTER"]
     ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0  # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
 

selfdrive/car/hyundai/hyundaican.py

@@ -126,11 +126,11 @@ def create_lfahda_mfc(packer, enabled, hda_set_speed=0):
   }
   return packer.make_can_msg("LFAHDA_MFC", 0, values)
 
-def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, use_fca):
+def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, use_fca, cruise_available):
   commands = []
 
   scc11_values = {
-    "MainMode_ACC": 1,
+    "MainMode_ACC": 1 if cruise_available else 0,
     "TauGapSet": 4,
     "VSetDis": set_speed if enabled else 0,
     "AliveCounterACC": idx % 0x10,

selfdrive/car/interfaces.py

@@ -360,6 +360,8 @@ class CarStateBase(ABC):
     self.param = Params()
     self.param_memory = Params("/dev/shm/params")
 
+    self.main_enabled = False
+
   def update_speed_kf(self, v_ego_raw):
     if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0:  # Prevent large accelerations when car starts at non zero speed
       self.v_ego_kf.set_x([[v_ego_raw], [0.0]])

selfdrive/controls/controlsd.py

@@ -108,6 +108,16 @@ class Controls:
     if not self.disengage_on_accelerator:
       self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
 
+    # Set "Always On Lateral" conditions
+    self.always_on_lateral = self.params.get_bool("AlwaysOnLateral")
+    self.always_on_lateral_main = self.params.get_bool("AlwaysOnLateralMain")
+    self.lateral_allowed = False
+    if self.always_on_lateral:
+      self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL
+      if self.disengage_on_accelerator:
+        self.disengage_on_accelerator = False
+        self.params.put_bool("DisengageOnAccelerator", False)
+
     self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX
 
     # read params
@@ -596,9 +606,18 @@ class Controls:
     gear = car.CarState.GearShifter
     driving_gear = CS.gearShifter not in (gear.neutral, gear.park, gear.reverse, gear.unknown)
 
+    # Always on lateral
+    if self.always_on_lateral:
+      self.lateral_allowed &= CS.cruiseState.available
+      self.lateral_allowed |= CS.cruiseState.enabled or (CS.cruiseState.available and self.always_on_lateral_main)
+
+      self.FPCC.alwaysOnLateral = self.lateral_allowed and driving_gear
+      if self.FPCC.alwaysOnLateral:
+        self.current_alert_types.append(ET.WARNING)
+
     # Check which actuators can be enabled
     standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill
-    CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
+    CC.latActive = (self.active or self.FPCC.alwaysOnLateral) and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
                    (not standstill or self.joystick_mode)
     CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
 

selfdrive/frogpilot/ui/control_settings.cc

@@ -3,6 +3,8 @@
 
 FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPilotListWidget(parent) {
   const std::vector<std::tuple<QString, QString, QString, QString>> controlToggles {
+    {"AlwaysOnLateral", "Always on Lateral", "Maintain openpilot lateral control when the brake or gas pedals are used.\n\nDeactivation occurs only through the 'Cruise Control' button.", "../frogpilot/assets/toggle_icons/icon_always_on_lateral.png"},
+
     {"LateralTune", "Lateral Tuning", "Modify openpilot's steering behavior.", "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"},
 
     {"LongitudinalTune", "Longitudinal Tuning", "Modify openpilot's acceleration and braking behavior.", "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"},
@@ -13,7 +15,22 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
   for (const auto &[param, title, desc, icon] : controlToggles) {
     ParamControl *toggle;
 
-    if (param == "LateralTune") {
+    if (param == "AlwaysOnLateral") {
+      std::vector<QString> aolToggles{tr("AlwaysOnLateralMain")};
+      std::vector<QString> aolToggleNames{tr("Enable On Cruise Main")};
+      toggle = new FrogPilotParamToggleControl(param, title, desc, icon, aolToggles, aolToggleNames);
+
+      QObject::connect(static_cast<FrogPilotParamToggleControl*>(toggle), &FrogPilotParamToggleControl::buttonClicked, [this](const bool checked) {
+        if (checked) {
+          FrogPilotConfirmationDialog::toggleAlert("WARNING: This is very experimental and isn't guaranteed to work. If you run into any issues, please report it in the FrogPilot Discord!",
+          "I understand the risks.", this);
+        }
+        if (FrogPilotConfirmationDialog::toggle("Reboot required to take effect.", "Reboot Now", this)) {
+          Hardware::reboot();
+        }
+      });
+
+    } else if (param == "LateralTune") {
       FrogPilotParamManageControl *lateralTuneToggle = new FrogPilotParamManageControl(param, title, desc, icon, this);
       QObject::connect(lateralTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
         parentToggleClicked();
@@ -68,7 +85,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     });
   }
 
-  std::set<std::string> rebootKeys = {};
+  std::set<std::string> rebootKeys = {"AlwaysOnLateral"};
   for (const std::string &key : rebootKeys) {
     QObject::connect(toggles[key], &ToggleControl::toggleFlipped, [this]() {
       if (FrogPilotConfirmationDialog::toggle("Reboot required to take effect.", "Reboot Now", this)) {

selfdrive/ui/qt/maps/map.cc

@@ -124,7 +124,7 @@ void MapWindow::updateState(const UIState &s) {
   if (sm.updated("modelV2")) {
     // set path color on change, and show map on rising edge of navigate on openpilot
     bool nav_enabled = sm["modelV2"].getModelV2().getNavEnabled() &&
-                       sm["controlsState"].getControlsState().getEnabled();
+                       (sm["controlsState"].getControlsState().getEnabled() || sm["frogpilotCarControl"].getFrogpilotCarControl().getAlwaysOnLateral());
     if (nav_enabled != uiState()->scene.navigate_on_openpilot) {
       if (loaded_once) {
         m_map->setPaintProperty("navLayer", "line-color", getNavPathColor(nav_enabled));

selfdrive/ui/qt/onroad.cc

@@ -178,7 +178,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) {
 
   int margin = 40;
   int radius = 30;
-  int offset = true ? 25 : 0;
+  int offset = scene.always_on_lateral ? 25 : 0;
   if (alert.size == cereal::ControlsState::AlertSize::FULL) {
     margin = 0;
     radius = 0;
@@ -258,7 +258,7 @@ void ExperimentalButton::updateState(const UIState &s) {
 void ExperimentalButton::paintEvent(QPaintEvent *event) {
   QPainter p(this);
   QPixmap img = experimental_mode ? experimental_img : engage_img;
-  drawIcon(p, QPoint(btn_size / 2, btn_size / 2), img, QColor(0, 0, 0, 166), (isDown() || !engageable) ? 0.6 : 1.0);
+  drawIcon(p, QPoint(btn_size / 2, btn_size / 2), img, QColor(0, 0, 0, 166), (isDown() || (!engageable && !scene.always_on_lateral_active)) ? 0.6 : 1.0);
 }
 
 
@@ -548,7 +548,7 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s)
 
   // base icon
   int offset = UI_BORDER_SIZE + btn_size / 2;
-  offset += true ? 25 : 0;
+  offset += alwaysOnLateral ? 25 : 0;
   int x = rightHandDM ? width() - offset : offset;
   int y = height() - offset;
   float opacity = dmActive ? 0.65 : 0.2;
@@ -739,9 +739,10 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() {
 }
 
 void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
+  alwaysOnLateral = scene.always_on_lateral_active;
   experimentalMode = scene.experimental_mode;
 
-  if (true) {
+  if (alwaysOnLateral) {
     drawStatusBar(p);
   }
 
@@ -772,6 +773,10 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
   p.setOpacity(1.0);
   p.drawRoundedRect(statusBarRect, 30, 30);
 
+  if (alwaysOnLateral) {
+    newStatus = QString("Always On Lateral active") + (". Press the \"Cruise Control\" button to disable");
+  }
+
   // Check if status has changed
   if (newStatus != lastShownStatus) {
     displayStatusText = true;

selfdrive/ui/qt/onroad.h

@@ -117,6 +117,7 @@ private:
 
   QHBoxLayout *bottom_layout;
 
+  bool alwaysOnLateral;
   bool experimentalMode;
 
 protected:

selfdrive/ui/ui.cc

@@ -213,6 +213,9 @@ static void update_state(UIState *s) {
   }
   if (sm.updated("frogpilotCarControl")) {
     auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl();
+    if (scene.always_on_lateral) {
+      scene.always_on_lateral_active = !scene.enabled && frogpilotCarControl.getAlwaysOnLateral();
+    }
   }
   if (sm.updated("frogpilotLateralPlan")) {
     auto frogpilotLateralPlan = sm["frogpilotLateralPlan"].getFrogpilotLateralPlan();
@@ -244,6 +247,8 @@ void ui_update_params(UIState *s) {
 
   // FrogPilot variables
   UIScene &scene = s->scene;
+
+  scene.always_on_lateral = params.getBool("AlwaysOnLateral");
 }
 
 void UIState::updateStatus() {
@@ -252,6 +257,8 @@ void UIState::updateStatus() {
     auto state = controls_state.getState();
     if (state == cereal::ControlsState::OpenpilotState::PRE_ENABLED || state == cereal::ControlsState::OpenpilotState::OVERRIDING) {
       status = STATUS_OVERRIDE;
+    } else if (scene.always_on_lateral_active) {
+      status = STATUS_LATERAL_ACTIVE;
     } else {
       status = controls_state.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED;
     }

selfdrive/ui/ui.h

@@ -107,6 +107,7 @@ typedef enum UIStatus {
   STATUS_ENGAGED,
 
   // FrogPilot statuses
+  STATUS_LATERAL_ACTIVE,
 } UIStatus;
 
 enum PrimeType {
@@ -125,6 +126,7 @@ const QColor bg_colors [] = {
   [STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1),
 
   // FrogPilot colors
+  [STATUS_LATERAL_ACTIVE] = QColor(0x0a, 0xba, 0xb5, 0xf1),
 };
 
 static std::map<cereal::ControlsState::AlertStatus, QColor> alert_colors = {
@@ -167,6 +169,8 @@ typedef struct UIScene {
   uint64_t started_frame;
 
   // FrogPilot variables
+  bool always_on_lateral;
+  bool always_on_lateral_active;
   bool enabled;
   bool experimental_mode;