Merge pull request #52 from firestar5683/Dom49

AOL: keep lateral active when braking disengages longitudinal on full OP for HKG with LKAS

opendbc_repo/opendbc/car/hyundai/values.py

@@ -70,6 +70,7 @@ class HyundaiSafetyFlags(IntFlag):
 
 class HyundaiStarPilotSafetyFlags(IntFlag):
   HAS_LDA_BUTTON = 1024
+  AOL_LKAS_ON_ENGAGE = 2048
 
 
 class HyundaiFlags(IntFlag):

opendbc_repo/opendbc/car/interfaces.py

@@ -219,6 +219,8 @@ class CarInterfaceBase(ABC):
 
         if CP.flags & HyundaiFlags.HAS_LDA_BUTTON:
           fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON.value
+        if CP.openpilotLongitudinalControl and starpilot_toggles.always_on_lateral_lkas:
+          fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value
 
       elif platform in TOYOTA:
         fp_ret.canUsePedal = not CP.autoResumeSng

opendbc_repo/opendbc/safety/modes/hyundai_common.h

@@ -45,6 +45,9 @@ bool hyundai_alt_limits_2 = false;
 extern bool hyundai_has_lda_button;
 bool hyundai_has_lda_button = false;
 
+extern bool hyundai_aol_lkas_on_engage;
+bool hyundai_aol_lkas_on_engage = false;
+
 static uint8_t hyundai_last_button_interaction;  // button messages since the user pressed an enable button
 
 void hyundai_common_init(uint16_t param) {
@@ -57,6 +60,7 @@ void hyundai_common_init(uint16_t param) {
   const uint16_t HYUNDAI_PARAM_ALT_LIMITS_2 = 512;
 
   const int HYUNDAI_PARAM_HAS_LDA_BUTTON = 1024;
+  const uint16_t HYUNDAI_PARAM_AOL_LKAS_ON_ENGAGE = 2048;
 
   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);
@@ -67,6 +71,7 @@ void hyundai_common_init(uint16_t param) {
   hyundai_alt_limits_2 = GET_FLAG(param, HYUNDAI_PARAM_ALT_LIMITS_2);
 
   hyundai_has_lda_button = GET_FLAG(param, HYUNDAI_PARAM_HAS_LDA_BUTTON);
+  hyundai_aol_lkas_on_engage = GET_FLAG(param, HYUNDAI_PARAM_AOL_LKAS_ON_ENGAGE);
 
   hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES;
 
@@ -108,6 +113,10 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const bool mai
     bool res = (cruise_button != HYUNDAI_BTN_RESUME) && (cruise_button_prev == HYUNDAI_BTN_RESUME);
     if (set || res) {
       controls_allowed = true;
+
+      if (hyundai_aol_lkas_on_engage && ((alternative_experience & ALT_EXP_ALWAYS_ON_LATERAL) != 0)) {
+        lkas_on = true;
+      }
     }
 
     // exit controls on cancel press

opendbc_repo/opendbc/safety/tests/hyundai_common.py

@@ -1,5 +1,6 @@
 import unittest
 
+from opendbc.safety import ALTERNATIVE_EXPERIENCE
 import opendbc.safety.tests.common as common
 from opendbc.safety.tests.libsafety import libsafety_py
 from opendbc.safety.tests.common import make_msg
@@ -171,3 +172,22 @@ class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest):
     self.assertFalse(self.safety.get_relay_malfunction())
     self._rx(make_msg(bus, addr, 8))
     self.assertTrue(self.safety.get_relay_malfunction())
+
+
+class HyundaiAolLkasOnEngageBase:
+  def test_aol_lkas_auto_enables_on_set_engagement(self):
+    torque_cmd = self.MAX_RATE_UP
+
+    self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL)
+    self.safety.set_controls_allowed(False)
+
+    self._set_prev_torque(0)
+    self.assertFalse(self._tx(self._torque_cmd_msg(torque_cmd)))
+
+    self._rx(self._button_msg(Buttons.SET))
+    self._rx(self._button_msg(Buttons.NONE))
+    self.assertTrue(self.safety.get_controls_allowed())
+
+    self.safety.set_controls_allowed(False)
+    self._set_prev_torque(0)
+    self.assertTrue(self._tx(self._torque_cmd_msg(torque_cmd)))

opendbc_repo/opendbc/safety/tests/test_hyundai.py

@@ -2,12 +2,12 @@
 import random
 import unittest
 
-from opendbc.car.hyundai.values import HyundaiSafetyFlags
+from opendbc.car.hyundai.values import HyundaiSafetyFlags, HyundaiStarPilotSafetyFlags
 from opendbc.car.structs import CarParams
 from opendbc.safety.tests.libsafety import libsafety_py
 import opendbc.safety.tests.common as common
 from opendbc.safety.tests.common import CANPackerSafety
-from opendbc.safety.tests.hyundai_common import HyundaiButtonBase, HyundaiLongitudinalBase
+from opendbc.safety.tests.hyundai_common import HyundaiAolLkasOnEngageBase, HyundaiButtonBase, HyundaiLongitudinalBase
 
 
 # 4 bit checkusm used in some hyundai messages
@@ -279,5 +279,14 @@ class TestHyundaiSafetyFCEVLong(TestHyundaiLongitudinalSafety, TestHyundaiSafety
     self.safety.init_tests()
 
 
+class TestHyundaiLongitudinalAolLkasOnEngageSafety(HyundaiAolLkasOnEngageBase, TestHyundaiLongitudinalSafety):
+  def setUp(self):
+    self.packer = CANPackerSafety("hyundai_kia_generic")
+    self.safety = libsafety_py.libsafety
+    self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai,
+                                 HyundaiSafetyFlags.LONG | HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE)
+    self.safety.init_tests()
+
+
 if __name__ == "__main__":
   unittest.main()

opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py

@@ -2,12 +2,12 @@
 from parameterized import parameterized_class
 import unittest
 
-from opendbc.car.hyundai.values import HyundaiSafetyFlags
+from opendbc.car.hyundai.values import HyundaiSafetyFlags, HyundaiStarPilotSafetyFlags
 from opendbc.car.structs import CarParams
 from opendbc.safety.tests.libsafety import libsafety_py
 import opendbc.safety.tests.common as common
 from opendbc.safety.tests.common import CANPackerSafety
-from opendbc.safety.tests.hyundai_common import HyundaiButtonBase, HyundaiLongitudinalBase
+from opendbc.safety.tests.hyundai_common import HyundaiAolLkasOnEngageBase, HyundaiButtonBase, HyundaiLongitudinalBase
 
 # All combinations of radar/camera-SCC and gas/hybrid/EV cars
 ALL_GAS_EV_HYBRID_COMBOS = [
@@ -326,6 +326,18 @@ class TestHyundaiCanfdLFASteeringLongAltButtons(TestHyundaiCanfdLFASteeringLongB
     pass
 
 
+class TestHyundaiCanfdLKASteeringLongAolLkasOnEngageEV(HyundaiAolLkasOnEngageBase, TestHyundaiCanfdLKASteeringLongEV):
+  def setUp(self):
+    self.packer = CANPackerSafety("hyundai_canfd_generated")
+    self.safety = libsafety_py.libsafety
+    self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd,
+                                 HyundaiSafetyFlags.CANFD_LKA_STEERING |
+                                 HyundaiSafetyFlags.LONG |
+                                 HyundaiSafetyFlags.EV_GAS |
+                                 HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE)
+    self.safety.init_tests()
+
+
 
 if __name__ == "__main__":
   unittest.main()

starpilot/controls/starpilot_card.py

@@ -21,6 +21,7 @@ class StarPilotCard:
 
     self.accel_pressed = False
     self.always_on_lateral_allowed = False
+    self.prev_active = False
     self.decel_pressed = False
     self.distancePressed_previously = False
     self.force_coast = False
@@ -78,6 +79,13 @@ class StarPilotCard:
     elif starpilot_toggles.always_on_lateral_main:
       self.always_on_lateral_allowed = carState.cruiseState.available
 
+    # On rising edge of engagement (SET press enabling lat+long), auto-enable AOL
+    # so that lateral persists when braking disengages longitudinal
+    if sm["selfdriveState"].active and not self.prev_active and self.always_on_lateral_set and starpilot_toggles.always_on_lateral_lkas:
+      self.always_on_lateral_allowed = True
+
+    self.prev_active = sm["selfdriveState"].active
+
     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["starpilotPlan"].lateralCheck