diff --git a/opendbc/car/car.capnp b/opendbc/car/car.capnp index 8d26c8eedb5..954f5cc87bb 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 (button-declared cars) + latBlocked @63 :Bool; # the EPS holds no lateral now: standstill, LKAS off, or the re-arm after a re-enable # cruise state cruiseState @10 :CruiseState; diff --git a/opendbc/car/mazda/carcontroller.py b/opendbc/car/mazda/carcontroller.py index 054b9ddce48..92c215b554f 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,21 @@ def update(self, CC, CC_SP, CS, now_nanos): if self.steer_to_zero and (CS.steer_undelivered or CS.steer_first_engage_hold): apply_torque = 0 + # With LKA off, a SET press starts a plain stock ACC run with openpilot fully out: the driver + # started it, so our cancel must not kill it. Stock-long only; under alpha-long it is a desync and still cancels. + if not CS.out.cruiseState.enabled: + self.cruise_joined = False + elif CC.enabled: + self.cruise_joined = True + # The declared TJA button switches lateral only, and stock never disarms MRCC for its press: + # the disable the press triggers hands the joined run back to the driver. Runs every cycle + # the bit is on, so it also clears the cycle before the disable lands. + if not self.CP.openpilotLongitudinalControl and CS.tja_button: + self.cruise_joined = False + stock_never_joined = not self.CP.openpilotLongitudinalControl and CS.out.cruiseState.enabled and not self.cruise_joined # Do not cancel a stock MRCC engagement while the stock radar still owns the bus. stock_mrcc_owns_cruise = self.CP.openpilotLongitudinalControl and not CS.radar_was_silenced - if CC.cruiseControl.cancel and not stock_mrcc_owns_cruise: + if CC.cruiseControl.cancel and not stock_mrcc_owns_cruise and not stock_never_joined: # If brake is pressed, let us wait >70ms before trying to disable crz to avoid # a race condition with the stock system, where the second cancel from openpilot # will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to diff --git a/opendbc/car/mazda/carstate.py b/opendbc/car/mazda/carstate.py index ec515fc8f51..c9c778a209c 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) # the EPS bit is steady; this only sheds a lost frame # Bus witnesses: independent vehicle messages whose silence says the bus is gone, not the radar. # Windows at the CANParser's own validity threshold, ten periods; a stricter window would revoke # radar ownership on a gap the parser still accepts. {message: (signal, fresh frames)} @@ -34,6 +35,7 @@ def __init__(self, CP, CP_SP): self.lkas_blocked = False self.lkas_effective = 0 self.lkas_track_state = False + self.lat_block_frames = 0 # LKAS non-delivery state is used only with the steer-to-zero EPS. self.params = CarControllerParams(CP) self.steer_undelivered_frames = 0 @@ -83,6 +85,11 @@ def __init__(self, CP, CP_SP): self.cam_laneinfo_seen = False self.cam_laneinfo_silent_frames = 0 self.cam_empty_seen = False + self.lkas_on_candidate = False + self.lkas_on_stable = False + self.lane_lines_armed = False + self.lkas_button = 0 + self.lkas_button_prev = 0 self.radar_session_refused = False self.radar_session_response = 0 self.fsc_settled_frames = 0 @@ -103,6 +110,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 @@ -111,7 +119,8 @@ def update_steer_undelivered(self, v_ego_raw: float, lkas_request: float) -> Non # Latch sustained zero LKAS_EFFECTIVE for a real request before the camera faults. Clear # with LKAS_BLOCK because a zeroed command provides no delivery signal. Driver torque does # not gate entry because torque in the requested direction does not reduce the request. - if not lkas_blocked: + # Lane keep off also clears the latch: zero delivery is then the driver's choice, not a fault. + if not lkas_blocked or lane_keep_off: self.steer_undelivered_frames = 0 self.steer_undelivered = False self.steer_undelivered_alert = False @@ -131,8 +140,10 @@ def update_steer_undelivered(self, v_ego_raw: float, lkas_request: float) -> Non # identifies normal low-speed standby, which can remain set briefly during a brisk # launch; the origin speed catches the standby blocks it does not, the ones carried # from a stop through a slow crawl until TRACK_STATE clears with the block still on. + # The latch can outlive the request that earned it: the request must still stand. self.steer_undelivered_frames += 1 if (not self.steer_undelivered_alert and not lkas_track_state and + abs(lkas_request) > self.params.STEER_UNDELIVERED_MIN and self.steer_undelivered_frames >= self.params.STEER_UNDELIVERED_FRAMES + self.params.STEER_UNDELIVERED_ALERT_FRAMES and v_ego_raw >= self.params.STEER_UNDELIVERED_ALERT_MIN_SPEED and self.lkas_block_origin_speed >= self.params.STEER_UNDELIVERED_ALERT_ORIGIN_SPEED): @@ -189,6 +200,11 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]: self.lkas_blocked = lkas_blocked self.lkas_effective = cp.vl["STEER_RATE"]["LKAS_EFFECTIVE"] self.lkas_track_state = cp.vl["STEER_RATE"]["LKAS_TRACK_STATE"] == 1 + # LKAS_BLOCK also covers standstill and the fixed ~3 s re-arm after a dash LKA re-enable, + # states where the EPS takes our request but holds no lateral: published so the UI can hold + # lateral as arming instead of claiming control the EPS has not granted yet. + self.lat_block_frames = self.lat_block_frames + 1 if lkas_blocked else 0 + ret.latBlocked = self.lat_block_frames >= LAT_BLOCK_CONFIRM_FRAMES # The 2022 EPS raises LKAS_FAULT once its 0x243 stream has stopped for about 0.6 s; the # camera's own fault follows 5.3 s later and neither clears before the next ignition cycle. # Decoded for the log and tooling; the driver-facing fault stays the camera's own. @@ -196,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) + # The dash LKA button has no CAN signal of its own; LANE_LINES is the state it drives. + self.lkas_button_prev = self.lkas_button + self.lkas_button = 0 + if self.CP_SP.flags & MazdaFlagsSP.LKA_BUTTON: + lane_lines_vals = cp_cam.vl_all["CAM_LANEINFO"]["LANE_LINES"] + if len(lane_lines_vals) > 0: + lkas_on = int(lane_lines_vals[-1]) != 0 + if lkas_on != self.lkas_on_candidate: + self.lkas_on_candidate = lkas_on + elif not self.lane_lines_armed: + self.lane_lines_armed = True + self.lkas_on_stable = lkas_on + elif lkas_on != self.lkas_on_stable: + self.lkas_on_stable = lkas_on + # one lateral switch per car: a declared TJA button owns the press + if not self.CP_SP.flags & MazdaFlagsSP.TJA_BUTTON: + self.lkas_button = 1 + # the state is not the switch: a TJA car reads it too, so its dash press names a driver-choice refusal + ret.lkaButtonOff = self.lane_lines_armed and not self.lkas_on_stable + if self.CP.flags & MazdaFlags.STEER_TO_ZERO_EPS: self.update_steer_undelivered(ret.vEgoRaw, cp.vl["STEER_RATE"]["LKAS_REQUEST"]) @@ -371,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 0d13dfe641a..13648864b02 100644 --- a/opendbc/car/mazda/tests/test_mazda_carstate.py +++ b/opendbc/car/mazda/tests/test_mazda_carstate.py @@ -14,9 +14,9 @@ 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.car.mazda.values import CAR, CarControllerParams from opendbc.sunnypilot.car.mazda.values import MazdaFlagsSP CAM_LANEINFO = 0x440 @@ -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): @@ -734,3 +810,106 @@ 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 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): + # an undeclared platform keeps no read of the button state at all + CI, pk = car_interface(alpha_long=False, candidate=CAR.MAZDA_CX5), packer() + 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..64971340960 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 @@ -240,6 +241,7 @@ class CAR(Platforms): [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")], diff --git a/opendbc/safety/modes/mazda.h b/opendbc/safety/modes/mazda.h index b8c861fcf58..503881da64d 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 with the 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: the dash LKA button owns lateral and the HUD frame is checked. +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; } } + + // The dash LKA button has no CAN signal: LANE_LINES is its state (0 = off). Only the on-press is + // reported: a press cannot exit controls, the off-toggle is the software's to act on. 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..70d702032e6 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 state publishes on both, only the press resolves to the physical button. + 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..8aa9bcd1659 100644 --- a/opendbc/sunnypilot/car/mazda/values.py +++ b/opendbc/sunnypilot/car/mazda/values.py @@ -12,8 +12,12 @@ 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. Declare only where the dash button is + # proven to drive LANE_LINES to zero; the whitelist liveness entry rides the same declaration. + LKA_BUTTON = 2