mazda: lateral toggle, independent TJA/MRCC, wheel HUD for lateral-engage

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
+  latBlocked @63 :Bool;  # EPS can't apply torque: standstill, LKA off, or the re-arm after a re-enable
 
   # cruise state
   cruiseState @10 :CruiseState;

opendbc/car/mazda/carcontroller.py

@@ -10,15 +10,25 @@
 from opendbc.car.mazda.longitudinal import BREAKAWAY_FRAMES, AdvertisedLead, StandstillHold
 from opendbc.car.mazda.radar_session import RadarSessionManager, RadarSessionState
 from opendbc.car.mazda.values import CarControllerParams, Buttons, MazdaFlags
+from opendbc.sunnypilot.car.mazda.values import MazdaFlagsSP
 
-from opendbc.sunnypilot.car.mazda.icbm import IntelligentCruiseButtonManagementInterface
+from opendbc.sunnypilot.car.mazda.icbm import BUTTONS, IntelligentCruiseButtonManagementInterface
 from opendbc.sunnypilot.car.stock_ecu import StockEcuState
 
 VisualAlert = structs.CarControl.HUDControl.VisualAlert
 LongCtrlState = structs.CarControl.Actuators.LongControlState
 
 # Send synthetic radar frames to both consumers; panda does not forward locally generated frames.
 LONG_BUSES = (0, 2)
+TJA_MRCC_MAX_TX_FRAMES = 3
+TJA_MRCC_RAW_OFF_CONFIRM_FRAMES = 5
+# The body ECU drops discrete presses faster than one per 200 ms (measured, docs/zoompilot/icbm.md);
+# the arm clears in PEDALS ~80 ms after a press plus the 50 ms confirm.
+TJA_MRCC_TX_PERIOD = 0.2  # s between undo presses
+# The press-induced arm appears ~80 ms after the press edge (docs/zoompilot/mazda-lateral.md).
+TJA_MRCC_ARM_WAIT_FRAMES = int(1.0 / DT_CTRL)
+# The white wheel waits this long on a fully-off, quiet cruise before it displays.
+MADS_WHITE_HUD_OFF_CONFIRM_FRAMES = int(0.5 / DT_CTRL)
 
 
 class CarController(CarControllerBase, IntelligentCruiseButtonManagementInterface):
@@ -38,6 +48,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
@@ -52,6 +63,16 @@ def __init__(self, dbc_names, CP, CP_SP):
     self.tja_press_count = 0
     self.tja_press_frame: int | None = None
     self.tja_episode_alerted = False
+    self.tja_button_prev = False
+    self.mrcc_armed_prev: bool | None = None
+    self.mrcc_undo_pending = False
+    self.mrcc_undo_saw_armed = False
+    self.mrcc_undo_frames = 0
+    self.mrcc_raw_off_frames = 0
+    self.mrcc_arm_wait_frames = 0
+    # The white wheel rides the alert frame; on_bus tracks that the white bit is on the wire.
+    self.mads_white_hud_off_frames = 0
+    self.mads_white_hud_on_bus = False
 
   def update(self, CC, CC_SP, CS, now_nanos):
     can_sends = []
@@ -103,9 +124,18 @@ 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
 
+    # Never cancel ACC set by user.
+    if not CS.out.cruiseState.enabled:
+      self.cruise_joined = False
+    elif CC.enabled:
+      self.cruise_joined = True
+    # A TJA press turns MADS lateral off, and stock keeps MRCC engaged through it.
+    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
@@ -120,27 +150,26 @@ def update(self, CC, CC_SP, CS, now_nanos):
       if self.resume_requested(CC) and self.frame % 5 == 0:
         can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.RESUME))
 
+    if self.CP_SP.flags & MazdaFlagsSP.TJA_BUTTON:
+      can_sends.extend(self.update_mrcc_cleanup(CC, CC_SP, CS))
+
     self.apply_torque_last = apply_torque
 
     if self.CP.openpilotLongitudinalControl:
       can_sends.extend(self.update_longitudinal(CC, CC_SP, CS))
 
     can_sends.extend(self.update_camera_tja(CC, CS))
 
-    # send HUD alerts
-    if self.frame % 50 == 0:
-      ldw = CC.hudControl.visualAlert == VisualAlert.ldw
-      steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired
-      # TODO: find a way to silence audible warnings so we can add more hud alerts
-      steer_required = steer_required and CS.lkas_allowed_speed
-      can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required))
+    can_sends.extend(self.update_hud(CC, CC_SP, CS))
 
     # send steering command
     can_sends.append(mazdacan.create_steering_control(self.packer, self.CP,
                                                       self.frame, apply_torque, CS.cam_lkas))
 
-    # Suppress ICBM while cancel or resume is active to avoid competing button frames.
-    icbm_suppress = CC.cruiseControl.cancel or CC.cruiseControl.resume or CS.cancel_button == 1
+    # Suppress ICBM while cancel/resume is active or the MRCC cleanup owns CRZ_BTNS:
+    # the wheel's press pattern owns the counter stream until release.
+    icbm_suppress = (CC.cruiseControl.cancel or CC.cruiseControl.resume or CS.cancel_button == 1 or
+                     self.mrcc_undo_pending or CS.tja_button == 1)
     if not icbm_suppress:
       can_sends.extend(IntelligentCruiseButtonManagementInterface.update(self, CC_SP, CS, self.packer, self.frame, self.last_button_frame))
 
@@ -194,6 +223,147 @@ def update_camera_tja(self, CC, CS):
         self.tja_episode_alerted = True
     return can_sends
 
+  def update_mrcc_cleanup(self, CC, CC_SP, CS):
+    # Undo the MRCC arm a physical TJA press causes: the wheel's press reaches every bus-0
+    # ECU directly, so the button the driver declared as the lateral switch also arms cruise.
+    # Answer with the driver's own MRCC master press until raw PEDALS confirms the arm is gone.
+    can_sends = []
+
+    raw_armed = bool(CS.mrcc_armed_raw)
+    # PEDALS drops both cruise bits through a brake transition; the filtered state bridges
+    # those samples, and only sustained raw-off is authoritative.
+    self.mrcc_raw_off_frames = 0 if raw_armed else self.mrcc_raw_off_frames + 1
+    raw_off_confirmed = self.mrcc_raw_off_frames >= TJA_MRCC_RAW_OFF_CONFIRM_FRAMES
+    # PEDALS holds both cruise bits low under braking: the clock restarts there and runs
+    # again once the pedal is off.
+    self.mrcc_arm_wait_frames = 0 if CS.out.brakePressed else self.mrcc_arm_wait_frames + 1
+    if self.CP.openpilotLongitudinalControl:
+      filtered_armed = CS.cruise_available
+    else:
+      filtered_armed = CS.out.cruiseState.available
+    mrcc_armed = raw_armed or (filtered_armed and not raw_off_confirmed)
+
+    if CS.tja_button and not self.tja_button_prev:
+      # A press before the previous press's arm reconciled sees that arm as its own
+      # artifact, not the driver's baseline.
+      if self.mrcc_undo_pending:
+        self.mrcc_undo_frames = 0
+      else:
+        # PEDALS can already show the press-induced arm in the same cycle as the edge; the
+        # previous stable sample is the state that existed before the press.
+        armed_before_press = self.mrcc_armed_prev if self.mrcc_armed_prev is not None else mrcc_armed
+        self.mrcc_undo_pending = not armed_before_press
+      self.mrcc_undo_saw_armed = False
+      self.mrcc_arm_wait_frames = 0
+    self.tja_button_prev = bool(CS.tja_button)
+    self.mrcc_armed_prev = mrcc_armed
+
+    # The arm is fully reconciled; the budget returns for the next press.
+    if not mrcc_armed and not CS.cruise_enabled and not CS.out.cruiseState.enabled:
+      self.mrcc_undo_frames = 0
+
+    if not self.mrcc_undo_pending:
+      return can_sends
+
+    self.mrcc_undo_saw_armed |= raw_armed
+
+    # The driver's own cruise presses own CRZ_BTNS from this cycle on, and restoring
+    # stock ECU ownership changes who owns the arm.
+    driver_activity = (CS.cancel_button or CS.resume_button or CS.accel_button or CS.decel_button or
+                       CS.mrcc_button or CS.distance_button)
+    if driver_activity or CS.radar_handback_active or CC_SP.stockEcuHandBack:
+      self.mrcc_undo_pending = False
+      return can_sends
+
+    # The arm never appeared: a takeover already disarmed it, or this car does not arm
+    # through the press. Past the bounded brake-free wait, stand down.
+    if not self.mrcc_undo_saw_armed:
+      if self.mrcc_arm_wait_frames >= TJA_MRCC_ARM_WAIT_FRAMES:
+        self.mrcc_undo_pending = False
+      return can_sends
+
+    if raw_off_confirmed:
+      self.mrcc_undo_pending = False
+      return can_sends
+
+    # openpilot's own cancel or resume interleaves button frames; wait it out rather
+    # than race the counter stream.
+    if CC.cruiseControl.cancel or CC.cruiseControl.resume:
+      return can_sends
+
+    # Never inside the driver's held press.
+    if CS.tja_button:
+      return can_sends
+
+    # One press per body-paced slot, within the episode budget; never while PEDALS reads disarmed,
+    # where the master press would arm instead of disarm. last_button_frame also paces ICBM.
+    if raw_armed and self.mrcc_undo_frames < TJA_MRCC_MAX_TX_FRAMES and \
+       (self.frame - self.last_button_frame) * DT_CTRL > TJA_MRCC_TX_PERIOD:
+      can_sends.append(mazdacan.create_mrcc_off_cmd(self.packer, CS.crz_btns_counter))
+      self.last_button_frame = self.frame
+      self.mrcc_undo_frames += 1
+      if self.mrcc_undo_frames >= TJA_MRCC_MAX_TX_FRAMES:
+        self.mrcc_undo_pending = False
+
+    return can_sends
+
+  def update_hud(self, CC, CC_SP, CS):
+    """The alert frame at 2 Hz, mirroring the camera's own HUD state.
+
+    On a TJA-declared car with MADS active and cruise fully off, the white-wheel TJA=2
+    state is XORed into the camera's current payload, but only onto an exact allowlisted
+    idle base: TJA=2 is not display-only, the body reads the same frame. Every cruise
+    interaction fails closed, and a white wheel HUD state that became unsafe is withdrawn
+    immediately, outside the cadence.
+    """
+    if self.CP.openpilotLongitudinalControl:
+      filtered_available, filtered_enabled = CS.cruise_available, CS.cruise_enabled
+    else:
+      filtered_available, filtered_enabled = CS.out.cruiseState.available, CS.out.cruiseState.enabled
+
+    session_ambiguous = CS.radar_handback_active or CC_SP.stockEcuHandBack
+    mrcc_off = (not session_ambiguous and not CS.mrcc_armed_raw and
+                not filtered_available and not filtered_enabled)
+
+    # Every button a TJA wheel carries: TJA, MRCC, SET+/-, RES, DISTANCE, plus the
+    # synthesized ICBM set presses and openpilot's own cancel/resume.
+    button_activity = (CS.tja_button or CS.mrcc_button or CS.cancel_button or CS.resume_button or
+                       CS.accel_button or CS.decel_button or CS.distance_button or
+                       CC_SP.intelligentCruiseButtonManagement.sendButton in BUTTONS or
+                       CC.cruiseControl.cancel or CC.cruiseControl.resume)
+
+    ldw = CC.hudControl.visualAlert == VisualAlert.ldw
+    steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired
+    # TODO: find a way to silence audible warnings so we can add more hud alerts
+    steer_required = steer_required and CS.lkas_allowed_speed
+    alert = mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required)
+
+    fsc_raw = CS.cam_laneinfo_raw
+    hud_base = mazdacan.white_hud_allowlist_base(fsc_raw)
+    white_allowed = (
+      bool(self.CP_SP.flags & MazdaFlagsSP.TJA_BUTTON) and
+      CC_SP.mads.active and
+      CS.cam_laneinfo_live and
+      hud_base is not None and
+      CC.hudControl.visualAlert == VisualAlert.none and
+      not button_activity and
+      mrcc_off
+    )
+    if white_allowed:
+      self.mads_white_hud_off_frames = min(self.mads_white_hud_off_frames + 1, MADS_WHITE_HUD_OFF_CONFIRM_FRAMES)
+    else:
+      self.mads_white_hud_off_frames = 0
+    white = white_allowed and self.mads_white_hud_off_frames >= MADS_WHITE_HUD_OFF_CONFIRM_FRAMES
+    withdraw_now = self.mads_white_hud_on_bus and not white
+
+    # Preserve the normal 2 Hz cadence; the exception is the immediate OEM withdraw.
+    if self.frame % 50 == 0 or withdraw_now:
+      payload = hud_base if white else alert[1]
+      alert = (alert[0], mazdacan.apply_mads_white_hud(fsc_raw, payload, white), alert[2])
+      self.mads_white_hud_on_bus = mazdacan.is_mads_white_hud(alert[1])
+      return [alert]
+    return []
+
   def resume_requested(self, CC) -> bool:
     """The resume button belongs to the stock-longitudinal path alone. Under openpilot longitudinal
     the hold is released in-protocol (stop bits drop, RESUME_UNLATCHING pulses, the command ramps),