From 1db28d09ec0ae15612b0d90a4c01f06c3d65d932 Mon Sep 17 00:00:00 2001 From: MazdaNick Date: Sat, 22 Aug 2026 02:53:42 -0400 Subject: [PATCH 01/12] mazda: pass the stock camera's LKAS and lane alerts through to the dash --- opendbc/car/mazda/carcontroller.py | 3 +- opendbc/car/mazda/mazdacan.py | 19 +- .../car/mazda/tests/test_mazda_carstate.py | 36 ++++ .../car/mazda/tests/test_mazda_controller.py | 176 +++++++++++++++++- opendbc/safety/modes/mazda.h | 28 ++- opendbc/safety/tests/common.py | 5 +- opendbc/safety/tests/test_mazda.py | 25 ++- 7 files changed, 265 insertions(+), 27 deletions(-) diff --git a/opendbc/car/mazda/carcontroller.py b/opendbc/car/mazda/carcontroller.py index d57c490eb6a..ed4d4f2d1fb 100644 --- a/opendbc/car/mazda/carcontroller.py +++ b/opendbc/car/mazda/carcontroller.py @@ -73,11 +73,10 @@ def update(self, CC, CC_SP, CS, now_nanos): # 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.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, steer_required)) # send steering command can_sends.append(mazdacan.create_steering_control(self.packer, self.CP, diff --git a/opendbc/car/mazda/mazdacan.py b/opendbc/car/mazda/mazdacan.py index a314bd847b5..5bdb7749c33 100644 --- a/opendbc/car/mazda/mazdacan.py +++ b/opendbc/car/mazda/mazdacan.py @@ -164,28 +164,13 @@ def create_steering_control(packer, CP, frame, apply_torque, lkas): 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", - ]} +def create_alert_command(packer, cam_msg: dict, steer_required: bool): + values = dict(cam_msg) 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) diff --git a/opendbc/car/mazda/tests/test_mazda_carstate.py b/opendbc/car/mazda/tests/test_mazda_carstate.py index d1f2ff75b7e..8379dbe1c64 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,41 @@ def test_gate_starts_closed_before_any_camera_frame(self): assert not CI.CS.fsc_settled +class TestCamRelaySources: + """CS.cam_lkas and CS.cam_laneinfo are the relay sources: the controller echoes them into + its own 0x243 and 0x440 frames, so the camera's values must survive the decode.""" + + @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 + + 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_decodes_the_camera_signals(self): + values = {"LANE_LINES": 2, "LDW_WARN_LL": 1, "LDW_WARN_RL": 0, "TJA": 3, + "TJA_TRANSITION": 1, "S1": 1, "S1_HBEAM": 1} + CI = _interface() + self._feed_cam(CI, CAM_LANEINFO, values) + for k, v in values.items(): + assert CI.CS.cam_laneinfo[k] == v + + 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..396b61e214f 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,180 @@ def test_lead_track_round_trips_through_the_dbc(self, d_rel, v_rel): assert dat[5:] == mazdacan.LEAD_TRACK_TEMPLATE[5:] +class TestAlertCommand: + """CAM_LANEINFO re-send: the camera's frame must reach the dash unchanged.""" + + @pytest.fixture + def packer(self): + return CANPacker("mazda_2017") + + @staticmethod + def _cam_laneinfo(packer, cam_msg, steer_required=False): + _, dat, _ = mazdacan.create_alert_command(packer, dict(cam_msg), steer_required) + parser = CANParser("mazda_2017", [("CAM_LANEINFO", 0)], 0) + parser.update([(0, [(0x440, dat, 0)])]) + return parser.vl["CAM_LANEINFO"] + + def test_every_camera_signal_relays(self, packer): + cam_msg = {s: 0 for s in ("LINE_VISIBLE", "LINE_NOT_VISIBLE", "LANE_LINES", "BIT1", "BIT2", + "BIT3", "NO_ERR_BIT", "ERR_BIT", "S1", "S1_HBEAM", + "LDW_WARN_LL", "LDW_WARN_RL", "TJA", "TJA_TRANSITION")} + cam_msg.update({"LANE_LINES": 2, "LDW_WARN_LL": 1, "LDW_WARN_RL": 0, + "TJA": 3, "TJA_TRANSITION": 2, "ERR_BIT": 1}) + vl = self._cam_laneinfo(packer, cam_msg) + for s in cam_msg: + assert vl[s] == cam_msg[s] + + def test_hands_override_keeps_camera_signals(self, packer): + vl = self._cam_laneinfo(packer, {"S1": 1, "LANE_LINES": 2}, steer_required=False) + assert vl["S1"] == 1 + assert vl["HANDS_WARN_3_BITS"] == 0 + + vl = self._cam_laneinfo(packer, {"S1": 1, "LANE_LINES": 2}, steer_required=True) + assert vl["S1"] == 1 + assert vl["HANDS_WARN_3_BITS"] == 0b111 + assert vl["HANDS_ON_STEER_WARN"] == 1 + assert vl["HANDS_ON_STEER_WARN_2"] == 1 + + +class TestSteeringCommand: + """CAM_LKAS re-send: the camera's health bits ride along on every steering command.""" + + @pytest.fixture + def packer(self): + return CANPacker("mazda_2017") + + @staticmethod + def _steer_msg(packer, lkas=None, frame=0, torque=0): + class FakeCP: + flags = MazdaFlags.GEN1 + lkas = {"BIT_1": 0, "ERR_BIT_1": 0, "ERR_BIT_2": 0} if lkas is None else lkas + _, dat, _ = mazdacan.create_steering_control(packer, FakeCP(), frame, 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} + vl = self._steer_msg(packer, lkas) + for k, v in lkas.items(): + assert vl[k] == v + assert vl["LKAS_REQUEST"] == 0 + + def test_counter_follows_the_frame(self, packer): + for frame in (0, 7, 15, 16, 33): + assert self._steer_msg(packer, frame=frame)["CTR"] == frame % 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 and its 0x440 + re-send every 50th frame regardless of engagement, relaying the camera's decoded values. + 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} + CAM_LANEINFO_VALUES = {"LANE_LINES": 2, "LDW_WARN_LL": 1, "LDW_WARN_RL": 0, "TJA": 3, + "TJA_TRANSITION": 1} + + @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])]) + + @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): + self._feed_camera(ci) + CC = self._control() + steer_frames = 0 + steer_at = {} + hud_at = {} + for i in range(200): + _, 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] = self._decode(sends, 0x440) + + # 0x243 at 100 Hz with zero torque; 0x440 at 2 Hz on the frame % 50 ticks + assert steer_frames == 200 + assert sorted(hud_at) == [0, 50, 100, 150] + + steer_vl = steer_at[100] + assert steer_vl["LKAS_REQUEST"] == 0 + for k, v in self.CAM_LKAS_VALUES.items(): + assert steer_vl[k] == v + + # the camera's lane frame reaches the dash unchanged, hands warnings dark + for vl in hud_at.values(): + for k, v in self.CAM_LANEINFO_VALUES.items(): + assert vl[k] == v + assert vl["HANDS_WARN_3_BITS"] == 0 + assert vl["HANDS_ON_STEER_WARN"] == 0 + assert vl["HANDS_ON_STEER_WARN_2"] == 0 + + def test_engaged_emission_and_relay(self, ci): + self._feed_camera(ci) + CC = self._control(enabled=True, lat_active=True, torque=0.1, steer_required=True) + steer_at = {} + hud_at = {} + for i in range(200): + _, sends = self._apply(ci, i, CC) + 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] = self._decode(sends, 0x440) + + assert sorted(hud_at) == [0, 50, 100, 150] + # torque flows once lateral is active + assert steer_at[100]["LKAS_REQUEST"] > 0 + for k, v in self.CAM_LKAS_VALUES.items(): + assert steer_at[100][k] == v + + # hands warnings on, and the camera's departure warnings survive the override + for vl in hud_at.values(): + assert vl["HANDS_WARN_3_BITS"] == 0b111 + assert vl["HANDS_ON_STEER_WARN"] == 1 + assert vl["HANDS_ON_STEER_WARN_2"] == 1 + assert vl["LDW_WARN_LL"] == 1 + assert vl["LDW_WARN_RL"] == 0 + + class TestStandstillHold: @pytest.fixture diff --git a/opendbc/safety/modes/mazda.h b/opendbc/safety/modes/mazda.h index e54a31fcdcd..57df9b118b1 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) { } } + // must stay off 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): From a7e5384b69bff5afc1d9da0317b6fc66fdffe6fd Mon Sep 17 00:00:00 2001 From: mzdnick Date: Sat, 22 Aug 2026 05:25:30 -0400 Subject: [PATCH 02/12] Update mazda.h --- opendbc/safety/modes/mazda.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc/safety/modes/mazda.h b/opendbc/safety/modes/mazda.h index 57df9b118b1..07693468ae7 100644 --- a/opendbc/safety/modes/mazda.h +++ b/opendbc/safety/modes/mazda.h @@ -175,7 +175,7 @@ static bool mazda_tx_hook(const CANPacket_t *msg) { } } - // must stay off bus while the stock camera's frames are being relayed + // 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; From f9f41c358dbd9f99727d67a9db76f0786cd4c953 Mon Sep 17 00:00:00 2001 From: MazdaNick Date: Sat, 22 Aug 2026 12:44:52 -0400 Subject: [PATCH 03/12] mazda: relay the camera's HUD frame byte-exact and continue its counter The rebuilt CAM_LANEINFO decoded and re-encoded the camera's frame through a DBC that describes 24 of its 64 bits, so 41 bits -- all of byte 2 among them -- went out as zeros whenever our frame was on the bus. The dash reads those bits, and on-device testing showed an intermittent front-camera fault and a missing engaged departure display. Relay the camera's exact bytes instead, on each new camera frame, with only the three hands-warn bits masked -- and only while steering. CAM_LKAS now carries the camera's LDW and LINE_NOT_VISIBLE bits so an engaged departure still reaches the dash, and its counter continues the camera's sequence at each engage edge instead of jumping by an arbitrary phase. --- opendbc/car/mazda/carcontroller.py | 39 +++- opendbc/car/mazda/carstate.py | 6 + opendbc/car/mazda/mazdacan.py | 38 ++-- .../car/mazda/tests/test_mazda_carstate.py | 9 + .../car/mazda/tests/test_mazda_controller.py | 199 ++++++++++++------ opendbc/dbc/mazda_2017.dbc | 2 + 6 files changed, 203 insertions(+), 90 deletions(-) diff --git a/opendbc/car/mazda/carcontroller.py b/opendbc/car/mazda/carcontroller.py index ed4d4f2d1fb..92b1555cf9f 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,6 +18,9 @@ # 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): @@ -32,6 +35,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_miss = 0 def update(self, CC, CC_SP, CS, now_nanos): can_sends = [] @@ -71,16 +78,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: - 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, steer_required)) - - # send steering command + # relay the camera's HUD frame the moment a new one lands, at the camera's own cadence; + # once the camera has been quiet past the stale window, hold the last frame at 2 Hz + 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 + cam_ts = CS.cam_laneinfo_ts + new_frame = cam_ts > 0 and cam_ts != self.last_laneinfo_ts + if new_frame or (self.laneinfo_miss > LANEINFO_STALE_FRAMES and self.frame % 50 == 0): + # not steering: the camera's own hands warning passes through untouched + hands = bool(steer_required) if CC.latActive else None + cam_raw = CS.cam_laneinfo_raw if cam_ts > 0 else None + can_sends.append(mazdacan.create_laneinfo_relay(cam_raw, hands)) + self.last_laneinfo_ts = cam_ts + self.laneinfo_miss = 0 if new_frame else self.laneinfo_miss + 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)) # 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..37a39391839 100644 --- a/opendbc/car/mazda/carstate.py +++ b/opendbc/car/mazda/carstate.py @@ -39,6 +39,8 @@ 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.fsc_settled_frames = 0 # the body ECU has taken the standstill hold over and is holding the brakes itself self.brake_hold = False @@ -211,6 +213,10 @@ 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 + arrival time: the HUD relay must carry bits the DBC doesn't describe + laneinfo = cp_cam.vl["CAM_LANEINFO"] + 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 5bdb7749c33..0798c61a12e 100644 --- a/opendbc/car/mazda/mazdacan.py +++ b/opendbc/car/mazda/mazdacan.py @@ -103,7 +103,7 @@ def create_radar_frames(bus, counter, lead): return frames -def create_steering_control(packer, CP, frame, apply_torque, lkas): +def create_steering_control(packer, CP, ctr, apply_torque, lkas): tmp = apply_torque + 2048 @@ -113,8 +113,8 @@ 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"]) - lnv = 0 - ldw = 0 + lnv = int(lkas["LINE_NOT_VISIBLE"]) + ldw = int(lkas["LDW"]) er2 = int(lkas["ERR_BIT_2"]) # Some older models do have these, newer models don't. @@ -128,7 +128,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) @@ -164,15 +164,27 @@ def create_steering_control(packer, CP, frame, apply_torque, lkas): return packer.make_can_msg("CAM_LKAS", 0, values) -def create_alert_command(packer, cam_msg: dict, steer_required: bool): - values = dict(cam_msg) - 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, - }) - return packer.make_can_msg("CAM_LANEINFO", 0, values) +CAM_LANEINFO_ADDR = 0x440 +# Hands-warn bits the controller owns; every other bit in the frame is the camera's. +# Byte positions match the packer mapping for this message, not naive DBC bit math. +HANDS_WARN_B6 = 0x0E # HANDS_WARN_3_BITS +HANDS_WARN_B7 = 0x09 # HANDS_ON_STEER_WARN | HANDS_ON_STEER_WARN_2 + + +def create_laneinfo_relay(cam_raw: int | None, steer_required: bool | None): + # Relays the camera's frame byte for byte, so bits the DBC does not describe (all of + # byte 2 among them) reach the dash exactly as sent. steer_required None means we are + # not steering and the camera's own hands warning passes through untouched. + # TODO: what's the difference between all these? do we need to send all? + dat = bytearray(8 if cam_raw is None else cam_raw.to_bytes(8, "big")) + if steer_required is not None: + if steer_required: + dat[6] |= HANDS_WARN_B6 + dat[7] |= HANDS_WARN_B7 + else: + dat[6] &= 0xFF ^ HANDS_WARN_B6 + dat[7] &= 0xFF ^ HANDS_WARN_B7 + 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 8379dbe1c64..03ae17a10ff 100644 --- a/opendbc/car/mazda/tests/test_mazda_carstate.py +++ b/opendbc/car/mazda/tests/test_mazda_carstate.py @@ -113,6 +113,15 @@ def test_cam_laneinfo_decodes_the_camera_signals(self): for k, v in values.items(): assert CI.CS.cam_laneinfo[k] == v + 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 diff --git a/opendbc/car/mazda/tests/test_mazda_controller.py b/opendbc/car/mazda/tests/test_mazda_controller.py index 396b61e214f..2b6b58802dd 100644 --- a/opendbc/car/mazda/tests/test_mazda_controller.py +++ b/opendbc/car/mazda/tests/test_mazda_controller.py @@ -186,69 +186,91 @@ def test_lead_track_round_trips_through_the_dbc(self, d_rel, v_rel): assert dat[5:] == mazdacan.LEAD_TRACK_TEMPLATE[5:] -class TestAlertCommand: - """CAM_LANEINFO re-send: the camera's frame must reach the dash unchanged.""" +class TestLaneinfoRelay: + """CAM_LANEINFO relay: the camera's frame reaches the dash byte for byte, and only the + hands-warn bits are ours.""" @pytest.fixture def packer(self): return CANPacker("mazda_2017") @staticmethod - def _cam_laneinfo(packer, cam_msg, steer_required=False): - _, dat, _ = mazdacan.create_alert_command(packer, dict(cam_msg), steer_required) + 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"), None) + assert relay.dat == bytes(cam_dat) + assert relay.address == 0x440 and relay.src == 0 + + def test_hands_override_touches_only_the_hands_bits(self, packer): + cam_dat = self._cam_dat(packer) + cam_dat[2] = 0xAB + raw = int.from_bytes(cam_dat, "big") + for steer_required in (True, False): + relay = mazdacan.create_laneinfo_relay(raw, steer_required) + assert relay.dat[:6] == bytes(cam_dat[:6]) + b6 = (cam_dat[6] | mazdacan.HANDS_WARN_B6) if steer_required else (cam_dat[6] & (0xFF ^ mazdacan.HANDS_WARN_B6)) + b7 = (cam_dat[7] | mazdacan.HANDS_WARN_B7) if steer_required else (cam_dat[7] & (0xFF ^ mazdacan.HANDS_WARN_B7)) + assert relay.dat[6] == b6 + assert relay.dat[7] == b7 + + def test_hands_masks_match_the_packer_mapping(self, packer): + # the masks must hit exactly the bits the packer assigns to the three 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.HANDS_WARN_B6), (7, mazdacan.HANDS_WARN_B7)] + + def test_no_camera_frame_sends_zeros(self): + relay = mazdacan.create_laneinfo_relay(None, True) + assert relay.dat == bytes([0, 0, 0, 0, 0, 0, mazdacan.HANDS_WARN_B6, mazdacan.HANDS_WARN_B7]) + + 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"), None) parser = CANParser("mazda_2017", [("CAM_LANEINFO", 0)], 0) - parser.update([(0, [(0x440, dat, 0)])]) - return parser.vl["CAM_LANEINFO"] - - def test_every_camera_signal_relays(self, packer): - cam_msg = {s: 0 for s in ("LINE_VISIBLE", "LINE_NOT_VISIBLE", "LANE_LINES", "BIT1", "BIT2", - "BIT3", "NO_ERR_BIT", "ERR_BIT", "S1", "S1_HBEAM", - "LDW_WARN_LL", "LDW_WARN_RL", "TJA", "TJA_TRANSITION")} - cam_msg.update({"LANE_LINES": 2, "LDW_WARN_LL": 1, "LDW_WARN_RL": 0, - "TJA": 3, "TJA_TRANSITION": 2, "ERR_BIT": 1}) - vl = self._cam_laneinfo(packer, cam_msg) - for s in cam_msg: - assert vl[s] == cam_msg[s] - - def test_hands_override_keeps_camera_signals(self, packer): - vl = self._cam_laneinfo(packer, {"S1": 1, "LANE_LINES": 2}, steer_required=False) - assert vl["S1"] == 1 - assert vl["HANDS_WARN_3_BITS"] == 0 - - vl = self._cam_laneinfo(packer, {"S1": 1, "LANE_LINES": 2}, steer_required=True) - assert vl["S1"] == 1 - assert vl["HANDS_WARN_3_BITS"] == 0b111 - assert vl["HANDS_ON_STEER_WARN"] == 1 - assert vl["HANDS_ON_STEER_WARN_2"] == 1 + 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 bits ride along on every steering command.""" + """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, frame=0, torque=0): + 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} if lkas is None else lkas - _, dat, _ = mazdacan.create_steering_control(packer, FakeCP(), frame, torque, dict(lkas)) + 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} + 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, v in lkas.items(): assert vl[k] == v assert vl["LKAS_REQUEST"] == 0 - def test_counter_follows_the_frame(self, packer): - for frame in (0, 7, 15, 16, 33): - assert self._steer_msg(packer, frame=frame)["CTR"] == frame % 16 + 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): @@ -256,13 +278,14 @@ def test_torque_round_trips(self, packer): class TestRelayEmission: - """Drives the real interface: the controller emits its own 0x243 every frame and its 0x440 - re-send every 50th frame regardless of engagement, relaying the camera's decoded values. - The panda, not the controller, yields the bus to the camera while disengaged.""" + """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} + CAM_LKAS_VALUES = {"BIT_1": 1, "ERR_BIT_1": 0, "ERR_BIT_2": 1, "LDW": 1, "LINE_NOT_VISIBLE": 0, "CTR": 5} CAM_LANEINFO_VALUES = {"LANE_LINES": 2, "LDW_WARN_LL": 1, "LDW_WARN_RL": 0, "TJA": 3, - "TJA_TRANSITION": 1} + "TJA_TRANSITION": 1, "HANDS_WARN_3_BITS": 0b101} @pytest.fixture def ci(self): @@ -279,6 +302,7 @@ def _feed_camera(self, ci): 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[1][1] @staticmethod def _control(enabled=False, lat_active=False, torque=0.0, steer_required=False): @@ -303,61 +327,102 @@ def _decode(sends, addr): return parser.vl[name] def test_disengaged_emission_and_relay(self, ci): - self._feed_camera(ci) + cam_dat = self._feed_camera(ci) CC = self._control() steer_frames = 0 steer_at = {} hud_at = {} - for i in range(200): + 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] = self._decode(sends, 0x440) + hud_at[i] = next(d for a, d, b in sends if a == 0x440) - # 0x243 at 100 Hz with zero torque; 0x440 at 2 Hz on the frame % 50 ticks - assert steer_frames == 200 - assert sorted(hud_at) == [0, 50, 100, 150] + # 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, 150, 200] steer_vl = steer_at[100] assert steer_vl["LKAS_REQUEST"] == 0 for k, v in self.CAM_LKAS_VALUES.items(): - assert steer_vl[k] == v + if k != "CTR": + assert steer_vl[k] == v + assert steer_vl["CTR"] == 100 % 16 - # the camera's lane frame reaches the dash unchanged, hands warnings dark - for vl in hud_at.values(): - for k, v in self.CAM_LANEINFO_VALUES.items(): - assert vl[k] == v - assert vl["HANDS_WARN_3_BITS"] == 0 - assert vl["HANDS_ON_STEER_WARN"] == 0 - assert vl["HANDS_ON_STEER_WARN_2"] == 0 + # the camera's HUD frame reaches the dash byte for byte, its hands warning intact + for dat in hud_at.values(): + assert dat == cam_dat def test_engaged_emission_and_relay(self, ci): - self._feed_camera(ci) + cam_dat = self._feed_camera(ci) CC = self._control(enabled=True, lat_active=True, torque=0.1, steer_required=True) steer_at = {} hud_at = {} - for i in range(200): + 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) if any(a == 0x440 for a, _, _ in sends): - hud_at[i] = self._decode(sends, 0x440) + hud_at[i] = next(d for a, d, b in sends if a == 0x440) + + # hands warnings on, every other bit exactly the camera's + expected = bytearray(cam_dat) + expected[6] |= mazdacan.HANDS_WARN_B6 + expected[7] |= mazdacan.HANDS_WARN_B7 + assert sorted(hud_at) == [0, 150, 200] + for dat in hud_at.values(): + assert dat == bytes(expected) - assert sorted(hud_at) == [0, 50, 100, 150] + # the counter continues the camera's sequence (camera CTR 5 -> ours starts at 6) and # torque flows once lateral is active + 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(): - assert steer_at[100][k] == v - - # hands warnings on, and the camera's departure warnings survive the override - for vl in hud_at.values(): - assert vl["HANDS_WARN_3_BITS"] == 0b111 - assert vl["HANDS_ON_STEER_WARN"] == 1 - assert vl["HANDS_ON_STEER_WARN_2"] == 1 - assert vl["LDW_WARN_LL"] == 1 - assert vl["LDW_WARN_RL"] == 0 + if k != "CTR": + assert steer_at[100][k] == v + + 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 our hands bits + assert sorted(hud_at) == [150] + assert hud_at[150] == bytes([0, 0, 0, 0, 0, 0, mazdacan.HANDS_WARN_B6, mazdacan.HANDS_WARN_B7]) class TestStandstillHold: diff --git a/opendbc/dbc/mazda_2017.dbc b/opendbc/dbc/mazda_2017.dbc index 0795eb466df..725c7251cc8 100644 --- a/opendbc/dbc/mazda_2017.dbc +++ b/opendbc/dbc/mazda_2017.dbc @@ -207,6 +207,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 From 341b303df058945bcb2bf25d2ff771d0782000c9 Mon Sep 17 00:00:00 2001 From: MazdaNick Date: Sat, 22 Aug 2026 13:14:32 -0400 Subject: [PATCH 04/12] mazda: rename laneinfo_miss to laneinfo_age_frames It counts controller frames since the last camera frame, not misses. --- opendbc/car/mazda/carcontroller.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/opendbc/car/mazda/carcontroller.py b/opendbc/car/mazda/carcontroller.py index 92b1555cf9f..67f1721dbc1 100644 --- a/opendbc/car/mazda/carcontroller.py +++ b/opendbc/car/mazda/carcontroller.py @@ -38,7 +38,7 @@ def __init__(self, dbc_names, CP, CP_SP): self.ctr_offset = 0 self.last_lat_active = False self.last_laneinfo_ts = None - self.laneinfo_miss = 0 + self.laneinfo_age_frames = 0 def update(self, CC, CC_SP, CS, now_nanos): can_sends = [] @@ -85,13 +85,13 @@ def update(self, CC, CC_SP, CS, now_nanos): steer_required = steer_required and CS.lkas_allowed_speed cam_ts = CS.cam_laneinfo_ts new_frame = cam_ts > 0 and cam_ts != self.last_laneinfo_ts - if new_frame or (self.laneinfo_miss > LANEINFO_STALE_FRAMES and self.frame % 50 == 0): + if new_frame or (self.laneinfo_age_frames > LANEINFO_STALE_FRAMES and self.frame % 50 == 0): # not steering: the camera's own hands warning passes through untouched hands = bool(steer_required) if CC.latActive else None cam_raw = CS.cam_laneinfo_raw if cam_ts > 0 else None can_sends.append(mazdacan.create_laneinfo_relay(cam_raw, hands)) self.last_laneinfo_ts = cam_ts - self.laneinfo_miss = 0 if new_frame else self.laneinfo_miss + 1 + 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: From f6108576994bac9e32241b9f7cf8110f9e03bd33 Mon Sep 17 00:00:00 2001 From: MazdaNick Date: Sat, 22 Aug 2026 16:30:59 -0400 Subject: [PATCH 05/12] mazda: overlay the camera's frame on 0x243 and let it own the hands warning Device testing of the byte-exact 0x440 relay still showed the v1 wrong-side signature engaged (right departure -> left flash, left -> nothing), so the dash reads departure-side bits from 0x243 as well. The engaged 0x243 is now an overlay on the camera's exact frame: openpilot writes only the torque field, the counter and the zero-angle pattern, adjusting the checksum by exactly the fields touched (a delta off the camera's own checksum, so bits outside the formula's model keep the camera's own contributions). Every other bit, defined or not, is the camera's. The dash hands warning is the camera's in both states now: relaying openpilot's steerRequired events painted an orange steering wheel for every wheel-touch and distraction alert. --- opendbc/car/mazda/carcontroller.py | 16 +-- opendbc/car/mazda/carstate.py | 8 +- opendbc/car/mazda/mazdacan.py | 45 +++++++- .../car/mazda/tests/test_mazda_carstate.py | 9 ++ .../car/mazda/tests/test_mazda_controller.py | 103 +++++++++++++++--- opendbc/dbc/mazda_2017.dbc | 2 + 6 files changed, 154 insertions(+), 29 deletions(-) diff --git a/opendbc/car/mazda/carcontroller.py b/opendbc/car/mazda/carcontroller.py index 67f1721dbc1..770ee67db67 100644 --- a/opendbc/car/mazda/carcontroller.py +++ b/opendbc/car/mazda/carcontroller.py @@ -11,7 +11,6 @@ from opendbc.sunnypilot.car.mazda.icbm import IntelligentCruiseButtonManagementInterface -VisualAlert = structs.CarControl.HUDControl.VisualAlert LongCtrlState = structs.CarControl.Actuators.LongControlState # Synthetic radar frames go to the car and to the camera; the panda only forwards @@ -20,8 +19,6 @@ # 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) @@ -79,17 +76,12 @@ def update(self, CC, CC_SP, CS, now_nanos): can_sends.extend(self.update_longitudinal(CC, CC_SP, CS)) # relay the camera's HUD frame the moment a new one lands, at the camera's own cadence; - # once the camera has been quiet past the stale window, hold the last frame at 2 Hz - 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 + # once the camera has been quiet past the stale window, hold the last frame at 2 Hz. + # the camera's own hands warning passes through untouched in both states 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.frame % 50 == 0): - # not steering: the camera's own hands warning passes through untouched - hands = bool(steer_required) if CC.latActive else None - cam_raw = CS.cam_laneinfo_raw if cam_ts > 0 else None - can_sends.append(mazdacan.create_laneinfo_relay(cam_raw, hands)) + can_sends.append(mazdacan.create_laneinfo_relay(CS.cam_laneinfo_raw if cam_ts > 0 else None)) self.last_laneinfo_ts = cam_ts self.laneinfo_age_frames = 0 if new_frame else self.laneinfo_age_frames + 1 @@ -99,7 +91,7 @@ def update(self, CC, CC_SP, CS, now_nanos): self.last_lat_active = CC.latActive can_sends.append(mazdacan.create_steering_control(self.packer, self.CP, self.frame + self.ctr_offset, - apply_torque, CS.cam_lkas)) + 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 37a39391839..18c6e9053ec 100644 --- a/opendbc/car/mazda/carstate.py +++ b/opendbc/car/mazda/carstate.py @@ -41,6 +41,7 @@ def __init__(self, CP, CP_SP): 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 @@ -213,7 +214,12 @@ 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 + arrival time: the HUD relay must carry bits the DBC doesn't describe + # 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 laneinfo = cp_cam.vl["CAM_LANEINFO"] 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"] diff --git a/opendbc/car/mazda/mazdacan.py b/opendbc/car/mazda/mazdacan.py index 0798c61a12e..5f4727bfeb1 100644 --- a/opendbc/car/mazda/mazdacan.py +++ b/opendbc/car/mazda/mazdacan.py @@ -103,7 +103,42 @@ def create_radar_frames(bus, counter, lead): return frames -def create_steering_control(packer, CP, ctr, 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. +# Every other bit is the camera's and rides through the overlay untouched. +LKAS_WRITE_MASKS = {0: 0xFF, 1: 0xFF, 4: 0x03, 5: 0xFF, 6: 0xD0} + + +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, ctr: int, apply_torque: int, lkas) -> bytes: + dat = bytearray(cam_raw.to_bytes(8, "big")) + tmp = (apply_torque + 2048) & 0xFFF + + # 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 -= (ctr % 16) - (dat[0] >> 4) + csum -= (tmp >> 8) - (dat[0] & 0x0F) + csum -= (tmp & 0xFF) - dat[1] + 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 @@ -161,7 +196,11 @@ def create_steering_control(packer, CP, ctr, apply_torque, lkas): "CHKSUM": csum } - return packer.make_can_msg("CAM_LKAS", 0, values) + if cam_raw is None: + return packer.make_can_msg("CAM_LKAS", 0, values) + # overlay: our 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, ctr, apply_torque, lkas), 0) CAM_LANEINFO_ADDR = 0x440 @@ -171,7 +210,7 @@ def create_steering_control(packer, CP, ctr, apply_torque, lkas): HANDS_WARN_B7 = 0x09 # HANDS_ON_STEER_WARN | HANDS_ON_STEER_WARN_2 -def create_laneinfo_relay(cam_raw: int | None, steer_required: bool | None): +def create_laneinfo_relay(cam_raw: int | None, steer_required: bool | None = None): # Relays the camera's frame byte for byte, so bits the DBC does not describe (all of # byte 2 among them) reach the dash exactly as sent. steer_required None means we are # not steering and the camera's own hands warning passes through untouched. diff --git a/opendbc/car/mazda/tests/test_mazda_carstate.py b/opendbc/car/mazda/tests/test_mazda_carstate.py index 03ae17a10ff..cdccf16190c 100644 --- a/opendbc/car/mazda/tests/test_mazda_carstate.py +++ b/opendbc/car/mazda/tests/test_mazda_carstate.py @@ -99,6 +99,15 @@ def test_cam_lkas_decodes_the_camera_bits(self): 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() diff --git a/opendbc/car/mazda/tests/test_mazda_controller.py b/opendbc/car/mazda/tests/test_mazda_controller.py index 2b6b58802dd..1dd8ec4b40b 100644 --- a/opendbc/car/mazda/tests/test_mazda_controller.py +++ b/opendbc/car/mazda/tests/test_mazda_controller.py @@ -186,6 +186,79 @@ 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; every other bit, defined or not, is the camera's.""" + + LKAS = {"BIT_1": 1, "ERR_BIT_1": 0, "ERR_BIT_2": 1, "LDW": 1, "LINE_NOT_VISIBLE": 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 + 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 + # the camera's defined bits we do not own also survive (LDW, LNV, ERR bits, BIT_1) + 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", "LINE_NOT_VISIBLE"): + assert vl[k] == self.LKAS[k] + assert vl["CTR"] == 9 + assert vl["LKAS_REQUEST"] == 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": 1, "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 << 7) - (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, and only the hands-warn bits are ours.""" @@ -302,7 +375,7 @@ def _feed_camera(self, ci): 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[1][1] + return msgs[0][1], msgs[1][1] @staticmethod def _control(enabled=False, lat_active=False, torque=0.0, steer_required=False): @@ -327,7 +400,7 @@ def _decode(sends, addr): return parser.vl[name] def test_disengaged_emission_and_relay(self, ci): - cam_dat = self._feed_camera(ci) + _, cam_dat = self._feed_camera(ci) CC = self._control() steer_frames = 0 steer_at = {} @@ -357,32 +430,36 @@ def test_disengaged_emission_and_relay(self, ci): assert dat == cam_dat def test_engaged_emission_and_relay(self, ci): - cam_dat = self._feed_camera(ci) - CC = self._control(enabled=True, lat_active=True, torque=0.1, steer_required=True) + 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) - # hands warnings on, every other bit exactly the camera's - expected = bytearray(cam_dat) - expected[6] |= mazdacan.HANDS_WARN_B6 - expected[7] |= mazdacan.HANDS_WARN_B7 + # the HUD frame is exactly the camera's, its hands warning included assert sorted(hud_at) == [0, 150, 200] for dat in hud_at.values(): - assert dat == bytes(expected) + assert dat == cam_dat - # the counter continues the camera's sequence (camera CTR 5 -> ours starts at 6) and - # torque flows once lateral is active + # 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 != "CTR": assert steer_at[100][k] == v + out = steer_dat[100] + assert out[2] == cam_lkas_dat[2] and 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_new_camera_frame_relays_immediately(self, ci): self._feed_camera(ci) @@ -420,9 +497,9 @@ def test_no_camera_frame_holds_the_zero_frame(self, ci): _, 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 our hands bits + # nothing from the camera: only the stale hold fires, and it is plain zeros assert sorted(hud_at) == [150] - assert hud_at[150] == bytes([0, 0, 0, 0, 0, 0, mazdacan.HANDS_WARN_B6, mazdacan.HANDS_WARN_B7]) + assert hud_at[150] == bytes(8) class TestStandstillHold: diff --git a/opendbc/dbc/mazda_2017.dbc b/opendbc/dbc/mazda_2017.dbc index 725c7251cc8..40609be7a7a 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 From 27753897f8ffeaf6ec85631b80855dfc5de6c554 Mon Sep 17 00:00:00 2001 From: MazdaNick Date: Sat, 22 Aug 2026 16:51:57 -0400 Subject: [PATCH 06/12] mazda: drop the unused hands-warn override and tidy the relay The steer_required parameter on the HUD relay was dead once the camera owned the hands warning; the masks, their tests and the stale comments went with it. cam_laneinfo was a dead write -- the FSC settle gate reads the parser directly -- and the overlay now derives the counter and torque for its checksum delta from the curated bytes instead of taking them as parameters. The FRAME_RAW container signals gain CM_ comments, since no other DBC uses that construct. --- opendbc/car/mazda/carstate.py | 5 +-- opendbc/car/mazda/mazdacan.py | 35 ++++++----------- .../car/mazda/tests/test_mazda_carstate.py | 13 ++----- .../car/mazda/tests/test_mazda_controller.py | 38 +++---------------- opendbc/dbc/mazda_2017.dbc | 4 ++ 5 files changed, 26 insertions(+), 69 deletions(-) diff --git a/opendbc/car/mazda/carstate.py b/opendbc/car/mazda/carstate.py index 18c6e9053ec..cfc2b92cf21 100644 --- a/opendbc/car/mazda/carstate.py +++ b/opendbc/car/mazda/carstate.py @@ -111,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. @@ -174,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: @@ -213,14 +214,12 @@ 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 - laneinfo = cp_cam.vl["CAM_LANEINFO"] 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 diff --git a/opendbc/car/mazda/mazdacan.py b/opendbc/car/mazda/mazdacan.py index 5f4727bfeb1..ac12bd4b41d 100644 --- a/opendbc/car/mazda/mazdacan.py +++ b/opendbc/car/mazda/mazdacan.py @@ -119,16 +119,16 @@ def _angle_checksum_terms(steering_angle: int, angle_enabled: int) -> int: return ahi + amd + alo + angle_enabled - (15 if ahi == 1 else 0) -def _overlay_steering_control(ours: bytes, cam_raw: int, ctr: int, apply_torque: int, lkas) -> bytes: +def _overlay_steering_control(ours: bytes, cam_raw: int, lkas) -> bytes: dat = bytearray(cam_raw.to_bytes(8, "big")) - tmp = (apply_torque + 2048) & 0xFFF # checksum delta over exactly the fields written below; the camera's own checksum - # already covers every other bit, defined or not + # already covers every other bit, defined or not. Counter and torque come out of the + # curated bytes: byte 0's nibbles and byte 1. csum = dat[7] - csum -= (ctr % 16) - (dat[0] >> 4) - csum -= (tmp >> 8) - (dat[0] & 0x0F) - csum -= (tmp & 0xFF) - dat[1] + csum -= (ours[0] >> 4) - (dat[0] >> 4) + csum -= (ours[0] & 0x0F) - (dat[0] & 0x0F) + csum -= ours[1] - dat[1] csum += _angle_checksum_terms(int(lkas["STEERING_ANGLE"]), int(lkas["ANGLE_ENABLED"])) csum -= _angle_checksum_terms(0, 0) @@ -200,30 +200,17 @@ def create_steering_control(packer, CP, ctr, apply_torque, lkas, cam_raw: int | return packer.make_can_msg("CAM_LKAS", 0, values) # overlay: our 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, ctr, apply_torque, lkas), 0) + return CanData(0x243, _overlay_steering_control(ours, cam_raw, lkas), 0) CAM_LANEINFO_ADDR = 0x440 -# Hands-warn bits the controller owns; every other bit in the frame is the camera's. -# Byte positions match the packer mapping for this message, not naive DBC bit math. -HANDS_WARN_B6 = 0x0E # HANDS_WARN_3_BITS -HANDS_WARN_B7 = 0x09 # HANDS_ON_STEER_WARN | HANDS_ON_STEER_WARN_2 -def create_laneinfo_relay(cam_raw: int | None, steer_required: bool | None = None): +def create_laneinfo_relay(cam_raw: int | None): # Relays the camera's frame byte for byte, so bits the DBC does not describe (all of - # byte 2 among them) reach the dash exactly as sent. steer_required None means we are - # not steering and the camera's own hands warning passes through untouched. - # TODO: what's the difference between all these? do we need to send all? - dat = bytearray(8 if cam_raw is None else cam_raw.to_bytes(8, "big")) - if steer_required is not None: - if steer_required: - dat[6] |= HANDS_WARN_B6 - dat[7] |= HANDS_WARN_B7 - else: - dat[6] &= 0xFF ^ HANDS_WARN_B6 - dat[7] &= 0xFF ^ HANDS_WARN_B7 - return CanData(CAM_LANEINFO_ADDR, bytes(dat), 0) + # byte 2 among them) reach the dash exactly as sent, hands warning included. + dat = bytes(8) if cam_raw is None else cam_raw.to_bytes(8, "big") + return CanData(CAM_LANEINFO_ADDR, 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 cdccf16190c..1e2e7bfdc15 100644 --- a/opendbc/car/mazda/tests/test_mazda_carstate.py +++ b/opendbc/car/mazda/tests/test_mazda_carstate.py @@ -80,8 +80,9 @@ def test_gate_starts_closed_before_any_camera_frame(self): class TestCamRelaySources: - """CS.cam_lkas and CS.cam_laneinfo are the relay sources: the controller echoes them into - its own 0x243 and 0x440 frames, so the camera's values must survive the decode.""" + """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): @@ -114,14 +115,6 @@ def test_steer_fault_follows_the_err_bit(self): 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_decodes_the_camera_signals(self): - values = {"LANE_LINES": 2, "LDW_WARN_LL": 1, "LDW_WARN_RL": 0, "TJA": 3, - "TJA_TRANSITION": 1, "S1": 1, "S1_HBEAM": 1} - CI = _interface() - self._feed_cam(CI, CAM_LANEINFO, values) - for k, v in values.items(): - assert CI.CS.cam_laneinfo[k] == v - 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]) diff --git a/opendbc/car/mazda/tests/test_mazda_controller.py b/opendbc/car/mazda/tests/test_mazda_controller.py index 1dd8ec4b40b..68287949b49 100644 --- a/opendbc/car/mazda/tests/test_mazda_controller.py +++ b/opendbc/car/mazda/tests/test_mazda_controller.py @@ -260,8 +260,8 @@ def test_write_masks_match_the_packer(self, packer): class TestLaneinfoRelay: - """CAM_LANEINFO relay: the camera's frame reaches the dash byte for byte, and only the - hands-warn bits are ours.""" + """CAM_LANEINFO relay: the camera's frame reaches the dash byte for byte, hands + warning included.""" @pytest.fixture def packer(self): @@ -277,39 +277,15 @@ def test_camera_frame_relays_byte_for_byte(self, packer): 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"), None) + 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_hands_override_touches_only_the_hands_bits(self, packer): - cam_dat = self._cam_dat(packer) - cam_dat[2] = 0xAB - raw = int.from_bytes(cam_dat, "big") - for steer_required in (True, False): - relay = mazdacan.create_laneinfo_relay(raw, steer_required) - assert relay.dat[:6] == bytes(cam_dat[:6]) - b6 = (cam_dat[6] | mazdacan.HANDS_WARN_B6) if steer_required else (cam_dat[6] & (0xFF ^ mazdacan.HANDS_WARN_B6)) - b7 = (cam_dat[7] | mazdacan.HANDS_WARN_B7) if steer_required else (cam_dat[7] & (0xFF ^ mazdacan.HANDS_WARN_B7)) - assert relay.dat[6] == b6 - assert relay.dat[7] == b7 - - def test_hands_masks_match_the_packer_mapping(self, packer): - # the masks must hit exactly the bits the packer assigns to the three 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.HANDS_WARN_B6), (7, mazdacan.HANDS_WARN_B7)] - - def test_no_camera_frame_sends_zeros(self): - relay = mazdacan.create_laneinfo_relay(None, True) - assert relay.dat == bytes([0, 0, 0, 0, 0, 0, mazdacan.HANDS_WARN_B6, mazdacan.HANDS_WARN_B7]) - 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"), None) + 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(): @@ -378,13 +354,11 @@ def _feed_camera(self, ci): return msgs[0][1], msgs[1][1] @staticmethod - def _control(enabled=False, lat_active=False, torque=0.0, steer_required=False): + def _control(enabled=False, lat_active=False, torque=0.0): 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() @@ -491,7 +465,7 @@ def test_reengage_reseeds_the_counter(self, ci): 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) + CC = self._control(enabled=True, lat_active=True) hud_at = {} for i in range(200): _, sends = self._apply(ci, i, CC) diff --git a/opendbc/dbc/mazda_2017.dbc b/opendbc/dbc/mazda_2017.dbc index 40609be7a7a..1ff9d3f3901 100644 --- a/opendbc/dbc/mazda_2017.dbc +++ b/opendbc/dbc/mazda_2017.dbc @@ -732,6 +732,10 @@ 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_ 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"; From 7e55dad57823591124ce662655fd58f5417ee143 Mon Sep 17 00:00:00 2001 From: MazdaNick Date: Sat, 22 Aug 2026 19:55:47 -0400 Subject: [PATCH 07/12] mazda: force LINE_NOT_VISIBLE off in the 0x243 overlay v4 on-device: engaged lane-departure warnings are correct on both sides, but openpilot could only steer while the camera saw lanes. The EPS gates torque on the camera's line-visibility state in 0x243, and the overlay relayed it -- the curated build had always forced it off ("they all work just fine if set to zero"). The overlay now clears the bit and the checksum delta pays for it; LDW and the undocumented side bits still ride through, so the dash alerts stay correct. The curated fallback returns to upstream's zeros for both bits. --- opendbc/car/mazda/mazdacan.py | 12 +++-- .../car/mazda/tests/test_mazda_controller.py | 44 +++++++++++++------ 2 files changed, 40 insertions(+), 16 deletions(-) diff --git a/opendbc/car/mazda/mazdacan.py b/opendbc/car/mazda/mazdacan.py index ac12bd4b41d..bfa94f444ab 100644 --- a/opendbc/car/mazda/mazdacan.py +++ b/opendbc/car/mazda/mazdacan.py @@ -105,8 +105,11 @@ def create_radar_frames(bus, counter, lead): # 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 the camera's line-visibility +# state, so relaying it left openpilot able to steer only when the camera saw lanes. # Every other bit is the camera's and rides through the overlay untouched. -LKAS_WRITE_MASKS = {0: 0xFF, 1: 0xFF, 4: 0x03, 5: 0xFF, 6: 0xD0} +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: @@ -129,6 +132,7 @@ def _overlay_steering_control(ours: bytes, cam_raw: int, lkas) -> bytes: 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) @@ -148,8 +152,10 @@ def create_steering_control(packer, CP, ctr, apply_torque, lkas, cam_raw: int | # copy values from camera b1 = int(lkas["BIT_1"]) er1 = int(lkas["ERR_BIT_1"]) - lnv = int(lkas["LINE_NOT_VISIBLE"]) - ldw = int(lkas["LDW"]) + # LINE_NOT_VISIBLE stays off and LDW is not ours to send here: the EPS gates torque on + # the visibility state, and the overlay carries the camera's alert bit from its raw bytes + lnv = 0 + ldw = 0 er2 = int(lkas["ERR_BIT_2"]) # Some older models do have these, newer models don't. diff --git a/opendbc/car/mazda/tests/test_mazda_controller.py b/opendbc/car/mazda/tests/test_mazda_controller.py index 68287949b49..2e47cafdb0c 100644 --- a/opendbc/car/mazda/tests/test_mazda_controller.py +++ b/opendbc/car/mazda/tests/test_mazda_controller.py @@ -188,9 +188,10 @@ def test_lead_track_round_trips_through_the_dbc(self, d_rel, v_rel): class TestSteeringOverlay: """The engaged 0x243 overlays the camera's exact frame: torque, counter and angle bits - are ours; every other bit, defined or not, is the camera's.""" + 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, "LDW": 1, "LINE_NOT_VISIBLE": 1, + 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} @@ -217,27 +218,37 @@ 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 - # the camera's defined bits we do not own also survive (LDW, LNV, ERR bits, BIT_1) 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", "LINE_NOT_VISIBLE"): - assert vl[k] == self.LKAS[k] + 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": 1, "LINE_NOT_VISIBLE": 1, "BIT_1": 1, "ERR_BIT_2": 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 << 7) - (1 << 4) - (1 << 5) + 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)) @@ -313,9 +324,13 @@ class FakeCP: 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, v in lkas.items(): - assert vl[k] == v + 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): @@ -332,7 +347,7 @@ class TestRelayEmission: 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": 0, "CTR": 5} + 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} @@ -395,8 +410,10 @@ def test_disengaged_emission_and_relay(self, ci): steer_vl = steer_at[100] assert steer_vl["LKAS_REQUEST"] == 0 for k, v in self.CAM_LKAS_VALUES.items(): - if k != "CTR": + 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 hands warning intact @@ -428,10 +445,11 @@ def test_engaged_emission_and_relay(self, ci): 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 != "CTR": + 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[2] == cam_lkas_dat[2] and out[3] == cam_lkas_dat[3] + 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) From f2c2f68173f57d51c1a8f0c3e369a1db81bacf19 Mon Sep 17 00:00:00 2001 From: MazdaNick Date: Sat, 22 Aug 2026 23:28:31 -0400 Subject: [PATCH 08/12] mazda: give the dash hands warning back to openpilot while it steers v5 on-device: steering and lane-departure warnings are correct in both states. Two gaps left. The camera's hands warning, relayed byte-exact while engaged, tracks "LAS applying torque" rather than the driver, so the orange wheel was on nearly whenever lane lines were drawn. And with the steerRequired mapping deleted, openpilot alerts -- the turn-limit warning above all -- lost their dash channel, which the stock setup had always given them. While openpilot steers, the three hands-warn bits now carry openpilot's hold-the-wheel alert: cleared when quiet, set when steerRequired is up (the pre-branch behavior). While not steering, the camera's own warning passes through untouched. --- opendbc/car/mazda/carcontroller.py | 11 +++- opendbc/car/mazda/mazdacan.py | 22 +++++-- .../car/mazda/tests/test_mazda_controller.py | 57 ++++++++++++++++--- 3 files changed, 76 insertions(+), 14 deletions(-) diff --git a/opendbc/car/mazda/carcontroller.py b/opendbc/car/mazda/carcontroller.py index 770ee67db67..fd09932732b 100644 --- a/opendbc/car/mazda/carcontroller.py +++ b/opendbc/car/mazda/carcontroller.py @@ -11,6 +11,7 @@ from opendbc.sunnypilot.car.mazda.icbm import IntelligentCruiseButtonManagementInterface +VisualAlert = structs.CarControl.HUDControl.VisualAlert LongCtrlState = structs.CarControl.Actuators.LongControlState # Synthetic radar frames go to the car and to the camera; the panda only forwards @@ -77,11 +78,17 @@ def update(self, CC, CC_SP, CS, now_nanos): # relay the camera's HUD frame the moment a new one lands, at the camera's own cadence; # once the camera has been quiet past the stale window, hold the last frame at 2 Hz. - # the camera's own hands warning passes through untouched in both states + # While openpilot steers, the hands-warn bits carry its hold-the-wheel alerts (the + # camera's own warning there tracks "LAS applying torque", not the driver); otherwise + # the camera's warning passes through untouched 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.frame % 50 == 0): - can_sends.append(mazdacan.create_laneinfo_relay(CS.cam_laneinfo_raw if cam_ts > 0 else None)) + hands = None + if CC.latActive: + steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired + hands = steer_required and CS.lkas_allowed_speed + can_sends.append(mazdacan.create_laneinfo_relay(CS.cam_laneinfo_raw if cam_ts > 0 else None, hands)) self.last_laneinfo_ts = cam_ts self.laneinfo_age_frames = 0 if new_frame else self.laneinfo_age_frames + 1 diff --git a/opendbc/car/mazda/mazdacan.py b/opendbc/car/mazda/mazdacan.py index bfa94f444ab..ddc3ac181fb 100644 --- a/opendbc/car/mazda/mazdacan.py +++ b/opendbc/car/mazda/mazdacan.py @@ -210,13 +210,27 @@ def create_steering_control(packer, CP, ctr, apply_torque, lkas, cam_raw: int | CAM_LANEINFO_ADDR = 0x440 +# Hands-warn bits, byte positions matching the packer mapping: HANDS_WARN_3_BITS sits in +# byte 6, the two single-bit warnings in byte 7 +HANDS_WARN_B6 = 0x0E +HANDS_WARN_B7 = 0x09 -def create_laneinfo_relay(cam_raw: int | None): +def create_laneinfo_relay(cam_raw: int | None, hands: bool | None = None): # Relays the camera's frame byte for byte, so bits the DBC does not describe (all of - # byte 2 among them) reach the dash exactly as sent, hands warning included. - dat = bytes(8) if cam_raw is None else cam_raw.to_bytes(8, "big") - return CanData(CAM_LANEINFO_ADDR, dat, 0) + # byte 2 among them) reach the dash exactly as sent. hands None passes the camera's own + # warning through; while openpilot steers, the camera's warning there tracks "LAS + # applying torque" rather than the driver, so the bits carry openpilot's hold-the-wheel + # alert instead. + dat = bytearray(8 if cam_raw is None else cam_raw.to_bytes(8, "big")) + if hands is not None: + if hands: + dat[6] |= HANDS_WARN_B6 + dat[7] |= HANDS_WARN_B7 + else: + dat[6] &= 0xFF ^ HANDS_WARN_B6 + dat[7] &= 0xFF ^ HANDS_WARN_B7 + 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_controller.py b/opendbc/car/mazda/tests/test_mazda_controller.py index 2e47cafdb0c..f6f54afc971 100644 --- a/opendbc/car/mazda/tests/test_mazda_controller.py +++ b/opendbc/car/mazda/tests/test_mazda_controller.py @@ -271,8 +271,8 @@ def test_write_masks_match_the_packer(self, packer): class TestLaneinfoRelay: - """CAM_LANEINFO relay: the camera's frame reaches the dash byte for byte, hands - warning included.""" + """CAM_LANEINFO relay: the camera's frame reaches the dash byte for byte; while + openpilot steers, only the hands-warn bits are its own.""" @pytest.fixture def packer(self): @@ -292,6 +292,26 @@ def test_camera_frame_relays_byte_for_byte(self, packer): assert relay.dat == bytes(cam_dat) assert relay.address == 0x440 and relay.src == 0 + def test_hands_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 hands in (True, False): + relay = mazdacan.create_laneinfo_relay(raw, hands) + assert relay.dat[:6] == bytes(cam_dat[:6]) + b6 = (cam_dat[6] | mazdacan.HANDS_WARN_B6) if hands else (cam_dat[6] & (0xFF ^ mazdacan.HANDS_WARN_B6)) + b7 = (cam_dat[7] | mazdacan.HANDS_WARN_B7) if hands else (cam_dat[7] & (0xFF ^ mazdacan.HANDS_WARN_B7)) + assert relay.dat[6] == b6 + assert relay.dat[7] == b7 + + def test_hands_masks_match_the_packer_mapping(self, packer): + # the masks must hit exactly the bits the packer assigns to the three 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.HANDS_WARN_B6), (7, mazdacan.HANDS_WARN_B7)] + 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} @@ -369,11 +389,13 @@ def _feed_camera(self, ci): return msgs[0][1], msgs[1][1] @staticmethod - def _control(enabled=False, lat_active=False, torque=0.0): + 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() @@ -434,10 +456,14 @@ def test_engaged_emission_and_relay(self, ci): 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 exactly the camera's, its hands warning included + # the HUD frame is the camera's with the hands warning suppressed: while openpilot + # steers without an alert of its own, the camera's "LAS applying torque" nag is off + expected = bytearray(cam_dat) + expected[6] &= 0xFF ^ mazdacan.HANDS_WARN_B6 + expected[7] &= 0xFF ^ mazdacan.HANDS_WARN_B7 assert sorted(hud_at) == [0, 150, 200] for dat in hud_at.values(): - assert dat == cam_dat + 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 @@ -453,6 +479,21 @@ def test_engaged_emission_and_relay(self, ci): 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_hands_warning(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.HANDS_WARN_B6 + expected[7] |= mazdacan.HANDS_WARN_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() @@ -483,15 +524,15 @@ def test_reengage_reseeds_the_counter(self, ci): 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) + 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, and it is plain zeros + # nothing from the camera: only the stale hold fires, zeros under our hands bits assert sorted(hud_at) == [150] - assert hud_at[150] == bytes(8) + assert hud_at[150] == bytes([0, 0, 0, 0, 0, 0, mazdacan.HANDS_WARN_B6, mazdacan.HANDS_WARN_B7]) class TestStandstillHold: From a45b90fe009aceec26c72834a32b4179c78a1b19 Mon Sep 17 00:00:00 2001 From: MazdaNick Date: Sat, 22 Aug 2026 23:53:25 -0400 Subject: [PATCH 09/12] mazda: blank the lane display while openpilot steers quietly The LAS visuals were a side effect of the passthrough: while engaged, the camera's lane lines drew on the dash and HUD alongside openpilot's own screen. Per the keep-the-alerts tradeoff, the relay now blanks LANE_LINES (LKAS disabled) whenever openpilot steers without an alert of its own, and relays the frame byte-exact while a hold-the-wheel alert is up -- the turn-limit warning keeps the rendering path the car already knows, lines and wheel together. Not steering, the camera's frame passes through untouched, flags-up windows included. --- opendbc/car/mazda/carcontroller.py | 11 +++++++---- opendbc/car/mazda/mazdacan.py | 9 +++++++-- .../car/mazda/tests/test_mazda_controller.py | 17 +++++++++++++++-- 3 files changed, 29 insertions(+), 8 deletions(-) diff --git a/opendbc/car/mazda/carcontroller.py b/opendbc/car/mazda/carcontroller.py index fd09932732b..817028a7754 100644 --- a/opendbc/car/mazda/carcontroller.py +++ b/opendbc/car/mazda/carcontroller.py @@ -78,9 +78,11 @@ def update(self, CC, CC_SP, CS, now_nanos): # relay the camera's HUD frame the moment a new one lands, at the camera's own cadence; # once the camera has been quiet past the stale window, hold the last frame at 2 Hz. - # While openpilot steers, the hands-warn bits carry its hold-the-wheel alerts (the - # camera's own warning there tracks "LAS applying torque", not the driver); otherwise - # the camera's warning passes through untouched + # While openpilot steers: the camera's hands warning there tracks "LAS applying + # torque" rather than the driver, so the bits carry openpilot's hold-the-wheel alert; + # the lane display is blanked while quiet and sent byte-exact during openpilot's own + # alerts, so those keep the rendering the car already knows. Not steering, the + # camera's frame passes through untouched 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.frame % 50 == 0): @@ -88,7 +90,8 @@ def update(self, CC, CC_SP, CS, now_nanos): if CC.latActive: steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired hands = steer_required and CS.lkas_allowed_speed - can_sends.append(mazdacan.create_laneinfo_relay(CS.cam_laneinfo_raw if cam_ts > 0 else None, hands)) + can_sends.append(mazdacan.create_laneinfo_relay(CS.cam_laneinfo_raw if cam_ts > 0 else None, + hands, hands is not None and not hands)) self.last_laneinfo_ts = cam_ts self.laneinfo_age_frames = 0 if new_frame else self.laneinfo_age_frames + 1 diff --git a/opendbc/car/mazda/mazdacan.py b/opendbc/car/mazda/mazdacan.py index ddc3ac181fb..aeabfc7bb18 100644 --- a/opendbc/car/mazda/mazdacan.py +++ b/opendbc/car/mazda/mazdacan.py @@ -214,14 +214,17 @@ def create_steering_control(packer, CP, ctr, apply_torque, lkas, cam_raw: int | # byte 6, the two single-bit warnings in byte 7 HANDS_WARN_B6 = 0x0E HANDS_WARN_B7 = 0x09 +LANE_LINES_MASK_B1 = 0x07 # LANE_LINES, 0 = LKAS disabled -def create_laneinfo_relay(cam_raw: int | None, hands: bool | None = None): +def create_laneinfo_relay(cam_raw: int | None, hands: bool | None = None, suppress_lines: bool = False): # Relays the camera's frame byte for byte, so bits the DBC does not describe (all of # byte 2 among them) reach the dash exactly as sent. hands None passes the camera's own # warning through; while openpilot steers, the camera's warning there tracks "LAS # applying torque" rather than the driver, so the bits carry openpilot's hold-the-wheel - # alert instead. + # alert instead. suppress_lines blanks the lane display (LANE_LINES = LKAS disabled) + # while openpilot steers quietly; the caller relays the frame untouched during + # openpilot's own alerts, so those keep the rendering the car already knows. dat = bytearray(8 if cam_raw is None else cam_raw.to_bytes(8, "big")) if hands is not None: if hands: @@ -230,6 +233,8 @@ def create_laneinfo_relay(cam_raw: int | None, hands: bool | None = None): else: dat[6] &= 0xFF ^ HANDS_WARN_B6 dat[7] &= 0xFF ^ HANDS_WARN_B7 + if suppress_lines: + dat[1] &= 0xFF ^ LANE_LINES_MASK_B1 return CanData(CAM_LANEINFO_ADDR, bytes(dat), 0) diff --git a/opendbc/car/mazda/tests/test_mazda_controller.py b/opendbc/car/mazda/tests/test_mazda_controller.py index f6f54afc971..70dc576a304 100644 --- a/opendbc/car/mazda/tests/test_mazda_controller.py +++ b/opendbc/car/mazda/tests/test_mazda_controller.py @@ -312,6 +312,17 @@ def test_hands_masks_match_the_packer_mapping(self, packer): diff = [(i, a ^ b) for i, (a, b) in enumerate(zip(dark, lit, strict=True)) if a != b] assert diff == [(6, mazdacan.HANDS_WARN_B6), (7, mazdacan.HANDS_WARN_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} @@ -456,11 +467,13 @@ def test_engaged_emission_and_relay(self, ci): 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 hands warning suppressed: while openpilot - # steers without an alert of its own, the camera's "LAS applying torque" nag is off + # the HUD frame is the camera's with the hands warning suppressed 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.HANDS_WARN_B6 expected[7] &= 0xFF ^ mazdacan.HANDS_WARN_B7 + expected[1] &= 0xFF ^ mazdacan.LANE_LINES_MASK_B1 assert sorted(hud_at) == [0, 150, 200] for dat in hud_at.values(): assert dat == bytes(expected) From 23f15f3aefb3df1f29e35543a7858d369e94fe41 Mon Sep 17 00:00:00 2001 From: MazdaNick Date: Sun, 23 Aug 2026 00:24:51 -0400 Subject: [PATCH 10/12] mazda: count the stale HUD hold from the camera's last frame The 2 Hz hold grid keyed off the controller frame counter, whose phase has no relation to when the camera went quiet. The first hold could land up to 0.5 s past the 1 s stale window, so the dash briefly lost its lane display on a camera dropout. Key the grid to laneinfo_age_frames instead: the first hold fires 1 s after the camera's last frame, then every 0.5 s. --- opendbc/car/mazda/carcontroller.py | 2 +- opendbc/car/mazda/tests/test_mazda_controller.py | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/opendbc/car/mazda/carcontroller.py b/opendbc/car/mazda/carcontroller.py index 817028a7754..1054072fded 100644 --- a/opendbc/car/mazda/carcontroller.py +++ b/opendbc/car/mazda/carcontroller.py @@ -85,7 +85,7 @@ def update(self, CC, CC_SP, CS, now_nanos): # camera's frame passes through untouched 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.frame % 50 == 0): + if new_frame or (self.laneinfo_age_frames >= LANEINFO_STALE_FRAMES and self.laneinfo_age_frames % 50 == 0): hands = None if CC.latActive: steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired diff --git a/opendbc/car/mazda/tests/test_mazda_controller.py b/opendbc/car/mazda/tests/test_mazda_controller.py index 70dc576a304..8c8f5162cd8 100644 --- a/opendbc/car/mazda/tests/test_mazda_controller.py +++ b/opendbc/car/mazda/tests/test_mazda_controller.py @@ -438,7 +438,7 @@ def test_disengaged_emission_and_relay(self, ci): # 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, 150, 200] + assert sorted(hud_at) == [0, 101, 151, 201] steer_vl = steer_at[100] assert steer_vl["LKAS_REQUEST"] == 0 @@ -474,7 +474,7 @@ def test_engaged_emission_and_relay(self, ci): expected[6] &= 0xFF ^ mazdacan.HANDS_WARN_B6 expected[7] &= 0xFF ^ mazdacan.HANDS_WARN_B7 expected[1] &= 0xFF ^ mazdacan.LANE_LINES_MASK_B1 - assert sorted(hud_at) == [0, 150, 200] + assert sorted(hud_at) == [0, 101, 151, 201] for dat in hud_at.values(): assert dat == bytes(expected) @@ -544,8 +544,8 @@ def test_no_camera_frame_holds_the_zero_frame(self, ci): 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 our hands bits - assert sorted(hud_at) == [150] - assert hud_at[150] == bytes([0, 0, 0, 0, 0, 0, mazdacan.HANDS_WARN_B6, mazdacan.HANDS_WARN_B7]) + assert sorted(hud_at) == [100, 150] + assert hud_at[100] == bytes([0, 0, 0, 0, 0, 0, mazdacan.HANDS_WARN_B6, mazdacan.HANDS_WARN_B7]) class TestStandstillHold: From 1aef617116345f2fff5bdad5861c5e7d4259d77b Mon Sep 17 00:00:00 2001 From: MazdaNick Date: Sun, 23 Aug 2026 00:40:53 -0400 Subject: [PATCH 11/12] mazda: trim the relay comments to the invariants Comment audit: 26 lines to 9. Each invariant is now stated once, at the place a reader needs it. The EPS-gates-torque fact appears once (at the write masks) instead of three times; the LAS-vs-driver story lives at the call site; fix history and caller behavior move out of mazdacan. No code change. --- opendbc/car/mazda/carcontroller.py | 9 ++------- opendbc/car/mazda/mazdacan.py | 20 +++++--------------- 2 files changed, 7 insertions(+), 22 deletions(-) diff --git a/opendbc/car/mazda/carcontroller.py b/opendbc/car/mazda/carcontroller.py index 1054072fded..f851b3c79b1 100644 --- a/opendbc/car/mazda/carcontroller.py +++ b/opendbc/car/mazda/carcontroller.py @@ -76,13 +76,8 @@ def update(self, CC, CC_SP, CS, now_nanos): if self.CP.openpilotLongitudinalControl: can_sends.extend(self.update_longitudinal(CC, CC_SP, CS)) - # relay the camera's HUD frame the moment a new one lands, at the camera's own cadence; - # once the camera has been quiet past the stale window, hold the last frame at 2 Hz. - # While openpilot steers: the camera's hands warning there tracks "LAS applying - # torque" rather than the driver, so the bits carry openpilot's hold-the-wheel alert; - # the lane display is blanked while quiet and sent byte-exact during openpilot's own - # alerts, so those keep the rendering the car already knows. Not steering, the - # camera's frame passes through untouched + # while openpilot steers, the camera's hands warning tracks "LAS applying torque" + # rather than the driver, so the relay carries openpilot's alert 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): diff --git a/opendbc/car/mazda/mazdacan.py b/opendbc/car/mazda/mazdacan.py index aeabfc7bb18..0d977f79091 100644 --- a/opendbc/car/mazda/mazdacan.py +++ b/opendbc/car/mazda/mazdacan.py @@ -105,9 +105,7 @@ def create_radar_frames(bus, counter, lead): # 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 the camera's line-visibility -# state, so relaying it left openpilot able to steer only when the camera saw lanes. -# Every other bit is the camera's and rides through the overlay untouched. +# 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 @@ -126,8 +124,7 @@ 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. Counter and torque come out of the - # curated bytes: byte 0's nibbles and byte 1. + # 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) @@ -152,8 +149,7 @@ def create_steering_control(packer, CP, ctr, apply_torque, lkas, cam_raw: int | # copy values from camera b1 = int(lkas["BIT_1"]) er1 = int(lkas["ERR_BIT_1"]) - # LINE_NOT_VISIBLE stays off and LDW is not ours to send here: the EPS gates torque on - # the visibility state, and the overlay carries the camera's alert bit from its raw bytes + # 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"]) @@ -204,7 +200,7 @@ def create_steering_control(packer, CP, ctr, apply_torque, lkas, cam_raw: int | if cam_raw is None: return packer.make_can_msg("CAM_LKAS", 0, values) - # overlay: our torque/counter/angle bits written into the camera's exact frame + # 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) @@ -218,13 +214,7 @@ def create_steering_control(packer, CP, ctr, apply_torque, lkas, cam_raw: int | def create_laneinfo_relay(cam_raw: int | None, hands: bool | None = None, suppress_lines: bool = False): - # Relays the camera's frame byte for byte, so bits the DBC does not describe (all of - # byte 2 among them) reach the dash exactly as sent. hands None passes the camera's own - # warning through; while openpilot steers, the camera's warning there tracks "LAS - # applying torque" rather than the driver, so the bits carry openpilot's hold-the-wheel - # alert instead. suppress_lines blanks the lane display (LANE_LINES = LKAS disabled) - # while openpilot steers quietly; the caller relays the frame untouched during - # openpilot's own alerts, so those keep the rendering the car already knows. + # byte-for-byte: bits the DBC does not describe must reach the dash as the camera sent them dat = bytearray(8 if cam_raw is None else cam_raw.to_bytes(8, "big")) if hands is not None: if hands: From b986c08045aaef5c74bca3f3c667e71f4135442e Mon Sep 17 00:00:00 2001 From: MazdaNick Date: Sun, 23 Aug 2026 00:59:01 -0400 Subject: [PATCH 12/12] mazda: name the steering-assist indicator what it is The orange steering wheel is not a hands-on-wheel icon: in stock it lights while the LAS corrects back to the lane, the EPS applying torque. The DBC's HANDS_* signal names misled the relay's naming and comments. The parameter becomes steer_indicator, the masks STEER_IND_B6/B7, the comments state the observed meaning, and the DBC signals carry CM_ notes recording it. Also picks up the stale-hold watchdog switching to the age counter and trimmed comments. --- opendbc/car/mazda/carcontroller.py | 11 +++--- opendbc/car/mazda/mazdacan.py | 27 +++++++------- .../car/mazda/tests/test_mazda_controller.py | 36 +++++++++---------- opendbc/dbc/mazda_2017.dbc | 3 ++ 4 files changed, 42 insertions(+), 35 deletions(-) diff --git a/opendbc/car/mazda/carcontroller.py b/opendbc/car/mazda/carcontroller.py index f851b3c79b1..4d839bd3af4 100644 --- a/opendbc/car/mazda/carcontroller.py +++ b/opendbc/car/mazda/carcontroller.py @@ -76,17 +76,18 @@ def update(self, CC, CC_SP, CS, now_nanos): if self.CP.openpilotLongitudinalControl: can_sends.extend(self.update_longitudinal(CC, CC_SP, CS)) - # while openpilot steers, the camera's hands warning tracks "LAS applying torque" - # rather than the driver, so the relay carries openpilot's alert and blanks the lines + # 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): - hands = None + steer_indicator = None if CC.latActive: steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired - hands = steer_required and CS.lkas_allowed_speed + 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, - hands, hands is not None and not hands)) + 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 diff --git a/opendbc/car/mazda/mazdacan.py b/opendbc/car/mazda/mazdacan.py index 0d977f79091..726a9daf054 100644 --- a/opendbc/car/mazda/mazdacan.py +++ b/opendbc/car/mazda/mazdacan.py @@ -206,23 +206,26 @@ def create_steering_control(packer, CP, ctr, apply_torque, lkas, cam_raw: int | CAM_LANEINFO_ADDR = 0x440 -# Hands-warn bits, byte positions matching the packer mapping: HANDS_WARN_3_BITS sits in -# byte 6, the two single-bit warnings in byte 7 -HANDS_WARN_B6 = 0x0E -HANDS_WARN_B7 = 0x09 +# 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, hands: 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 +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 hands is not None: - if hands: - dat[6] |= HANDS_WARN_B6 - dat[7] |= HANDS_WARN_B7 + if steer_indicator is not None: + if steer_indicator: + dat[6] |= STEER_IND_B6 + dat[7] |= STEER_IND_B7 else: - dat[6] &= 0xFF ^ HANDS_WARN_B6 - dat[7] &= 0xFF ^ HANDS_WARN_B7 + 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) diff --git a/opendbc/car/mazda/tests/test_mazda_controller.py b/opendbc/car/mazda/tests/test_mazda_controller.py index 8c8f5162cd8..9530a78faef 100644 --- a/opendbc/car/mazda/tests/test_mazda_controller.py +++ b/opendbc/car/mazda/tests/test_mazda_controller.py @@ -272,7 +272,7 @@ def test_write_masks_match_the_packer(self, packer): class TestLaneinfoRelay: """CAM_LANEINFO relay: the camera's frame reaches the dash byte for byte; while - openpilot steers, only the hands-warn bits are its own.""" + openpilot steers, only the steering-assist indicator bits are its own.""" @pytest.fixture def packer(self): @@ -292,25 +292,25 @@ def test_camera_frame_relays_byte_for_byte(self, packer): assert relay.dat == bytes(cam_dat) assert relay.address == 0x440 and relay.src == 0 - def test_hands_bits_set_and_clear_touch_nothing_else(self, packer): + 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 hands in (True, False): - relay = mazdacan.create_laneinfo_relay(raw, hands) + 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.HANDS_WARN_B6) if hands else (cam_dat[6] & (0xFF ^ mazdacan.HANDS_WARN_B6)) - b7 = (cam_dat[7] | mazdacan.HANDS_WARN_B7) if hands else (cam_dat[7] & (0xFF ^ mazdacan.HANDS_WARN_B7)) + 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_hands_masks_match_the_packer_mapping(self, packer): - # the masks must hit exactly the bits the packer assigns to the three hands signals + 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.HANDS_WARN_B6), (7, mazdacan.HANDS_WARN_B7)] + 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}) @@ -449,7 +449,7 @@ def test_disengaged_emission_and_relay(self, ci): 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 hands warning intact + # 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 @@ -467,12 +467,12 @@ def test_engaged_emission_and_relay(self, ci): 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 hands warning suppressed and the lane + # 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.HANDS_WARN_B6 - expected[7] &= 0xFF ^ mazdacan.HANDS_WARN_B7 + 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(): @@ -492,7 +492,7 @@ def test_engaged_emission_and_relay(self, ci): 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_hands_warning(self, ci): + 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 = {} @@ -502,8 +502,8 @@ def test_engaged_steer_required_lights_the_hands_warning(self, ci): 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.HANDS_WARN_B6 - expected[7] |= mazdacan.HANDS_WARN_B7 + expected[6] |= mazdacan.STEER_IND_B6 + expected[7] |= mazdacan.STEER_IND_B7 for dat in hud_at.values(): assert dat == bytes(expected) @@ -543,9 +543,9 @@ def test_no_camera_frame_holds_the_zero_frame(self, ci): _, 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 our hands bits + # 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.HANDS_WARN_B6, mazdacan.HANDS_WARN_B7]) + assert hud_at[100] == bytes([0, 0, 0, 0, 0, 0, mazdacan.STEER_IND_B6, mazdacan.STEER_IND_B7]) class TestStandstillHold: diff --git a/opendbc/dbc/mazda_2017.dbc b/opendbc/dbc/mazda_2017.dbc index 1ff9d3f3901..a879716be46 100644 --- a/opendbc/dbc/mazda_2017.dbc +++ b/opendbc/dbc/mazda_2017.dbc @@ -736,6 +736,9 @@ CM_ SG_ 579 FRAME_RAW_HI "whole-frame capture, bytes 0-3; source for the byte-ex 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";