AOL/TESLA

starpilot/common/controller_actions.py

@@ -66,7 +66,7 @@ CONTROLLER_ACTION_OPTIONS = (
   {
     "key": CONTROLLER_ACTION_TOGGLE_AOL,
     "label": "Toggle AOL",
-    "description": "Toggles Always On Lateral like the vehicle LKAS button; it does not change the AOL setting.",
+    "description": "Arms or disarms Always On Lateral for the current drive and updates the blue AOL border. The AOL master setting is unchanged.",
     "section": "Controller Actions",
   },
   {

starpilot/common/tests/test_favorite_slots.py

@@ -2,6 +2,7 @@ import json
 from typing import cast
 
 from openpilot.common.params import ParamKeyType, Params
+from openpilot.starpilot.common.controller_actions import CONTROLLER_ACTION_TOGGLE_AOL
 from openpilot.starpilot.common.favorite_slots import (
   FAVORITE_ACTION_ACCEL_COUNTER,
   FAVORITE_ACTION_DECEL_COUNTER,
@@ -37,6 +38,7 @@ class FakeParams:
       FAVORITE_SLOTS_PARAM: ParamKeyType.JSON,
       PERSONALITY_PROFILES_PARAM: ParamKeyType.JSON,
       "AlphaLongitudinalEnabled": ParamKeyType.BOOL,
+      "AlwaysOnLateral": ParamKeyType.BOOL,
       "ForceOffroad": ParamKeyType.BOOL,
       "RedneckCruise": ParamKeyType.BOOL,
       "NotBool": ParamKeyType.INT,
@@ -111,6 +113,26 @@ def test_galaxy_only_ford_controls_are_not_available_to_device_favorites():
   assert ford_keys.isdisjoint({option["key"] for option in options})
 
 
+def test_aol_master_favorite_is_preserved_alongside_transient_toggle_action():
+  options = build_favorite_slot_options(lambda _key: True, alpha_longitudinal_available=True)
+  option_keys = {option["key"] for option in options}
+
+  assert "AlwaysOnLateral" in option_keys
+  assert CONTROLLER_ACTION_TOGGLE_AOL in option_keys
+
+  params = FakeParams()
+  memory = FakeParams()
+  params.put_bool("AlwaysOnLateral", True)
+  params.put(FAVORITE_SLOTS_PARAM, [
+    {"enabled": True, "show_onroad": True, "key": "AlwaysOnLateral", "label": "Always On Lateral"},
+  ])
+
+  slots = load_favorite_slots(params, eligible_keys={"AlwaysOnLateral"})
+  assert slots[0]["key"] == "AlwaysOnLateral"
+  assert execute_favorite_key("AlwaysOnLateral", params, memory, eligible_keys={"AlwaysOnLateral"}) is True
+  assert params.get_bool("AlwaysOnLateral") is False
+
+
 def test_parked_only_personality_keys_are_never_exposed_or_mutated_as_favorites():
   blocked_keys = PERSONALITY_PARKED_PARAM_KEYS | {PERSONALITY_PROFILES_PARAM}
   options = build_favorite_slot_options(lambda _key: True, alpha_longitudinal_available=True)

starpilot/controls/starpilot_card.py

@@ -58,6 +58,8 @@ class StarPilotCard:
 
     self.accel_pressed = False
     self.always_on_lateral_allowed = False
+    self.controller_aol_override = None
+    self.pacifica_aol_set_seen = False
     hyundai_flags = getattr(self.CP, "flags", 0)
     self.kia_forte_non_scc = (
       getattr(self.CP, "carFingerprint", None) in (HYUNDAI_CAR.KIA_FORTE_2019_NON_SCC, HYUNDAI_CAR.KIA_FORTE_2021_NON_SCC) and
@@ -160,7 +162,7 @@ class StarPilotCard:
     self._controller_action_counters[key] = current
     return max(0, current - previous)
 
-  def _toggle_controller_aol(self, carState, starpilot_toggles):
+  def _toggle_controller_aol(self, carState, starpilot_toggles, main_cruise_aol=False):
     if not self.always_on_lateral_supported or not getattr(starpilot_toggles, "always_on_lateral", False):
       return False
     tesla_disengage_on_brake = (
@@ -169,16 +171,26 @@ class StarPilotCard:
     )
     if tesla_disengage_on_brake and not self.always_on_lateral_allowed and carState.brakePressed:
       return False
+    pacifica_requires_set = pacifica_hybrid_aol_requires_set_press(
+      getattr(self.CP, "carFingerprint", None), getattr(self.CP, "pcmCruise", False),
+    )
+    if main_cruise_aol and (
+      not carState.cruiseState.available or
+      (pacifica_requires_set and not self.pacifica_aol_set_seen)
+    ):
+      return False
     if self.hyundai_aol_needs_engagement:
       self.hyundai_aol_ready = True
     self.always_on_lateral_allowed = not self.always_on_lateral_allowed
+    if main_cruise_aol:
+      self.controller_aol_override = self.always_on_lateral_allowed
     if tesla_disengage_on_brake and self.always_on_lateral_allowed:
       self.tesla_aol_brake_disengaged = False
     if carState.cruiseState.enabled or self.pause_lateral:
       self.pause_lateral = not self.always_on_lateral_allowed
     return True
 
-  def _handle_controller_actions(self, carState, sm, starpilot_toggles):
+  def _handle_controller_actions(self, carState, sm, starpilot_toggles, main_cruise_aol=False):
     force_coast_count = self._pending_controller_action_count(
       CONTROLLER_ACTION_FORCE_COAST
     )
@@ -196,7 +208,7 @@ class StarPilotCard:
       CONTROLLER_ACTION_TOGGLE_AOL
     )
     if aol_count % 2:
-      self._toggle_controller_aol(carState, starpilot_toggles)
+      self._toggle_controller_aol(carState, starpilot_toggles, main_cruise_aol)
 
   def _handle_favorite_traffic_mode_action(self, sm):
     counter = self.params_memory.get_int(FAVORITE_ACTION_TRAFFIC_MODE_COUNTER)
@@ -336,24 +348,30 @@ class StarPilotCard:
     if forte_main_cruise_aol_managed:
       self.always_on_lateral_allowed = carState.cruiseState.available
 
-    if starpilot_toggles.always_on_lateral_main and not button_managed_aol:
+    main_cruise_aol = starpilot_toggles.always_on_lateral_main and not button_managed_aol
+    if main_cruise_aol:
       car_fingerprint = getattr(self.CP, "carFingerprint", None)
       pcm_cruise = getattr(self.CP, "pcmCruise", False)
       if pacifica_hybrid_aol_requires_set_press(car_fingerprint, pcm_cruise):
         # Chrysler Pacifica Hybrid stock ACC can fall back to plain cruise if AOL
         # starts steering before the driver presses SET.
         if not carState.cruiseState.available:
+          self.pacifica_aol_set_seen = False
           self.always_on_lateral_allowed = False
         elif carState.cruiseState.enabled and not self.prev_cruise_enabled:
+          self.pacifica_aol_set_seen = True
           self.always_on_lateral_allowed = True
+        if self.controller_aol_override is not None:
+          self.always_on_lateral_allowed = self.pacifica_aol_set_seen and self.controller_aol_override
       else:
-        self.always_on_lateral_allowed = carState.cruiseState.available
+        self.always_on_lateral_allowed = carState.cruiseState.available and self.controller_aol_override is not False
 
     # On rising edge of engagement (SET press enabling lat+long), auto-enable AOL
     # so that lateral persists when braking disengages longitudinal
     engagement_started = sm["selfdriveState"].active and not self.prev_active
     if (engagement_started and self.always_on_lateral_set and
-        (starpilot_toggles.always_on_lateral_lkas or tesla_disengage_on_brake)):
+        (starpilot_toggles.always_on_lateral_lkas or tesla_disengage_on_brake) and
+        (not main_cruise_aol or self.controller_aol_override is not False)):
       if hyundai_aol_needs_engagement:
         self.hyundai_aol_ready = True
       self.tesla_aol_brake_disengaged = False
@@ -374,19 +392,6 @@ class StarPilotCard:
     if not self.always_on_lateral_supported:
       self.always_on_lateral_allowed = False
 
-    self.always_on_lateral_enabled = self.always_on_lateral_allowed and self.always_on_lateral_set
-    if getattr(self.CP, "carFingerprint", None) == "TESLA_MODEL_S_PREAP":
-      self.always_on_lateral_enabled &= preap_authorized
-    self.always_on_lateral_enabled &= carState.gearShifter not in NON_DRIVING_GEARS
-    self.always_on_lateral_enabled &= not hyundai_aol_needs_engagement or self.hyundai_aol_ready
-    self.always_on_lateral_enabled &= sm["starpilotPlan"].lateralCheck
-    self.always_on_lateral_enabled &= sm["liveCalibration"].calPerc >= 1
-    self.always_on_lateral_enabled &= not aol_blocked_by_immediate_disable(
-      sm["selfdriveState"].alertType, sm["starpilotSelfdriveState"].alertType,
-    )
-    self.always_on_lateral_enabled &= not (carState.brakePressed and carState.vEgo < starpilot_toggles.always_on_lateral_pause_speed) or carState.standstill
-    self.always_on_lateral_enabled &= not self.error_log.is_file()
-
     if sm.updated["starpilotPlan"] or any(be_type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be_type in button_event_types):
       self.accel_pressed = any(be_type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be_type in button_event_types)
 
@@ -458,7 +463,20 @@ class StarPilotCard:
         else:
           self.handle_button_event("lkas", sm, starpilot_toggles)
 
-    self._handle_controller_actions(carState, sm, starpilot_toggles)
+    self._handle_controller_actions(carState, sm, starpilot_toggles, main_cruise_aol)
+
+    self.always_on_lateral_enabled = self.always_on_lateral_allowed and self.always_on_lateral_set
+    if getattr(self.CP, "carFingerprint", None) == "TESLA_MODEL_S_PREAP":
+      self.always_on_lateral_enabled &= preap_authorized
+    self.always_on_lateral_enabled &= carState.gearShifter not in NON_DRIVING_GEARS
+    self.always_on_lateral_enabled &= not hyundai_aol_needs_engagement or self.hyundai_aol_ready
+    self.always_on_lateral_enabled &= sm["starpilotPlan"].lateralCheck
+    self.always_on_lateral_enabled &= sm["liveCalibration"].calPerc >= 1
+    self.always_on_lateral_enabled &= not aol_blocked_by_immediate_disable(
+      sm["selfdriveState"].alertType, sm["starpilotSelfdriveState"].alertType,
+    )
+    self.always_on_lateral_enabled &= not (carState.brakePressed and carState.vEgo < starpilot_toggles.always_on_lateral_pause_speed) or carState.standstill
+    self.always_on_lateral_enabled &= not self.error_log.is_file()
 
     if getattr(starpilot_toggles, "has_canfd_media_buttons", False):
       if starpilotCarState.modePressed:

starpilot/controls/tests/test_starpilot_card.py

@@ -398,6 +398,88 @@ def test_controller_aol_does_not_require_physical_lkas_button_mapping(monkeypatc
   assert ret.alwaysOnLateralAllowed is True
 
 
+@pytest.mark.parametrize("brand", ["tesla", "gm"])
+def test_controller_aol_owns_main_latch_only_while_cruise_is_available(monkeypatch, tmp_path, brand):
+  monkeypatch.setattr(spc, "Params", FakeParams)
+  monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
+
+  card = spc.StarPilotCard(
+    SimpleNamespace(brand=brand),
+    SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL),
+  )
+  sm = make_sm()
+  toggles = make_toggles(always_on_lateral=True, always_on_lateral_main=True)
+  starpilot_car_state = SimpleNamespace(distancePressed=False)
+  counter = spc.CONTROLLER_ACTION_COUNTERS[spc.CONTROLLER_ACTION_TOGGLE_AOL]
+
+  initial = card.update(make_car_state(available=True), starpilot_car_state, sm, toggles)
+  assert initial.alwaysOnLateralAllowed is True
+  assert initial.alwaysOnLateralEnabled is True
+
+  card.params_memory.put_int(counter, 1)
+  toggled_off = card.update(make_car_state(available=True), starpilot_car_state, sm, toggles)
+  assert toggled_off.alwaysOnLateralAllowed is False
+  assert toggled_off.alwaysOnLateralEnabled is False
+
+  persisted_off = card.update(make_car_state(available=True), starpilot_car_state, sm, toggles)
+  assert persisted_off.alwaysOnLateralAllowed is False
+  assert persisted_off.alwaysOnLateralEnabled is False
+
+  card.params_memory.put_int(counter, 2)
+  toggled_on = card.update(make_car_state(available=True), starpilot_car_state, sm, toggles)
+  assert toggled_on.alwaysOnLateralAllowed is True
+  assert toggled_on.alwaysOnLateralEnabled is True
+
+  unavailable = card.update(make_car_state(available=False), starpilot_car_state, sm, toggles)
+  assert unavailable.alwaysOnLateralAllowed is False
+  assert unavailable.alwaysOnLateralEnabled is False
+
+  available_again = card.update(make_car_state(available=True), starpilot_car_state, sm, toggles)
+  assert available_again.alwaysOnLateralAllowed is True
+  assert available_again.alwaysOnLateralEnabled is True
+
+  card.params_memory.put_int(counter, 3)
+  card.update(make_car_state(available=True), starpilot_car_state, sm, toggles)
+  card.update(make_car_state(available=False), starpilot_car_state, sm, toggles)
+  still_off = card.update(make_car_state(available=True), starpilot_car_state, sm, toggles)
+  assert still_off.alwaysOnLateralAllowed is False
+  assert still_off.alwaysOnLateralEnabled is False
+
+
+def test_controller_aol_cannot_arm_main_latch_without_cruise(monkeypatch, tmp_path):
+  monkeypatch.setattr(spc, "Params", FakeParams)
+  monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
+  card = spc.StarPilotCard(SimpleNamespace(brand="tesla"), SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL))
+  toggles = make_toggles(always_on_lateral=True, always_on_lateral_main=True)
+  starpilot_car_state = SimpleNamespace(distancePressed=False)
+  card.params_memory.put_int(spc.CONTROLLER_ACTION_COUNTERS[spc.CONTROLLER_ACTION_TOGGLE_AOL], 1)
+
+  unavailable = card.update(make_car_state(available=False), starpilot_car_state, make_sm(), toggles)
+  assert unavailable.alwaysOnLateralEnabled is False
+  assert card.controller_aol_override is None
+
+  available = card.update(make_car_state(available=True), starpilot_car_state, make_sm(), toggles)
+  assert available.alwaysOnLateralEnabled is True
+
+
+def test_tesla_controller_disarm_survives_engagement(monkeypatch, tmp_path):
+  monkeypatch.setattr(spc, "Params", FakeParams)
+  monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
+  card = spc.StarPilotCard(SimpleNamespace(brand="tesla"), SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL))
+  toggles = make_toggles(always_on_lateral=True, always_on_lateral_main=True, tesla_aol_disengage_on_brake=True)
+  starpilot_car_state = SimpleNamespace(distancePressed=False)
+  sm = make_sm()
+  counter = spc.CONTROLLER_ACTION_COUNTERS[spc.CONTROLLER_ACTION_TOGGLE_AOL]
+  card.update(make_car_state(available=True), starpilot_car_state, sm, toggles)
+  card.params_memory.put_int(counter, 1)
+  card.update(make_car_state(available=True), starpilot_car_state, sm, toggles)
+
+  sm["selfdriveState"].active = True
+  engaged = card.update(make_car_state(available=True, enabled=True), starpilot_car_state, sm, toggles)
+  assert engaged.alwaysOnLateralAllowed is False
+  assert engaged.alwaysOnLateralEnabled is False
+
+
 def test_hyundai_lkas_button_can_start_aol_before_normal_engagement(monkeypatch, tmp_path):
   monkeypatch.setattr(spc, "Params", FakeParams)
   monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
@@ -1341,19 +1423,30 @@ def test_non_button_aol_platform_keeps_main_aol_when_main_cruise_is_mapped(monke
   assert ret.alwaysOnLateralEnabled is True
 
 
-def test_main_aol_still_follows_cruise_main_for_other_platforms(monkeypatch, tmp_path):
+@pytest.mark.parametrize("brand", ["tesla", "gm", "toyota"])
+def test_main_aol_without_controller_action_still_follows_cruise_main(monkeypatch, tmp_path, brand):
   monkeypatch.setattr(spc, "Params", FakeParams)
   monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
 
-  card = spc.StarPilotCard(SimpleNamespace(brand="toyota", carFingerprint="TOYOTA_TEST", pcmCruise=True),
+  card = spc.StarPilotCard(SimpleNamespace(brand=brand, pcmCruise=True),
                            SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL))
 
-  ret = card.update(make_car_state(available=True), SimpleNamespace(distancePressed=False), make_sm(),
-                    make_toggles(always_on_lateral_main=True))
+  sm = make_sm()
+  toggles = make_toggles(always_on_lateral_main=True)
+  starpilot_car_state = SimpleNamespace(distancePressed=False)
+  ret = card.update(make_car_state(available=True), starpilot_car_state, sm, toggles)
 
   assert ret.alwaysOnLateralAllowed is True
   assert ret.alwaysOnLateralEnabled is True
 
+  ret = card.update(make_car_state(available=False), starpilot_car_state, sm, toggles)
+  assert ret.alwaysOnLateralAllowed is False
+  assert ret.alwaysOnLateralEnabled is False
+
+  ret = card.update(make_car_state(available=True), starpilot_car_state, sm, toggles)
+  assert ret.alwaysOnLateralAllowed is True
+  assert ret.alwaysOnLateralEnabled is True
+
 
 def test_pacifica_hybrid_main_aol_waits_for_set_press(monkeypatch, tmp_path):
   monkeypatch.setattr(spc, "Params", FakeParams)
@@ -1389,6 +1482,38 @@ def test_pacifica_hybrid_main_aol_waits_for_set_press(monkeypatch, tmp_path):
   assert ret.alwaysOnLateralEnabled is False
 
 
+def test_pacifica_hybrid_controller_aol_still_requires_set_press(monkeypatch, tmp_path):
+  monkeypatch.setattr(spc, "Params", FakeParams)
+  monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
+  card = spc.StarPilotCard(
+    SimpleNamespace(brand="chrysler", carFingerprint=CHRYSLER_CAR.CHRYSLER_PACIFICA_2019_HYBRID, pcmCruise=True),
+    SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL),
+  )
+  toggles = make_toggles(always_on_lateral=True, always_on_lateral_main=True)
+  starpilot_car_state = SimpleNamespace(distancePressed=False)
+  counter = spc.CONTROLLER_ACTION_COUNTERS[spc.CONTROLLER_ACTION_TOGGLE_AOL]
+  card.params_memory.put_int(counter, 1)
+
+  before_set = card.update(make_car_state(available=True), starpilot_car_state, make_sm(), toggles)
+  assert before_set.alwaysOnLateralEnabled is False
+  assert card.controller_aol_override is None
+
+  after_set = card.update(make_car_state(available=True, enabled=True), starpilot_car_state, make_sm(), toggles)
+  assert after_set.alwaysOnLateralEnabled is True
+
+  card.params_memory.put_int(counter, 2)
+  disarmed = card.update(make_car_state(available=True, enabled=True), starpilot_car_state, make_sm(), toggles)
+  assert disarmed.alwaysOnLateralEnabled is False
+  card.params_memory.put_int(counter, 3)
+  rearmed = card.update(make_car_state(available=True), starpilot_car_state, make_sm(), toggles)
+  assert rearmed.alwaysOnLateralEnabled is True
+
+  cruise_off = card.update(make_car_state(available=False), starpilot_car_state, make_sm(), toggles)
+  assert cruise_off.alwaysOnLateralEnabled is False
+  before_next_set = card.update(make_car_state(available=True), starpilot_car_state, make_sm(), toggles)
+  assert before_next_set.alwaysOnLateralEnabled is False
+
+
 def test_conditional_chill_wheel_override_cycles_manual_state(monkeypatch, tmp_path):
   monkeypatch.setattr(spc, "Params", FakeParams)
   monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)

starpilot/system/wheel_controls/tests/test_wheel_controlsd.py

@@ -328,6 +328,27 @@ def test_only_key_down_is_dispatched(monkeypatch):
   daemon.close()
 
 
+def test_duplicate_key_down_packets_execute_only_once_until_release(monkeypatch):
+  params = FakeParams({"IsOffroad": False})
+  memory = FakeParams()
+  wheel_controlsd.upsert_mapping(source(), 30, 0, params)
+  daemon = wheel_controlsd.WheelControlsDaemon(params, memory)
+  triggered = []
+  monkeypatch.setattr(wheel_controlsd, "execute_favorite_slot", lambda slot, *_args: triggered.append(slot) or True)
+  read_fd, write_fd = os.pipe()
+  os.set_blocking(read_fd, False)
+  daemon.sources[read_fd] = source()
+  daemon.buffers[read_fd] = bytearray()
+
+  for value in (1, 1, 1, 0, 1):
+    os.write(write_fd, wheel_controlsd.INPUT_EVENT.pack(0, 0, wheel_controlsd.EV_KEY, 30, value))
+  daemon._read_events(read_fd)
+
+  assert triggered == [0, 0]
+  os.close(write_fd)
+  daemon.close()
+
+
 def test_stale_selector_event_after_controller_disconnect_is_ignored():
   params = FakeParams({"IsOffroad": False})
   memory = FakeParams()

starpilot/system/wheel_controls/wheel_controlsd.py

@@ -650,7 +650,7 @@ class WheelControlsDaemon:
           if key not in self.pressed_keys:
             self.pressed_keys.add(key)
             self._publish_button_press(time.monotonic_ns())
-          self._handle_key(source, code)
+            self._handle_key(source, code)
       elif event_type == EV_ABS and ABS_HAT0X <= code <= ABS_HAT3Y:
         previous = self.hat_values.get((fd, code), 0)
         self.hat_values[(fd, code)] = value