From ea23fc4fd717c97d5278998e1f387c66d97e1e10 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Tue, 23 Jul 2024 00:12:48 -0700
Subject: [PATCH] Controls - Always On Lateral - Control Via LKAS Button

Enable or disable 'Always On Lateral' by clicking your 'LKAS' button.
---
 .../chrysler_pacifica_2017_hybrid_generated.dbc    |  1 +
 opendbc/chrysler_ram_hd_generated.dbc              |  6 ++++++
 opendbc/hyundai_canfd.dbc                          |  2 +-
 panda/python/__init__.py                           |  1 +
 selfdrive/car/chrysler/carstate.py                 | 10 ++++++++++
 selfdrive/car/chrysler/interface.py                |  6 +++++-
 selfdrive/car/ford/carstate.py                     |  4 ++++
 selfdrive/car/ford/interface.py                    |  6 +++++-
 selfdrive/car/gm/carstate.py                       |  6 ++++++
 selfdrive/car/gm/interface.py                      |  5 ++++-
 selfdrive/car/honda/carstate.py                    |  4 ++++
 selfdrive/car/honda/interface.py                   |  2 ++
 selfdrive/car/hyundai/carstate.py                  | 12 ++++++++++++
 selfdrive/car/hyundai/interface.py                 | 10 +++++++++-
 selfdrive/car/hyundai/values.py                    |  2 ++
 selfdrive/car/interfaces.py                        | 14 +++++++++++++-
 selfdrive/car/mazda/carstate.py                    |  4 ++++
 selfdrive/car/mazda/interface.py                   |  6 +++++-
 selfdrive/car/nissan/carstate.py                   |  1 +
 selfdrive/car/nissan/interface.py                  |  6 +++++-
 selfdrive/car/subaru/carstate.py                   |  7 ++++++-
 selfdrive/car/subaru/interface.py                  |  7 ++++++-
 selfdrive/car/subaru/values.py                     |  2 ++
 selfdrive/car/toyota/carstate.py                   |  4 ++++
 selfdrive/car/toyota/interface.py                  |  6 +++++-
 selfdrive/controls/controlsd.py                    |  5 +++--
 26 files changed, 126 insertions(+), 13 deletions(-)

diff --git a/opendbc/chrysler_pacifica_2017_hybrid_generated.dbc b/opendbc/chrysler_pacifica_2017_hybrid_generated.dbc
index d8f4c662..98f29016 100644
--- a/opendbc/chrysler_pacifica_2017_hybrid_generated.dbc
+++ b/opendbc/chrysler_pacifica_2017_hybrid_generated.dbc
@@ -312,6 +312,7 @@ BO_ 764 ACCEL_RELATED_2FC: 8 XXX
 BO_ 816 TRACTION_BUTTON: 8 XXX
  SG_ TRACTION_OFF : 19|1@0+ (1,0) [0|3] "" XXX
  SG_ TOGGLE_PARKSENSE : 52|1@0+ (1,0) [0|3] "" XXX
+ SG_ TOGGLE_LKAS : 53|1@0+ (1,0) [0|1] "" XXX
 
 BO_ 878 ACCEL_RELATED_36E: 8 XXX
  SG_ ACCEL_OR_RPM_2 : 15|8@0+ (1,0) [0|255] "" XXX
diff --git a/opendbc/chrysler_ram_hd_generated.dbc b/opendbc/chrysler_ram_hd_generated.dbc
index 31e62ec6..19b57434 100644
--- a/opendbc/chrysler_ram_hd_generated.dbc
+++ b/opendbc/chrysler_ram_hd_generated.dbc
@@ -221,3 +221,9 @@ BO_ 630 LKAS_COMMAND: 8 XXX
  SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
  SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
 
+BO_ 650 Center_Stack_2: 8 XXX
+ SG_ LKAS_Button : 57|1@1+ (1,0) [0|0] "" XXX
+
+BO_ 816 Center_Stack_1: 8 XXX
+ SG_ LKAS_Button : 53|1@1+ (1,0) [0|0] "" XXX
+ SG_ Traction_Button : 54|1@0+ (1,0) [0|1] "" XXX
diff --git a/opendbc/hyundai_canfd.dbc b/opendbc/hyundai_canfd.dbc
index 1ef67c04..00991a71 100644
--- a/opendbc/hyundai_canfd.dbc
+++ b/opendbc/hyundai_canfd.dbc
@@ -316,7 +316,7 @@ BO_ 441 CAM_0x1b9: 32 CAMERA
 
 BO_ 463 CRUISE_BUTTONS: 8 XXX
  SG_ _CHECKSUM : 0|8@1+ (1,0) [0|65535] "" XXX
- SG_ LKAS_BTN : 23|1@1+ (1,0) [0|1] "" XXX
+ SG_ LFA_BTN : 23|1@1+ (1,0) [0|1] "" XXX
  SG_ SET_ME_1 : 29|1@1+ (1,0) [0|1] "" XXX
  SG_ ADAPTIVE_CRUISE_MAIN_BTN : 19|1@1+ (1,0) [0|1] "" XXX
  SG_ NORMAL_CRUISE_MAIN_BTN : 21|1@1+ (1,0) [0|1] "" XXX
diff --git a/panda/python/__init__.py b/panda/python/__init__.py
index 110d45f5..d3123507 100644
--- a/panda/python/__init__.py
+++ b/panda/python/__init__.py
@@ -209,6 +209,7 @@ class Panda:
   FLAG_HYUNDAI_CANFD_ALT_BUTTONS = 32
   FLAG_HYUNDAI_ALT_LIMITS = 64
   FLAG_HYUNDAI_CANFD_HDA2_ALT_STEERING = 128
+  FLAG_HYUNDAI_LFA_BTN = 256
 
   FLAG_TESLA_POWERTRAIN = 1
   FLAG_TESLA_LONG_CONTROL = 2
diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py
index 8b504189..0e2a82bf 100644
--- a/selfdrive/car/chrysler/carstate.py
+++ b/selfdrive/car/chrysler/carstate.py
@@ -103,6 +103,13 @@ class CarState(CarStateBase):
     self.lkas_car_model = cp_cam.vl["DAS_6"]["CAR_MODEL"]
     self.button_counter = cp.vl[self.button_message]["COUNTER"]
 
+    # FrogPilot CarState functions
+    self.lkas_previously_enabled = self.lkas_enabled
+    if self.CP.carFingerprint in RAM_CARS:
+      self.lkas_enabled = cp.vl["Center_Stack_2"]["LKAS_Button"] or cp.vl["Center_Stack_1"]["LKAS_Button"]
+    else:
+      self.lkas_enabled = cp.vl["TRACTION_BUTTON"]["TOGGLE_LKAS"] == 1
+
     return ret, fp_ret
 
   @staticmethod
@@ -137,11 +144,14 @@ class CarState(CarStateBase):
         ("ESP_8", 50),
         ("EPS_3", 50),
         ("Transmission_Status", 50),
+        ("Center_Stack_1", 1),
+        ("Center_Stack_2", 1),
       ]
     else:
       messages += [
         ("GEAR", 50),
         ("SPEED_1", 100),
+        ("TRACTION_BUTTON", 1),
       ]
       messages += CarState.get_cruise_messages()
 
diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py
index da7e5205..10a31404 100755
--- a/selfdrive/car/chrysler/interface.py
+++ b/selfdrive/car/chrysler/interface.py
@@ -6,6 +6,7 @@ from openpilot.selfdrive.car.chrysler.values import CAR, RAM_HD, RAM_DT, RAM_CAR
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase
 
 ButtonType = car.CarState.ButtonEvent.Type
+FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
 
 
 class CarInterface(CarInterfaceBase):
@@ -88,7 +89,10 @@ class CarInterface(CarInterfaceBase):
   def _update(self, c, frogpilot_toggles):
     ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles)
 
-    ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
+    ret.buttonEvents = [
+      *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
+      *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
+    ]
 
     # events
     events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low])
diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py
index ab79d091..db9abdf6 100644
--- a/selfdrive/car/ford/carstate.py
+++ b/selfdrive/car/ford/carstate.py
@@ -108,6 +108,10 @@ class CarState(CarStateBase):
     self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"]
     self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"]
 
+    # FrogPilot CarState functions
+    self.lkas_previously_enabled = self.lkas_enabled
+    self.lkas_enabled = ret.genericToggle
+
     return ret, fp_ret
 
   @staticmethod
diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py
index c73201b6..611d8035 100644
--- a/selfdrive/car/ford/interface.py
+++ b/selfdrive/car/ford/interface.py
@@ -7,6 +7,7 @@ from openpilot.selfdrive.car.ford.values import Ecu, FordFlags
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase
 
 ButtonType = car.CarState.ButtonEvent.Type
+FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
 TransmissionType = car.CarParams.TransmissionType
 GearShifter = car.CarState.GearShifter
 
@@ -70,7 +71,10 @@ class CarInterface(CarInterfaceBase):
   def _update(self, c, frogpilot_toggles):
     ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles)
 
-    ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
+    ret.buttonEvents = [
+      *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
+      *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
+    ]
 
     events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic])
     if not self.CS.vehicle_sensors_valid:
diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py
index 4d249497..06e68a97 100644
--- a/selfdrive/car/gm/carstate.py
+++ b/selfdrive/car/gm/carstate.py
@@ -172,6 +172,12 @@ class CarState(CarStateBase):
     # FrogPilot CarState functions
     fp_ret.hasMenu = not (self.CP.flags & GMFlags.NO_CAMERA.value or self.CP.carFingerprint in CC_ONLY_CAR)
 
+    self.lkas_previously_enabled = self.lkas_enabled
+    if self.CP.carFingerprint in SDGM_CAR:
+      self.lkas_enabled = cam_cp.vl["ASCMSteeringButton"]["LKAButton"]
+    else:
+      self.lkas_enabled = pt_cp.vl["ASCMSteeringButton"]["LKAButton"]
+
     self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]["CruiseState"]
 
     return ret, fp_ret
diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py
index 6d68782d..9305e59c 100644
--- a/selfdrive/car/gm/interface.py
+++ b/selfdrive/car/gm/interface.py
@@ -13,6 +13,7 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLater
 from openpilot.selfdrive.controls.lib.drive_helpers import get_friction
 
 ButtonType = car.CarState.ButtonEvent.Type
+FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
 GearShifter = car.CarState.GearShifter
 TransmissionType = car.CarParams.TransmissionType
@@ -317,7 +318,9 @@ class CarInterface(CarInterfaceBase):
         *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT,
                               unpressed_btn=CruiseButtons.UNPRESS),
         *create_button_events(self.CS.distance_button, self.CS.prev_distance_button,
-                              {1: ButtonType.gapAdjustCruise})
+                              {1: ButtonType.gapAdjustCruise}),
+        *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled,
+                              {1: FrogPilotButtonType.lkas}),
       ]
 
     # The ECM allows enabling on falling edge of set, but only rising edge of resume
diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
index 18ec601c..554b3b1c 100644
--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@@ -273,6 +273,10 @@ class CarState(CarStateBase):
       ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]["BSM_ALERT"] == 1
       ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]["BSM_ALERT"] == 1
 
+    # FrogPilot CarState functions
+    self.lkas_previously_enabled = self.lkas_enabled
+    self.lkas_enabled = self.cruise_setting == 1
+
     return ret, fp_ret
 
   def get_can_parser(self, CP):
diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
index 67086b05..6405a13c 100755
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@@ -12,6 +12,7 @@ from openpilot.selfdrive.car.disable_ecu import disable_ecu
 
 
 ButtonType = car.CarState.ButtonEvent.Type
+FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
 TransmissionType = car.CarParams.TransmissionType
 BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise,
@@ -253,6 +254,7 @@ class CarInterface(CarInterfaceBase):
     ret.buttonEvents = [
       *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT),
       *create_button_events(self.CS.cruise_setting, self.CS.prev_cruise_setting, SETTINGS_BUTTONS_DICT),
+      *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
     ]
 
     # events
diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py
index 0f40cdd4..c9938ab1 100644
--- a/selfdrive/car/hyundai/carstate.py
+++ b/selfdrive/car/hyundai/carstate.py
@@ -172,6 +172,11 @@ class CarState(CarStateBase):
     if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
       self.main_enabled = not self.main_enabled
 
+    # FrogPilot CarState functions
+    self.lkas_previously_enabled = self.lkas_enabled
+    if self.CP.flags & HyundaiFlags.CAN_LFA_BTN:
+      self.lkas_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"]
+
     return ret, fp_ret
 
   def update_canfd(self, cp, cp_cam, frogpilot_toggles):
@@ -258,6 +263,10 @@ class CarState(CarStateBase):
       self.hda2_lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x362"] if self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING
                                           else cp_cam.vl["CAM_0x2a4"])
 
+    # FrogPilot CarState functions
+    self.lkas_previously_enabled = self.lkas_enabled
+    self.lkas_enabled = cp.vl[self.cruise_btns_msg_canfd]["LFA_BTN"]
+
     return ret, fp_ret
 
   def get_can_parser(self, CP):
@@ -308,6 +317,9 @@ class CarState(CarStateBase):
     else:
       messages.append(("LVR12", 100))
 
+    if CP.flags & HyundaiFlags.CAN_LFA_BTN:
+      messages.append(("BCM_PO_11", 50))
+
     return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
 
   @staticmethod
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 5880b499..12a3a7bf 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -11,6 +11,7 @@ from openpilot.selfdrive.car.disable_ecu import disable_ecu
 
 Ecu = car.CarParams.Ecu
 ButtonType = car.CarState.ButtonEvent.Type
+FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
 GearShifter = car.CarState.GearShifter
 ENABLE_BUTTONS = (Buttons.RES_ACCEL, Buttons.SET_DECEL, Buttons.CANCEL)
@@ -124,6 +125,10 @@ class CarInterface(CarInterfaceBase):
       if candidate in CAMERA_SCC_CAR:
         ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC
 
+      if 0x391 in fingerprint[0]:
+        ret.flags |= HyundaiFlags.CAN_LFA_BTN.value
+        ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_LFA_BTN
+
     if ret.openpilotLongitudinalControl:
       ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_LONG
     if ret.flags & HyundaiFlags.HYBRID:
@@ -155,7 +160,10 @@ class CarInterface(CarInterfaceBase):
     ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles)
 
     if self.CS.CP.openpilotLongitudinalControl:
-      ret.buttonEvents = create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT)
+      ret.buttonEvents = [
+        *create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT),
+        *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
+      ]
 
     # On some newer model years, the CANCEL button acts as a pause/resume button based on the PCM state
     # To avoid re-engaging when openpilot cancels, check user engagement intention via buttons
diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py
index 8cb29c80..3d9a8fc9 100644
--- a/selfdrive/car/hyundai/values.py
+++ b/selfdrive/car/hyundai/values.py
@@ -95,6 +95,8 @@ class HyundaiFlags(IntFlag):
 
   MIN_STEER_32_MPH = 2 ** 23
 
+  # FrogPilot HKG flags
+  CAN_LFA_BTN = 2 ** 24
 
 class Footnote(Enum):
   CANFD = CarFootnote(
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
index 297184e3..00ed4974 100644
--- a/selfdrive/car/interfaces.py
+++ b/selfdrive/car/interfaces.py
@@ -8,7 +8,7 @@ from typing import Any, NamedTuple
 from collections.abc import Callable
 from functools import cache
 
-from cereal import car
+from cereal import car, custom
 from openpilot.common.basedir import BASEDIR
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.simple_kalman import KF1D, get_kalman_gain
@@ -22,6 +22,7 @@ from openpilot.selfdrive.controls.lib.events import Events
 from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
 
 ButtonType = car.CarState.ButtonEvent.Type
+FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
 GearShifter = car.CarState.GearShifter
 EventName = car.CarEvent.EventName
 
@@ -115,6 +116,7 @@ class CarInterfaceBase(ABC):
     self.params = Params()
     self.params_memory = Params("/dev/shm/params")
 
+    self.always_on_lateral_disabled = False
     self.belowSteerSpeed_shown = False
     self.disable_belowSteerSpeed = False
     self.disable_resumeRequired = False
@@ -266,6 +268,9 @@ class CarInterfaceBase(ABC):
     if ret.cruiseState.speedCluster == 0:
       ret.cruiseState.speedCluster = ret.cruiseState.speed
 
+    # Add any additional frogpilotCarStates
+    fp_ret.alwaysOnLateralDisabled = self.always_on_lateral_disabled
+
     # copy back for next iteration
     if self.CS is not None:
       self.CS.out = ret.as_reader()
@@ -320,6 +325,10 @@ class CarInterfaceBase(ABC):
       if b.type == ButtonType.cancel:
         events.add(EventName.buttonCancel)
 
+      # FrogPilot button presses
+      if b.type == FrogPilotButtonType.lkas and b.pressed:
+        self.always_on_lateral_disabled = not self.always_on_lateral_disabled
+
     # Handle permanent and temporary steering faults
     self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1
     if cs_out.steerFaultTemporary:
@@ -389,6 +398,9 @@ class CarStateBase(ABC):
     K = get_kalman_gain(DT_CTRL, np.array(A), np.array(C), np.array(Q), R)
     self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K)
 
+    # FrogPilot variables
+    self.lkas_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]])
diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py
index bf006550..b7a64659 100644
--- a/selfdrive/car/mazda/carstate.py
+++ b/selfdrive/car/mazda/carstate.py
@@ -111,6 +111,10 @@ class CarState(CarStateBase):
     self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"]
     ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
 
+    # FrogPilot CarState functions
+    self.lkas_previously_enabled = self.lkas_enabled
+    self.lkas_enabled = not self.lkas_disabled
+
     return ret, fp_ret
 
   @staticmethod
diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py
index f6b72b17..7920f0ed 100755
--- a/selfdrive/car/mazda/interface.py
+++ b/selfdrive/car/mazda/interface.py
@@ -6,6 +6,7 @@ from openpilot.selfdrive.car import create_button_events, get_safety_config
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase
 
 ButtonType = car.CarState.ButtonEvent.Type
+FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
 
 class CarInterface(CarInterfaceBase):
@@ -33,7 +34,10 @@ class CarInterface(CarInterfaceBase):
     ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles)
 
      # TODO: add button types for inc and dec
-    ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
+    ret.buttonEvents = [
+      *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
+      *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
+    ]
 
     # events
     events = self.create_common_events(ret)
diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py
index 5144765e..3a67c59e 100644
--- a/selfdrive/car/nissan/carstate.py
+++ b/selfdrive/car/nissan/carstate.py
@@ -108,6 +108,7 @@ class CarState(CarStateBase):
     can_gear = int(cp.vl["GEARBOX"]["GEAR_SHIFTER"])
     ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
 
+    self.lkas_previously_enabled = self.lkas_enabled
     if self.CP.carFingerprint == CAR.NISSAN_ALTIMA:
       self.lkas_enabled = bool(cp.vl["LKAS_SETTINGS"]["LKAS_ENABLED"])
     else:
diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py
index 0176c17c..f3141b9d 100644
--- a/selfdrive/car/nissan/interface.py
+++ b/selfdrive/car/nissan/interface.py
@@ -5,6 +5,7 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase
 from openpilot.selfdrive.car.nissan.values import CAR
 
 ButtonType = car.CarState.ButtonEvent.Type
+FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
 
 
 class CarInterface(CarInterfaceBase):
@@ -32,7 +33,10 @@ class CarInterface(CarInterfaceBase):
   def _update(self, c, frogpilot_toggles):
     ret, fp_ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam, frogpilot_toggles)
 
-    ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
+    ret.buttonEvents = [
+      *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
+      *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
+    ]
 
     events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.brake])
 
diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py
index fe438d17..9c96076a 100644
--- a/selfdrive/car/subaru/carstate.py
+++ b/selfdrive/car/subaru/carstate.py
@@ -4,7 +4,7 @@ from opendbc.can.can_define import CANDefine
 from openpilot.common.conversions import Conversions as CV
 from openpilot.selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
-from openpilot.selfdrive.car.subaru.values import DBC, CanBus, SubaruFlags
+from openpilot.selfdrive.car.subaru.values import DBC, CanBus, PREGLOBAL_CARS, SubaruFlags
 from openpilot.selfdrive.car import CanSignalRateCalculator
 
 
@@ -126,6 +126,11 @@ class CarState(CarStateBase):
     if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
       self.es_infotainment_msg = copy.copy(cp_cam.vl["ES_Infotainment"])
 
+    # FrogPilot CarState functions
+    self.lkas_previously_enabled = self.lkas_enabled
+    if self.car_fingerprint not in PREGLOBAL_CARS:
+      self.lkas_enabled = cp_cam.vl["ES_LKAS_State"]["LKAS_Dash_State"]
+
     return ret, fp_ret
 
   @staticmethod
diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py
index 9bc874f1..ea8e196e 100644
--- a/selfdrive/car/subaru/interface.py
+++ b/selfdrive/car/subaru/interface.py
@@ -1,10 +1,11 @@
 from cereal import car, custom
 from panda import Panda
-from openpilot.selfdrive.car import get_safety_config
+from openpilot.selfdrive.car import create_button_events, get_safety_config
 from openpilot.selfdrive.car.disable_ecu import disable_ecu
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase
 from openpilot.selfdrive.car.subaru.values import CAR, GLOBAL_ES_ADDR, SubaruFlags
 
+FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
 
 class CarInterface(CarInterfaceBase):
 
@@ -101,6 +102,10 @@ class CarInterface(CarInterfaceBase):
 
     ret, fp_ret = self.CS.update(self.cp, self.cp_cam, self.cp_body, frogpilot_toggles)
 
+    ret.buttonEvents = [
+      *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
+    ]
+
     ret.events = self.create_common_events(ret).to_msg()
 
     return ret, fp_ret
diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py
index b138b7f8..bfdb8aae 100644
--- a/selfdrive/car/subaru/values.py
+++ b/selfdrive/car/subaru/values.py
@@ -209,6 +209,8 @@ class CAR(Platforms):
   )
 
 
+PREGLOBAL_CARS = {CAR.SUBARU_FORESTER_PREGLOBAL, CAR.SUBARU_LEGACY_PREGLOBAL, CAR.SUBARU_OUTBACK_PREGLOBAL, CAR.SUBARU_OUTBACK_PREGLOBAL_2018}
+
 SUBARU_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
   p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION)
 SUBARU_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 7dd5da32..11adbb58 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -191,6 +191,10 @@ class CarState(CarStateBase):
         self.distance_button = cp.vl["SDSU"]["FD_BUTTON"]
 
     # FrogPilot CarState functions
+    if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:
+      self.lkas_previously_enabled = self.lkas_enabled
+      message_keys = ["LDA_ON_MESSAGE", "SET_ME_X02"]
+      self.lkas_enabled = any(self.lkas_hud.get(key) == 1 for key in message_keys)
 
     # ZSS Support - Credit goes to the DragonPilot team!
     if self.CP.flags & ToyotaFlags.ZSS and self.zss_threshold_count < ZSS_THRESHOLD_COUNT:
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index c1e9ff61..2f81d420 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -8,6 +8,7 @@ from openpilot.selfdrive.car.disable_ecu import disable_ecu
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase
 
 ButtonType = car.CarState.ButtonEvent.Type
+FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
 SteerControlType = car.CarParams.SteerControlType
 
@@ -156,7 +157,10 @@ class CarInterface(CarInterfaceBase):
     ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles)
 
     if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER):
-      ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
+      ret.buttonEvents = [
+        *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
+        *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
+      ]
 
     # events
     events = self.create_common_events(ret)
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index fb297f8f..4fb00a7c 100644
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -680,7 +680,7 @@ class Controls:
 
     self.display_timer -= 1
 
-    FPCC = self.update_frogpilot_variables(CS)
+    FPCC = self.update_frogpilot_variables(CS, self.sm['frogpilotCarState'])
 
     return CC, lac_log, FPCC
 
@@ -892,12 +892,13 @@ class Controls:
       self.events.add(EventName.openpilotCrashed)
       self.openpilot_crashed_triggered = True
 
-  def update_frogpilot_variables(self, CS):
+  def update_frogpilot_variables(self, CS, frogpilotCarState):
     driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown)
 
     self.always_on_lateral_active |= self.frogpilot_toggles.always_on_lateral_main or CS.cruiseState.enabled
     self.always_on_lateral_active &= self.frogpilot_toggles.always_on_lateral and CS.cruiseState.available
     self.always_on_lateral_active &= driving_gear
+    self.always_on_lateral_active &= not (self.frogpilot_toggles.always_on_lateral_lkas and frogpilotCarState.alwaysOnLateralDisabled)
     self.always_on_lateral_active &= not (CS.brakePressed and CS.vEgo < self.frogpilot_toggles.always_on_lateral_pause_speed) or CS.standstill
 
     self.drive_distance += CS.vEgo * DT_CTRL