From 4113bf035dc3f97090871025e73bd8b89f41a51f Mon Sep 17 00:00:00 2001 From: MazdaNick Date: Sun, 20 Sep 2026 12:30:17 -0400 Subject: [PATCH 1/4] mazda: read invalidLkasSetting from the camera's intervention bits Issue: a settings-menu or camera state can leave the EPS taking our steer request and applying none of it, with no block or fault. Fix: the lane-assist-off state clears both CAM_SETTINGS intervention bits, LKAS_INERVENTION_ON1 and ILKAS_NTERVENTION_ON2, together; either bit alone is a live setting, and cameras assert them unevenly, so CarState raises invalidLkasSetting only when both are clear, replacing the LANE_LINES term. Latch that the camera sent CAM_SETTINGS once, so a car that never sends it cannot read as off, and the state holds through the message's silent cycles. Validation: mazda car suite 546 passed; corpus replay: a single-bit camera no longer reads as off, the lane-assist-off drive stays blocked. --- opendbc/car/mazda/carstate.py | 8 +++-- .../car/mazda/tests/test_mazda_carstate.py | 30 +++++++++++++++++++ 2 files changed, 35 insertions(+), 3 deletions(-) diff --git a/opendbc/car/mazda/carstate.py b/opendbc/car/mazda/carstate.py index ec515fc8f51..4d468e95c6a 100644 --- a/opendbc/car/mazda/carstate.py +++ b/opendbc/car/mazda/carstate.py @@ -83,6 +83,7 @@ def __init__(self, CP, CP_SP): self.cam_laneinfo_seen = False self.cam_laneinfo_silent_frames = 0 self.cam_empty_seen = False + self.cam_settings_seen = False self.radar_session_refused = False self.radar_session_response = 0 self.fsc_settled_frames = 0 @@ -305,9 +306,9 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]: ret.cruiseState.standstill = cp.vl["PEDALS"]["STANDSTILL"] == 1 and not self.CP.openpilotLongitudinalControl ret.cruiseState.speed = cp.vl["CRZ_EVENTS"]["CRZ_SPEED"] * CV.KPH_TO_MS - # Stock LKAS must be active. - # TODO: is this needed? - ret.invalidLkasSetting = cam_laneinfo_fresh and cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0 + self.cam_settings_seen |= len(cp_cam.vl_all["CAM_SETTINGS"]["LKAS_INERVENTION_ON1"]) > 0 + # Either bit set is an enabled setting + ret.invalidLkasSetting = self.cam_settings_seen and not any(cp_cam.vl["CAM_SETTINGS"][s] for s in ("LKAS_INERVENTION_ON1", "ILKAS_NTERVENTION_ON2")) if ret.cruiseState.enabled: if not self.lkas_allowed_speed and self.acc_active_last: @@ -387,6 +388,7 @@ def get_can_parsers(CP, CP_SP): cam_messages = [ # Read these optional camera messages without making them part of canValid. ("CAM_LANEINFO", float("nan")), + ("CAM_SETTINGS", float("nan")), ("CAM_TRAFFIC_SIGNS", float("nan")), ("CAM_EMPTY", float("nan")), ("CAM_PEDESTRIAN", float("nan")), diff --git a/opendbc/car/mazda/tests/test_mazda_carstate.py b/opendbc/car/mazda/tests/test_mazda_carstate.py index 0d13dfe641a..e015833c125 100644 --- a/opendbc/car/mazda/tests/test_mazda_carstate.py +++ b/opendbc/car/mazda/tests/test_mazda_carstate.py @@ -734,3 +734,33 @@ def test_rolling_or_leaving_standby_releases(self, release): kw.update(release) rig.step(100, 0, kw.pop('blocked'), **kw) assert not rig.CS.steer_first_engage_hold + + +def test_intervention_bits_become_an_invalid_lkas_setting(): + CI, pk = car_interface(alpha_long=False), packer() + healthy = pk.make_can_msg("CAM_SETTINGS", 2, {"LKAS_INERVENTION_ON1": 1, "ILKAS_NTERVENTION_ON2": 1}) + assert not feed(CI, 0, healthy)[0].invalidLkasSetting + off = pk.make_can_msg("CAM_SETTINGS", 2, {"LKAS_INERVENTION_ON1": 0, "ILKAS_NTERVENTION_ON2": 0}) + assert feed(CI, 1, off)[0].invalidLkasSetting + # the flag holds through the silent cycles between the message's arrivals + for i in range(2, 12): + assert feed(CI, i)[0].invalidLkasSetting + # either bit alone is a live setting: cameras assert them unevenly, so the + # single-bit states must not read as off + on1_only = pk.make_can_msg("CAM_SETTINGS", 2, {"LKAS_INERVENTION_ON1": 1, "ILKAS_NTERVENTION_ON2": 0}) + assert not feed(CI, 12, on1_only)[0].invalidLkasSetting + on2_only = pk.make_can_msg("CAM_SETTINGS", 2, {"LKAS_INERVENTION_ON1": 0, "ILKAS_NTERVENTION_ON2": 1}) + assert not feed(CI, 13, on2_only)[0].invalidLkasSetting + + +def test_cam_settings_absence_never_reads_as_off(): + # the parser's default zeros must not read as off on a car that never sends CAM_SETTINGS + CI = car_interface(alpha_long=False) + for i in range(10): + assert not feed(CI, i)[0].invalidLkasSetting + + +def test_lane_lines_zero_alone_is_not_an_invalid_lkas_setting(): + CI, pk = car_interface(alpha_long=False), packer() + lanes = pk.make_can_msg("CAM_LANEINFO", 2, {"LANE_LINES": 0}) + assert not feed(CI, 0, lanes)[0].invalidLkasSetting From 6fff61a6c5c524793f7a0f13eb6ca3345e8c0d71 Mon Sep 17 00:00:00 2001 From: MazdaNick Date: Thu, 17 Sep 2026 19:42:33 -0400 Subject: [PATCH 2/4] mazda: turn the dash LKAS button into the lateral toggle Issue: non-TJA trims expose no wheel button for lateral; the dash LKAS button is the only switch, and with MADS on its press read as a steering fault - the delivery watchdog sees the EPS stop applying torque the instant the button goes off - instead of the driver choice it is. Fix: read the button's edge from LANE_LINES, the state the dash button drives (0 lane keep off, 1-4 lane state): two agreeing frames make a 0 <-> nonzero edge one ButtonType.lkas press, the debounced off state publishes as lkaButtonOff for a lateral-only refusal, and the panda arms lateral on the on-press from the same frame, its HUD-frame liveness check riding the declaration. One lateral switch per car: a declared TJA button owns the press, the state still publishes. The delivery watchdog holds through a button-off window and alerts only while a request stands: the latch can outlive the request that earned it. Stock-long only: a SET press with LKA off is a stock ACC run openpilot never joined, so cancel no longer kills it. The TJA press hands a joined cruise back to the driver: on a TJA-declared car under stock longitudinal, the press that turns MADS lateral off leaves the driver's stock MRCC engaged underneath, and controlsd raises cruiseControl.cancel for exactly that state; the cancel spam disengaged an MRCC the press never asked about. Clear cruise_joined every cycle CRZ_BTNS carries the TJA bit, on the press cycle itself and before the MADS disable lands, so the standing engagement falls into the never-joined protection; joining the cruise again re-arms the sync cancel. Alpha-long still cancels, the engagement there being openpilot's own. Without the declaration carstate publishes tja_button as zero, so the clause is unreachable on other cars. The re-enabled button stops overselling control too: the EPS holds LKAS_BLOCK through standstill, a dash LKA-off, and a fixed ~3 s re-arm after a re-enable (3.03 to 3.06 s, nine of nine measured dash cycles) before it takes lateral. carstate publishes the bit debounced (0.2 s) as latBlocked on carState, so the UI can hold lateral as arming until the EPS actually grants it. The declaration rides every platform now: sp_flags=LKA_BUTTON on all eight, not the one body the feature was written on. The 2022-generation cameras (CX-5 2022, CX-9 2021-23, CX-8 2023) share the family the feature was tuned on. The 2016.5-era camera bodies (CX-5 KE and 2017-21, CX-9 2016-20, Mazda 3, Mazda 6) are declared on the shared decode alone: a 121 s KE firmware capture shows that camera streaming the HUD frame continuously at 1 Hz with LANE_LINES in one steady state, but no capture holds a button edge on the older family, so a drive log per body closes the press semantics. Declaring also carries the HUD-frame disappearance check to those bodies' drivers, MADS on or off. Suites on the rebuilt line: 620 car + 296 safety passed, 44 skipped, 220 subtests; ruff clean. Validation: mazda suites 562 car + 289 safety passed, 44 skipped, 215 subtests; ruff clean. Route 9ff65375 (2026-09-16): every toggle drove LANE_LINES to zero and back on marked road, unmarked streets held the count at one. Stage drive the same evening: boot-off silent arm, no-entry while off, ON edges engage lateral, and the standing-request gate silenced the four false warnings seen before it. The re-arm window was measured on the stage drive's dedicated capture, ten dash cycles. --- opendbc/car/car.capnp | 2 + opendbc/car/mazda/carcontroller.py | 12 +- opendbc/car/mazda/carstate.py | 38 +++- opendbc/car/mazda/tests/conftest.py | 3 +- opendbc/car/mazda/tests/test_mazda_buttons.py | 61 +++++- .../car/mazda/tests/test_mazda_carstate.py | 199 +++++++++++++++++- opendbc/car/mazda/values.py | 9 + opendbc/safety/modes/mazda.h | 47 ++++- opendbc/safety/tests/test_mazda.py | 175 +++++++++++++++ opendbc/sunnypilot/car/interfaces.py | 3 + opendbc/sunnypilot/car/mazda/values.py | 3 + 11 files changed, 532 insertions(+), 20 deletions(-) diff --git a/opendbc/car/car.capnp b/opendbc/car/car.capnp index 8d26c8eedb5..4270d4c76e0 100644 --- a/opendbc/car/car.capnp +++ b/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; diff --git a/opendbc/car/mazda/carcontroller.py b/opendbc/car/mazda/carcontroller.py index 054b9ddce48..254b374eb8f 100644 --- a/opendbc/car/mazda/carcontroller.py +++ b/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,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 diff --git a/opendbc/car/mazda/carstate.py b/opendbc/car/mazda/carstate.py index 4d468e95c6a..663650d51b9 100644 --- a/opendbc/car/mazda/carstate.py +++ b/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) # 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 @@ -84,6 +86,11 @@ def __init__(self, CP, CP_SP): self.cam_laneinfo_silent_frames = 0 self.cam_empty_seen = False self.cam_settings_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 @@ -104,6 +111,7 @@ def stock_radar_gone(self) -> bool: 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: + lane_keep_off = self.lane_lines_armed and not (self.lkas_on_stable and self.lkas_on_candidate) 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 @@ -112,7 +120,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: + # Dash LKA off also clears the latch. + if not lkas_blocked or lane_keep_off: self.steer_undelivered_frames = 0 self.steer_undelivered = False self.steer_undelivered_alert = False @@ -132,8 +141,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. 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): @@ -190,6 +201,10 @@ 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 ~3 s re-arm after a re-enable: + # the UI shows arming until the EPS grants lateral. + 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,6 +212,26 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]: # The panda refuses every LKA frame while it is not controlling, so a refused zero-torque # 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) + # LANE_LINES is the state of the dash LKA button + 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 publishes it too + ret.lkaButtonOff = self.lane_lines_armed and not self.lkas_on_stable + if self.CP.flags & MazdaFlags.STEER_TO_ZERO_EPS: self.update_steer_undelivered(ret.vEgoRaw, cp.vl["STEER_RATE"]["LKAS_REQUEST"]) @@ -372,6 +407,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) diff --git a/opendbc/car/mazda/tests/conftest.py b/opendbc/car/mazda/tests/conftest.py index 542143ec797..b47acfce90d 100644 --- a/opendbc/car/mazda/tests/conftest.py +++ b/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 diff --git a/opendbc/car/mazda/tests/test_mazda_buttons.py b/opendbc/car/mazda/tests/test_mazda_buttons.py index 5e8d1c16784..3a6cd0e2509 100644 --- a/opendbc/car/mazda/tests/test_mazda_buttons.py +++ b/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 diff --git a/opendbc/car/mazda/tests/test_mazda_carstate.py b/opendbc/car/mazda/tests/test_mazda_carstate.py index e015833c125..8811dbbb76d 100644 --- a/opendbc/car/mazda/tests/test_mazda_carstate.py +++ b/opendbc/car/mazda/tests/test_mazda_carstate.py @@ -14,7 +14,7 @@ from opendbc.car import structs from opendbc.car.common.conversions import Conversions as CV from opendbc.car.mazda import mazdacan -from opendbc.car.mazda.carstate import CAM_LANEINFO_FRESH_FRAMES, STOCK_CTS_ALERT_FRAMES +from opendbc.car.mazda.carstate import CAM_LANEINFO_FRESH_FRAMES, LAT_BLOCK_CONFIRM_FRAMES, STOCK_CTS_ALERT_FRAMES from opendbc.car.mazda.tests.conftest import car_interface, packer from opendbc.car.mazda.values import CarControllerParams from opendbc.sunnypilot.car.mazda.values import MazdaFlagsSP @@ -436,13 +436,15 @@ def __init__(self): self.packer = packer() self.frame = 0 - def step(self, request, effective, blocked, speed_kph=40., driver_torque=0, track_state=0, fault=0): + def step(self, request, effective, blocked, speed_kph=40., driver_torque=0, track_state=0, fault=0, lane_lines=None): self.frame += 1 - ret, _ = feed(self.CI, self.frame, - self.packer.make_can_msg("STEER_RATE", 0, {"LKAS_REQUEST": request, "LKAS_EFFECTIVE": effective, "LKAS_BLOCK": blocked, - "LKAS_TRACK_STATE": track_state, "LKAS_FAULT": fault}), - self.packer.make_can_msg("WHEEL_SPEEDS", 0, {"FL": speed_kph, "FR": speed_kph, "RL": speed_kph, "RR": speed_kph}), - self.packer.make_can_msg("STEER_TORQUE", 0, {"STEER_TORQUE_SENSOR": driver_torque})) + msgs = [self.packer.make_can_msg("STEER_RATE", 0, {"LKAS_REQUEST": request, "LKAS_EFFECTIVE": effective, "LKAS_BLOCK": blocked, + "LKAS_TRACK_STATE": track_state, "LKAS_FAULT": fault}), + self.packer.make_can_msg("WHEEL_SPEEDS", 0, {"FL": speed_kph, "FR": speed_kph, "RL": speed_kph, "RR": speed_kph}), + self.packer.make_can_msg("STEER_TORQUE", 0, {"STEER_TORQUE_SENSOR": driver_torque})] + if lane_lines is not None: + msgs.append(self.packer.make_can_msg("CAM_LANEINFO", 2, {"LANE_LINES": lane_lines})) + ret, _ = feed(self.CI, self.frame, *msgs) return ret @@ -460,6 +462,38 @@ def test_the_bit_is_decoded_but_the_camera_still_owns_the_fault(self): assert not ret.steerFaultPermanent # the camera's ERR_BIT_1 reports it, as before +class TestLatBlocked: + """LKAS_BLOCK published for the UI: the EPS holds it through standstill, a dash LKA-off, + and the fixed ~3 s re-arm after a re-enable, states where our request stands but no lateral + is applied. The border reads it to hold lateral as arming until the EPS takes the wheel.""" + + def test_block_latches_after_the_confirm_window(self): + rig = UndeliveredRig() + rig.step(0, 0, 1) # the first frame only arms the lazy parser + ret = None + for _ in range(LAT_BLOCK_CONFIRM_FRAMES - 1): + ret = rig.step(0, 0, 1) + assert not ret.latBlocked + ret = rig.step(0, 0, 1) + assert ret.latBlocked + + def test_a_lost_frame_never_latches(self): + rig = UndeliveredRig() + rig.step(0, 0, 1) + ret = None + for _ in range(LAT_BLOCK_CONFIRM_FRAMES * 3): + ret = rig.step(0, 0, 1 if rig.frame % 7 else 0) + assert not ret.latBlocked + + def test_an_unblocked_frame_clears_immediately(self): + rig = UndeliveredRig() + rig.step(0, 0, 1) + for _ in range(LAT_BLOCK_CONFIRM_FRAMES): + rig.step(0, 0, 1) + ret = rig.step(0, 0, 0) # the EPS grants lateral; the border turns cyan this frame + assert not ret.latBlocked + + class TestRejectionReport: """The panda puts every frame its tx hook refused back on the can stream with src = bus + 0xC0; carstate counts our refused torque requests so the controller can restart its ramp.""" @@ -526,14 +560,39 @@ def test_rejected_request_latches_then_alerts(self): assert not ret.steerFaultTemporary # Alert only after the additional configured hold time. for _ in range(rig.params.STEER_UNDELIVERED_ALERT_FRAMES - 2): - ret = rig.step(0, 0, 1) + ret = rig.step(600, 0, 1) assert not ret.steerFaultTemporary - ret = rig.step(0, 0, 1) + ret = rig.step(600, 0, 1) assert ret.steerFaultTemporary ret = rig.step(600, 600, 0) assert not rig.CS.steer_undelivered assert not ret.steerFaultTemporary + def test_lane_keep_off_zero_delivery_is_not_a_fault(self): + # The EPS stops delivery the instant the button goes off; the debounced state lands up to + # a second later, so the off evidence must suppress the latch across that gap. + rig = UndeliveredRig() + for _ in range(2): + rig.step(600, 600, 0, lane_lines=2) + for _ in range(rig.params.STEER_UNDELIVERED_FRAMES + rig.params.STEER_UNDELIVERED_ALERT_FRAMES + 50): + ret = rig.step(600, 0, 1, lane_lines=0) + assert not rig.CS.steer_undelivered + assert not ret.steerFaultTemporary + + def test_the_fault_still_alerts_once_lane_keep_returns(self): + rig = UndeliveredRig() + for _ in range(2): + rig.step(600, 600, 0, lane_lines=2) + for _ in range(50): + rig.step(600, 0, 1, lane_lines=0) + assert not rig.CS.steer_undelivered + # same-cycle off evidence starts the alert ladder one frame sooner than the old previous-cycle read + for _ in range(rig.params.STEER_UNDELIVERED_FRAMES + rig.params.STEER_UNDELIVERED_ALERT_FRAMES - 2): + ret = rig.step(600, 0, 1, lane_lines=2) + assert not ret.steerFaultTemporary + ret = rig.step(600, 0, 1, lane_lines=2) + assert ret.steerFaultTemporary + def test_small_or_delivered_requests_never_latch(self): rig = UndeliveredRig() for _ in range(200): @@ -558,8 +617,25 @@ def test_launch_block_stays_silent_however_fast_it_goes(self): ret = rig.step(600, 0, 1, speed_kph=25., track_state=1) assert rig.CS.steer_undelivered # the command is still zeroed, only the banner is withheld assert not ret.steerFaultTemporary - # Clearing standby while still blocked makes the condition alertable. + # Clearing standby while still blocked makes the condition alertable once a request stands. ret = rig.step(0, 0, 1, speed_kph=25., track_state=0) + assert not ret.steerFaultTemporary + ret = rig.step(600, 0, 1, speed_kph=25., track_state=0) + assert ret.steerFaultTemporary + + def test_the_alert_waits_for_a_standing_request(self): + # The latch can outlive the request that earned it: at a lane-keep re-arm the EPS clears + # standby a frame or two before the block, and a lockout after cruise-off holds the block + # with nobody asking. Four warning activations on one drive came from exactly that. + rig = UndeliveredRig() + for _ in range(rig.params.STEER_UNDELIVERED_FRAMES + rig.params.STEER_UNDELIVERED_ALERT_FRAMES + 10): + rig.step(600, 0, 1, track_state=1) + assert rig.CS.steer_undelivered + # Standby clears while blocked, but the request has stopped: no alert. + ret = rig.step(0, 0, 1, track_state=0) + assert not ret.steerFaultTemporary + # The request returns under the same block: now it alerts. + ret = rig.step(600, 0, 1, track_state=0) assert ret.steerFaultTemporary def test_driver_steering_with_the_request_still_latches(self): @@ -764,3 +840,106 @@ def test_lane_lines_zero_alone_is_not_an_invalid_lkas_setting(): CI, pk = car_interface(alpha_long=False), packer() lanes = pk.make_can_msg("CAM_LANEINFO", 2, {"LANE_LINES": 0}) assert not feed(CI, 0, lanes)[0].invalidLkasSetting + +class TestLkaButtonToggle: + """Each confirmed LANE_LINES 0 <-> nonzero edge is one ButtonType.lkas press, the same event the TJA button produces on TJA cars.""" + + ON = 2 + OFF = 0 + + @staticmethod + def _laneinfo(pk, lane_lines): + return pk.make_can_msg("CAM_LANEINFO", 2, {"LANE_LINES": lane_lines}) + + def _lkas(self, ret): + lkas = structs.CarState.ButtonEvent.Type.lkas + return [be.pressed for be in ret.buttonEvents if be.type == lkas] + + def _presses(self, ret): + return sum(self._lkas(ret)) + + def _feed(self, CI, pk, lane_lines, count=1, i0=0): + ret = None + for i in range(i0, i0 + count): + ret, _ = feed(CI, i, self._laneinfo(pk, lane_lines)) + return ret, i0 + count + + def _armed(self, CI, pk, lane_lines=ON): + # two agreeing frames arm the baseline without firing + ret, n = self._feed(CI, pk, lane_lines, count=2) + assert self._presses(ret) == 0 + return n + + def test_the_first_read_only_seeds_the_baseline(self): + # a boot with lane keep already off must not emit an enable press when the first frames land + for lane_lines in (self.ON, self.OFF): + CI, pk = car_interface(alpha_long=False), packer() + ret, _ = self._feed(CI, pk, lane_lines, count=3) + assert self._presses(ret) == 0, f"a drive starting at LANE_LINES {lane_lines} emitted a press" + + def test_each_edge_is_one_press(self): + CI, pk = car_interface(alpha_long=False), packer() + n = self._armed(CI, pk) + ret, n = self._feed(CI, pk, self.OFF, count=2, i0=n) + assert self._presses(ret) == 1 and ret.lkaButtonOff + ret, n = self._feed(CI, pk, self.OFF, count=3, i0=n) + assert self._presses(ret) == 0, "the held state kept pressing" + assert ret.lkaButtonOff + ret, n = self._feed(CI, pk, self.ON, count=2, i0=n) + assert self._presses(ret) == 1 and not ret.lkaButtonOff + + def test_the_press_is_released(self): + # a press with no release leaves the button held for the rest of the drive + CI, pk = car_interface(alpha_long=False), packer() + n = self._armed(CI, pk) + ret, n = self._feed(CI, pk, self.OFF, count=2, i0=n) + assert self._lkas(ret) == [True] + ret, _ = feed(CI, n) + assert self._lkas(ret) == [False], "the press must be released on the next cycle" + + def test_lane_state_changes_are_not_presses(self): + CI, pk = car_interface(alpha_long=False), packer() + n = self._armed(CI, pk) + for v in (3, 4, 1, 2): + ret, n = self._feed(CI, pk, v, count=2, i0=n) + assert self._presses(ret) == 0, f"LANE_LINES {v} fired a press" + assert not ret.lkaButtonOff + + def test_single_frame_glitch_is_ignored(self): + CI, pk = car_interface(alpha_long=False), packer() + n = self._armed(CI, pk) + _, n = self._feed(CI, pk, self.OFF, count=1, i0=n) + ret, _ = self._feed(CI, pk, self.ON, count=3, i0=n) + assert self._presses(ret) == 0 + + def test_dropout_does_not_fire_and_a_real_edge_still_does(self): + CI, pk = car_interface(alpha_long=False), packer() + n = self._armed(CI, pk) + for i in range(n, n + 300): # 3 s of silence, past any freshness window + CI.update([(t_ns(i), [])]) + ret, _ = self._feed(CI, pk, self.ON, count=2, i0=n + 300) + assert self._presses(ret) == 0, "the dropout itself fired a press" + ret, _ = self._feed(CI, pk, self.OFF, count=2, i0=n + 302) + assert self._presses(ret) == 1 + + def test_only_declared_platforms_read_the_toggle(self): + # every platform declares the toggle now; a cleared flag stands in for one that does not + CI, pk = car_interface(alpha_long=False), packer() + CI.CP_SP.flags &= ~MazdaFlagsSP.LKA_BUTTON + assert not CI.CP_SP.flags & MazdaFlagsSP.LKA_BUTTON + ret, n = self._feed(CI, pk, self.ON, count=2) + for lane_lines in (self.OFF, self.OFF, self.ON, self.ON): + ret, n = self._feed(CI, pk, lane_lines, count=1, i0=n) + assert self._presses(ret) == 0, "an undeclared platform emitted a press" + assert not ret.lkaButtonOff + + def test_a_declared_tja_button_owns_the_press_not_the_state(self): + # one lateral switch per car: the physical button owns the press, but the dash + # button's off state still names the lateral refusal instead of a steering fault + CI, pk = car_interface(alpha_long=False), packer() + CI.CP_SP.flags |= MazdaFlagsSP.TJA_BUTTON + ret, n = self._feed(CI, pk, self.ON, count=2) + for lane_lines in (self.OFF, self.OFF, self.ON, self.ON, self.OFF): + ret, n = self._feed(CI, pk, lane_lines, count=2, i0=n) + assert self._presses(ret) == 0, "LANE_LINES pressed the toggle" + assert ret.lkaButtonOff == (lane_lines == self.OFF) diff --git a/opendbc/car/mazda/values.py b/opendbc/car/mazda/values.py index 961b4ee70cd..d866b0149d3 100644 --- a/opendbc/car/mazda/values.py +++ b/opendbc/car/mazda/values.py @@ -8,6 +8,7 @@ from opendbc.car.docs_definitions import CarHarness, CarDocs, CarParts from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries from opendbc.car.vin import Vin, is_valid_vin +from opendbc.sunnypilot.car.mazda.values import MazdaFlagsSP Ecu = CarParams.Ecu @@ -208,11 +209,13 @@ class CAR(Platforms): # This radar does not publish 0x361-0x366 tracks on bus 0. dbc_dict={Bus.pt: 'mazda_2017'}, wmis={WMI.JAPAN_CROSSOVER}, chassis_codes={'KE'}, years={'C', 'D', 'E', 'F', 'G'}, # 2012-16 + sp_flags=MazdaFlagsSP.LKA_BUTTON, ) MAZDA_CX5 = MazdaPlatformConfig( [MazdaCarDocs("Mazda CX-5 2017-21")], MazdaCarSpecs(mass=3655 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=18.1), # steer ratio from the 2022 CX-5: same rack hardware wmis={WMI.JAPAN_CROSSOVER}, chassis_codes={'KF'}, years={'H', 'J', 'K', 'L', 'M'}, # 2017-21 + sp_flags=MazdaFlagsSP.LKA_BUTTON, ) MAZDA_CX9 = MazdaPlatformConfig( [MazdaCarDocs("Mazda CX-9 2016-20")], @@ -220,26 +223,31 @@ class CAR(Platforms): # This radar does not publish 0x361-0x366 tracks on bus 0. dbc_dict={Bus.pt: 'mazda_2017'}, wmis={WMI.JAPAN_CROSSOVER}, chassis_codes={'TC'}, years={'G', 'H', 'J', 'K', 'L'}, # 2016-20 + sp_flags=MazdaFlagsSP.LKA_BUTTON, ) MAZDA_3 = MazdaPlatformConfig( [MazdaCarDocs("Mazda 3 2017-18")], MazdaCarSpecs(mass=2875 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=14.0), wmis={WMI.JAPAN_PASSENGER, WMI.MEXICO_PASSENGER}, chassis_codes={'BN'}, years={'H', 'J'}, # 2017-18 + sp_flags=MazdaFlagsSP.LKA_BUTTON, ) MAZDA_6 = MazdaPlatformConfig( [MazdaCarDocs("Mazda 6 2017-20")], MazdaCarSpecs(mass=3443 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=15.5), wmis={WMI.JAPAN_PASSENGER}, chassis_codes={'GL'}, years={'H', 'J', 'K', 'L', 'M'}, # 2017-21 + sp_flags=MazdaFlagsSP.LKA_BUTTON, ) MAZDA_CX9_2021 = MazdaPlatformConfig( [MazdaCarDocs("Mazda CX-9 2021-23", video="https://youtu.be/dA3duO4a0O4")], MazdaCarSpecs(mass=4409 * CV.LB_TO_KG, wheelbase=2.93, steerRatio=17.6), wmis={WMI.JAPAN_CROSSOVER}, chassis_codes={'TC'}, years={'M', 'N', 'P'}, # 2021-23 + sp_flags=MazdaFlagsSP.LKA_BUTTON, ) MAZDA_CX5_2022 = MazdaPlatformConfig( [MazdaCarDocs("Mazda CX-5 2022-25")], MazdaCX5_2022CarSpecs(mass=3728 * CV.LB_TO_KG, wheelbase=2.698, steerRatio=18.1), # 15.5 is factory spec; 18.1 from paramsd learner (2.9M samples) wmis={WMI.JAPAN_CROSSOVER}, chassis_codes={'KF'}, years={'N', 'P', 'R', 'S'}, # 2022-25 + sp_flags=MazdaFlagsSP.LKA_BUTTON, ) MAZDA_CX8_2023 = MazdaPlatformConfig( [MazdaCarDocs("Mazda CX-8 2023")], @@ -247,6 +255,7 @@ class CAR(Platforms): # specs stand in until a learned set exists. Japan-market cars carry a chassis number, not a VIN, # and Australian JM0 VINs have no model-year field, so it fingerprints by firmware alone. MAZDA_CX9_2021.specs, + sp_flags=MazdaFlagsSP.LKA_BUTTON, ) diff --git a/opendbc/safety/modes/mazda.h b/opendbc/safety/modes/mazda.h index b8c861fcf58..5ecb394c1b1 100644 --- a/opendbc/safety/modes/mazda.h +++ b/opendbc/safety/modes/mazda.h @@ -13,6 +13,11 @@ #define MAZDA_TJA_BUTTON_BIT 11U // sunnypilot safety param: the TJA button is the MADS lateral switch #define MAZDA_PARAM_SP_TJA_BUTTON 1U +// sunnypilot safety param: the dash LKA button is the MADS lateral switch +#define MAZDA_PARAM_SP_LKA_BUTTON 2U +// Lane-keep line count, low 3 bits of byte 1 (Motorola bits 10..8): 0 = dash button off, 1-4 = lane state. +#define MAZDA_LANE_LINES_BYTE 1U +#define MAZDA_LANE_LINES_MASK 0x07U #define MAZDA_RADAR_STATIC 0x499U #define MAZDA_RADAR_TRACK_1 0x361U #define MAZDA_RADAR_TRACK_2 0x362U @@ -29,6 +34,8 @@ #define MAZDA_MAIN 0 #define MAZDA_CAM 2 +#define MAZDA_LKAS_HUD_RX_CHECK {.msg = {{MAZDA_LKAS_HUD, MAZDA_CAM, 8, 1U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true, .ignore_frequency_check = true}, { 0 }, { 0 }}} + #define MAZDA_PARAM_LONGITUDINAL 1U // Select the steer-to-zero EPS envelope from the firmware-derived interface flag. #define MAZDA_PARAM_STEER_TO_ZERO_EPS 2U @@ -49,6 +56,11 @@ static bool mazda_steer_to_zero_eps = false; static bool mazda_legacy_fw_eps = false; static uint32_t mazda_engage_btn_frames = 0U; static uint32_t mazda_cancel_context_frames = 0U; +// Declared by the platform. +static bool mazda_lka_button = false; +static bool mazda_lka_on = false; +static bool mazda_lka_candidate = false; +static bool mazda_lka_read = false; // Pin replaced-radar traffic to captured stock patterns where possible. @@ -202,6 +214,23 @@ static void mazda_rx_hook(const CANPacket_t *msg) { brake_pressed = brake; } } + + // LANE_LINES is the dash LKA button state (0 = off); only the on-press is reported. + // Two agreeing frames. + if (((int)msg->bus == MAZDA_CAM) && (msg->addr == MAZDA_LKAS_HUD) && mazda_lka_button) { + bool lka_on = (msg->data[MAZDA_LANE_LINES_BYTE] & MAZDA_LANE_LINES_MASK) != 0U; + if (lka_on != mazda_lka_candidate) { + mazda_lka_candidate = lka_on; + mads_button_press = MADS_BUTTON_NOT_PRESSED; + } else if (!mazda_lka_read) { + mazda_lka_read = true; + mazda_lka_on = lka_on; + mads_button_press = MADS_BUTTON_NOT_PRESSED; + } else { + mads_button_press = (lka_on && !mazda_lka_on) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED; + mazda_lka_on = lka_on; + } + } } static bool mazda_is_lka_addr(int addr) { @@ -376,6 +405,9 @@ static bool mazda_fwd_hook(int bus_num, int addr) { static safety_config mazda_init(uint16_t param) { mazda_engage_btn_frames = 0U; mazda_cancel_context_frames = 0U; + mazda_lka_on = false; + mazda_lka_candidate = false; + mazda_lka_read = false; static const CanMsg MAZDA_TX_MSGS[] = { {MAZDA_LKAS, 0, 8, .check_relay = true, .disable_static_blocking = true}, @@ -419,6 +451,8 @@ static safety_config mazda_init(uint16_t param) { {.msg = {{MAZDA_STEER_TORQUE, 0, 8, 83U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, {.msg = {{MAZDA_ENGINE_DATA, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, {.msg = {{MAZDA_PEDALS, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + // MUST STAY LAST (dropped by length when undeclared); no rate floor: 2 Hz road data, only disappearance faults (Toyota 0x365 opts out) + MAZDA_LKAS_HUD_RX_CHECK, }; // CRZ_CTRL intentionally disappears after radar teardown. @@ -427,16 +461,25 @@ static safety_config mazda_init(uint16_t param) { {.msg = {{MAZDA_STEER_TORQUE, 0, 8, 83U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, {.msg = {{MAZDA_ENGINE_DATA, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, {.msg = {{MAZDA_PEDALS, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + // MUST STAY LAST (dropped by length when undeclared); no rate floor: 2 Hz road data, only disappearance faults (Toyota 0x365 opts out) + MAZDA_LKAS_HUD_RX_CHECK, }; mazda_longitudinal = GET_FLAG(param, MAZDA_PARAM_LONGITUDINAL); mazda_steer_to_zero_eps = GET_FLAG(param, MAZDA_PARAM_STEER_TO_ZERO_EPS); mazda_legacy_fw_eps = GET_FLAG(param, MAZDA_PARAM_LEGACY_FW_EPS); mazda_tja_button = GET_FLAG(current_safety_param_sp, MAZDA_PARAM_SP_TJA_BUTTON); + // one lateral switch per car: the physical TJA button wins when both are declared + mazda_lka_button = GET_FLAG(current_safety_param_sp, MAZDA_PARAM_SP_LKA_BUTTON) && !mazda_tja_button; acc_main_on = false; - return mazda_longitudinal ? BUILD_SAFETY_CFG(mazda_long_rx_checks, MAZDA_LONG_TX_MSGS) : - BUILD_SAFETY_CFG(mazda_rx_checks, MAZDA_TX_MSGS); + safety_config cfg = mazda_longitudinal ? BUILD_SAFETY_CFG(mazda_long_rx_checks, MAZDA_LONG_TX_MSGS) : + BUILD_SAFETY_CFG(mazda_rx_checks, MAZDA_TX_MSGS); + // the lag check has no opt-out, so a camera that may not send the HUD frame must not carry it + if (!mazda_lka_button) { + cfg.rx_checks_len -= 1; + } + return cfg; } const safety_hooks mazda_hooks = { diff --git a/opendbc/safety/tests/test_mazda.py b/opendbc/safety/tests/test_mazda.py index 66fb4395b8c..76a8663ce8c 100755 --- a/opendbc/safety/tests/test_mazda.py +++ b/opendbc/safety/tests/test_mazda.py @@ -789,6 +789,181 @@ def test_declaration_is_read_at_init(self): self.assertTrue(self.safety.get_acc_main_on()) +class TestMazdaLkaButtonMads(unittest.TestCase): + """The dash LKA button as the MADS lateral switch: the panda reads LANE_LINES itself to re-arm lateral.""" + + def setUp(self): + self.packer = CANPackerSafety("mazda_2017") + self.safety = libsafety_py.libsafety + self._init(tja_button=False) + + def _init(self, tja_button=False, lka_button=True, param=0): + param_sp = 0 + if tja_button: + param_sp |= MazdaSafetyFlagsSP.TJA_BUTTON + if lka_button: + param_sp |= MazdaSafetyFlagsSP.LKA_BUTTON + self.safety.set_current_safety_param_sp(param_sp) + self.safety.set_safety_hooks(CarParams.SafetyModel.mazda, param) + self.safety.init_tests() + self.safety.set_mads_params(True, False, False) + self._t = 0.0 # init_tests zeroes the panda clock; _advance tracks it from there + + def tearDown(self): + self.safety.set_current_safety_param_sp(0) + self.safety.set_mads_params(False, False, False) + + def _laneinfo(self, lane_lines, bus=2): + return self.packer.make_can_msg_safety("CAM_LANEINFO", bus, {"LANE_LINES": lane_lines}) + + def _feed(self, lane_lines, count=1, bus=2): + for _ in range(count): + self.safety.safety_rx_hook(self._laneinfo(lane_lines, bus)) + + def test_the_first_read_only_seeds_the_baseline(self): + # a drive that starts with lane keep on must not request lateral before the driver asks + self._feed(2, count=4) + self.assertEqual(0, self.safety.get_mads_button_press()) # NOT_PRESSED + self.assertFalse(self.safety.get_controls_allowed_lateral()) + + def test_the_on_press_re_arms_a_dropped_lateral(self): + # the feature: after the heartbeat check drops lateral, the on-press must re-arm it + self._feed(2, count=2) + self.safety.set_controls_allowed_lateral(True) + self._feed(0, count=2) + self.assertEqual(0, self.safety.get_mads_button_press()) + self.safety.set_controls_allowed_lateral(False) # what the heartbeat check does + + self._feed(2, count=2) # debounced: the second agreeing frame is the press + self.assertEqual(1, self.safety.get_mads_button_press()) # PRESSED + self.assertTrue(self.safety.get_controls_allowed_lateral()) + + def test_the_press_is_released_on_the_next_frame(self): + self._feed(2, count=2) + self._feed(0, count=2) + self._feed(2, count=2) + self.assertEqual(1, self.safety.get_mads_button_press()) + self._feed(2) + self.assertEqual(0, self.safety.get_mads_button_press()) + + def test_a_single_glitched_frame_cannot_press(self): + # one zero frame between steady nonzero frames only moves the candidate, never presses + self._feed(2, count=2) + self.safety.set_controls_allowed_lateral(False) + self._feed(0) + self._feed(2) + self.assertEqual(0, self.safety.get_mads_button_press()) + self.assertFalse(self.safety.get_controls_allowed_lateral()) + + def test_a_lane_state_change_is_not_an_edge(self): + self._feed(2, count=2) + self.safety.set_controls_allowed_lateral(False) + self._feed(4, count=2) # still lane keep on: lane state, not the button + self.assertEqual(0, self.safety.get_mads_button_press()) + self._feed(1, count=2) + self.assertEqual(0, self.safety.get_mads_button_press()) + self._feed(0, count=2) + self._feed(1, count=2) + self.assertEqual(1, self.safety.get_mads_button_press()) + self.assertTrue(self.safety.get_controls_allowed_lateral()) + + def test_the_main_bus_is_ignored(self): + # only the camera reports the lane state + self._feed(2, count=2) + self.safety.set_controls_allowed_lateral(False) + self._feed(0, count=2, bus=0) + self._feed(2, count=2, bus=0) + self.assertFalse(self.safety.get_controls_allowed_lateral()) + + def test_an_undeclared_car_never_checks_the_hud_frame(self): + # undeclared: the frame never arrives, the clock runs past the 10 s window, and controls + # stay allowed because the entry is not in the config at all + self._init(lka_button=False) + for seconds in (1, 10, 20): + with self.subTest(after=self._t / 1e6 + seconds): + self._advance(seconds, laneinfo=False) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertTrue(self.safety.safety_config_valid()) + + def test_a_declared_car_does_require_the_hud_frame(self): + # the other half: declared, the frame is checked, so its absence is caught + self._init() + self._advance(11, laneinfo=False) + self.assertFalse(self.safety.get_controls_allowed()) + self.assertFalse(self.safety.safety_config_valid()) + + def test_an_undeclared_car_reads_no_toggle(self): + self._init(lka_button=False) + self._feed(2, count=2) + self._feed(0, count=2) + self._feed(2, count=2) + self.assertEqual(-1, self.safety.get_mads_button_press()) # UNAVAILABLE + self.assertFalse(self.safety.get_controls_allowed_lateral()) + + def test_both_declarations_at_once_leave_the_physical_button_in_charge(self): + # the interface passes one safety param, but if both arrive the param read resolves it: + # the dash read must be inert and its liveness check gone, not just one of the two + self._init(tja_button=True, lka_button=True) + self._feed(2, count=2) + self._feed(0, count=2) + self._feed(2, count=2) + self.assertEqual(-1, self.safety.get_mads_button_press()) # UNAVAILABLE: the dash read is inert + self.assertFalse(self.safety.get_controls_allowed_lateral()) + # and the HUD frame is not a liveness input for this car + self._advance(20, laneinfo=False) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertTrue(self.safety.safety_config_valid()) + + def test_the_toggle_works_under_openpilot_longitudinal(self): + self._init(param=MazdaSafetyFlags.LONG | MazdaSafetyFlags.STEER_TO_ZERO_EPS) + self._feed(2, count=2) + self._feed(0, count=2) + self.safety.set_controls_allowed_lateral(False) + self._feed(2, count=2) + self.assertTrue(self.safety.get_controls_allowed_lateral()) + + def _feed_every_rx_check(self, laneinfo=True): + # everything mazda_rx_checks requires, so a tick below is about the lag windows alone + msgs = [self.packer.make_can_msg_safety("CRZ_CTRL", 0, {}), + self.packer.make_can_msg_safety("CRZ_BTNS", 0, {}), + self.packer.make_can_msg_safety("STEER_TORQUE", 0, {}), + self.packer.make_can_msg_safety("ENGINE_DATA", 0, {}), + self.packer.make_can_msg_safety("PEDALS", 0, {})] + if laneinfo: + msgs.append(self._laneinfo(2)) + for msg in msgs: + self.safety.safety_rx_hook(msg) + + def _advance(self, seconds, laneinfo=True, step=0.5): + # run the clock forward re-feeding the rx-check messages, so only the withheld frame can age; + # controls are raised just before the tick so the assertion is about the tick alone. + end = self._t + (seconds * 1e6) + while self._t < end: + self._t = min(self._t + (step * 1e6), end) + self.safety.set_timer(int(self._t)) + self._feed_every_rx_check(laneinfo=laneinfo) + self.safety.set_controls_allowed(True) + self.safety.set_controls_allowed_lateral(True) + self.safety.safety_tick_current_safety_config() + + def test_a_healthy_tick_never_drops_controls(self): + # a declaration under 10 Hz without ignore_frequency_check is invalid on every tick, + # whatever the traffic; safety_config_valid() cannot see that flag, so assert controls + self._advance(0.1) # 100 ms: nothing can be lagging + self.assertTrue(self.safety.get_controls_allowed()) + self.assertTrue(self.safety.get_controls_allowed_lateral()) + self.assertTrue(self.safety.safety_config_valid()) + + def test_the_hud_frame_is_never_the_binding_lag_window(self): + # measured 2 Hz, worst period ~1 s: the 10 s window is ten times the 1 s floor the + # rest sit on, so a camera hiccup must not disengage. Five seconds quiet stays inside it. + self._advance(1) + self.assertTrue(self.safety.safety_config_valid()) + self.assertTrue(self.safety.get_controls_allowed()) + self._advance(5, laneinfo=False) + self.assertTrue(self.safety.get_controls_allowed()) + + class TestMazdaIgnition(unittest.TestCase): TX_MSGS: list = [] diff --git a/opendbc/sunnypilot/car/interfaces.py b/opendbc/sunnypilot/car/interfaces.py index 984b53289e5..651023f7c5e 100644 --- a/opendbc/sunnypilot/car/interfaces.py +++ b/opendbc/sunnypilot/car/interfaces.py @@ -299,6 +299,9 @@ def _initialize_mazda(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params_ if int(params_dict.get("MazdaTjaButton", 0)) == 1: CP_SP.flags |= MazdaFlagsSP.TJA_BUTTON.value CP_SP.safetyParam |= MazdaSafetyFlagsSP.TJA_BUTTON + elif CP_SP.flags & MazdaFlagsSP.LKA_BUTTON: + # A TJA declaration keeps the flag: the dash state still publishes. + CP_SP.safetyParam |= MazdaSafetyFlagsSP.LKA_BUTTON # A developer's declaration, off by default: the radar takeover may be requested while the # car is moving (a fresh session after a forced-offroad exit or a process restart). No # radar has a moving handover on record yet; when one does, this becomes a firmware rule. diff --git a/opendbc/sunnypilot/car/mazda/values.py b/opendbc/sunnypilot/car/mazda/values.py index 734bf17c6b4..8ea51984705 100644 --- a/opendbc/sunnypilot/car/mazda/values.py +++ b/opendbc/sunnypilot/car/mazda/values.py @@ -12,8 +12,11 @@ class MazdaSafetyFlagsSP: DEFAULT = 0 # The physical TJA button is the MADS lateral switch; MRCC no longer drives the main edge. TJA_BUTTON = 1 + LKA_BUTTON = 2 class MazdaFlagsSP(IntFlag): # Fitted to some trims only and not predicted by the fingerprint, so the driver declares it. TJA_BUTTON = 1 + # The dash LKA button is the MADS lateral switch. + LKA_BUTTON = 2 From 695bc1adcdc396ac523be9a6ce23d4fce45aa019 Mon Sep 17 00:00:00 2001 From: MazdaNick Date: Sun, 20 Sep 2026 15:49:06 -0400 Subject: [PATCH 3/4] mazda: undo the TJA-press MRCC arm Issue: the physical TJA button arms MRCC on bus 0 as a side effect of the press the driver declared as the lateral switch; the frame reaches bus-0 ECUs directly, so the arm cannot be prevented. Fix: when MRCC was off before the press, the controller answers the arm with the same MRCC master press the driver would use: one exact active-low CRZ_BTNS shape, one frame per body-paced 200 ms slot, at most three frames per unreconciled arm, never sent while PEDALS reads disarmed, stopped by raw PEDALS confirming the arm is gone. A second press before the first arm reconciled read that arm as the driver's baseline and cancelled the undo; a press edge inside a live episode now keeps the episode with its frame budget back, since the visible arm is the first press's artifact, and only a press with no live episode samples the armed state. The driver's own cruise presses, a held TJA press, and radar or stock-ECU transitions all stand down, and ICBM stands down through the whole TJA hold. A car that never arms through the press stands down after 1 s of brake-free time, since PEDALS cannot witness an arm under braking. Validation: 620 mazda car tests + 296 safety tests host-side; the double-press regression tests fail without the episode rule. --- opendbc/car/mazda/carcontroller.py | 108 ++++++++++- opendbc/car/mazda/carstate.py | 22 ++- opendbc/car/mazda/mazdacan.py | 13 ++ opendbc/car/mazda/tests/conftest.py | 15 +- .../car/mazda/tests/test_mazda_carstate.py | 33 +++- .../car/mazda/tests/test_mazda_tja_press.py | 183 +++++++++++++++++- opendbc/dbc/mazda_2017.dbc | 1 + opendbc/safety/modes/mazda.h | 23 ++- opendbc/safety/tests/test_mazda.py | 77 ++++++++ 9 files changed, 455 insertions(+), 20 deletions(-) diff --git a/opendbc/car/mazda/carcontroller.py b/opendbc/car/mazda/carcontroller.py index 254b374eb8f..218e92d8aa9 100644 --- a/opendbc/car/mazda/carcontroller.py +++ b/opendbc/car/mazda/carcontroller.py @@ -10,6 +10,7 @@ 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.stock_ecu import StockEcuState @@ -19,6 +20,13 @@ # 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) class CarController(CarControllerBase, IntelligentCruiseButtonManagementInterface): @@ -53,6 +61,13 @@ 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 def update(self, CC, CC_SP, CS, now_nanos): can_sends = [] @@ -130,6 +145,9 @@ 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: @@ -149,8 +167,10 @@ def update(self, CC, CC_SP, CS, now_nanos): 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)) @@ -204,6 +224,90 @@ 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 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), diff --git a/opendbc/car/mazda/carstate.py b/opendbc/car/mazda/carstate.py index 663650d51b9..635d4309b42 100644 --- a/opendbc/car/mazda/carstate.py +++ b/opendbc/car/mazda/carstate.py @@ -67,9 +67,13 @@ def __init__(self, CP, CP_SP): self.resume_button = 0 self.main_button = 0 self.tja_button = 0 + self.mrcc_button = 0 + self.crz_btns_seen = False self.cruise_available = False self.cruise_enabled = False + # Unfiltered PEDALS cruise state; the filtered public state bridges brake dropouts. + self.mrcc_armed_raw = False self.cruise_enabled_blocked = True self.brake_pressed_prev = False self.stock_radar_silent_frames = 0 @@ -261,11 +265,14 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]: ret.stockFcw = (self.cam_empty_seen and cam_empty["STATUS"] != 0x7F) or \ ped["PED_WARNING"] == 1 or ped["BRAKE_WARNING"] == 1 + acc_armed = cp.vl["PEDALS"]["ACC_OFF"] == 1 + acc_active = cp.vl["PEDALS"]["ACC_ACTIVE"] == 1 + # Unfiltered, both longitudinal modes: the TJA-press cleanup reads it. + self.mrcc_armed_raw = acc_armed or acc_active + if self.CP.openpilotLongitudinalControl: # After radar teardown, derive cruise state from PEDALS. Hold the previous state through # brake-only samples where both cruise bits are transiently low. - acc_armed = cp.vl["PEDALS"]["ACC_OFF"] == 1 - acc_active = cp.vl["PEDALS"]["ACC_ACTIVE"] == 1 brake_free = not ret.brakePressed and not self.brake_pressed_prev # Retain wheel-cancel context until PEDALS reflects the main-state change. if cp.vl["CRZ_BTNS"]["CAN_OFF"] == 1: @@ -387,6 +394,7 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]: prev_cancel_button = self.cancel_button prev_resume_button = self.resume_button prev_main_button = self.main_button + prev_mrcc_button = self.mrcc_button prev_tja_button = self.tja_button self.distance_button = cp.vl["CRZ_BTNS"]["DISTANCE_LESS"] # SET_P is the wheel's increase button; RES is a distinct resume button. @@ -396,6 +404,14 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]: self.cancel_button = cp.vl["CRZ_BTNS"]["CAN_OFF"] self.resume_button = cp.vl["CRZ_BTNS"]["RES"] self.main_button = int(cp.vl["CRZ_BTNS"]["MODE_X"] == 1 and cp.vl["CRZ_BTNS"]["MODE_Y"] == 1) + # BIT1 is active-low: a 0 on the bus-0 parser is the wheel's MRCC master press. Gated on + # the declaration (an undeclared wheel's idle level is unknown) and held unpressed until + # the wheel's first frame: parser zeros before it would decode as a phantom press. + if self.CP_SP.flags & MazdaFlagsSP.TJA_BUTTON: + self.crz_btns_seen = self.crz_btns_seen or len(cp.vl_all["CRZ_BTNS"]["BIT1"]) > 0 + self.mrcc_button = int(cp.vl["CRZ_BTNS"]["BIT1"] == 0) if self.crz_btns_seen else 0 + else: + self.mrcc_button = 0 # Only a car declared to have the physical TJA button reports it as the MADS switch. self.tja_button = int(cp.vl["CRZ_BTNS"]["TJA_BUTTON"] == 1) if self.CP_SP.flags & MazdaFlagsSP.TJA_BUTTON else 0 @@ -406,6 +422,8 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]: *create_button_events(self.cancel_button, prev_cancel_button, {1: ButtonType.cancel}), *create_button_events(self.resume_button, prev_resume_button, {1: ButtonType.resumeCruise}), *create_button_events(self.main_button, prev_main_button, {1: ButtonType.mainCruise}), + # A held press of this button must freeze ICBM through the cruise button timers in the openpilot tree. + *create_button_events(self.mrcc_button, prev_mrcc_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}), ] diff --git a/opendbc/car/mazda/mazdacan.py b/opendbc/car/mazda/mazdacan.py index 8f737f9bccf..b1822dbc30e 100644 --- a/opendbc/car/mazda/mazdacan.py +++ b/opendbc/car/mazda/mazdacan.py @@ -230,3 +230,16 @@ def create_button_cmd(packer, CP, counter, button, bus=0): } return packer.make_can_msg("CRZ_BTNS", bus, values) + + +def create_mrcc_off_cmd(packer, counter): + # The wheel's MRCC master press, active-low: every button bit 0 with its inversion 1, + # the master signature in BIT1/BIT1_INV plus BIT2/BIT3, counter plus one. Only the + # TJA-press cleanup sends it, and the panda pins the exact bytes in mazda_mrcc_off_msg_valid. + values = { + "CAN_OFF_INV": 1, "SET_P_INV": 1, "RES_INV": 1, "SET_M_INV": 1, + "DISTANCE_LESS_INV": 1, "DISTANCE_MORE_INV": 1, "MODE_X_INV": 1, "MODE_Y_INV": 1, + "BIT1_INV": 1, "BIT2": 1, "BIT3": 1, + "CTR": (counter + 1) % 16, + } + return packer.make_can_msg("CRZ_BTNS", 0, values) diff --git a/opendbc/car/mazda/tests/conftest.py b/opendbc/car/mazda/tests/conftest.py index b47acfce90d..d21104ec487 100644 --- a/opendbc/car/mazda/tests/conftest.py +++ b/opendbc/car/mazda/tests/conftest.py @@ -139,18 +139,24 @@ 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, tja_button=0, **out_kwargs) -> CarState: + cancel_button=0, accel_button=0, decel_button=0, + tja_button=0, mrcc_button=0, + mrcc_armed_raw=False, cruise_available=None, cruise_enabled=None, + radar_handback_active=False, + **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 gets the same semantics as a fresh state each frame. The stock-radar and FSC properties are derived from their frame counters, so those are what get seeded: a silent radar reads as gone (guard-long silence) unless a test is about the gap between the two windows. + The filtered cruise fields mirror cruiseState unless a test overrides them explicitly, + which is how a brake-dropout (raw low, filtered held) is staged. """ cs.out = out if out is not None else car_state(**out_kwargs) cs.brake_hold = brake_hold - cs.cruise_enabled = cs.out.cruiseState.enabled - cs.cruise_available = cs.out.cruiseState.available + cs.cruise_enabled = cs.out.cruiseState.enabled if cruise_enabled is None else cruise_enabled + cs.cruise_available = cs.out.cruiseState.available if cruise_available is None else cruise_available cs.stock_radar_seen = True cs.radar_bus_healthy = radar_bus_healthy cs.radar_session_response = radar_session_response @@ -178,6 +184,9 @@ def set_car_state(cs: CarState, out=None, *, brake_hold=False, stock_radar_alive cs.accel_button = accel_button cs.decel_button = decel_button cs.tja_button = tja_button + cs.mrcc_button = mrcc_button + cs.mrcc_armed_raw = mrcc_armed_raw + cs.radar_handback_active = radar_handback_active return cs diff --git a/opendbc/car/mazda/tests/test_mazda_carstate.py b/opendbc/car/mazda/tests/test_mazda_carstate.py index 8811dbbb76d..1590a83ed9f 100644 --- a/opendbc/car/mazda/tests/test_mazda_carstate.py +++ b/opendbc/car/mazda/tests/test_mazda_carstate.py @@ -14,7 +14,7 @@ from opendbc.car import structs from opendbc.car.common.conversions import Conversions as CV from opendbc.car.mazda import mazdacan -from opendbc.car.mazda.carstate import CAM_LANEINFO_FRESH_FRAMES, LAT_BLOCK_CONFIRM_FRAMES, STOCK_CTS_ALERT_FRAMES +from opendbc.car.mazda.carstate import ButtonType, CAM_LANEINFO_FRESH_FRAMES, LAT_BLOCK_CONFIRM_FRAMES, STOCK_CTS_ALERT_FRAMES from opendbc.car.mazda.tests.conftest import car_interface, packer from opendbc.car.mazda.values import CarControllerParams from opendbc.sunnypilot.car.mazda.values import MazdaFlagsSP @@ -716,15 +716,15 @@ def _declared(self): def test_tja_press_emits_an_lkas_event(self): CI, pk = self._declared(), packer() - self._btns(CI, pk, 0, TJA_BUTTON=0) - ret = self._btns(CI, pk, 1, TJA_BUTTON=1) + self._btns(CI, pk, 0, TJA_BUTTON=0, BIT1=1) + ret = self._btns(CI, pk, 1, TJA_BUTTON=1, BIT1=1) assert [be.type for be in ret.buttonEvents] == [self.ButtonType.lkas] assert ret.buttonEvents[0].pressed def test_no_event_without_the_button(self): CI, pk = self._declared(), packer() for i in range(10): - ret = self._btns(CI, pk, i, TJA_BUTTON=0) + ret = self._btns(CI, pk, i, TJA_BUTTON=0, BIT1=1) assert not [be for be in ret.buttonEvents if be.type == self.ButtonType.lkas] def test_undeclared_ignores_the_bit(self): @@ -943,3 +943,28 @@ def test_a_declared_tja_button_owns_the_press_not_the_state(self): ret, n = self._feed(CI, pk, lane_lines, count=2, i0=n) assert self._presses(ret) == 0, "LANE_LINES pressed the toggle" assert ret.lkaButtonOff == (lane_lines == self.OFF) + + +class TestMrccButtonEvent: + """The wheel's MRCC master press (CRZ_BTNS BIT1, active low) as a buttonEvent. + + Declared cars publish mainCruise like any master button; undeclared cars never read the + bit, so an unknown idle level there cannot produce events. + """ + + def test_press_publishes_maincruise_on_a_declared_car(self): + CI = car_interface() + CI.CP_SP.flags |= MazdaFlagsSP.TJA_BUTTON + pk = packer() + ret, _ = feed(CI, 0, pk.make_can_msg("CRZ_BTNS", 0, {"BIT1": 1})) + assert not ret.buttonEvents + ret, _ = feed(CI, 1, pk.make_can_msg("CRZ_BTNS", 0, {"BIT1": 0, "BIT1_INV": 1})) + assert [(be.type, be.pressed) for be in ret.buttonEvents] == [(ButtonType.mainCruise, True)] + + def test_press_is_invisible_on_an_undeclared_car(self): + CI = car_interface() + pk = packer() + ret, _ = feed(CI, 0, pk.make_can_msg("CRZ_BTNS", 0, {"BIT1": 1})) + ret, _ = feed(CI, 1, pk.make_can_msg("CRZ_BTNS", 0, {"BIT1": 0, "BIT1_INV": 1})) + assert not ret.buttonEvents + assert CI.CS.mrcc_button == 0 diff --git a/opendbc/car/mazda/tests/test_mazda_tja_press.py b/opendbc/car/mazda/tests/test_mazda_tja_press.py index d463e6426d6..222fbc3eab5 100644 --- a/opendbc/car/mazda/tests/test_mazda_tja_press.py +++ b/opendbc/car/mazda/tests/test_mazda_tja_press.py @@ -4,27 +4,43 @@ This file is part of zoompilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. -The camera press: whenever the camera's own TJA/CTS is armed (0x440 TJA nonzero), steering -or not, the controller presses the camera's button off on its own bus so the two lane-centering -systems never run at once and a MADS-off press cannot hand the wheel to the camera. One frame -per press, at least one 0x440 period between presses, three per arming episode, then one -stockLkas pulse if openpilot is steering; the episode resets when the camera reads 0. Not gated -on the TJA button declaration. +The TJA press, both sides of it. The camera press: whenever the camera's own TJA/CTS is +armed (0x440 TJA nonzero), steering or not, the controller presses the camera's button +off on its own bus so the two lane-centering systems never run at once and a MADS-off +press cannot hand the wheel to the camera. One frame per press, at least one 0x440 +period between presses, three per arming episode, then one stockLkas pulse if openpilot +is steering; the episode resets when the camera reads 0. Not gated on the TJA button +declaration. The MRCC undo: the same physical press arms MRCC on the car's bus, so on a +declared car with cruise off before the press the controller answers with the driver's +own MRCC master press until raw PEDALS confirms the arm is gone. Driver activity wins; +nothing sends while the button is held. """ +import pytest + from opendbc.car import DT_CTRL -from opendbc.car.mazda.tests.conftest import CRZ_BTNS, car_controller, frames, mazda_car_state, step +from opendbc.car.mazda.tests.conftest import CRZ_BTNS, SendButtonState, car_controller, frames, mazda_car_state, step from opendbc.car.mazda.values import CarControllerParams +from opendbc.sunnypilot.car.mazda.values import MazdaFlagsSP INTERVAL = int(CarControllerParams.TJA_PRESS_INTERVAL_T / DT_CTRL) PRESS = bytes.fromhex("0009ff") +HOLD_CYCLES = 15 # past the shared button pacing before the release -def rig(alpha_long=False): +def rig(alpha_long=False, tja_button=False): cc = car_controller(alpha_long=alpha_long) + cc.CP_SP.flags |= MazdaFlagsSP.TJA_BUTTON if tja_button else 0 cs = mazda_car_state(cc.CP, cc.CP_SP) return cc, cs +def drive(cc, cs, cycles, **kwargs): + sends = [] + for _ in range(cycles): + sends.extend(step(cc, cs, **kwargs)[1]) + return sends + + def presses(sends): # the one frame openpilot may put on the camera-side CRZ_BTNS out = frames(sends, CRZ_BTNS, bus=2) @@ -34,6 +50,20 @@ def presses(sends): return len(out) +def mrcc_off_frames(sends) -> list[bytes]: + # the wheel's MRCC master press on the car's bus: exact shape, CTR the only variable bits + return [d for d in frames(sends, CRZ_BTNS) + if d[:3] == b"\x00\x81\xfe" and d[3] & 0xc3 == 0xc0 and d[4:] == bytes(4)] + + +def undo_episode(alpha_long): + # cruise off before a TJA press-and-hold: the press-induced arm is live, undo pending + cc, cs = rig(alpha_long, tja_button=True) + drive(cc, cs, 5, available=False) + held = drive(cc, cs, HOLD_CYCLES, available=True, mrcc_armed_raw=True, tja_button=1) + return cc, cs, held + + class TestCameraPress: def test_one_press_on_the_first_steering_frame_with_the_camera_armed(self): @@ -101,3 +131,140 @@ def test_same_press_under_openpilot_longitudinal(self): cc, cs = rig(alpha_long=True) _, sends = step(cc, cs, lat_active=True, stock_tja=3, radar_was_silenced=True) assert presses(sends) == 1 + + +@pytest.mark.parametrize("alpha_long", [False, True]) +class TestMrccUndo: + + def test_off_before_press_undoes_the_arm(self, alpha_long): + cc, cs, held = undo_episode(alpha_long) + assert mrcc_off_frames(held) == [] # nothing while the driver holds the button + released = drive(cc, cs, 40, available=True, mrcc_armed_raw=True) + assert 0 < len(mrcc_off_frames(released)) <= 3 # the hold: at most the episode budget + settled = drive(cc, cs, 10, available=False) # arm reconciled + assert mrcc_off_frames(settled) == [] + assert not cc.mrcc_undo_pending + assert cc.mrcc_undo_frames == 0 # budget returned for the next press + + @pytest.mark.parametrize("pre_armed", [True, False]) + def test_the_drivers_own_arm_and_no_arm_are_left_alone(self, alpha_long, pre_armed): + # cruise already on before the press, or a press that never arms: no undo either way + cc, cs = rig(alpha_long, tja_button=True) + drive(cc, cs, 5, available=pre_armed, mrcc_armed_raw=pre_armed) + sends = drive(cc, cs, HOLD_CYCLES, available=pre_armed, mrcc_armed_raw=pre_armed, tja_button=1) + sends += drive(cc, cs, 40, available=pre_armed, mrcc_armed_raw=pre_armed) + assert mrcc_off_frames(sends) == [] + + def test_budget_caps_the_hold_and_returns_after_reconciliation(self, alpha_long): + cc, cs, _ = undo_episode(alpha_long) + stuck = drive(cc, cs, 80, available=True, mrcc_armed_raw=True) # the arm never clears + assert len(mrcc_off_frames(stuck)) == 3 + assert not cc.mrcc_undo_pending + drive(cc, cs, 10, available=False) # reconciled: the budget is back + drive(cc, cs, 5, available=False) + again = drive(cc, cs, HOLD_CYCLES, available=True, mrcc_armed_raw=True, tja_button=1) + again += drive(cc, cs, 40, available=True, mrcc_armed_raw=True) + assert mrcc_off_frames(again) != [] + + def test_a_second_press_before_reconciliation_keeps_the_episode(self, alpha_long): + # a fast double-press: the second edge lands while the first press's arm is still + # live, so the "armed before the press" sample is the first press's artifact; the + # episode must survive and keep undoing, or MRCC stays armed until the driver clears it + cc, cs, _ = undo_episode(alpha_long) + released = drive(cc, cs, 10, available=True, mrcc_armed_raw=True) # one press out, arm unreconciled + assert len(mrcc_off_frames(released)) == 1 + again = drive(cc, cs, HOLD_CYCLES, available=True, mrcc_armed_raw=True, tja_button=1) # second press + assert mrcc_off_frames(again) == [] # nothing while held + assert cc.mrcc_undo_pending # not cancelled by the second edge + after = drive(cc, cs, 40, available=True, mrcc_armed_raw=True) + assert 0 < len(mrcc_off_frames(after)) <= 3 # the budget returned and undoes the second arm + + def test_a_second_press_at_reconciliation_waits_for_its_own_arm(self, alpha_long): + # the first arm clears under the second press: PEDALS confirms the disarm inside the + # hold, and the second press's own arm lands only after the release; the episode must + # wait it out instead of standing down on the confirmed disarm + cc, cs, _ = undo_episode(alpha_long) + drive(cc, cs, 10, available=True, mrcc_armed_raw=True) + held = drive(cc, cs, HOLD_CYCLES, available=False, mrcc_armed_raw=False, tja_button=1) + assert mrcc_off_frames(held) == [] + assert cc.mrcc_undo_pending # waiting for this press's arm, not stood down + armed = drive(cc, cs, 40, available=True, mrcc_armed_raw=True) + assert 0 < len(mrcc_off_frames(armed)) <= 3 + + def test_brake_dropout_does_not_end_the_episode(self, alpha_long): + # both PEDALS cruise bits read low through a brake transition; the filtered state + # bridges it, so three raw-off cycles stay under the five-frame confirmation + cc, cs, _ = undo_episode(alpha_long) + drive(cc, cs, 3, available=True, mrcc_armed_raw=False) + assert cc.mrcc_undo_pending + + def test_arm_clearing_stands_down_without_a_second_press(self, alpha_long): + # the undo's own press disarms the car ~80 ms before PEDALS confirms it; the raw gate + # and the 200 ms slot must hold the second press back, or it would re-arm the car + cc, cs, _ = undo_episode(alpha_long) + pressing = drive(cc, cs, 10, available=True, mrcc_armed_raw=True) # one press out, arm still reflected + assert len(mrcc_off_frames(pressing)) == 1 + clearing = drive(cc, cs, 40, available=False, mrcc_armed_raw=False) # the arm clears mid-budget + assert mrcc_off_frames(clearing) == [] + assert not cc.mrcc_undo_pending # one press was enough: stood down without spending the budget + + def test_no_arm_wait_is_bounded_and_brake_held(self, alpha_long): + # a car that never arms through the press: the undo stands down after 1 s brake-free + # and ICBM gets the button stream back; a held brake restarts the clock, since PEDALS + # cannot witness an arm under braking + cc, cs = rig(alpha_long, tja_button=True) + drive(cc, cs, 5, available=False) + drive(cc, cs, HOLD_CYCLES, available=False, tja_button=1) + drive(cc, cs, 60, available=False) # brake-free wait, still inside 1 s + assert cc.mrcc_undo_pending + held = drive(cc, cs, 100, available=False, brake_pressed=True, send_button=SendButtonState.increase) + assert cc.mrcc_undo_pending # the brake restarts the clock + assert frames(held, CRZ_BTNS, bus=0) == [] # ICBM still suppressed while the undo owns the stream + resumed = drive(cc, cs, 110, available=False, send_button=SendButtonState.increase) + assert not cc.mrcc_undo_pending # 1 s brake-free elapsed + assert frames(resumed, CRZ_BTNS, bus=0) != [] # ICBM has the stream back + + @pytest.mark.parametrize("activity", [dict(mrcc_button=1), dict(cancel_button=1), dict(accel_button=1), + dict(handback=True)]) + def test_driver_activity_aborts(self, alpha_long, activity): + # the driver's own cruise presses and a stock ECU hand-back all stand down at once + cc, cs, _ = undo_episode(alpha_long) + drive(cc, cs, 30, available=True, mrcc_armed_raw=True, **activity) + assert not cc.mrcc_undo_pending + + def test_openpilot_cancel_waits_then_resumes(self, alpha_long): + cc, cs, _ = undo_episode(alpha_long) + waiting = drive(cc, cs, 20, available=True, mrcc_armed_raw=True, cancel=True) + assert mrcc_off_frames(waiting) == [] # no frame races openpilot's own cancel + assert cc.mrcc_undo_pending # waited out, not aborted + resumed = drive(cc, cs, 30, available=True, mrcc_armed_raw=True) + assert 0 < len(mrcc_off_frames(resumed)) <= 3 + + +class TestMrccUndoShipsDark: + + def test_radar_handback_aborts_with_the_budget_frozen(self): + # under alpha-long the session manager owns radar_handback_active and overwrites a + # seeded flag, so the abort runs on a stock-long controller + cc, cs, _ = undo_episode(alpha_long=False) + drive(cc, cs, 3, available=True, mrcc_armed_raw=True) # episode live, at most one frame out + spent = cc.mrcc_undo_frames + sends = drive(cc, cs, 30, available=True, mrcc_armed_raw=True, radar_handback_active=True) + assert not cc.mrcc_undo_pending + assert cc.mrcc_undo_frames == spent # aborted, not budget-exhausted + assert mrcc_off_frames(sends) == [] + + def test_undeclared_button_never_sends(self): + cc, cs = rig() + drive(cc, cs, 5, available=False) + drive(cc, cs, HOLD_CYCLES, available=True, mrcc_armed_raw=True, tja_button=1) + sends = drive(cc, cs, 60, available=True, mrcc_armed_raw=True) + assert mrcc_off_frames(sends) == [] + assert not cc.mrcc_undo_pending + + def test_icbm_suppressed_through_the_hold(self): + cc, cs = rig(tja_button=True) + held = drive(cc, cs, HOLD_CYCLES, available=True, mrcc_armed_raw=True, tja_button=1, + send_button=SendButtonState.increase) + # the wheel owns the counter stream during the hold: ICBM stays quiet too + assert frames(held, CRZ_BTNS, bus=0) == [] diff --git a/opendbc/dbc/mazda_2017.dbc b/opendbc/dbc/mazda_2017.dbc index 3784aa09178..d4c7a6f2778 100644 --- a/opendbc/dbc/mazda_2017.dbc +++ b/opendbc/dbc/mazda_2017.dbc @@ -416,6 +416,7 @@ BO_ 158 MSG_05: 8 XXX BO_ 157 CRZ_BTNS: 8 XXX SG_ TJA_BUTTON : 11|1@0+ (1,0) [0|1] "" XXX + SG_ BIT1_INV : 15|1@0+ (1,0) [0|1] "" XXX SG_ BIT1 : 16|1@0+ (1,0) [0|1] "" XXX SG_ BIT2 : 18|1@0+ (1,0) [0|1] "" XXX SG_ BIT3 : 20|1@0+ (1,0) [0|1] "" XXX diff --git a/opendbc/safety/modes/mazda.h b/opendbc/safety/modes/mazda.h index 5ecb394c1b1..dd53be483d0 100644 --- a/opendbc/safety/modes/mazda.h +++ b/opendbc/safety/modes/mazda.h @@ -54,6 +54,8 @@ static bool mazda_longitudinal = false; static bool mazda_tja_button = false; static bool mazda_steer_to_zero_eps = false; static bool mazda_legacy_fw_eps = false; +// Live cruise arming, from CRZ_CTRL under stock longitudinal and PEDALS after teardown. +static bool mazda_acc_armed = false; static uint32_t mazda_engage_btn_frames = 0U; static uint32_t mazda_cancel_context_frames = 0U; // Declared by the platform. @@ -62,6 +64,15 @@ static bool mazda_lka_on = false; static bool mazda_lka_candidate = false; static bool mazda_lka_read = false; +static bool mazda_mrcc_off_msg_valid(const CANPacket_t *msg) { + // Exact active-low MRCC master tap. CTR occupies the variable bits in byte 3; + // all other buttons and payload bits remain pinned. + return (GET_LEN(msg) == 8U) && (msg->data[0] == 0x00U) && + (msg->data[1] == 0x81U) && (msg->data[2] == 0xfeU) && + ((msg->data[3] & 0xc3U) == 0xc0U) && (msg->data[4] == 0x00U) && + (msg->data[5] == 0x00U) && (msg->data[6] == 0x00U) && (msg->data[7] == 0x00U); +} + // Pin replaced-radar traffic to captured stock patterns where possible. // Each radar generation sends its own static capture; the controller picks the dialect (mazdacan.py) @@ -140,6 +151,7 @@ static void mazda_rx_hook(const CANPacket_t *msg) { if ((msg->addr == MAZDA_CRZ_CTRL) && !mazda_longitudinal) { bool cruise_engaged = msg->data[0] & 0x8U; pcm_cruise_check(cruise_engaged); + mazda_acc_armed = GET_BIT(msg, 17U); // With the TJA button owning lateral, MRCC no longer drives the MADS main edge: its // falling edge would exit the panda's lateral while the software's MADS stays on. if (!mazda_tja_button) { @@ -180,6 +192,7 @@ static void mazda_rx_hook(const CANPacket_t *msg) { bool cruise_engaged = GET_BIT(msg, 3U); bool acc_armed = GET_BIT(msg, 2U) || cruise_engaged; bool brake_free = !brake && !brake_pressed_prev; + mazda_acc_armed = acc_armed; // Main mirrors carstate's cruise_available: it follows arming, and a both-low sample is // held under braking unless a wheel cancel explains it. Without the cancel path, main @@ -370,7 +383,14 @@ static bool mazda_tx_hook(const CANPacket_t *msg) { if (main_bus && (msg->addr == MAZDA_CRZ_BTNS)) { // Permit resume only while controlling and cancel only while not controlling. bool cancel_cmd = (msg->data[0] == 0x1U); - if (!controls_allowed && !cancel_cmd) { + const bool mrcc_off_candidate = !GET_BIT(msg, 16U) && GET_BIT(msg, 15U); + const bool mrcc_off_cmd = mazda_tja_button && mazda_acc_armed && mazda_mrcc_off_msg_valid(msg); + if (!controls_allowed && !cancel_cmd && !mrcc_off_cmd) { + tx = false; + } + // An MRCC-off-shaped frame is either the exact narrow exception or invalid; it cannot + // borrow the normal cancel authorization as a composite command. + if (mrcc_off_candidate && !mrcc_off_cmd) { tx = false; } // The TJA button is never pressed on the car's side: it would toggle MADS through the @@ -408,6 +428,7 @@ static safety_config mazda_init(uint16_t param) { mazda_lka_on = false; mazda_lka_candidate = false; mazda_lka_read = false; + mazda_acc_armed = false; static const CanMsg MAZDA_TX_MSGS[] = { {MAZDA_LKAS, 0, 8, .check_relay = true, .disable_static_blocking = true}, diff --git a/opendbc/safety/tests/test_mazda.py b/opendbc/safety/tests/test_mazda.py index 76a8663ce8c..259bd45f4ea 100755 --- a/opendbc/safety/tests/test_mazda.py +++ b/opendbc/safety/tests/test_mazda.py @@ -986,5 +986,82 @@ def test_ignition_off(self): self.assertFalse(self.safety.get_ignition_can()) +class TestMazdaMrccOffCleanup(unittest.TestCase): + """The exact-bytes MRCC master tap that undoes the arm a physical TJA press causes.""" + + # Nonzero signals only; unlisted DBC signals pack as 0 (see create_mrcc_off_cmd). + MRCC_OFF_VALUES = { + "CAN_OFF_INV": 1, "SET_P_INV": 1, "RES_INV": 1, "SET_M_INV": 1, + "DISTANCE_LESS_INV": 1, "DISTANCE_MORE_INV": 1, "MODE_X_INV": 1, "MODE_Y_INV": 1, + "BIT1_INV": 1, "BIT2": 1, "BIT3": 1, "CTR": 4, + } + + def setUp(self): + self.packer = CANPackerSafety("mazda_2017") + self.safety = libsafety_py.libsafety + self._init(tja_button=True) + + def _init(self, tja_button, param=0): + self.safety.set_current_safety_param_sp(MazdaSafetyFlagsSP.TJA_BUTTON if tja_button else 0) + self.safety.set_safety_hooks(CarParams.SafetyModel.mazda, param) + self.safety.init_tests() + self.safety.set_mads_params(True, False, False) + + def tearDown(self): + self.safety.set_current_safety_param_sp(0) + self.safety.set_mads_params(False, False, False) + + def _mrcc_off(self, **over): + values = dict(self.MRCC_OFF_VALUES) + values.update(over) + return self.packer.make_can_msg_safety("CRZ_BTNS", 0, values) + + def _crz_ctrl(self, armed): + return self.packer.make_can_msg_safety("CRZ_CTRL", 0, {"CRZ_AVAILABLE": armed}) + + def _pedals(self, acc_off): + return self.packer.make_can_msg_safety("PEDALS", 0, {"ACC_OFF": acc_off}) + + def test_exact_tap_allowed_declared_and_armed(self): + # not controlling: this is the whole point of the exception + self.safety.safety_rx_hook(self._crz_ctrl(True)) + self.assertTrue(self.safety.safety_tx_hook(self._mrcc_off())) + + def test_exact_tap_blocked_undeclared(self): + self._init(tja_button=False) + self.safety.safety_rx_hook(self._crz_ctrl(True)) + self.assertFalse(self.safety.safety_tx_hook(self._mrcc_off())) + + def test_exact_tap_blocked_when_cruise_not_armed(self): + self.safety.safety_rx_hook(self._crz_ctrl(False)) + self.assertFalse(self.safety.safety_tx_hook(self._mrcc_off())) + + def test_armed_tracks_pedals_after_teardown(self): + self._init(tja_button=True, param=MazdaSafetyFlags.LONG | MazdaSafetyFlags.STEER_TO_ZERO_EPS) + self.safety.safety_rx_hook(self._pedals(True)) + self.assertTrue(self.safety.safety_tx_hook(self._mrcc_off())) + self.safety.safety_rx_hook(self._pedals(False)) + self.assertFalse(self.safety.safety_tx_hook(self._mrcc_off())) + + def test_lookalike_tap_is_blocked_everywhere(self): + self.safety.safety_rx_hook(self._crz_ctrl(True)) + for over in ({"SET_P": 1}, {"RES": 1}, {"TJA_BUTTON": 1}, {"BIT2": 0}, {"CTR": 12}): + with self.subTest(over=over): + msg = self._mrcc_off(**over) + # only the CTR variation stays valid; anything else is a composite command + allowed = over == {"CTR": 12} + self.assertEqual(allowed, self.safety.safety_tx_hook(msg)) + + def test_cancel_still_allowed_while_not_controlling(self): + self.safety.safety_rx_hook(self._crz_ctrl(False)) + msg = self.packer.make_can_msg_safety("CRZ_BTNS", 0, {"CAN_OFF": 1}) + self.assertTrue(self.safety.safety_tx_hook(msg)) + + def test_tja_bit_still_blocked_on_the_main_bus(self): + self.safety.safety_rx_hook(self._crz_ctrl(True)) + msg = self.packer.make_can_msg_safety("CRZ_BTNS", 0, {"TJA_BUTTON": 1}) + self.assertFalse(self.safety.safety_tx_hook(msg)) + + if __name__ == "__main__": unittest.main() From 75f75bfcc2ef466d5e381a6d2da7cffa49e24432 Mon Sep 17 00:00:00 2001 From: MazdaNick Date: Fri, 18 Sep 2026 01:07:15 -0400 Subject: [PATCH 4/4] mazda: show the dash white wheel when MADS lateral is ours While the EPS takes our request and holds no lateral (the arming window after engagement, the re-arm hold after a dash LKA re-enable), the stock HUD shows nothing to explain the missing assistance. Draw the steering wheel glyph on the HUD while MADS lateral is ours, dimmed while arming and full once the EPS delivers, on declared cars only. Validation: 620 mazda car tests + 296 safety tests host-side, ruff clean. --- opendbc/car/mazda/carcontroller.py | 72 +++++++++-- opendbc/car/mazda/carstate.py | 10 +- opendbc/car/mazda/interface.py | 16 +++ opendbc/car/mazda/mazdacan.py | 61 +++++++++ opendbc/car/mazda/tests/conftest.py | 10 +- .../car/mazda/tests/test_mazda_interface.py | 28 ++++- .../tests/test_mazda_mads_white_wheel.py | 116 ++++++++++++++++++ .../car/mazda/tests/test_mazda_mazdacan.py | 22 ++++ 8 files changed, 322 insertions(+), 13 deletions(-) create mode 100644 opendbc/car/mazda/tests/test_mazda_mads_white_wheel.py diff --git a/opendbc/car/mazda/carcontroller.py b/opendbc/car/mazda/carcontroller.py index 218e92d8aa9..eae6a80925d 100644 --- a/opendbc/car/mazda/carcontroller.py +++ b/opendbc/car/mazda/carcontroller.py @@ -12,7 +12,7 @@ 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 @@ -27,6 +27,8 @@ 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): @@ -68,6 +70,9 @@ def __init__(self, dbc_names, CP, CP_SP): 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 = [] @@ -155,13 +160,7 @@ def update(self, CC, CC_SP, CS, now_nanos): 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, @@ -308,6 +307,63 @@ def update_mrcc_cleanup(self, CC, CC_SP, CS): 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), diff --git a/opendbc/car/mazda/carstate.py b/opendbc/car/mazda/carstate.py index 635d4309b42..7cad233a989 100644 --- a/opendbc/car/mazda/carstate.py +++ b/opendbc/car/mazda/carstate.py @@ -67,6 +67,7 @@ def __init__(self, CP, CP_SP): self.resume_button = 0 self.main_button = 0 self.tja_button = 0 + # Active-low wheel MRCC master (CRZ_BTNS.BIT1), read from the bus-0 parser: a 0 is a press. self.mrcc_button = 0 self.crz_btns_seen = False @@ -88,6 +89,9 @@ def __init__(self, CP, CP_SP): self.cancel_context_frames = 0 self.cam_laneinfo_seen = False self.cam_laneinfo_silent_frames = 0 + # The camera's last CAM_LANEINFO payload and its staleness, for the white-wheel HUD gate. + self.cam_laneinfo_raw: bytes | None = None + self.cam_laneinfo_stale_frames = CAM_LANEINFO_FRESH_FRAMES self.cam_empty_seen = False self.cam_settings_seen = False self.lkas_on_candidate = False @@ -109,6 +113,10 @@ def fsc_settled(self) -> bool: def stock_radar_alive(self) -> bool: return self.stock_radar_seen and self.stock_radar_silent_frames < STOCK_RADAR_ALIVE_FRAMES + @property + def cam_laneinfo_live(self) -> bool: + return self.cam_laneinfo_raw is not None and self.cam_laneinfo_stale_frames < CAM_LANEINFO_FRESH_FRAMES + @property def stock_radar_gone(self) -> bool: # This silence duration establishes radar ownership rather than a dropped frame. @@ -267,7 +275,7 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]: acc_armed = cp.vl["PEDALS"]["ACC_OFF"] == 1 acc_active = cp.vl["PEDALS"]["ACC_ACTIVE"] == 1 - # Unfiltered, both longitudinal modes: the TJA-press cleanup reads it. + # Both longitudinal modes: the TJA-press cleanup and the white-wheel HUD gate read it. self.mrcc_armed_raw = acc_armed or acc_active if self.CP.openpilotLongitudinalControl: diff --git a/opendbc/car/mazda/interface.py b/opendbc/car/mazda/interface.py index 72c99e9d952..a4ba4beaf48 100755 --- a/opendbc/car/mazda/interface.py +++ b/opendbc/car/mazda/interface.py @@ -93,3 +93,19 @@ def _get_params_sp(stock_cp: structs.CarParams, ret: structs.CarParamsSP, candid "hint": "the selected platform bundle does not match the VIN's platform"}) return ret + + def update(self, can_packets): + """Latch the camera's last CAM_LANEINFO payload and staleness for the white-wheel HUD gate.""" + # card sends [(t, frames), ...]; the model tests send one bare (t, frames) tuple. CANParser.update takes both. + if can_packets and not isinstance(can_packets[0], (list, tuple)): + can_packets = [can_packets] + raw, received = self.CS.cam_laneinfo_raw, False + for _t, frames in can_packets: + for msg in frames: + # tests pass CanData objects; card passes plain (addr, dat, src) tuples + addr, dat, src = (msg.address, msg.dat, msg.src) if hasattr(msg, "address") else msg + if addr == 0x440 and src == 2 and len(dat) == 8: + raw, received = bytes(dat), True + self.CS.cam_laneinfo_raw = raw + self.CS.cam_laneinfo_stale_frames = 0 if received else self.CS.cam_laneinfo_stale_frames + 1 + return super().update(can_packets) diff --git a/opendbc/car/mazda/mazdacan.py b/opendbc/car/mazda/mazdacan.py index b1822dbc30e..40301f40869 100644 --- a/opendbc/car/mazda/mazdacan.py +++ b/opendbc/car/mazda/mazdacan.py @@ -186,6 +186,67 @@ def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool) return packer.make_can_msg("CAM_LANEINFO", 0, values) +# The MADS white wheel: the dash draws it when the camera's own HUD frame carries TJA=2, +# and the body reads the same frame. The white bit is only ever XORed into an exact +# camera payload audited to be an idle frame, never a frame we composed. Every observed +# FSC idle family on TJA-declared cars is enumerated: the OFF family with its +# counter-nibble twins, the LINE_VISIBLE families and their high-beam variants, and the +# partial-lane LANE_LINES=3/4 encodings. Exact bases only; do not widen to a field-based +# rule until more captures are audited. +MADS_HUD_SAFE_BASE_PAYLOADS = frozenset(bytes.fromhex(h) for h in ( + "4201000000001040", "4201000000001060", "4221000000004040", "4221000000001040", + "4221000000001060", "4201000000004040", "0221000000000040", "4201000000000040", + "4221000000000040", "0221000000001040", "4361000000000040", "4102000000001040", + "4122000000001040", "4361000000000060", "4102000000004040", "4122000000004040", + "4221000000004060", "4122000000000040", "4103000000001040", "4104000000001040", + "4123000000000040", "4124000000000040", "4123000000001040", "4124000000001040", + "4123000000004040", "4124000000004040", "4102000000001060", "4102000000004060", + "4122000000001060", "4122000000004060", "0122000000000040", "0122000000004040", + "4202000000001040", "4102000000000040", +)) +# OFF to WHITE is TJA 0 to 2 only: one bit, byte 4 0x20, XORed in, never a frame swap. +MADS_HUD_WHITE_TJA_XOR = bytes.fromhex("0000000020000000") +# 64-bit big-endian keep-mask over the bits an idle camera may still move between +# samples: TJA (byte 4, 0x70), TJA_TRANSITION (byte 3, 0x0C), and the unnamed byte-3 +# transition bits 0x03. Do not clear byte-4 0x80 or unrelated byte-0 family bits. +# Every allowlisted base carries zero in the masked bits, so no two bases share a key. +CAM_LANEINFO_TJA_NORMALIZE_MASK = 0xFFFFFFF08FFFFFFF +_MADS_HUD_SAFE_BASE_BY_INT = { + int.from_bytes(b, "big") & CAM_LANEINFO_TJA_NORMALIZE_MASK: b + for b in MADS_HUD_SAFE_BASE_PAYLOADS +} + + +def white_hud_allowlist_base(fsc_raw: bytes | None) -> bytes | None: + """The allowlisted idle base for the camera's current frame, TJA/transition bits ignored.""" + if fsc_raw is None or len(fsc_raw) != 8: + return None + return _MADS_HUD_SAFE_BASE_BY_INT.get( + int.from_bytes(fsc_raw, "big") & CAM_LANEINFO_TJA_NORMALIZE_MASK + ) + + +def apply_mads_white_hud(fsc_raw: bytes | None, packed_dat: bytes, enabled: bool) -> bytes: + """Set TJA=2 on the camera's own allowlisted idle frame, and on nothing else. + + packed_dat must be exactly the base the camera's current frame normalizes to: any other + payload, or an unknown camera frame, passes through untouched. + """ + if not enabled or len(packed_dat) != 8: + return packed_dat + if packed_dat != white_hud_allowlist_base(fsc_raw): + return packed_dat + return bytes(a ^ b for a, b in zip(packed_dat, MADS_HUD_WHITE_TJA_XOR, strict=True)) + + +def is_mads_white_hud(dat: bytes) -> bool: + """True when dat is an allowlisted base with only the WHITE TJA bit set.""" + if len(dat) != 8: + return False + base = bytes(a ^ b for a, b in zip(dat, MADS_HUD_WHITE_TJA_XOR, strict=True)) + return base in MADS_HUD_SAFE_BASE_PAYLOADS and dat != base + + def create_button_cmd(packer, CP, counter, button, bus=0): can = int(button == Buttons.CANCEL) res = int(button == Buttons.RESUME) diff --git a/opendbc/car/mazda/tests/conftest.py b/opendbc/car/mazda/tests/conftest.py index d21104ec487..1ba36521d90 100644 --- a/opendbc/car/mazda/tests/conftest.py +++ b/opendbc/car/mazda/tests/conftest.py @@ -16,7 +16,7 @@ from opendbc.can import CANPacker, CANParser from opendbc.car import Bus, gen_empty_fingerprint, structs from opendbc.car.mazda.carcontroller import CarController -from opendbc.car.mazda.carstate import CarState, FSC_SETTLE_FRAMES, STOCK_RADAR_ALIVE_FRAMES, STOCK_RADAR_GUARD_FRAMES +from opendbc.car.mazda.carstate import CarState, CAM_LANEINFO_FRESH_FRAMES, FSC_SETTLE_FRAMES, STOCK_RADAR_ALIVE_FRAMES, STOCK_RADAR_GUARD_FRAMES from opendbc.car.mazda.interface import CarInterface from opendbc.car.mazda.values import CAR, CarControllerParams @@ -123,12 +123,14 @@ def car_control(enabled=None, long_active=True, lat_active=False, accel=0.5, tor return cc.as_reader() -def car_control_sp(handback=False, lead_d_rel=12.0, lead_v_rel=0.0, send_button=SendButtonState.none) -> structs.CarControlSP: +def car_control_sp(handback=False, lead_d_rel=12.0, lead_v_rel=0.0, send_button=SendButtonState.none, + mads_active=False) -> structs.CarControlSP: cc_sp = structs.CarControlSP() cc_sp.stockEcuHandBack = handback cc_sp.leadOne.dRel = lead_d_rel cc_sp.leadOne.vRel = lead_v_rel cc_sp.intelligentCruiseButtonManagement.sendButton = send_button + cc_sp.mads.active = mads_active return cc_sp @@ -142,7 +144,7 @@ def set_car_state(cs: CarState, out=None, *, brake_hold=False, stock_radar_alive cancel_button=0, accel_button=0, decel_button=0, tja_button=0, mrcc_button=0, mrcc_armed_raw=False, cruise_available=None, cruise_enabled=None, - radar_handback_active=False, + radar_handback_active=False, cam_laneinfo_raw=None, cam_laneinfo_live=False, **out_kwargs) -> CarState: """Put the controller-facing state of a real CarState where a test wants it. @@ -187,6 +189,8 @@ def set_car_state(cs: CarState, out=None, *, brake_hold=False, stock_radar_alive cs.mrcc_button = mrcc_button cs.mrcc_armed_raw = mrcc_armed_raw cs.radar_handback_active = radar_handback_active + cs.cam_laneinfo_raw = cam_laneinfo_raw + cs.cam_laneinfo_stale_frames = 0 if cam_laneinfo_live else CAM_LANEINFO_FRESH_FRAMES return cs diff --git a/opendbc/car/mazda/tests/test_mazda_interface.py b/opendbc/car/mazda/tests/test_mazda_interface.py index 9d8cc5d24a5..3f3ceb3b158 100644 --- a/opendbc/car/mazda/tests/test_mazda_interface.py +++ b/opendbc/car/mazda/tests/test_mazda_interface.py @@ -10,11 +10,12 @@ import pytest from opendbc.car import Bus, structs +from opendbc.car.can_definitions import CanData from opendbc.car.common.conversions import Conversions as CV from opendbc.car.mazda.carcontroller import CarController from opendbc.car.mazda.fingerprints import FW_VERSIONS from opendbc.car.mazda.interface import CarInterface -from opendbc.car.mazda.tests.conftest import DBC_NAME, car_params, car_params_sp +from opendbc.car.mazda.tests.conftest import CAM_LKAS, CAM_LANEINFO, DBC_NAME, car_interface, car_params, car_params_sp from opendbc.car.mazda.values import CAR, DBC, G46L_RADAR_FW, LKAS_LIMITS, STEER_TO_ZERO_EPS_FW, STEER_TO_ZERO_PLATFORMS, MazdaFlags, MazdaSafetyFlags Ecu = structs.CarParams.Ecu @@ -336,3 +337,28 @@ def test_param_sets_the_flag_under_alpha_long_only(self): CP_SP = car_params_sp(CP, CAR.MAZDA_CX5_2022, alpha_long=alpha_long) setup_interfaces(CarInterface, CP, CP_SP, [{"MazdaMovingTakeover": declared}]) assert bool(CP.flags & MazdaFlags.MOVING_TAKEOVER) == expect + + +class TestCamLaneinfoLatch: + """CI.update() latches the camera's raw CAM_LANEINFO frame, so it must accept every shape callers pass.""" + + LANEINFO_IDLE = bytes.fromhex("4201000000001040") + + def test_bare_tuple_from_the_model_tests(self): + ci = car_interface(alpha_long=False) + ci.update((0, [CanData(CAM_LANEINFO, self.LANEINFO_IDLE, 2)])) + assert ci.CS.cam_laneinfo_raw == self.LANEINFO_IDLE + assert ci.CS.cam_laneinfo_stale_frames == 0 + + def test_tuple_list_from_card(self): + ci = car_interface(alpha_long=False) + ci.update([(0, [(CAM_LANEINFO, self.LANEINFO_IDLE, 2)])]) + assert ci.CS.cam_laneinfo_raw == self.LANEINFO_IDLE + assert ci.CS.cam_laneinfo_stale_frames == 0 + + def test_silent_cycle_keeps_the_last_payload_and_grows_stale(self): + ci = car_interface(alpha_long=False) + ci.update([(0, [(CAM_LANEINFO, self.LANEINFO_IDLE, 2)])]) + ci.update([(1, [(CAM_LKAS, b"\x00" * 8, 2)])]) + assert ci.CS.cam_laneinfo_raw == self.LANEINFO_IDLE + assert ci.CS.cam_laneinfo_stale_frames == 1 diff --git a/opendbc/car/mazda/tests/test_mazda_mads_white_wheel.py b/opendbc/car/mazda/tests/test_mazda_mads_white_wheel.py new file mode 100644 index 00000000000..0e4fce255e4 --- /dev/null +++ b/opendbc/car/mazda/tests/test_mazda_mads_white_wheel.py @@ -0,0 +1,116 @@ +""" +Copyright (c) 2026-, Zeph Leggett. + +This file is part of zoompilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. + +The MADS white wheel: with lateral active on a TJA-declared car and cruise completely +off, the TJA=2 state is XORed into the camera's own allowlisted idle frame. Under test: +an undeclared car is byte-identical to the base behavior, every fail-closed deny holds, +and an unsafe state already on the bus is withdrawn immediately, outside the 2 Hz cadence. +""" + +import pytest + +from opendbc.car.mazda import mazdacan +from opendbc.sunnypilot.car.mazda.values import MazdaFlagsSP + +from opendbc.car.mazda.tests.conftest import (CAM_LANEINFO, SendButtonState, VisualAlert, car_controller, + mazda_car_state, packer, step) + +BASE = bytes.fromhex("4201000000001040") # the canonical OFF-family idle base +CADENCE = 50 # the normal 2 Hz alert slot + + +def hud_frames(sends) -> list[bytes]: + return [d for a, d, b in sends if a == CAM_LANEINFO and b == 0] + + +def tja_controller(alpha_long=True): + cc = car_controller(alpha_long=alpha_long) + cc.CP_SP.flags |= MazdaFlagsSP.TJA_BUTTON + cs = mazda_car_state(cc.CP, cc.CP_SP) + return cc, cs + + +def drive(cc, cs, cycles, **kwargs): + hud, sends = [], [] + for _ in range(cycles): + _, out = step(cc, cs, **kwargs) + hud.extend((cc.frame - 1, d) for d in hud_frames(out)) + sends.extend(out) + return hud, sends + + +class TestShipsDark: + def test_undeclared_car_is_byte_identical_and_on_cadence(self): + cc, cs = tja_controller() + cc.CP_SP.flags &= ~MazdaFlagsSP.TJA_BUTTON + hud, _ = drive(cc, cs, 2 * CADENCE + 5, mads_active=True, cam_laneinfo_raw=BASE, + cam_laneinfo_live=True, available=False, mrcc_armed_raw=False) + assert hud, "the alert cadence itself must not change" + assert all(frame % CADENCE == 0 for frame, _ in hud) + assert not any(mazdacan.is_mads_white_hud(d) for _, d in hud) + expected = mazdacan.create_alert_command(packer(), cs.cam_laneinfo, False, False)[1] + assert all(d == expected for _, d in hud) + + +class TestWhiteWheelGate: + def kwargs(self, **over): + kw = dict(mads_active=True, cam_laneinfo_raw=BASE, cam_laneinfo_live=True, + available=False, mrcc_armed_raw=False) + kw.update(over) + return kw + + def test_white_wheel_displays_on_the_allowlisted_base(self): + cc, cs = tja_controller() + hud, _ = drive(cc, cs, 2 * CADENCE + 5, **self.kwargs()) + whites = [d for _, d in hud if mazdacan.is_mads_white_hud(d)] + assert whites, "the white wheel must display once confirmed" + assert all(d == bytes.fromhex("4201000020001040") for d in whites) + assert cc.mads_white_hud_on_bus + + def test_confirmation_delay_before_display(self): + cc, cs = tja_controller() + hud, _ = drive(cc, cs, CADENCE, **self.kwargs()) + # the frame-0 slot is inside the 0.5 s confirmation window: base frame, no white + assert hud and not any(mazdacan.is_mads_white_hud(d) for _, d in hud) + + @pytest.mark.parametrize("deny,over", [ + ("MADS off", dict(mads_active=False)), + ("cruise armed", dict(mrcc_armed_raw=True)), + ("cruise available", dict(available=True)), + ("stock ECU hand-back", dict(handback=True)), + ("stale camera frame", dict(cam_laneinfo_live=False)), + ("unknown camera payload", dict(cam_laneinfo_raw=b"\xff" * 8)), + ("no camera payload", dict(cam_laneinfo_raw=None)), + ("wheel button held", dict(tja_button=1)), + ("MRCC button held", dict(mrcc_button=1)), + ("ICBM set button", dict(send_button=SendButtonState.increase)), + ("visual alert", dict(visual_alert=VisualAlert.steerRequired)), + ]) + def test_every_deny_blocks_the_white_state(self, deny, over): + cc, cs = tja_controller() + hud, _ = drive(cc, cs, 2 * CADENCE + 5, **self.kwargs(**over)) + assert not any(mazdacan.is_mads_white_hud(d) for _, d in hud), deny + + def test_radar_handback_blocks_the_white_state(self): + # under alpha-long the session manager owns radar_handback_active and overwrites a + # seeded flag, so the deny is exercised on a stock-long controller + cc, cs = tja_controller(alpha_long=False) + hud, _ = drive(cc, cs, 2 * CADENCE + 5, **self.kwargs(radar_handback_active=True)) + assert not any(mazdacan.is_mads_white_hud(d) for _, d in hud) + + def test_unsafe_state_is_withdrawn_immediately(self): + cc, cs = tja_controller() + hud, _ = drive(cc, cs, 2 * CADENCE + 5, **self.kwargs()) + assert cc.mads_white_hud_on_bus + + # cruise arms mid-display: the white bit leaves the bus on the very next cycle, + # not at the next 2 Hz slot + withdrawn, _ = drive(cc, cs, 3, **self.kwargs(mrcc_armed_raw=True)) + frames_out = [f for f, d in withdrawn] + assert frames_out, "the withdraw frame must go out at once" + assert all(f % CADENCE != 0 for f in frames_out) + assert all(not mazdacan.is_mads_white_hud(d) for _, d in withdrawn) + assert not cc.mads_white_hud_on_bus diff --git a/opendbc/car/mazda/tests/test_mazda_mazdacan.py b/opendbc/car/mazda/tests/test_mazda_mazdacan.py index 514d1777b89..fe3600d91be 100644 --- a/opendbc/car/mazda/tests/test_mazda_mazdacan.py +++ b/opendbc/car/mazda/tests/test_mazda_mazdacan.py @@ -36,6 +36,28 @@ def test_alert_command_relays_state_but_not_the_tja_churn(packer): assert out["TJA"] == 0 and out["TJA_TRANSITION"] == 0 +WHITE_HUD_BASE = bytes.fromhex("4201000000001040") # the canonical OFF-family idle base + + +def test_white_hud_allowlist_maps_tja_states_to_their_idle_base(): + # a frame that already carries a TJA/transition state maps back to its idle base + assert mazdacan.white_hud_allowlist_base(bytes.fromhex("4201000020001040")) == WHITE_HUD_BASE + # the counter-nibble twins are separately audited entries, not normalized away + assert mazdacan.white_hud_allowlist_base(bytes.fromhex("4201000000001060")) == bytes.fromhex("4201000000001060") + + +def test_apply_mads_white_hud_only_touches_an_allowlisted_base(): + assert mazdacan.apply_mads_white_hud(WHITE_HUD_BASE, WHITE_HUD_BASE, True) == bytes.fromhex("4201000020001040") + assert mazdacan.apply_mads_white_hud(b"\xff" * 8, b"\xff" * 8, True) == b"\xff" * 8 + assert mazdacan.apply_mads_white_hud(WHITE_HUD_BASE, WHITE_HUD_BASE, False) == WHITE_HUD_BASE + + +def test_is_mads_white_hud_requires_the_exact_xor(): + assert mazdacan.is_mads_white_hud(bytes.fromhex("4201000020001040")) + assert not mazdacan.is_mads_white_hud(WHITE_HUD_BASE) + assert not mazdacan.is_mads_white_hud(bytes.fromhex("4201000030001040")) + + @pytest.mark.parametrize("counter", range(16)) def test_camera_tja_press_bytes(packer, counter): # the wheel's idle pattern with the TJA bit, counter plus one, on the camera bus only