diff --git a/opendbc/car/mazda/carcontroller.py b/opendbc/car/mazda/carcontroller.py index d57c490eb6a..4d839bd3af4 100644 --- a/opendbc/car/mazda/carcontroller.py +++ b/opendbc/car/mazda/carcontroller.py @@ -1,7 +1,7 @@ import numpy as np from opendbc.can import CANPacker -from opendbc.car import Bus, make_tester_present_msg, rate_limit, structs, uds +from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, rate_limit, structs, uds from opendbc.car.lateral import apply_driver_steer_torque_limits from opendbc.car.interfaces import CarControllerBase from opendbc.car.mazda import mazdacan @@ -18,7 +18,8 @@ # received frames between those buses, not our own transmissions. LONG_BUSES = (0, 2) - +# a quiet camera longer than this drops the HUD relay to the 2 Hz hold on the last frame +LANEINFO_STALE_FRAMES = int(1.0 / DT_CTRL) class CarController(CarControllerBase, IntelligentCruiseButtonManagementInterface): def __init__(self, dbc_names, CP, CP_SP): CarControllerBase.__init__(self, dbc_names, CP, CP_SP) @@ -32,6 +33,10 @@ def __init__(self, dbc_names, CP, CP_SP): self.radar_counter = 0 self.radar_session = RadarSessionManager() self.accel_last = 0. + self.ctr_offset = 0 + self.last_lat_active = False + self.last_laneinfo_ts = None + self.laneinfo_age_frames = 0 def update(self, CC, CC_SP, CS, now_nanos): can_sends = [] @@ -71,17 +76,28 @@ def update(self, CC, CC_SP, CS, now_nanos): if self.CP.openpilotLongitudinalControl: can_sends.extend(self.update_longitudinal(CC, CC_SP, 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)) - - # send steering command + # while openpilot steers it drives the steering-assist indicator (the orange wheel + # stock lights while the EPS corrects) as its alert channel, and blanks the lines + cam_ts = CS.cam_laneinfo_ts + new_frame = cam_ts > 0 and cam_ts != self.last_laneinfo_ts + if new_frame or (self.laneinfo_age_frames >= LANEINFO_STALE_FRAMES and self.laneinfo_age_frames % 50 == 0): + steer_indicator = None + if CC.latActive: + steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired + steer_indicator = steer_required and CS.lkas_allowed_speed + can_sends.append(mazdacan.create_laneinfo_relay(CS.cam_laneinfo_raw if cam_ts > 0 else None, + steer_indicator, + steer_indicator is not None and not steer_indicator)) + self.last_laneinfo_ts = cam_ts + self.laneinfo_age_frames = 0 if new_frame else self.laneinfo_age_frames + 1 + + # send steering command; the counter continues the camera's sequence across an engage + if CC.latActive and not self.last_lat_active: + self.ctr_offset = (int(CS.cam_lkas["CTR"]) + 1 - self.frame) % 16 + self.last_lat_active = CC.latActive can_sends.append(mazdacan.create_steering_control(self.packer, self.CP, - self.frame, apply_torque, CS.cam_lkas)) + self.frame + self.ctr_offset, + apply_torque, CS.cam_lkas, CS.cam_lkas_raw)) # Intelligent Cruise Button Management # Suppress ICBM CRZ_BTNS spam while cancel/resume are in flight or while the driver is diff --git a/opendbc/car/mazda/carstate.py b/opendbc/car/mazda/carstate.py index 5b5e7cfba0c..cfc2b92cf21 100644 --- a/opendbc/car/mazda/carstate.py +++ b/opendbc/car/mazda/carstate.py @@ -39,6 +39,9 @@ def __init__(self, CP, CP_SP): self.radar_was_silenced = False self.cancel_context_frames = 0 self.cam_laneinfo_seen = False + self.cam_laneinfo_raw = 0 + self.cam_laneinfo_ts = 0 + self.cam_lkas_raw = None self.fsc_settled_frames = 0 # the body ECU has taken the standstill hold over and is holding the brakes itself self.brake_hold = False @@ -108,6 +111,8 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]: else: self.lkas_allowed_speed = True + laneinfo = cp_cam.vl["CAM_LANEINFO"] + if self.CP.openpilotLongitudinalControl: # The radar teardown silences the radar-owned CRZ_CTRL frame, so cruise state comes # from PEDALS: ACC_OFF means MRCC is armed but idle, ACC_ACTIVE means it is engaged. @@ -171,7 +176,6 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]: # timer at zero, so the radar was never silenced and the two-master guard held # accFaulted for the entire drive with nothing to tell the driver why. self.cam_laneinfo_seen |= len(cp_cam.vl_all["CAM_LANEINFO"]["LANE_LINES"]) > 0 - laneinfo = cp_cam.vl["CAM_LANEINFO"] settled = self.cam_laneinfo_seen and not any(laneinfo[s] for s in ("NO_ERR_BIT", "ERR_BIT")) self.fsc_settled_frames = self.fsc_settled_frames + 1 if settled else 0 else: @@ -210,7 +214,14 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]: # camera signals self.cam_lkas = cp_cam.vl["CAM_LKAS"] - self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"] + # exact frame bytes: both relays must carry bits the DBC doesn't describe + if cp_cam.ts_nanos["CAM_LKAS"]["CTR"] > 0: + lkas_raw = cp_cam.vl["CAM_LKAS"] + self.cam_lkas_raw = (int(lkas_raw["FRAME_RAW_HI"]) << 32) | int(lkas_raw["FRAME_RAW_LO"]) + else: + self.cam_lkas_raw = None + self.cam_laneinfo_raw = (int(laneinfo["FRAME_RAW_HI"]) << 32) | int(laneinfo["FRAME_RAW_LO"]) + self.cam_laneinfo_ts = cp_cam.ts_nanos["CAM_LANEINFO"]["FRAME_RAW_HI"] ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1 # cruise control button events: distance, inc, dec, resume, cancel, and main diff --git a/opendbc/car/mazda/mazdacan.py b/opendbc/car/mazda/mazdacan.py index a314bd847b5..726a9daf054 100644 --- a/opendbc/car/mazda/mazdacan.py +++ b/opendbc/car/mazda/mazdacan.py @@ -103,7 +103,43 @@ def create_radar_frames(bus, counter, lead): return frames -def create_steering_control(packer, CP, frame, apply_torque, lkas): +# CAM_LKAS bits the controller owns, probed against the packer: CTR owns byte 0's high +# nibble, the torque field byte 0's low nibble plus byte 1, the angle fields bytes 4-6. +# LINE_NOT_VISIBLE is forced off (the EPS gates torque on it); every other bit rides through. +LKAS_WRITE_MASKS = {0: 0xFF, 1: 0xFF, 2: 0x08, 4: 0x03, 5: 0xFF, 6: 0xD0} +LKAS_LNV_MASK_B2 = 0x08 + + +def _angle_checksum_terms(steering_angle: int, angle_enabled: int) -> int: + # the checksum contribution the curated formula assigns to the angle fields + tmp = steering_angle + 2048 + ahi = tmp >> 10 + amd = (tmp & 0x3FF) >> 2 + amd = (amd >> 4) | ((amd & 0xF) << 4) + alo = (tmp & 0x3) << 2 + return ahi + amd + alo + angle_enabled - (15 if ahi == 1 else 0) + + +def _overlay_steering_control(ours: bytes, cam_raw: int, lkas) -> bytes: + dat = bytearray(cam_raw.to_bytes(8, "big")) + + # checksum delta over exactly the fields written below; the camera's own checksum + # already covers every other bit, defined or not + csum = dat[7] + csum -= (ours[0] >> 4) - (dat[0] >> 4) + csum -= (ours[0] & 0x0F) - (dat[0] & 0x0F) + csum -= ours[1] - dat[1] + csum += dat[2] & LKAS_LNV_MASK_B2 # visibility bit forced off; removing it adds back + csum += _angle_checksum_terms(int(lkas["STEERING_ANGLE"]), int(lkas["ANGLE_ENABLED"])) + csum -= _angle_checksum_terms(0, 0) + + for i, mask in LKAS_WRITE_MASKS.items(): + dat[i] = (dat[i] & (0xFF ^ mask)) | (ours[i] & mask) + dat[7] = csum % 256 + return bytes(dat) + + +def create_steering_control(packer, CP, ctr, apply_torque, lkas, cam_raw: int | None = None): tmp = apply_torque + 2048 @@ -113,6 +149,7 @@ def create_steering_control(packer, CP, frame, apply_torque, lkas): # copy values from camera b1 = int(lkas["BIT_1"]) er1 = int(lkas["ERR_BIT_1"]) + # LDW stays zero: the overlay carries the camera's alert bit from its raw bytes lnv = 0 ldw = 0 er2 = int(lkas["ERR_BIT_2"]) @@ -128,7 +165,7 @@ def create_steering_control(packer, CP, frame, apply_torque, lkas): amd = (amd >> 4) | ((amd & 0xF) << 4) alo = (tmp & 0x3) << 2 - ctr = frame % 16 + ctr = ctr % 16 # bytes: [ 1 ] [ 2 ] [ 3 ] [ 4 ] csum = 249 - ctr - hi - lo - (lnv << 3) - er1 - (ldw << 7) - (er2 << 4) - (b1 << 5) @@ -161,33 +198,37 @@ def create_steering_control(packer, CP, frame, apply_torque, lkas): "CHKSUM": csum } - return packer.make_can_msg("CAM_LKAS", 0, values) - - -def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool): - values = {s: cam_msg[s] for s in [ - "LINE_VISIBLE", - "LINE_NOT_VISIBLE", - "LANE_LINES", - "BIT1", - "BIT2", - "BIT3", - "NO_ERR_BIT", - "S1", - "S1_HBEAM", - ]} - values.update({ - # TODO: what's the difference between all these? do we need to send all? - "HANDS_WARN_3_BITS": 0b111 if steer_required else 0, - "HANDS_ON_STEER_WARN": steer_required, - "HANDS_ON_STEER_WARN_2": steer_required, - - # TODO: right lane works, left doesn't - # TODO: need to do something about L/R - "LDW_WARN_LL": 0, - "LDW_WARN_RL": 0, - }) - return packer.make_can_msg("CAM_LANEINFO", 0, values) + if cam_raw is None: + return packer.make_can_msg("CAM_LKAS", 0, values) + # overlay: the controller's torque/counter/angle bits written into the camera's exact frame + ours = packer.make_can_msg("CAM_LKAS", 0, values)[1] + return CanData(0x243, _overlay_steering_control(ours, cam_raw, lkas), 0) + + +CAM_LANEINFO_ADDR = 0x440 +# Steering-assist indicator bits: the orange wheel the dash lights while the EPS applies +# corrective torque (the DBC's HANDS_* names are misleading). Byte positions match the +# packer mapping: the three-bit field in byte 6, the single bits in byte 7 +STEER_IND_B6 = 0x0E +STEER_IND_B7 = 0x09 +LANE_LINES_MASK_B1 = 0x07 # LANE_LINES, 0 = LKAS disabled + + +def create_laneinfo_relay(cam_raw: int | None, steer_indicator: bool | None = None, suppress_lines: bool = False): + # byte-for-byte: bits the DBC does not describe must reach the dash as the camera sent + # them. steer_indicator None relays the camera's own indicator state, True/False light + # or clear it for openpilot's hold-the-wheel alerts + dat = bytearray(8 if cam_raw is None else cam_raw.to_bytes(8, "big")) + if steer_indicator is not None: + if steer_indicator: + dat[6] |= STEER_IND_B6 + dat[7] |= STEER_IND_B7 + else: + dat[6] &= 0xFF ^ STEER_IND_B6 + dat[7] &= 0xFF ^ STEER_IND_B7 + if suppress_lines: + dat[1] &= 0xFF ^ LANE_LINES_MASK_B1 + return CanData(CAM_LANEINFO_ADDR, bytes(dat), 0) def create_button_cmd(packer, CP, counter, button): diff --git a/opendbc/car/mazda/tests/test_mazda_carstate.py b/opendbc/car/mazda/tests/test_mazda_carstate.py index d1f2ff75b7e..1e2e7bfdc15 100644 --- a/opendbc/car/mazda/tests/test_mazda_carstate.py +++ b/opendbc/car/mazda/tests/test_mazda_carstate.py @@ -6,6 +6,7 @@ from opendbc.car.mazda.values import CAR, CarControllerParams CAM_LANEINFO = 0x440 +CAM_LKAS = 0x243 # Real CAM_LANEINFO prefixes, captured on two CX-5 2022s running the same FSC firmware # (GSH7-67XK2-U). Only byte 1 differs: bit 5 is BIT2, bit 6 is NO_ERR_BIT. @@ -78,6 +79,52 @@ def test_gate_starts_closed_before_any_camera_frame(self): assert not CI.CS.fsc_settled +class TestCamRelaySources: + """CS.cam_lkas (decoded bits) and CS.cam_lkas_raw / CS.cam_laneinfo_raw (exact frame + bytes) are the relay sources: the controller overlays its steering command onto the + camera's 0x243 bytes and re-sends the 0x440 bytes verbatim.""" + + @staticmethod + def _feed_cam(CI, addr, values, frames=2): + # CANParser registers a message lazily on first access, so the first frame only arms it + from opendbc.can import CANPacker + packer = CANPacker("mazda_2017") + msg = packer.make_can_msg("CAM_LKAS" if addr == CAM_LKAS else "CAM_LANEINFO", 2, values) + for i in range(frames): + CI.update([(int(i * DT_CTRL * 1e9), [(addr, msg[1], 2)])]) + + def test_cam_lkas_decodes_the_camera_bits(self): + values = {"BIT_1": 1, "ERR_BIT_1": 1, "ERR_BIT_2": 1, "LDW": 1, "LINE_NOT_VISIBLE": 1} + CI = _interface() + self._feed_cam(CI, CAM_LKAS, values) + for k, v in values.items(): + assert CI.CS.cam_lkas[k] == v + assert CI.CS.out.steerFaultPermanent + assert CI.CS.cam_lkas_raw is not None + + def test_cam_lkas_raw_carries_undefined_bits(self): + # byte 2's bits 1,2,4,5,6 and byte 3's low five bits carry no DBC signal + payload = bytes([0x35, 0xC7, 0xF6, 0x3B, 0xA8, 0x51, 0x7E, 0x42]) + CI = _interface() + for i in range(2): + CI.update([(int(i * DT_CTRL * 1e9), [(CAM_LKAS, payload, 2)])]) + assert CI.CS.cam_lkas_raw == int.from_bytes(payload, "big") + + def test_steer_fault_follows_the_err_bit(self): + CI = _interface() + self._feed_cam(CI, CAM_LKAS, {"BIT_1": 1, "ERR_BIT_1": 0, "ERR_BIT_2": 1}) + assert not CI.CS.out.steerFaultPermanent + + def test_cam_laneinfo_raw_carries_undefined_bits(self): + # bytes 2 and 5 carry no DBC signal at all, but the dash reads bits there + payload = bytes([0x42, 0x41, 0xAB, 0x00, 0x00, 0xCD, 0x71, 0x3C]) + CI = _interface() + for i in range(2): + CI.update([(int(i * DT_CTRL * 1e9), [(CAM_LANEINFO, payload, 2)])]) + assert CI.CS.cam_laneinfo_raw == int.from_bytes(payload, "big") + assert CI.CS.cam_laneinfo_ts > 0 + + class TestBrakeHold: """GEAR.BRAKE_HOLD is the body ECU reporting that it owns the standstill hold. Stock relaxes its own command the instant this sets, so the payloads below come straight off the two logs diff --git a/opendbc/car/mazda/tests/test_mazda_controller.py b/opendbc/car/mazda/tests/test_mazda_controller.py index 70a3d0d371b..9530a78faef 100644 --- a/opendbc/car/mazda/tests/test_mazda_controller.py +++ b/opendbc/car/mazda/tests/test_mazda_controller.py @@ -13,7 +13,7 @@ from opendbc.car.mazda.carcontroller import CarController from opendbc.car.mazda.longitudinal import LEAD_DEBOUNCE_FRAMES, RESUME_UNLATCH_FRAMES, StandstillHold from opendbc.car.mazda.interface import CarInterface -from opendbc.car.mazda.values import CAR, CarControllerParams +from opendbc.car.mazda.values import CAR, CarControllerParams, MazdaFlags class TestCarControllerParams: @@ -186,6 +186,368 @@ def test_lead_track_round_trips_through_the_dbc(self, d_rel, v_rel): assert dat[5:] == mazdacan.LEAD_TRACK_TEMPLATE[5:] +class TestSteeringOverlay: + """The engaged 0x243 overlays the camera's exact frame: torque, counter and angle bits + are ours, the line-visibility bit is forced off (the EPS gates torque on it), and every + other bit, defined or not, is the camera's.""" + + LKAS = {"BIT_1": 1, "ERR_BIT_1": 0, "ERR_BIT_2": 1, + "STEERING_ANGLE": 0, "ANGLE_ENABLED": 0} + # bits no DBC signal describes, per byte + UNDEFINED = {2: 0x76, 3: 0x9F, 4: 0xFC, 6: 0x2F} + + @pytest.fixture + def packer(self): + return CANPacker("mazda_2017") + + @classmethod + def _steer(cls, packer, ctr=0, torque=0, lkas=None, cam_raw=None): + class FakeCP: + flags = MazdaFlags.GEN1 + return mazdacan.create_steering_control(packer, FakeCP(), ctr, torque, + dict(cls.LKAS if lkas is None else lkas), cam_raw)[1] + + def test_overlay_matches_curated_on_a_defined_camera_frame(self, packer): + # a camera frame carrying no undefined bits: the overlay must reproduce the curated + # build byte for byte, checksum included + cam = self._steer(packer, ctr=5, torque=-200) + overlay = self._steer(packer, ctr=9, torque=300, cam_raw=int.from_bytes(cam, "big")) + assert overlay == self._steer(packer, ctr=9, torque=300) + + def test_undefined_bits_ride_through(self, packer): + cam = bytearray(self._steer(packer, ctr=5, torque=-200)) + for i, mask in self.UNDEFINED.items(): + cam[i] |= mask + cam[2] |= 0x80 # the camera's LDW alert bit rides through too + overlay = self._steer(packer, ctr=9, torque=300, cam_raw=int.from_bytes(cam, "big")) + for i, mask in self.UNDEFINED.items(): + assert overlay[i] & mask == cam[i] & mask + parser = CANParser("mazda_2017", [("CAM_LKAS", 0)], 0) + parser.update([(0, [(0x243, overlay, 0)])]) + vl = parser.vl["CAM_LKAS"] + for k in ("BIT_1", "ERR_BIT_2", "LDW"): + assert vl[k] == 1 + assert vl["CTR"] == 9 + assert vl["LKAS_REQUEST"] == 300 + + def test_line_not_visible_is_forced_off(self, packer): + # the EPS gates torque on the camera's visibility state (v4 on-device: openpilot + # could only steer while the camera saw lanes), so the overlay clears it and the + # checksum delta pays for the removal + cam = bytearray(self._steer(packer, ctr=5, torque=-200)) + cam[2] |= mazdacan.LKAS_LNV_MASK_B2 + cam[7] = (cam[7] - mazdacan.LKAS_LNV_MASK_B2) % 256 # a checksum valid for LNV set + overlay = self._steer(packer, ctr=9, torque=300, cam_raw=int.from_bytes(cam, "big")) + assert overlay == self._steer(packer, ctr=9, torque=300) + + def test_camera_angle_bits_replaced_with_ours(self, packer): + # a camera frame carrying a nonzero angle and enable: the overlay swaps in our + # zero-angle pattern and the checksum delta pays for the removal, so the result is + # the curated build byte for byte (the curated path never writes the camera's angle) + vals = {"CTR": 5, "LKAS_REQUEST": -200, "STEERING_ANGLE": -300, "ANGLE_ENABLED": 1, + "LDW": 0, "LINE_NOT_VISIBLE": 1, "BIT_1": 1, "ERR_BIT_2": 1} + angled = bytearray(packer.make_can_msg("CAM_LKAS", 0, dict(vals, CHKSUM=0))[1]) + tmp = -200 + 2048 + angled[7] = (249 - 5 - (tmp >> 8) - (tmp & 0xFF) - (1 << 3) - (1 << 4) - (1 << 5) + - mazdacan._angle_checksum_terms(-300, 1)) % 256 + overlay = self._steer(packer, ctr=9, torque=300, cam_raw=int.from_bytes(angled, "big"), + lkas=dict(self.LKAS, STEERING_ANGLE=-300, ANGLE_ENABLED=1)) + assert overlay == self._steer(packer, ctr=9, torque=300) + + def test_write_masks_match_the_packer(self, packer): + # flipping every owned field (counter, torque, angle, enable) may only flip bits + # inside the write masks; bytes 2 and 3 must never move + base = self._steer(packer, ctr=0, torque=0) + variants = ( + self._steer(packer, ctr=10, torque=900), + self._steer(packer, ctr=0, torque=0, lkas=dict(self.LKAS, STEERING_ANGLE=-300, ANGLE_ENABLED=1)), + ) + for other in variants: + assert other[2] == base[2] and other[3] == base[3] + for i in range(7): + diff = base[i] ^ other[i] + assert diff & mazdacan.LKAS_WRITE_MASKS.get(i, 0) == diff, \ + f"byte {i}: packer flipped bits outside the write mask" + + +class TestLaneinfoRelay: + """CAM_LANEINFO relay: the camera's frame reaches the dash byte for byte; while + openpilot steers, only the steering-assist indicator bits are its own.""" + + @pytest.fixture + def packer(self): + return CANPacker("mazda_2017") + + @staticmethod + def _cam_dat(packer, values=None): + _, dat, _ = packer.make_can_msg("CAM_LANEINFO", 2, dict(values or {})) + return bytearray(dat) + + def test_camera_frame_relays_byte_for_byte(self, packer): + # bits the DBC does not describe (all of byte 2 among them) must survive the trip + cam_dat = self._cam_dat(packer) + cam_dat[2] = 0xAB + cam_dat[5] = 0xCD + relay = mazdacan.create_laneinfo_relay(int.from_bytes(cam_dat, "big")) + assert relay.dat == bytes(cam_dat) + assert relay.address == 0x440 and relay.src == 0 + + def test_indicator_bits_set_and_clear_touch_nothing_else(self, packer): + cam_dat = self._cam_dat(packer) + cam_dat[2] = 0xAB + raw = int.from_bytes(cam_dat, "big") + for lit in (True, False): + relay = mazdacan.create_laneinfo_relay(raw, lit) + assert relay.dat[:6] == bytes(cam_dat[:6]) + b6 = (cam_dat[6] | mazdacan.STEER_IND_B6) if lit else (cam_dat[6] & (0xFF ^ mazdacan.STEER_IND_B6)) + b7 = (cam_dat[7] | mazdacan.STEER_IND_B7) if lit else (cam_dat[7] & (0xFF ^ mazdacan.STEER_IND_B7)) + assert relay.dat[6] == b6 + assert relay.dat[7] == b7 + + def test_indicator_masks_match_the_packer_mapping(self, packer): + # the masks must hit exactly the bits the packer assigns to the HANDS_* signals + dark = self._cam_dat(packer) + lit = self._cam_dat(packer, {"HANDS_WARN_3_BITS": 0b111, "HANDS_ON_STEER_WARN": 1, + "HANDS_ON_STEER_WARN_2": 1}) + diff = [(i, a ^ b) for i, (a, b) in enumerate(zip(dark, lit, strict=True)) if a != b] + assert diff == [(6, mazdacan.STEER_IND_B6), (7, mazdacan.STEER_IND_B7)] + + def test_line_suppression_touches_only_the_lanes_field(self, packer): + cam_dat = self._cam_dat(packer, {"LANE_LINES": 4}) + cam_dat[2] = 0xAB + relay = mazdacan.create_laneinfo_relay(int.from_bytes(cam_dat, "big"), suppress_lines=True) + expected = bytearray(cam_dat) + expected[1] &= 0xFF ^ mazdacan.LANE_LINES_MASK_B1 + assert relay.dat == bytes(expected) + # the mask covers exactly the bits the packer assigns to LANE_LINES + other = self._cam_dat(packer, {"LANE_LINES": 1}) + assert (cam_dat[1] ^ other[1]) & mazdacan.LANE_LINES_MASK_B1 == cam_dat[1] ^ other[1] + + def test_camera_signals_decode_back(self, packer): + values = {"LANE_LINES": 2, "LDW_WARN_LL": 1, "LDW_WARN_RL": 0, "TJA": 3, + "TJA_TRANSITION": 2, "S1": 1, "S1_HBEAM": 1, "ERR_BIT": 1} + cam_dat = self._cam_dat(packer, values) + relay = mazdacan.create_laneinfo_relay(int.from_bytes(cam_dat, "big")) + parser = CANParser("mazda_2017", [("CAM_LANEINFO", 0)], 0) + parser.update([(0, [(0x440, relay.dat, 0)])]) + for k, v in values.items(): + assert parser.vl["CAM_LANEINFO"][k] == v + + +class TestSteeringCommand: + """CAM_LKAS re-send: the camera's health and lane-departure bits ride along on every + steering command.""" + + @pytest.fixture + def packer(self): + return CANPacker("mazda_2017") + + @staticmethod + def _steer_msg(packer, lkas=None, ctr=0, torque=0): + class FakeCP: + flags = MazdaFlags.GEN1 + lkas = {"BIT_1": 0, "ERR_BIT_1": 0, "ERR_BIT_2": 0, "LDW": 0, "LINE_NOT_VISIBLE": 0} if lkas is None else lkas + _, dat, _ = mazdacan.create_steering_control(packer, FakeCP(), ctr, torque, dict(lkas)) + parser = CANParser("mazda_2017", [("CAM_LKAS", 0)], 0) + parser.update([(0, [(0x243, dat, 0)])]) + return parser.vl["CAM_LKAS"] + + def test_camera_bits_relay(self, packer): + lkas = {"BIT_1": 1, "ERR_BIT_1": 1, "ERR_BIT_2": 1, "LDW": 1, "LINE_NOT_VISIBLE": 1} + vl = self._steer_msg(packer, lkas) + for k in ("BIT_1", "ERR_BIT_1", "ERR_BIT_2"): + assert vl[k] == 1 + assert vl["LKAS_REQUEST"] == 0 + # the EPS gates torque on the visibility state, so the curated build never sends it; + # the overlay carries the camera's alert bit from the raw bytes instead + assert vl["LDW"] == 0 + assert vl["LINE_NOT_VISIBLE"] == 0 + + def test_counter_wraps_at_sixteen(self, packer): + for ctr in (0, 7, 15, 16, 33): + assert self._steer_msg(packer, ctr=ctr)["CTR"] == ctr % 16 + + def test_torque_round_trips(self, packer): + for torque in (0, 100, -100, 1200): + assert self._steer_msg(packer, torque=torque)["LKAS_REQUEST"] == torque + + +class TestRelayEmission: + """Drives the real interface: the controller emits its own 0x243 every frame regardless of + engagement, and relays the camera's 0x440 the moment a new camera frame lands (with a 2 Hz + hold on the last frame once the camera has been quiet past the stale window). The panda, + not the controller, yields the bus to the camera while disengaged.""" + + CAM_LKAS_VALUES = {"BIT_1": 1, "ERR_BIT_1": 0, "ERR_BIT_2": 1, "LDW": 1, "LINE_NOT_VISIBLE": 1, "CTR": 5} + CAM_LANEINFO_VALUES = {"LANE_LINES": 2, "LDW_WARN_LL": 1, "LDW_WARN_RL": 0, "TJA": 3, + "TJA_TRANSITION": 1, "HANDS_WARN_3_BITS": 0b101} + + @pytest.fixture + def ci(self): + CP = CarInterface.get_params(CAR.MAZDA_CX5_2022, {0: {}, 1: {}, 2: {}}, [], alpha_long=False, + is_release=False, docs=False) + CP_SP = CarInterface.get_params_sp(CP, CAR.MAZDA_CX5_2022, {0: {}, 1: {}, 2: {}}, [], + alpha_long=False, is_release_sp=False, docs=False) + assert not CP.openpilotLongitudinalControl + return CarInterface(CP, CP_SP) + + def _feed_camera(self, ci): + packer = CANPacker("mazda_2017") + msgs = [packer.make_can_msg("CAM_LKAS", 2, self.CAM_LKAS_VALUES), + packer.make_can_msg("CAM_LANEINFO", 2, self.CAM_LANEINFO_VALUES)] + for i in range(2): + ci.update([(int(i * DT_CTRL * 1e9), [(m[0], m[1], m[2]) for m in msgs])]) + return msgs[0][1], msgs[1][1] + + @staticmethod + def _control(enabled=False, lat_active=False, torque=0.0, steer_required=False): + CC = structs.CarControl.new_message() + CC.enabled = enabled + CC.latActive = lat_active + CC.actuators.torque = torque + if steer_required: + CC.hudControl.visualAlert = structs.CarControl.HUDControl.VisualAlert.steerRequired + # card hands the controller a reader off the wire; update() calls actuators.as_builder() + return CC.as_reader() + + def _apply(self, ci, i, CC): + return ci.apply(CC, structs.CarControlSP(), int(i * DT_CTRL * 1e9)) + + @staticmethod + def _decode(sends, addr): + dat = next(d for a, d, b in sends if a == addr) + name = "CAM_LKAS" if addr == 0x243 else "CAM_LANEINFO" + parser = CANParser("mazda_2017", [(name, 0)], 0) + parser.update([(0, [(addr, dat, 0)])]) + return parser.vl[name] + + def test_disengaged_emission_and_relay(self, ci): + _, cam_dat = self._feed_camera(ci) + CC = self._control() + steer_frames = 0 + steer_at = {} + hud_at = {} + for i in range(250): + _, sends = self._apply(ci, i, CC) + steer_frames += sum(1 for a, _, _ in sends if a == 0x243) + if any(a == 0x243 for a, _, _ in sends): + steer_at[i] = self._decode(sends, 0x243) + if any(a == 0x440 for a, _, _ in sends): + hud_at[i] = next(d for a, d, b in sends if a == 0x440) + + # 0x243 at 100 Hz with zero torque; the HUD relay fires on the first camera frame, + # then only the 2 Hz hold fires while the camera stays quiet + assert steer_frames == 250 + assert sorted(hud_at) == [0, 101, 151, 201] + + steer_vl = steer_at[100] + assert steer_vl["LKAS_REQUEST"] == 0 + for k, v in self.CAM_LKAS_VALUES.items(): + if k not in ("CTR", "LINE_NOT_VISIBLE"): + assert steer_vl[k] == v + # the camera says the line is not visible; the EPS must not be told that + assert steer_vl["LINE_NOT_VISIBLE"] == 0 + assert steer_vl["CTR"] == 100 % 16 + + # the camera's HUD frame reaches the dash byte for byte, its indicator state intact + for dat in hud_at.values(): + assert dat == cam_dat + + def test_engaged_emission_and_relay(self, ci): + cam_lkas_dat, cam_dat = self._feed_camera(ci) + CC = self._control(enabled=True, lat_active=True, torque=0.1) + steer_at = {} + steer_dat = {} + hud_at = {} + for i in range(250): + _, sends = self._apply(ci, i, CC) + if any(a == 0x243 for a, _, _ in sends): + steer_at[i] = self._decode(sends, 0x243) + steer_dat[i] = next(d for a, d, b in sends if a == 0x243) + if any(a == 0x440 for a, _, _ in sends): + hud_at[i] = next(d for a, d, b in sends if a == 0x440) + + # the HUD frame is the camera's with the indicator cleared and the lane + # display blanked: while openpilot steers quietly, neither the camera's "LAS applying + # torque" nag nor its lines belong on the dash + expected = bytearray(cam_dat) + expected[6] &= 0xFF ^ mazdacan.STEER_IND_B6 + expected[7] &= 0xFF ^ mazdacan.STEER_IND_B7 + expected[1] &= 0xFF ^ mazdacan.LANE_LINES_MASK_B1 + assert sorted(hud_at) == [0, 101, 151, 201] + for dat in hud_at.values(): + assert dat == bytes(expected) + + # the steering frame overlays the camera's: the counter continues the camera's + # sequence (camera CTR 5 -> ours starts at 6), torque flows, and every bit outside + # the write masks stays the camera's own -- LDW included + assert [steer_at[i]["CTR"] for i in range(6)] == [6, 7, 8, 9, 10, 11] + assert steer_at[100]["LKAS_REQUEST"] > 0 + for k, v in self.CAM_LKAS_VALUES.items(): + if k not in ("CTR", "LINE_NOT_VISIBLE"): + assert steer_at[100][k] == v + assert steer_at[100]["LINE_NOT_VISIBLE"] == 0 + out = steer_dat[100] + assert out[3] == cam_lkas_dat[3] + for i, mask in mazdacan.LKAS_WRITE_MASKS.items(): + assert out[i] & (0xFF ^ mask) == cam_lkas_dat[i] & (0xFF ^ mask) + + def test_engaged_steer_required_lights_the_steering_assist_indicator(self, ci): + cam_dat = self._feed_camera(ci)[1] + CC = self._control(enabled=True, lat_active=True, steer_required=True) + hud_at = {} + for i in range(250): + _, sends = self._apply(ci, i, CC) + if any(a == 0x440 for a, _, _ in sends): + hud_at[i] = next(d for a, d, b in sends if a == 0x440) + # the pre-branch channel is back: openpilot's hold-the-wheel alerts reach the dash + expected = bytearray(cam_dat) + expected[6] |= mazdacan.STEER_IND_B6 + expected[7] |= mazdacan.STEER_IND_B7 + for dat in hud_at.values(): + assert dat == bytes(expected) + + def test_new_camera_frame_relays_immediately(self, ci): + self._feed_camera(ci) + CC = self._control() + for i in range(10): + _, sends = self._apply(ci, i, CC) + if i > 0: + assert not any(a == 0x440 for a, _, _ in sends) + + # a fresh camera frame goes out on the very cycle it arrives + values = dict(self.CAM_LANEINFO_VALUES, LDW_WARN_RL=1) + dat = CANPacker("mazda_2017").make_can_msg("CAM_LANEINFO", 2, values)[1] + ci.update([(int(11 * DT_CTRL * 1e9), [(0x440, dat, 2)])]) + _, sends = self._apply(ci, 11, CC) + assert next(d for a, d, b in sends if a == 0x440) == dat + + def test_reengage_reseeds_the_counter(self, ci): + self._feed_camera(ci) + engaged = self._control(enabled=True, lat_active=True) + off = self._control() + ctrs = [] + for CC in (engaged, off, engaged): + for _ in range(5): + _, sends = self._apply(ci, 0, CC) + ctrs.append(self._decode(sends, 0x243)["CTR"]) + # camera CTR 5: every engage edge restarts our sequence at 6 + assert ctrs[0] == ctrs[10] == 6 + + def test_no_camera_frame_holds_the_zero_frame(self, ci): + for i in range(2): + ci.update([(int(i * DT_CTRL * 1e9), [])]) + CC = self._control(enabled=True, lat_active=True, steer_required=True) + hud_at = {} + for i in range(200): + _, sends = self._apply(ci, i, CC) + if any(a == 0x440 for a, _, _ in sends): + hud_at[i] = next(d for a, d, b in sends if a == 0x440) + # nothing from the camera: only the stale hold fires, zeros under the indicator bits + assert sorted(hud_at) == [100, 150] + assert hud_at[100] == bytes([0, 0, 0, 0, 0, 0, mazdacan.STEER_IND_B6, mazdacan.STEER_IND_B7]) + + class TestStandstillHold: @pytest.fixture diff --git a/opendbc/dbc/mazda_2017.dbc b/opendbc/dbc/mazda_2017.dbc index 0795eb466df..a879716be46 100644 --- a/opendbc/dbc/mazda_2017.dbc +++ b/opendbc/dbc/mazda_2017.dbc @@ -142,6 +142,8 @@ BO_ 579 CAM_LKAS: 8 XXX SG_ ERR_BIT_2 : 30|1@0+ (1,0) [0|1] "" XXX SG_ ANGLE_ENABLED : 52|1@0+ (1,0) [0|1] "" XXX SG_ STEERING_ANGLE : 33|12@0+ (1,-2048) [-2048|2047] "" XXX + SG_ FRAME_RAW_HI : 7|32@0+ (1,0) [0|4294967295] "" XXX + SG_ FRAME_RAW_LO : 39|32@0+ (1,0) [0|4294967295] "" XXX BO_ 580 CAM_DISTANCE: 8 XXX SG_ S1 : 0|8@1+ (1,0) [0|127] "" XXX @@ -207,6 +209,8 @@ BO_ 1088 CAM_LANEINFO: 8 XXX SG_ LDW_WARN_LL : 57|1@0+ (1,0) [0|1] "" XXX SG_ TJA : 38|3@0+ (1,0) [0|7] "" XXX SG_ TJA_TRANSITION : 27|2@0+ (1,0) [0|63] "" XXX + SG_ FRAME_RAW_HI : 7|32@0+ (1,0) [0|4294967295] "" XXX + SG_ FRAME_RAW_LO : 39|32@0+ (1,0) [0|4294967295] "" XXX BO_ 1479 NEW_MSG_470: 8 XXX @@ -728,6 +732,13 @@ CM_ SG_ 540 RADAR_LEAD_RELATIVE_DISTANCE "stop-and-go phase: 1 cruise, 2 follow, CM_ SG_ 605 PED_BRAKE "3: no brake, 4: brake"; CM_ SG_ 605 BRAKE_WARNING "Flashing brake warning and audible alert for potential forward collision"; CM_ SG_ 579 STEERING_ANGLE "steering angle aligns with 0.022 factor and -45.06 offset"; +CM_ SG_ 579 FRAME_RAW_HI "whole-frame capture, bytes 0-3; source for the byte-exact steering relay"; +CM_ SG_ 579 FRAME_RAW_LO "whole-frame capture, bytes 4-7; source for the byte-exact steering relay"; +CM_ SG_ 1088 FRAME_RAW_HI "whole-frame capture, bytes 0-3; source for the byte-exact HUD relay"; +CM_ SG_ 1088 FRAME_RAW_LO "whole-frame capture, bytes 4-7; source for the byte-exact HUD relay"; +CM_ SG_ 1088 HANDS_WARN_3_BITS "with the HANDS_ON_STEER_WARN bits, lights the steering-assist indicator: the orange wheel shown while the EPS applies corrective torque"; +CM_ SG_ 1088 HANDS_ON_STEER_WARN "steering-assist indicator, see HANDS_WARN_3_BITS"; +CM_ SG_ 1088 HANDS_ON_STEER_WARN_2 "steering-assist indicator, see HANDS_WARN_3_BITS"; CM_ SG_ 863 SPEED_SIGN "displayed speed limit, unit per SPEED_SIGN_ON"; CM_ SG_ 863 SPEED_SIGN_ON "0: no limit displayed, 1: limit displayed in MPH, 2: limit displayed in km/h"; CM_ SG_ 863 SPEED_SIGN_CAM "1: The speed limit is recognized by the camera. 0: speed limit is map based or is not available"; diff --git a/opendbc/safety/modes/mazda.h b/opendbc/safety/modes/mazda.h index e54a31fcdcd..07693468ae7 100644 --- a/opendbc/safety/modes/mazda.h +++ b/opendbc/safety/modes/mazda.h @@ -175,6 +175,12 @@ static bool mazda_tx_hook(const CANPacket_t *msg) { } } + // stay off the bus while the stock camera's frames are being relayed + if (main_bus && ((msg->addr == MAZDA_LKAS) || (msg->addr == MAZDA_LKAS_HUD)) && + !(controls_allowed || controls_allowed_lateral)) { + tx = false; + } + if (mazda_longitudinal && long_replacement_bus && (msg->addr == MAZDA_CRZ_INFO)) { // the stock standby pattern pegs the command field high; allow it byte-exactly // (checksum included) instead of decoding it as a huge accel command @@ -233,17 +239,30 @@ static bool mazda_tx_hook(const CANPacket_t *msg) { return tx; } +// LKAS and HUD frames reach the dash while openpilot isn't engaged +static bool mazda_fwd_hook(int bus_num, int addr) { + bool block_msg = false; + + if (bus_num == MAZDA_CAM) { + if ((addr == MAZDA_LKAS) || (addr == MAZDA_LKAS_HUD)) { + block_msg = controls_allowed || controls_allowed_lateral; + } + } + + return block_msg; +} + static safety_config mazda_init(uint16_t param) { static const CanMsg MAZDA_TX_MSGS[] = { - {MAZDA_LKAS, 0, 8, .check_relay = true}, + {MAZDA_LKAS, 0, 8, .check_relay = true, .disable_static_blocking = true}, {MAZDA_CRZ_BTNS, 0, 8, .check_relay = false}, - {MAZDA_LKAS_HUD, 0, 8, .check_relay = true}, + {MAZDA_LKAS_HUD, 0, 8, .check_relay = true, .disable_static_blocking = true}, }; static const CanMsg MAZDA_LONG_TX_MSGS[] = { - {MAZDA_LKAS, 0, 8, .check_relay = true}, + {MAZDA_LKAS, 0, 8, .check_relay = true, .disable_static_blocking = true}, {MAZDA_CRZ_BTNS, 0, 8, .check_relay = false}, - {MAZDA_LKAS_HUD, 0, 8, .check_relay = true}, + {MAZDA_LKAS_HUD, 0, 8, .check_relay = true, .disable_static_blocking = true}, {MAZDA_CRZ_INFO, 0, 8, .check_relay = false}, {MAZDA_CRZ_CTRL, 0, 8, .check_relay = false}, {MAZDA_RADAR_STATIC, 0, 8, .check_relay = false}, @@ -292,4 +311,5 @@ const safety_hooks mazda_hooks = { .init = mazda_init, .rx = mazda_rx_hook, .tx = mazda_tx_hook, + .fwd = mazda_fwd_hook, }; diff --git a/opendbc/safety/tests/common.py b/opendbc/safety/tests/common.py index 7dd39316192..6f2876c5ab8 100644 --- a/opendbc/safety/tests/common.py +++ b/opendbc/safety/tests/common.py @@ -234,6 +234,7 @@ class TorqueSteeringSafetyTestBase(SafetyTestBase, abc.ABC): MAX_RT_DELTA = 0 NO_STEER_REQ_BIT = False + DISENGAGED_IDLE_STEER_TX = True @classmethod def setUpClass(cls): @@ -284,7 +285,7 @@ def test_steer_safety_check(self): for t in range(int(-max_torque * 1.5), int(max_torque * 1.5)): self.safety.set_controls_allowed(enabled) self._set_prev_torque(t) - if abs(t) > max_torque or (not enabled and abs(t) > 0): + if abs(t) > max_torque or (not enabled and (abs(t) > 0 or not self.DISENGAGED_IDLE_STEER_TX)): self.assertFalse(self._tx(self._torque_cmd_msg(t))) else: self.assertTrue(self._tx(self._torque_cmd_msg(t))) @@ -566,7 +567,7 @@ def test_torque_absolute_limits(self): if controls_allowed: send = (-max_torque <= torque <= max_torque) else: - send = torque == 0 + send = torque == 0 and self.DISENGAGED_IDLE_STEER_TX self.assertEqual(send, self._tx(self._torque_cmd_msg(torque))) diff --git a/opendbc/safety/tests/test_mazda.py b/opendbc/safety/tests/test_mazda.py index be9c79bff68..3d040c66498 100755 --- a/opendbc/safety/tests/test_mazda.py +++ b/opendbc/safety/tests/test_mazda.py @@ -13,7 +13,7 @@ class TestMazdaSafety(common.CarSafetyTest, common.DriverTorqueSteeringSafetyTes TX_MSGS = [[0x243, 0], [0x09d, 0], [0x440, 0]] STANDSTILL_THRESHOLD = .1 RELAY_MALFUNCTION_ADDRS = {0: (0x243, 0x440)} - FWD_BLACKLISTED_ADDRS = {2: [0x243, 0x440]} + FWD_BLACKLISTED_ADDRS = {2: []} MAX_RATE_UP = 12 MAX_RATE_DOWN = 25 @@ -27,6 +27,9 @@ class TestMazdaSafety(common.CarSafetyTest, common.DriverTorqueSteeringSafetyTes # Mazda actually does not set any bit when requesting torque NO_STEER_REQ_BIT = True + # while onroad disengaged relay 0x243 and 0x440 for stock lane departure warnings + DISENGAGED_IDLE_STEER_TX = False + def setUp(self): self.packer = CANPackerSafety("mazda_2017") self.safety = libsafety_py.libsafety @@ -70,6 +73,10 @@ def _button_msg(self, resume=False, cancel=False): } return self.packer.make_can_msg_safety("CRZ_BTNS", 0, values) + def _hud_msg(self): + values = {"LANE_LINES": 2} + return self.packer.make_can_msg_safety("CAM_LANEINFO", 0, values) + def test_buttons(self): # only cancel allows while controls not allowed self.safety.set_controls_allowed(0) @@ -81,6 +88,22 @@ def test_buttons(self): self.assertTrue(self._tx(self._button_msg(cancel=True))) self.assertTrue(self._tx(self._button_msg(resume=True))) + def test_stock_relay(self): + # every engagement combination: the camera keeps the bus while neither leg + # is allowed; either leg takes the bus and puts ours on + for controls, lateral, cam_fwd, own_tx in ( + (0, 0, 0, False), # disengaged / neither active + (0, 1, -1, True), # lateral engaged + (1, 0, -1, True), # longitudinal engaged + (1, 1, -1, True), # both engaged + ): + self.safety.set_controls_allowed(controls) + self.safety.set_controls_allowed_lateral(lateral) + self.assertEqual(cam_fwd, self.safety.safety_fwd_hook(2, 0x243)) + self.assertEqual(cam_fwd, self.safety.safety_fwd_hook(2, 0x440)) + self.assertEqual(own_tx, self._tx(self._torque_cmd_msg(0))) + self.assertEqual(own_tx, self._tx(self._hud_msg())) + class TestMazdaLongitudinalSafety(TestMazdaSafety, common.LongitudinalAccelSafetyTest):