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..eae6a80925d 100644 --- a/opendbc/car/mazda/carcontroller.py +++ b/opendbc/car/mazda/carcontroller.py @@ -10,8 +10,9 @@ from opendbc.car.mazda.longitudinal import BREAKAWAY_FRAMES, AdvertisedLead, StandstillHold from opendbc.car.mazda.radar_session import RadarSessionManager, RadarSessionState from opendbc.car.mazda.values import CarControllerParams, Buttons, MazdaFlags +from opendbc.sunnypilot.car.mazda.values import MazdaFlagsSP -from opendbc.sunnypilot.car.mazda.icbm import IntelligentCruiseButtonManagementInterface +from opendbc.sunnypilot.car.mazda.icbm import BUTTONS, IntelligentCruiseButtonManagementInterface from opendbc.sunnypilot.car.stock_ecu import StockEcuState VisualAlert = structs.CarControl.HUDControl.VisualAlert @@ -19,6 +20,15 @@ # Send synthetic radar frames to both consumers; panda does not forward locally generated frames. LONG_BUSES = (0, 2) +TJA_MRCC_MAX_TX_FRAMES = 3 +TJA_MRCC_RAW_OFF_CONFIRM_FRAMES = 5 +# The body ECU drops discrete presses faster than one per 200 ms (measured, docs/zoompilot/icbm.md); +# the arm clears in PEDALS ~80 ms after a press plus the 50 ms confirm. +TJA_MRCC_TX_PERIOD = 0.2 # s between undo presses +# The press-induced arm appears ~80 ms after the press edge (docs/zoompilot/mazda-lateral.md). +TJA_MRCC_ARM_WAIT_FRAMES = int(1.0 / DT_CTRL) +# The white wheel waits this long on a fully-off, quiet cruise before it displays. +MADS_WHITE_HUD_OFF_CONFIRM_FRAMES = int(0.5 / DT_CTRL) class CarController(CarControllerBase, IntelligentCruiseButtonManagementInterface): @@ -38,6 +48,7 @@ def __init__(self, dbc_names, CP, CP_SP): self.driver_torque_samples: deque[float] = deque(maxlen=self.params.STEER_DRIVER_SAMPLES if self.eps_2022 else 1) self.packer = CANPacker(dbc_names[Bus.pt]) self.brake_counter = 0 + self.cruise_joined = False self.stop_and_go = StandstillHold() self.lead_adv = AdvertisedLead() self.long_counter = 0 @@ -52,6 +63,16 @@ def __init__(self, dbc_names, CP, CP_SP): self.tja_press_count = 0 self.tja_press_frame: int | None = None self.tja_episode_alerted = False + self.tja_button_prev = False + self.mrcc_armed_prev: bool | None = None + self.mrcc_undo_pending = False + self.mrcc_undo_saw_armed = False + self.mrcc_undo_frames = 0 + self.mrcc_raw_off_frames = 0 + self.mrcc_arm_wait_frames = 0 + # The white wheel rides the alert frame; on_bus tracks that the white bit is on the wire. + self.mads_white_hud_off_frames = 0 + self.mads_white_hud_on_bus = False def update(self, CC, CC_SP, CS, now_nanos): can_sends = [] @@ -103,9 +124,18 @@ def update(self, CC, CC_SP, CS, now_nanos): if self.steer_to_zero and (CS.steer_undelivered or CS.steer_first_engage_hold): apply_torque = 0 + # Never cancel ACC set by user. + if not CS.out.cruiseState.enabled: + self.cruise_joined = False + elif CC.enabled: + self.cruise_joined = True + # A TJA press turns MADS lateral off, and stock keeps MRCC engaged through it. + if not self.CP.openpilotLongitudinalControl and CS.tja_button: + self.cruise_joined = False + stock_never_joined = not self.CP.openpilotLongitudinalControl and CS.out.cruiseState.enabled and not self.cruise_joined # Do not cancel a stock MRCC engagement while the stock radar still owns the bus. stock_mrcc_owns_cruise = self.CP.openpilotLongitudinalControl and not CS.radar_was_silenced - if CC.cruiseControl.cancel and not stock_mrcc_owns_cruise: + if CC.cruiseControl.cancel and not stock_mrcc_owns_cruise and not stock_never_joined: # If brake is pressed, let us wait >70ms before trying to disable crz to avoid # a race condition with the stock system, where the second cancel from openpilot # will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to @@ -120,6 +150,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: @@ -127,20 +160,16 @@ 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, self.frame, apply_torque, CS.cam_lkas)) - # Suppress ICBM while cancel or resume is active to avoid competing button frames. - icbm_suppress = CC.cruiseControl.cancel or CC.cruiseControl.resume or CS.cancel_button == 1 + # Suppress ICBM while cancel/resume is active or the MRCC cleanup owns CRZ_BTNS: + # the wheel's press pattern owns the counter stream until release. + icbm_suppress = (CC.cruiseControl.cancel or CC.cruiseControl.resume or CS.cancel_button == 1 or + self.mrcc_undo_pending or CS.tja_button == 1) if not icbm_suppress: can_sends.extend(IntelligentCruiseButtonManagementInterface.update(self, CC_SP, CS, self.packer, self.frame, self.last_button_frame)) @@ -194,6 +223,147 @@ def update_camera_tja(self, CC, CS): self.tja_episode_alerted = True return can_sends + def update_mrcc_cleanup(self, CC, CC_SP, CS): + # Undo the MRCC arm a physical TJA press causes: the wheel's press reaches every bus-0 + # ECU directly, so the button the driver declared as the lateral switch also arms cruise. + # Answer with the driver's own MRCC master press until raw PEDALS confirms the arm is gone. + can_sends = [] + + raw_armed = bool(CS.mrcc_armed_raw) + # PEDALS drops both cruise bits through a brake transition; the filtered state bridges + # those samples, and only sustained raw-off is authoritative. + self.mrcc_raw_off_frames = 0 if raw_armed else self.mrcc_raw_off_frames + 1 + raw_off_confirmed = self.mrcc_raw_off_frames >= TJA_MRCC_RAW_OFF_CONFIRM_FRAMES + # PEDALS holds both cruise bits low under braking: the clock restarts there and runs + # again once the pedal is off. + self.mrcc_arm_wait_frames = 0 if CS.out.brakePressed else self.mrcc_arm_wait_frames + 1 + if self.CP.openpilotLongitudinalControl: + filtered_armed = CS.cruise_available + else: + filtered_armed = CS.out.cruiseState.available + mrcc_armed = raw_armed or (filtered_armed and not raw_off_confirmed) + + if CS.tja_button and not self.tja_button_prev: + # A press before the previous press's arm reconciled sees that arm as its own + # artifact, not the driver's baseline. + if self.mrcc_undo_pending: + self.mrcc_undo_frames = 0 + else: + # PEDALS can already show the press-induced arm in the same cycle as the edge; the + # previous stable sample is the state that existed before the press. + armed_before_press = self.mrcc_armed_prev if self.mrcc_armed_prev is not None else mrcc_armed + self.mrcc_undo_pending = not armed_before_press + self.mrcc_undo_saw_armed = False + self.mrcc_arm_wait_frames = 0 + self.tja_button_prev = bool(CS.tja_button) + self.mrcc_armed_prev = mrcc_armed + + # The arm is fully reconciled; the budget returns for the next press. + if not mrcc_armed and not CS.cruise_enabled and not CS.out.cruiseState.enabled: + self.mrcc_undo_frames = 0 + + if not self.mrcc_undo_pending: + return can_sends + + self.mrcc_undo_saw_armed |= raw_armed + + # The driver's own cruise presses own CRZ_BTNS from this cycle on, and restoring + # stock ECU ownership changes who owns the arm. + driver_activity = (CS.cancel_button or CS.resume_button or CS.accel_button or CS.decel_button or + CS.mrcc_button or CS.distance_button) + if driver_activity or CS.radar_handback_active or CC_SP.stockEcuHandBack: + self.mrcc_undo_pending = False + return can_sends + + # The arm never appeared: a takeover already disarmed it, or this car does not arm + # through the press. Past the bounded brake-free wait, stand down. + if not self.mrcc_undo_saw_armed: + if self.mrcc_arm_wait_frames >= TJA_MRCC_ARM_WAIT_FRAMES: + self.mrcc_undo_pending = False + return can_sends + + if raw_off_confirmed: + self.mrcc_undo_pending = False + return can_sends + + # openpilot's own cancel or resume interleaves button frames; wait it out rather + # than race the counter stream. + if CC.cruiseControl.cancel or CC.cruiseControl.resume: + return can_sends + + # Never inside the driver's held press. + if CS.tja_button: + return can_sends + + # One press per body-paced slot, within the episode budget; never while PEDALS reads disarmed, + # where the master press would arm instead of disarm. last_button_frame also paces ICBM. + if raw_armed and self.mrcc_undo_frames < TJA_MRCC_MAX_TX_FRAMES and \ + (self.frame - self.last_button_frame) * DT_CTRL > TJA_MRCC_TX_PERIOD: + can_sends.append(mazdacan.create_mrcc_off_cmd(self.packer, CS.crz_btns_counter)) + self.last_button_frame = self.frame + self.mrcc_undo_frames += 1 + if self.mrcc_undo_frames >= TJA_MRCC_MAX_TX_FRAMES: + self.mrcc_undo_pending = False + + return can_sends + + def update_hud(self, CC, CC_SP, CS): + """The alert frame at 2 Hz, mirroring the camera's own HUD state. + + On a TJA-declared car with MADS active and cruise fully off, the white-wheel TJA=2 + state is XORed into the camera's current payload, but only onto an exact allowlisted + idle base: TJA=2 is not display-only, the body reads the same frame. Every cruise + interaction fails closed, and a white wheel HUD state that became unsafe is withdrawn + immediately, outside the cadence. + """ + if self.CP.openpilotLongitudinalControl: + filtered_available, filtered_enabled = CS.cruise_available, CS.cruise_enabled + else: + filtered_available, filtered_enabled = CS.out.cruiseState.available, CS.out.cruiseState.enabled + + session_ambiguous = CS.radar_handback_active or CC_SP.stockEcuHandBack + mrcc_off = (not session_ambiguous and not CS.mrcc_armed_raw and + not filtered_available and not filtered_enabled) + + # Every button a TJA wheel carries: TJA, MRCC, SET+/-, RES, DISTANCE, plus the + # synthesized ICBM set presses and openpilot's own cancel/resume. + button_activity = (CS.tja_button or CS.mrcc_button or CS.cancel_button or CS.resume_button or + CS.accel_button or CS.decel_button or CS.distance_button or + CC_SP.intelligentCruiseButtonManagement.sendButton in BUTTONS or + CC.cruiseControl.cancel or CC.cruiseControl.resume) + + ldw = CC.hudControl.visualAlert == VisualAlert.ldw + steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired + # TODO: find a way to silence audible warnings so we can add more hud alerts + steer_required = steer_required and CS.lkas_allowed_speed + alert = mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required) + + fsc_raw = CS.cam_laneinfo_raw + hud_base = mazdacan.white_hud_allowlist_base(fsc_raw) + white_allowed = ( + bool(self.CP_SP.flags & MazdaFlagsSP.TJA_BUTTON) and + CC_SP.mads.active and + CS.cam_laneinfo_live and + hud_base is not None and + CC.hudControl.visualAlert == VisualAlert.none and + not button_activity and + mrcc_off + ) + if white_allowed: + self.mads_white_hud_off_frames = min(self.mads_white_hud_off_frames + 1, MADS_WHITE_HUD_OFF_CONFIRM_FRAMES) + else: + self.mads_white_hud_off_frames = 0 + white = white_allowed and self.mads_white_hud_off_frames >= MADS_WHITE_HUD_OFF_CONFIRM_FRAMES + withdraw_now = self.mads_white_hud_on_bus and not white + + # Preserve the normal 2 Hz cadence; the exception is the immediate OEM withdraw. + if self.frame % 50 == 0 or withdraw_now: + payload = hud_base if white else alert[1] + alert = (alert[0], mazdacan.apply_mads_white_hud(fsc_raw, payload, white), alert[2]) + self.mads_white_hud_on_bus = mazdacan.is_mads_white_hud(alert[1]) + return [alert] + return [] + def resume_requested(self, CC) -> bool: """The resume button belongs to the stock-longitudinal path alone. Under openpilot longitudinal the hold is released in-protocol (stop bits drop, RESUME_UNLATCHING pulses, the command ramps), diff --git a/opendbc/car/mazda/carstate.py b/opendbc/car/mazda/carstate.py index ec515fc8f51..7cad233a989 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 @@ -65,9 +67,14 @@ 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 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 @@ -82,7 +89,16 @@ 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 + 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 @@ -97,12 +113,17 @@ 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. 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 +132,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 @@ -131,8 +153,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): @@ -189,6 +213,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. @@ -196,6 +224,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"]) @@ -225,11 +273,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 + # 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: # 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: @@ -305,9 +356,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: @@ -351,6 +402,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 +412,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 @@ -370,7 +430,10 @@ 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}), ] CarStateExt.update(self, ret, ret_sp, can_parsers) @@ -387,6 +450,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/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 8f737f9bccf..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) @@ -230,3 +291,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..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 @@ -139,18 +141,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, 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. 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 +185,12 @@ 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 + 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_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..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, 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 @@ -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): @@ -640,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): @@ -734,3 +810,161 @@ 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 + +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) + + +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_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 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/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/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..dd53be483d0 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 @@ -47,8 +54,24 @@ 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. +static bool mazda_lka_button = false; +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. @@ -128,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) { @@ -168,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 @@ -202,6 +227,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) { @@ -341,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 @@ -376,6 +425,10 @@ 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; + mazda_acc_armed = false; static const CanMsg MAZDA_TX_MSGS[] = { {MAZDA_LKAS, 0, 8, .check_relay = true, .disable_static_blocking = true}, @@ -419,6 +472,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 +482,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..259bd45f4ea 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 = [] @@ -811,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() 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