diff --git a/opendbc/car/mazda/carcontroller.py b/opendbc/car/mazda/carcontroller.py index 054b9ddce48..635e0bf91ea 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,16 @@ # 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), +# and the undo must also stand down before its next slot: the arm clears in PEDALS ~80 ms after a +# press plus the 50 ms confirm, so 200 ms clears both constraints with margin. +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); +# 1 s is 12x that. PEDALS cannot witness an arm under braking, so the wait counts brake-free +# cycles only. +TJA_MRCC_ARM_WAIT_FRAMES = int(1.0 / DT_CTRL) class CarController(CarControllerBase, IntelligentCruiseButtonManagementInterface): @@ -52,6 +63,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 = [] @@ -120,6 +138,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: @@ -139,8 +160,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 + # whole TJA hold counts, since the wheel's press pattern owns the counter stream until release. + icbm_suppress = (CC.cruiseControl.cancel or CC.cruiseControl.resume or CS.cancel_button == 1 or + self.mrcc_undo_pending or CS.tja_button == 1) if not icbm_suppress: can_sends.extend(IntelligentCruiseButtonManagementInterface.update(self, CC_SP, CS, self.packer, self.frame, self.last_button_frame)) @@ -194,6 +217,87 @@ 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 + # The no-arm wait counts brake-free cycles only: PEDALS holds both cruise bits low under + # braking, so 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: + # 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: either way, stand down. + 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: ICBM gets the button + # stream back instead of staying suppressed until the next cruise press. + 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, so nothing else transmits over the same slots. + 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 ec515fc8f51..0043f46d697 100644 --- a/opendbc/car/mazda/carstate.py +++ b/opendbc/car/mazda/carstate.py @@ -65,9 +65,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 @@ -225,11 +229,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: @@ -351,6 +358,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. @@ -360,6 +368,15 @@ 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 because an undeclared wheel's BIT1 idle level is unknown, and held + # unpressed until the wheel's first frame: the parser reads zeros before it, which the + # active-low decode would turn into a phantom press at startup. + 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 @@ -370,6 +387,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}), ] 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 542143ec797..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, **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 @@ -177,6 +183,10 @@ 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 + 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 0d13dfe641a..ac627d9c9e9 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, STOCK_CTS_ALERT_FRAMES, ButtonType 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 @@ -640,15 +640,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): @@ -734,3 +734,28 @@ 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 + + +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..a82d9a74387 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,115 @@ 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_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 b8c861fcf58..a828042283a 100644 --- a/opendbc/safety/modes/mazda.h +++ b/opendbc/safety/modes/mazda.h @@ -47,9 +47,20 @@ 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; +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) @@ -128,6 +139,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) { @@ -168,6 +180,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 @@ -341,7 +354,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 @@ -376,6 +396,7 @@ 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_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 66fb4395b8c..c240d14da6a 100755 --- a/opendbc/safety/tests/test_mazda.py +++ b/opendbc/safety/tests/test_mazda.py @@ -811,5 +811,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()