pre-ap

opendbc_repo/opendbc/car/tesla/preap/engagement.py

@@ -14,6 +14,7 @@ class PreAPEngagement:
     self.double_pull_window_ms = double_pull_window_ms
     self.cruiseEnabled = False
     self.lateralEnabled = False
+    self.lateralRearmRequired = False
     self.enableLongControl = False
     self.enableJustCC = False
     self.pending_enable = False
@@ -30,6 +31,7 @@ class PreAPEngagement:
   def handle_steering_disengage(self, steering_disengage: bool) -> None:
     if steering_disengage and not self.prev_steering_disengage:
       self.lateralEnabled = False
+      self.lateralRearmRequired = True
       self.cruiseEnabled = False
       self.enableLongControl = False
       self.enableJustCC = False
@@ -48,6 +50,7 @@ class PreAPEngagement:
 
     if cruise_buttons == CruiseButtons.MAIN and prev_cruise_buttons != CruiseButtons.MAIN:
       self.lateralEnabled = True
+      self.lateralRearmRequired = False
       if self.enableDoublePull:
         self._handle_double_pull(curr_time_ms, v_ego, speed_units, use_pedal, pedal_long_allowed, long_control_allowed, di_cruise_state)
       else:
@@ -79,6 +82,7 @@ class PreAPEngagement:
     can_engage = not door_open and gear_shifter == structs.CarState.GearShifter.drive and not seatbelt_unlatched
     if not can_engage:
       self.lateralEnabled = False
+      self.lateralRearmRequired = True
       self.cruiseEnabled = False
       self.enableLongControl = False
       self.enableJustCC = False
@@ -123,6 +127,7 @@ class PreAPEngagement:
       be.type = ButtonType.unknown if is_echo else ButtonType.cancel
       if not is_echo:
         self.lateralEnabled = False
+        self.lateralRearmRequired = True
         self.cruiseEnabled = False
         self.enableLongControl = False
         self.enableJustCC = False

opendbc_repo/opendbc/car/tesla/preap/lateral.py

@@ -6,6 +6,8 @@ def preap_lateral_authorized(CP, CS, panda_states, panda_states_valid: bool) ->
   """Match Pre-AP's existing safety authorization without treating software CC availability as ACC main."""
   if not panda_states_valid or CS.out.gearShifter != structs.CarState.GearShifter.drive or CS.out.doorOpen or CS.out.steeringDisengage:
     return False
+  if CS.engagement.lateralRearmRequired:
+    return False
   config = CP.safetyConfigs[0]
   matching = [p for p in panda_states if p.safetyModel == config.safetyModel and p.safetyParam == config.safetyParam]
   if len(matching) != 1 or matching[0].safetyRxChecksInvalid:

starpilot/common/tests/test_preap_steering_engagement.py

@@ -63,14 +63,14 @@ def test_preap_aol_stalk_cancel_and_reengagement(stock_main):
       assert safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, dat)), (frame, hex(addr), dat.hex())
       if addr == 0x488 and dat[2] >> 6 == 1:
         active_commands += 1
-  assert active_commands == (290 if stock_main else 140)
+  assert active_commands == (240 if stock_main else 140)
 
 
 @pytest.mark.parametrize('failure', ['stale', 'wrong_mode', 'wrong_param', 'rx_invalid', 'park', 'door', 'override', 'cancel'])
 def test_preap_authorization_fails_closed(failure):
   cp = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_PREAP)
   out = SimpleNamespace(gearShifter=structs.CarState.GearShifter.drive, doorOpen=False, steeringDisengage=False)
-  cs = SimpleNamespace(out=out, engagement=SimpleNamespace(lateralEnabled=True), di_cruise_state='OFF')
+  cs = SimpleNamespace(out=out, engagement=SimpleNamespace(lateralEnabled=True, lateralRearmRequired=False), di_cruise_state='OFF')
   panda = SimpleNamespace(safetyModel=cp.safetyConfigs[0].safetyModel, safetyParam=0,
                           safetyRxChecksInvalid=False, alternativeExperience=32, controlsAllowed=True)
   assert preap_lateral_authorized(cp, cs, [panda], True)
@@ -106,5 +106,13 @@ def test_preap_physical_lateral_session_requires_new_pull_after_reset(reset):
     engagement.check_can_engage(reset == 'door', gear, False)
     engagement.check_can_engage(False, structs.CarState.GearShifter.drive, False)
   assert not engagement.lateralEnabled
+  cp = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_PREAP)
+  cs = SimpleNamespace(engagement=engagement, di_cruise_state='STANDBY',
+                       out=SimpleNamespace(gearShifter=structs.CarState.GearShifter.drive, doorOpen=False, steeringDisengage=False))
+  panda = SimpleNamespace(safetyModel=cp.safetyConfigs[0].safetyModel, safetyParam=0,
+                          safetyRxChecksInvalid=False, alternativeExperience=32, controlsAllowed=True)
+  for _ in range(50):
+    assert not preap_lateral_authorized(cp, cs, [panda], True)
   engagement.process_buttons(2, 0, 12000, 10., 'KPH', False, False, True, False)
   assert engagement.lateralEnabled
+  assert preap_lateral_authorized(cp, cs, [panda], True)

starpilot/controls/lib/starpilot_events.py

@@ -23,6 +23,7 @@ class StarPilotEvents:
     self.events = Events(starpilot=True)
 
     self.always_on_lateral_allowed_previously = False
+    self.aol_alert_active_previously = False
     self.previous_traffic_mode = False
     self.previous_switchback_mode = False
     self.random_event_playing = False
@@ -58,11 +59,7 @@ class StarPilotEvents:
     else:
       self.max_acceleration = 0
 
-    if sm["starpilotCarState"].alwaysOnLateralAllowed != self.always_on_lateral_allowed_previously:
-      if sm["starpilotCarState"].alwaysOnLateralAllowed:
-        self.events.add(StarPilotEventName.lkasEnable)
-      else:
-        self.events.add(StarPilotEventName.lkasDisable)
+    self.update_aol_alerts(sm["carParams"], sm["starpilotCarState"])
 
     if self.starpilot_planner.starpilot_vcruise.forcing_stop:
       self.events.add(StarPilotEventName.forcingStop)
@@ -220,3 +217,10 @@ class StarPilotEvents:
 
     self.always_on_lateral_allowed_previously = sm["starpilotCarState"].alwaysOnLateralAllowed
     self.played_events.update(STARPILOT_EVENT_NAME[event] for event in self.events.names)
+
+  def update_aol_alerts(self, car_params, car_state):
+    active = (car_state.alwaysOnLateralEnabled if car_params.carFingerprint == "TESLA_MODEL_S_PREAP"
+              else car_state.alwaysOnLateralAllowed)
+    if active != self.aol_alert_active_previously:
+      self.events.add(StarPilotEventName.lkasEnable if active else StarPilotEventName.lkasDisable)
+    self.aol_alert_active_previously = active

starpilot/controls/tests/test_preap_aol_alerts.py

@@ -0,0 +1,22 @@
+from types import SimpleNamespace
+
+from openpilot.starpilot.controls.lib.starpilot_events import StarPilotEvents, StarPilotEventName
+
+
+def test_preap_chimes_when_authorized_not_when_merely_selected():
+  events = StarPilotEvents(None, None, None)
+  cp = SimpleNamespace(carFingerprint='TESLA_MODEL_S_PREAP')
+  cs = SimpleNamespace(alwaysOnLateralAllowed=True, alwaysOnLateralEnabled=False)
+  for active, expected in ((False, []), (True, [StarPilotEventName.lkasEnable]),
+                           (True, []), (False, [StarPilotEventName.lkasDisable]), (False, [])):
+    events.events.clear()
+    cs.alwaysOnLateralEnabled = active
+    events.update_aol_alerts(cp, cs)
+    assert events.events.names == expected
+
+
+def test_ap1_preserves_selection_chime():
+  events = StarPilotEvents(None, None, None)
+  events.update_aol_alerts(SimpleNamespace(carFingerprint='TESLA_MODEL_S_HW1'),
+                           SimpleNamespace(alwaysOnLateralAllowed=True, alwaysOnLateralEnabled=False))
+  assert events.events.names == [StarPilotEventName.lkasEnable]