mazda: LKA button toggles lateral

opendbc/car/car.capnp

@@ -204,6 +204,8 @@ struct CarState {
   lowSpeedAlert @56 :Bool;  # lost steering control due to a dynamic min steering speed
   blockPcmEnable @60 :Bool;  # whether to allow PCM to enable this frame
   carNotReady @61 :Bool;  # car is transiently refusing engagement, used to prevent a fault if engaged
+  lkaButtonOff @62 :Bool;  # stock lane keep is off at the dash LKA button (button-declared cars)
+  latBlocked @63 :Bool;  # the EPS holds no lateral now: standstill, LKAS off, or the re-arm after a re-enable
 
   # cruise state
   cruiseState @10 :CruiseState;

opendbc/car/mazda/carcontroller.py

@@ -38,6 +38,7 @@ def __init__(self, dbc_names, CP, CP_SP):
     self.driver_torque_samples: deque[float] = deque(maxlen=self.params.STEER_DRIVER_SAMPLES if self.eps_2022 else 1)
     self.packer = CANPacker(dbc_names[Bus.pt])
     self.brake_counter = 0
+    self.cruise_joined = False
     self.stop_and_go = StandstillHold()
     self.lead_adv = AdvertisedLead()
     self.long_counter = 0
@@ -103,9 +104,21 @@ def update(self, CC, CC_SP, CS, now_nanos):
     if self.steer_to_zero and (CS.steer_undelivered or CS.steer_first_engage_hold):
       apply_torque = 0
 
+    # With LKA off, a SET press starts a plain stock ACC run with openpilot fully out: the driver
+    # started it, so our cancel must not kill it. Stock-long only; under alpha-long it is a desync and still cancels.
+    if not CS.out.cruiseState.enabled:
+      self.cruise_joined = False
+    elif CC.enabled:
+      self.cruise_joined = True
+    # The declared TJA button switches lateral only, and stock never disarms MRCC for its press:
+    # the disable the press triggers hands the joined run back to the driver. Runs every cycle
+    # the bit is on, so it also clears the cycle before the disable lands.
+    if not self.CP.openpilotLongitudinalControl and CS.tja_button:
+      self.cruise_joined = False
+    stock_never_joined = not self.CP.openpilotLongitudinalControl and CS.out.cruiseState.enabled and not self.cruise_joined
     # Do not cancel a stock MRCC engagement while the stock radar still owns the bus.
     stock_mrcc_owns_cruise = self.CP.openpilotLongitudinalControl and not CS.radar_was_silenced
-    if CC.cruiseControl.cancel and not stock_mrcc_owns_cruise:
+    if CC.cruiseControl.cancel and not stock_mrcc_owns_cruise and not stock_never_joined:
       # If brake is pressed, let us wait >70ms before trying to disable crz to avoid
       # a race condition with the stock system, where the second cancel from openpilot
       # will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to

opendbc/car/mazda/carstate.py

@@ -14,6 +14,7 @@
 CANCEL_CONTEXT_FRAMES = int(CarControllerParams.CANCEL_CONTEXT_T / DT_CTRL)
 CAM_LANEINFO_FRESH_FRAMES = int(CarControllerParams.CAM_LANEINFO_FRESH_T / DT_CTRL)
 STOCK_CTS_ALERT_FRAMES = int(CarControllerParams.STOCK_CTS_ALERT_T / DT_CTRL)
+LAT_BLOCK_CONFIRM_FRAMES = round(0.2 / DT_CTRL)  # the EPS bit is steady; this only sheds a lost frame
 # Bus witnesses: independent vehicle messages whose silence says the bus is gone, not the radar.
 # Windows at the CANParser's own validity threshold, ten periods; a stricter window would revoke
 # radar ownership on a gap the parser still accepts. {message: (signal, fresh frames)}
@@ -34,6 +35,7 @@ def __init__(self, CP, CP_SP):
     self.lkas_blocked = False
     self.lkas_effective = 0
     self.lkas_track_state = False
+    self.lat_block_frames = 0
     # LKAS non-delivery state is used only with the steer-to-zero EPS.
     self.params = CarControllerParams(CP)
     self.steer_undelivered_frames = 0
@@ -83,6 +85,11 @@ def __init__(self, CP, CP_SP):
     self.cam_laneinfo_seen = False
     self.cam_laneinfo_silent_frames = 0
     self.cam_empty_seen = False
+    self.lkas_on_candidate = False
+    self.lkas_on_stable = False
+    self.lane_lines_armed = False
+    self.lkas_button = 0
+    self.lkas_button_prev = 0
     self.radar_session_refused = False
     self.radar_session_response = 0
     self.fsc_settled_frames = 0
@@ -102,7 +109,7 @@ def stock_radar_gone(self) -> bool:
     # This silence duration establishes radar ownership rather than a dropped frame.
     return self.radar_bus_healthy and self.stock_radar_silent_frames >= STOCK_RADAR_GUARD_FRAMES
 
-  def update_steer_undelivered(self, v_ego_raw: float, lkas_request: float) -> None:
+  def update_steer_undelivered(self, v_ego_raw: float, lkas_request: float, lane_keep_off: bool = False) -> None:
     lkas_blocked, lkas_track_state = self.lkas_blocked, self.lkas_track_state
     self.lkas_delivered |= self.lkas_effective != 0
     self.steer_first_engage_hold = (not self.lkas_delivered and lkas_blocked and lkas_track_state and
@@ -111,7 +118,8 @@ def update_steer_undelivered(self, v_ego_raw: float, lkas_request: float) -> Non
     # Latch sustained zero LKAS_EFFECTIVE for a real request before the camera faults. Clear
     # with LKAS_BLOCK because a zeroed command provides no delivery signal. Driver torque does
     # not gate entry because torque in the requested direction does not reduce the request.
-    if not lkas_blocked:
+    # Lane keep off also clears the latch: zero delivery is then the driver's choice, not a fault.
+    if not lkas_blocked or lane_keep_off:
       self.steer_undelivered_frames = 0
       self.steer_undelivered = False
       self.steer_undelivered_alert = False
@@ -131,8 +139,10 @@ def update_steer_undelivered(self, v_ego_raw: float, lkas_request: float) -> Non
       # identifies normal low-speed standby, which can remain set briefly during a brisk
       # launch; the origin speed catches the standby blocks it does not, the ones carried
       # from a stop through a slow crawl until TRACK_STATE clears with the block still on.
+      # The latch can outlive the request that earned it: the request must still stand.
       self.steer_undelivered_frames += 1
       if (not self.steer_undelivered_alert and not lkas_track_state and
+          abs(lkas_request) > self.params.STEER_UNDELIVERED_MIN and
           self.steer_undelivered_frames >= self.params.STEER_UNDELIVERED_FRAMES + self.params.STEER_UNDELIVERED_ALERT_FRAMES and
           v_ego_raw >= self.params.STEER_UNDELIVERED_ALERT_MIN_SPEED and
           self.lkas_block_origin_speed >= self.params.STEER_UNDELIVERED_ALERT_ORIGIN_SPEED):
@@ -189,6 +199,11 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]:
     self.lkas_blocked = lkas_blocked
     self.lkas_effective = cp.vl["STEER_RATE"]["LKAS_EFFECTIVE"]
     self.lkas_track_state = cp.vl["STEER_RATE"]["LKAS_TRACK_STATE"] == 1
+    # LKAS_BLOCK also covers standstill and the fixed ~3 s re-arm after a dash LKA re-enable,
+    # states where the EPS takes our request but holds no lateral: published so the UI can hold
+    # lateral as arming instead of claiming control the EPS has not granted yet.
+    self.lat_block_frames = self.lat_block_frames + 1 if lkas_blocked else 0
+    ret.latBlocked = self.lat_block_frames >= LAT_BLOCK_CONFIRM_FRAMES
     # The 2022 EPS raises LKAS_FAULT once its 0x243 stream has stopped for about 0.6 s; the
     # camera's own fault follows 5.3 s later and neither clears before the next ignition cycle.
     # Decoded for the log and tooling; the driver-facing fault stays the camera's own.
@@ -197,7 +212,9 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]:
     # frame carries nothing the controller needs; count the torque requests it turned away.
     self.lkas_rejected = sum(1 for v in can_parsers[Bus.loopback].vl_all["CAM_LKAS"]["LKAS_REQUEST"] if v != 0)
     if self.CP.flags & MazdaFlags.STEER_TO_ZERO_EPS:
-      self.update_steer_undelivered(ret.vEgoRaw, cp.vl["STEER_RATE"]["LKAS_REQUEST"])
+      # lane keep off read from the previous cycle: the debounce runs later in update
+      lane_keep_off = self.lane_lines_armed and not (self.lkas_on_stable and self.lkas_on_candidate)
+      self.update_steer_undelivered(ret.vEgoRaw, cp.vl["STEER_RATE"]["LKAS_REQUEST"], lane_keep_off)
 
     if not self.CP.flags & MazdaFlags.STEER_TO_ZERO_EPS:
       # LKAS is enabled at 52kph going up and disabled at 45kph going down
@@ -309,6 +326,27 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]:
     # TODO: is this needed?
     ret.invalidLkasSetting = cam_laneinfo_fresh and cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0
 
+    # The dash LKA button has no CAN signal of its own; LANE_LINES is the state it drives
+    # (0 = lane keep off, nonzero = lane state on); two agreeing frames make a 0 <-> nonzero edge the press.
+    self.lkas_button_prev = self.lkas_button
+    self.lkas_button = 0
+    if self.CP_SP.flags & MazdaFlagsSP.LKA_BUTTON:
+      lane_lines_vals = cp_cam.vl_all["CAM_LANEINFO"]["LANE_LINES"]
+      if len(lane_lines_vals) > 0:
+        lkas_on = int(lane_lines_vals[-1]) != 0
+        if lkas_on != self.lkas_on_candidate:
+          self.lkas_on_candidate = lkas_on
+        elif not self.lane_lines_armed:
+          self.lane_lines_armed = True
+          self.lkas_on_stable = lkas_on
+        elif lkas_on != self.lkas_on_stable:
+          self.lkas_on_stable = lkas_on
+          # one lateral switch per car: a declared TJA button owns the press
+          if not self.CP_SP.flags & MazdaFlagsSP.TJA_BUTTON:
+            self.lkas_button = 1
+      # the state is not the switch: a TJA car reads it too, so its dash press names a driver-choice refusal
+      ret.lkaButtonOff = self.lane_lines_armed and not self.lkas_on_stable
+
     if ret.cruiseState.enabled:
       if not self.lkas_allowed_speed and self.acc_active_last:
         self.low_speed_alert = True
@@ -371,6 +409,7 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]:
       *create_button_events(self.resume_button, prev_resume_button, {1: ButtonType.resumeCruise}),
       *create_button_events(self.main_button, prev_main_button, {1: ButtonType.mainCruise}),
       *create_button_events(self.tja_button, prev_tja_button, {1: ButtonType.lkas}),
+      *create_button_events(self.lkas_button, self.lkas_button_prev, {1: ButtonType.lkas}),
     ]
 
     CarStateExt.update(self, ret, ret_sp, can_parsers)

opendbc/car/mazda/tests/conftest.py

@@ -139,7 +139,7 @@ def set_car_state(cs: CarState, out=None, *, brake_hold=False, stock_radar_alive
                   radar_bus_healthy=True, steer_undelivered=False,
                   lkas_blocked=False, lkas_effective=0, steer_first_engage_hold=False, lkas_allowed_speed=True, lkas_rejected=0,
                   lkas_fault=False, crz_btns_counter=0, stock_tja=0,
-                  cancel_button=0, accel_button=0, decel_button=0, **out_kwargs) -> CarState:
+                  cancel_button=0, accel_button=0, decel_button=0, tja_button=0, **out_kwargs) -> CarState:
   """Put the controller-facing state of a real CarState where a test wants it.
 
   Every keyword is reset to its default on each call, so a test that drives frame by frame
@@ -177,6 +177,7 @@ def set_car_state(cs: CarState, out=None, *, brake_hold=False, stock_radar_alive
   cs.cancel_button = cancel_button
   cs.accel_button = accel_button
   cs.decel_button = decel_button
+  cs.tja_button = tja_button
   return cs
 
 

opendbc/car/mazda/tests/test_mazda_buttons.py

@@ -43,11 +43,11 @@ def test_body_latched_hold_releases_in_protocol(self, cc, cs):
     assert cc.stop_and_go.resume_unlatching, "the pulse must fire with the release"
 
 
-def cancel_frame(cc, cs, cancel, radar_was_silenced, stock_radar_alive):
+def cancel_frame(cc, cs, cancel, radar_was_silenced, stock_radar_alive, enabled=False, cruise_engaged=True, tja_button=0):
   cc.frame = 10  # off the 50-frame alert cadence, on the 10-frame cancel cadence
-  _, sends = step(cc, cs, long_active=False, enabled=False, accel=0., long_state=LongCtrlState.off, available=False,
-                  cruise_engaged=True, cancel=cancel, stock_radar_alive=stock_radar_alive, fsc_settled=False,
-                  radar_was_silenced=radar_was_silenced)
+  _, sends = step(cc, cs, long_active=False, enabled=enabled, accel=0., long_state=LongCtrlState.off, available=False,
+                  cruise_engaged=cruise_engaged, cancel=cancel, stock_radar_alive=stock_radar_alive, fsc_settled=False,
+                  radar_was_silenced=radar_was_silenced, tja_button=tja_button)
   return addrs(sends)
 
 
@@ -69,6 +69,57 @@ def test_cancel_still_sent_after_the_teardown(self, cc, cs):
     sent = cancel_frame(cc, cs, cancel=True, radar_was_silenced=True, stock_radar_alive=False)
     assert CRZ_BTNS in sent
 
-  def test_stock_longitudinal_cancel_unaffected(self, stock_cc, stock_cs):
+  def test_stock_longitudinal_never_joined_is_not_canceled(self, stock_cc, stock_cs):
+    # the feature: ACC with the LKA button off, openpilot fully out
+    for _ in range(3):
+      sent = cancel_frame(stock_cc, stock_cs, cancel=True, radar_was_silenced=False, stock_radar_alive=True)
+      assert CRZ_BTNS not in sent, "CANCELed a cruise openpilot never joined"
+
+  def test_stock_longitudinal_joined_cruise_still_canceled(self, stock_cc, stock_cs):
+    # once openpilot has been enabled on the engagement, a later disengage cancels as before
+    cancel_frame(stock_cc, stock_cs, cancel=True, radar_was_silenced=False, stock_radar_alive=True, enabled=True)
+    sent = cancel_frame(stock_cc, stock_cs, cancel=True, radar_was_silenced=False, stock_radar_alive=True)
+    assert CRZ_BTNS in sent
+
+  def test_the_latch_resets_at_cruise_idle(self, stock_cc, stock_cs):
+    # the suppressed engagement ends; the next engagement is again the driver's until
+    # openpilot joins it
+    cancel_frame(stock_cc, stock_cs, cancel=True, radar_was_silenced=False, stock_radar_alive=True)
+    cancel_frame(stock_cc, stock_cs, cancel=True, radar_was_silenced=False, stock_radar_alive=True, cruise_engaged=False)
     sent = cancel_frame(stock_cc, stock_cs, cancel=True, radar_was_silenced=False, stock_radar_alive=True)
+    assert CRZ_BTNS not in sent
+
+
+class TestTjaHandBack:
+  """The declared TJA button turns MADS lateral off with a press stock treats as lane-centering
+  only: MRCC never disarms for it. The disable the press triggers must hand the joined cruise
+  run back to the driver, the same protection a never-joined run already has."""
+
+  def test_the_press_hands_the_joined_cruise_back(self, stock_cc, stock_cs):
+    # MADS ran on the engagement, then the driver pressed TJA: through the press window and
+    # past it, the cruise underneath stays theirs. The press cycle still reads CC.enabled,
+    # so the clear must land before the disable does.
+    cancel_frame(stock_cc, stock_cs, cancel=True, radar_was_silenced=False, stock_radar_alive=True, enabled=True)
+    for _ in range(2):
+      sent = cancel_frame(stock_cc, stock_cs, cancel=True, radar_was_silenced=False, stock_radar_alive=True,
+                          enabled=True, tja_button=1)
+      assert CRZ_BTNS not in sent
+    for _ in range(2):
+      sent = cancel_frame(stock_cc, stock_cs, cancel=True, radar_was_silenced=False, stock_radar_alive=True)
+      assert CRZ_BTNS not in sent, "CANCELed the cruise the TJA press handed back"
+
+  def test_re_engaging_re_arms_the_sync_cancel(self, stock_cc, stock_cs):
+    # the protection belongs to the press, not to the car: joining the same cruise again
+    # brings the desync cancel back
+    cancel_frame(stock_cc, stock_cs, cancel=True, radar_was_silenced=False, stock_radar_alive=True, enabled=True)
+    cancel_frame(stock_cc, stock_cs, cancel=True, radar_was_silenced=False, stock_radar_alive=True, tja_button=1)
+    cancel_frame(stock_cc, stock_cs, cancel=True, radar_was_silenced=False, stock_radar_alive=True, enabled=True)
+    sent = cancel_frame(stock_cc, stock_cs, cancel=True, radar_was_silenced=False, stock_radar_alive=True)
+    assert CRZ_BTNS in sent
+
+  def test_alpha_long_still_cancels_through_the_press(self, cc, cs):
+    # under alpha-long the engagement is openpilot's own, so the lateral-only disable is a
+    # desync and cancels, the same split as the never-joined latch
+    cancel_frame(cc, cs, cancel=True, radar_was_silenced=True, stock_radar_alive=False, enabled=True)
+    sent = cancel_frame(cc, cs, cancel=True, radar_was_silenced=True, stock_radar_alive=False, tja_button=1)
     assert CRZ_BTNS in sent