mazda: fix HBC under radar takeover

opendbc/car/mazda/carcontroller.py

@@ -312,7 +312,7 @@ def update_longitudinal(self, CC, CC_SP, CS):
                                                      stopping=sm.stop_bits, resume_unlatching=sm.resume_unlatching))
         can_sends.append(mazdacan.create_crz_ctrl(self.packer, bus, long_engaged, acc_available, gap,
                                                   self.lead_adv.has_lead, self.lead_adv.ctrl_phase,
-                                                  acc_active_2))
+                                                  acc_active_2, hbc_armed=CS.hbc_armed))
       self.long_counter += 1
 
     return can_sends

opendbc/car/mazda/carstate.py

@@ -53,6 +53,9 @@ def __init__(self, CP, CP_SP):
     # The camera's own TJA/CTS state from its 0x440: 0 off, 2 armed, 3 to 5 steering. Live,
     # never latched; 0 when the camera is stale.
     self.stock_tja = 0
+    # The camera's HBC arm state, 0x440 BIT2. The stock radar relays it to CRZ_CTRL bit 13 for
+    # the dash's green HBC light; under the radar takeover the controller relays it instead.
+    self.hbc_armed = False
     # Raised by the controller once per arming episode when its camera presses did not clear
     # stock_tja; consumed here into a stockLkas pulse.
     self.stock_cts_stuck = False
@@ -291,7 +294,7 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]:
       ret.cruiseState.enabled = self.cruise_enabled and not self.cruise_enabled_blocked
 
       # The FSC teardown gate requires fresh, settled CAM_LANEINFO without ERR_BIT. BIT2 is
-      # excluded because it may remain set for an entire ignition cycle.
+      # excluded: it is the auto high-beam arming bit and stays set for as long as HBC is armed.
       laneinfo = cp_cam.vl["CAM_LANEINFO"]
       settled = cam_laneinfo_fresh and not (laneinfo["NO_ERR_BIT"] or laneinfo["ERR_BIT"])
       self.fsc_settled_frames = self.fsc_settled_frames + 1 if settled else 0
@@ -334,6 +337,7 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]:
     self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"]
     ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
     self.stock_tja = int(self.cam_laneinfo["TJA"]) if cam_laneinfo_fresh else 0
+    self.hbc_armed = cam_laneinfo_fresh and self.cam_laneinfo["BIT2"] == 1
 
     # The camera stayed armed through the controller's presses: one pulse of stockLkas, which
     # the Mazda event hook turns into a one-shot warning. openpilot keeps steering; the panda

opendbc/car/mazda/mazdacan.py

@@ -6,18 +6,28 @@
 RADAR_STATIC_MSG = (0x499, bytes.fromhex("0008c00000000000"))
 RADAR_TRACK_MSGS = {
   0x361: bytes.fromhex("fff7fefe1fc00080"),
-  0x362: bytes.fromhex("fff7fefe1fc78c80"),
+  0x362: bytes.fromhex("fff7fefe1fc00080"),
   0x363: bytes.fromhex("fff7fefe1fc00000"),
   0x364: bytes.fromhex("fff7fefe1fc00000"),
-  0x365: bytes.fromhex("fff7fe7ffbff3fc0"),
-  0x366: bytes.fromhex("fff7fe7ffbff3fc0"),
 }
 LEAD_TRACK_ADDR = 0x364
 # Constant bytes for an occupied 0x364 track. create_lead_track replaces its measurements.
 LEAD_TRACK_TEMPLATE = bytes.fromhex("000e00001c000000")
 DIST_OBJ_SCALE = 0.0625   # m per bit, DIST_OBJ and RELV_OBJ share it
 DIST_OBJ_MAX = 255.875    # m, the full-scale DIST_OBJ reading a track can carry
 
+# The camera gates auto high beams on a living 5/6 object world: empty-only templates read as
+# a dead radar and HBC stops deciding under the takeover. The occupied 0x365 track, encoded
+# on the 0x364 field layout, sits at 175 m nominal (2x-uncalibrated scale, 87-350 m band),
+# far enough out to never suppress beams. It is camera-facing; the body bus carries the
+# empty templates stop-and-go needs.
+RADAR_TRACK_56_EMPTY = bytes.fromhex("fff7fe7ffbff3fc0")
+SYNTHETIC_TRACK_ADDRS = (0x365, 0x366)
+SYNTHETIC_TRACK_CYCLE = {
+  0x365: bytes.fromhex("af00a4001bff37c1"),
+  0x366: RADAR_TRACK_56_EMPTY,
+}
+
 # The G46L radar (2016.5 bodies) sends only this static frame and no track messages at
 # all, so the lead rides CRZ_CTRL alone; fully static — no counter, no checksum.
 G46L_RADAR_STATIC_MSG = (0x499, bytes.fromhex("0098400000000000"))
@@ -54,8 +64,11 @@ def create_acc_command(packer, bus, counter, accel, *, long_active, acc_availabl
   return packer.make_can_msg("CRZ_INFO", bus, values)
 
 
-def create_crz_ctrl(packer, bus, long_active, acc_available, gap_setting, radar_has_lead, stop_go_phase, acc_active_2):
+def create_crz_ctrl(packer, bus, long_active, acc_available, gap_setting, radar_has_lead, stop_go_phase, acc_active_2,
+                    *, hbc_armed=False):
   # CRZ_CTRL replaces radar cruise state and mirrors stop phase and driver gap selection.
+  # NEW_SIGNAL_3 (bit 13) mirrors the camera's HBC-arming bit (0x440 BIT2), matching stock;
+  # the dash's green HBC light follows it.
   values = {
     "MSG_1_INV": 1,
     "MSG_1_INV_COPY": 1,
@@ -66,6 +79,7 @@ def create_crz_ctrl(packer, bus, long_active, acc_available, gap_setting, radar_
     "RADAR_HAS_LEAD": int(radar_has_lead),
     "RADAR_LEAD_RELATIVE_DISTANCE": stop_go_phase,
     "ACC_ACTIVE_2": int(acc_active_2),
+    "NEW_SIGNAL_3": int(hbc_armed),
   }
   return packer.make_can_msg("CRZ_CTRL", bus, values)
 
@@ -86,6 +100,10 @@ def create_lead_track(d_rel: float, v_rel: float) -> bytes:
   return bytes(dat)
 
 
+# The FSC camera listens on bus 2; the body ECU rides bus 0 and needs the empty tracks.
+CAM_BUS = 2
+
+
 def create_radar_frames(bus, counter, lead, g46l=False):
   """lead is the (dRel, vRel) of the object to advertise on 0x364, or None for an empty slot."""
   if g46l:
@@ -95,6 +113,11 @@ def create_radar_frames(bus, counter, lead, g46l=False):
     if lead is not None and addr == LEAD_TRACK_ADDR:
       dat = create_lead_track(*lead)
     frames.append(CanData(addr, dat[:7] + bytes([(dat[7] & 0xf0) | (counter % 16)]), bus))
+  for addr in SYNTHETIC_TRACK_ADDRS:
+    cycle = SYNTHETIC_TRACK_CYCLE[addr] if bus == CAM_BUS else RADAR_TRACK_56_EMPTY
+    i = (counter % (len(cycle) // 8)) * 8
+    dat = cycle[i:i + 8]
+    frames.append(CanData(addr, dat[:7] + bytes([(dat[7] & 0xf0) | (counter % 16)]), bus))
   return frames
 
 

opendbc/car/mazda/tests/conftest.py

@@ -33,6 +33,7 @@
 CAM_LKAS = 0x243
 CAM_LANEINFO = 0x440
 LEAD_TRACK = 0x364
+SYNTHETIC_TRACK = 0x365
 RADAR_STATIC = 0x499
 RADAR_UDS = 0x764
 
@@ -138,7 +139,7 @@ def set_car_state(cs: CarState, out=None, *, brake_hold=False, stock_radar_alive
                   fsc_settled=True, radar_was_silenced=False, radar_session_refused=False, radar_session_response=0,
                   radar_bus_healthy=True, steer_undelivered=False,
                   lkas_blocked=False, lkas_effective=0, steer_first_engage_hold=False, lkas_allowed_speed=True, lkas_rejected=0,
-                  lkas_fault=False, crz_btns_counter=0, stock_tja=0,
+                  lkas_fault=False, crz_btns_counter=0, stock_tja=0, hbc_armed=False,
                   cancel_button=0, accel_button=0, decel_button=0, **out_kwargs) -> CarState:
   """Put the controller-facing state of a real CarState where a test wants it.
 
@@ -174,6 +175,7 @@ def set_car_state(cs: CarState, out=None, *, brake_hold=False, stock_radar_alive
   cs.lkas_fault = lkas_fault
   cs.crz_btns_counter = crz_btns_counter
   cs.stock_tja = stock_tja
+  cs.hbc_armed = hbc_armed
   cs.cancel_button = cancel_button
   cs.accel_button = accel_button
   cs.decel_button = decel_button

opendbc/car/mazda/tests/test_mazda_carstate.py

@@ -30,7 +30,7 @@
 # (GSH7-67XK2-U). Only byte 1 differs: bit 5 is BIT2, bit 6 is NO_ERR_BIT.
 BOOTING = bytes([0x42, 0b01000001, 0, 0, 0, 0, 0, 0])       # NO_ERR_BIT set: still booting
 SETTLED = bytes([0x42, 0b00000001, 0, 0, 0, 0, 0, 0])       # markers clear: settled
-BIT2_LATCHED = bytes([0x41, 0b00100001, 0, 0, 0, 0, 0, 0])  # BIT2 stuck high for a whole cycle
+BIT2_LATCHED = bytes([0x41, 0b00100001, 0, 0, 0, 0, 0, 0])  # BIT2 (HBC armed) high for a whole cycle
 FAULTED = bytes([0x42, 0b00000001, 0, 0, 0, 0x01, 0, 0])    # ERR_BIT (bit 40) set
 
 # Exercise CAM_LANEINFO at its longest measured period so freshness tests match the bus cadence.
@@ -103,6 +103,18 @@ def test_camera_dropout_resets_the_settle_timer(self):
     assert not feed_laneinfo(CI, SETTLED, SETTLE_T * 0.5)
     assert feed_laneinfo(CI, SETTLED, SETTLE_T * 0.6)
 
+  def test_hbc_arming_follows_bit2_while_fresh(self):
+    # BIT2 is the camera's auto high-beam arming, relayed to the cluster through CRZ_CTRL
+    CI = car_interface(alpha_long=True)
+    feed_laneinfo(CI, BIT2_LATCHED, 1.0)
+    assert CI.CS.hbc_armed
+    feed_laneinfo(CI, SETTLED, 1.0)
+    assert not CI.CS.hbc_armed
+    # a silent camera relays nothing
+    feed_laneinfo(CI, BIT2_LATCHED, 1.0)
+    feed_laneinfo(CI, None, CarControllerParams.CAM_LANEINFO_FRESH_T + 0.5)
+    assert not CI.CS.hbc_armed
+
   def test_gate_starts_closed_before_any_camera_frame(self):
     # the parser reads all-zero before the first frame, which would otherwise look settled
     CI = car_interface()
@@ -311,9 +323,7 @@ class TestSpeedSignLimit:
   1-bit SPEED_SIGN_ON at bit 12 is its low bit): 1 = limit displayed in mph, 2 = displayed in
   km/h, 0 = none. Which value an FSC emits tracks its market, not the cluster's unit setting.
   Payloads are real captures: mph frames from a US CX-5 2022 (drive_1x local set), km/h
-  frames from a NZ CX-5 (route
-  ded445e51c0e1830|00000007--4b5a89a1ce) where the old 1-bit decode at bit 12 read 0 and SLA
-  never saw a limit."""
+  frames from a NZ CX-5, where the old 1-bit decode at bit 12 read 0 and SLA never saw a limit."""
 
   @pytest.mark.parametrize("payload, expected_ms", [
     ("0000000002005300", 0.0),                 # no limit displayed

opendbc/car/mazda/tests/test_mazda_golden_tx.py

@@ -96,9 +96,10 @@ def _cancel(i, _):
 SCENARIO = [
   _phase("boot_stock_radar", 100, BOOT),
   _phase("fsc_settled_silencing", 120, dict(BOOT, fsc_settled=True)),
-  _phase("radar_silenced_armed_idle", 100, dict(BOOT, **SILENCED, available=True), lambda i, _: {"brake_pressed": i < 50}),
+  _phase("radar_silenced_armed_idle", 100, dict(BOOT, **SILENCED, available=True),
+         lambda i, _: {"brake_pressed": i < 50, "hbc_armed": 25 <= i < 75}),
   _phase("engage_steer_ramp", 220, dict(ENGAGED, **LEAD_30, v_ego=10.0, accel=1.0), _driver_fight),
-  _phase("highway_rail", 170, dict(ENGAGED, **NO_LEAD, v_ego=20.0, accel=0.2), _highway),
+  _phase("highway_rail", 170, dict(ENGAGED, **NO_LEAD, v_ego=20.0, accel=0.2, hbc_armed=True), _highway),
   _phase("approach_stop", 100, dict(ENGAGED, lead_visible=True, lead_d_rel=6.0, lead_v_rel=-1.0,
                                      long_state=LongCtrlState.stopping, accel=-1.5, torque=0.1), _approach),
   _phase("hold_on_the_plan", 150, dict(ENGAGED, **LEAD_4, long_state=LongCtrlState.stopping, accel=-1.024,

opendbc/car/mazda/tests/test_mazda_longitudinal.py

@@ -68,6 +68,17 @@ def test_engaged_frame_rates_and_counters(cc, cs):
   assert cc.long_counter == 50 and cc.radar_counter == 10
 
 
+@pytest.mark.parametrize("engaged", [False, True])
+def test_crz_ctrl_relays_hbc_arming_on_both_buses(cc, cs, engaged):
+  # Stock radar relays camera's HBC arming into CRZ_CTRL; synthetic radar must also carry the state.
+  kwargs = dict(accel=0.5) if engaged else dict(enabled=False, long_active=False, accel=0., long_state=OFF)
+  for armed in (False, True, True, False):
+    cc.frame = 0  # force emission
+    sends = step_long(cc, cs, hbc_armed=armed, available=True, **kwargs)
+    for bus in (0, 2):
+      assert parse_frame(CRZ_CTRL, frame(sends, CRZ_CTRL, bus), bus)["NEW_SIGNAL_3"] == armed
+
+
 @pytest.mark.parametrize("gap", [1, 2, 3])
 def test_gap_setting_mirrors_driver(cc, cs, gap):
   cc.frame = 0  # force emission on the first step

opendbc/car/mazda/tests/test_mazda_mazdacan.py

@@ -11,7 +11,7 @@
 
 from opendbc.car.mazda import mazdacan
 from opendbc.car.mazda.values import Buttons
-from opendbc.car.mazda.tests.conftest import CAM_LANEINFO, LEAD_TRACK, parse_frame
+from opendbc.car.mazda.tests.conftest import CAM_LANEINFO, LEAD_TRACK, SYNTHETIC_TRACK, parse_frame
 
 
 def crz_info_reference_checksum(dat):
@@ -123,18 +123,54 @@ def test_crz_ctrl_golden_bytes(packer, long_active, acc_available, gap, has_lead
   assert dat.hex() == expected
 
 
+@pytest.mark.parametrize("long_active, acc_available, gap, has_lead, phase, acc_active_2, expected", [
+  (False, False, 0, False, 0, False, "0221010000000000"),  # standby: the stock radar's frame
+  (False, True, 2, False, 0, False, "02210b0000000000"),   # MRCC armed
+  (True, True, 2, True, 1, True, "0a218b2000001000"),      # engaged
+])
+def test_crz_ctrl_relays_hbc_arming(packer, long_active, acc_available, gap, has_lead, phase, acc_active_2, expected):
+  dat = mazdacan.create_crz_ctrl(packer, 0, long_active, acc_available, gap, has_lead, phase, acc_active_2,
+                                 hbc_armed=True)[1]
+  assert dat.hex() == expected
+  bare = mazdacan.create_crz_ctrl(packer, 0, long_active, acc_available, gap, has_lead, phase, acc_active_2)[1]
+  assert bytes(a ^ b for a, b in zip(dat, bare, strict=True)) == bytes([0, 0x20, 0, 0, 0, 0, 0, 0])
+
+
 def test_radar_frames_match_stock():
   expected = [
     (0x499, "0008c00000000000"),
     (0x361, "fff7fefe1fc00080"),
-    (0x362, "fff7fefe1fc78c80"),
+    (0x362, "fff7fefe1fc00080"),
     (0x363, "fff7fefe1fc00000"),
     (0x364, "fff7fefe1fc00000"),
-    (0x365, "fff7fe7ffbff3fc0"),
-    (0x366, "fff7fe7ffbff3fc0"),
   ]
   frames = mazdacan.create_radar_frames(0, 0, None)
-  assert [(f.address, f.dat.hex()) for f in frames] == expected
+  # the static frame and the empty 1-4 tracks stay the stock capture; 5/6 open the cycle
+  assert [(f.address, f.dat.hex()) for f in frames[:5]] == expected
+  assert [f.address for f in frames[5:]] == [0x365, 0x366]
+
+
+def test_radar_frames_send_the_synthetic_object_on_the_camera_bus():
+  # Slot 5 holds the synthetic far object and slot 6 the empty template, with only the
+  # counter nibble moving; the body bus carries the empty templates for both slots.
+  for k in (0, 1, 7, 15, 16, 100):
+    for bus, cycle in ((2, mazdacan.SYNTHETIC_TRACK_CYCLE), (0, None)):
+      frames = {f.address: f.dat for f in mazdacan.create_radar_frames(bus, k, None)}
+      for addr in mazdacan.SYNTHETIC_TRACK_ADDRS:
+        want = cycle[addr] if cycle else mazdacan.RADAR_TRACK_56_EMPTY
+        assert frames[addr] == want[:7] + bytes([(want[7] & 0xf0) | (k % 16)])
+  assert mazdacan.SYNTHETIC_TRACK_CYCLE[0x365] == bytes.fromhex("af00a4001bff37c1")
+  assert mazdacan.SYNTHETIC_TRACK_CYCLE[0x366] == mazdacan.RADAR_TRACK_56_EMPTY
+
+
+def test_synthetic_track_decodes_to_a_far_static_object():
+  # The never-suppress claim is about the decoded fields, not the bytes: 175 m nominal
+  # (the 2x-uncalibrated scale puts the true range in 87-350 m), zero closing speed,
+  # 2.6 deg off bore.
+  vl = parse_frame(SYNTHETIC_TRACK, mazdacan.SYNTHETIC_TRACK_CYCLE[SYNTHETIC_TRACK])
+  assert vl["DIST_OBJ"] == pytest.approx(175.0)
+  assert vl["RELV_OBJ"] == 0.
+  assert vl["ANG_OBJ"] == pytest.approx(2.5625)
 
 
 def test_radar_frames_counter_and_lead_track():

opendbc/safety/modes/mazda.h

@@ -74,10 +74,7 @@ static bool mazda_empty_radar_track_msg_valid(const CANPacket_t *msg) {
             (msg->data[2] == 0xfeU) && (msg->data[3] == 0xfeU) &&
             (msg->data[4] == 0x1fU);
 
-    if (msg->addr == MAZDA_RADAR_TRACK_2) {
-      valid = valid && (msg->data[5] == 0xc7U) && (msg->data[6] == 0x8cU) &&
-              ((msg->data[7] & 0xf0U) == 0x80U);
-    } else if ((msg->addr == MAZDA_RADAR_TRACK_3) || (msg->addr == MAZDA_RADAR_TRACK_4)) {
+    if ((msg->addr == MAZDA_RADAR_TRACK_3) || (msg->addr == MAZDA_RADAR_TRACK_4)) {
       valid = valid && (msg->data[5] == 0xc0U) && (msg->data[6] == 0x00U) &&
               ((msg->data[7] & 0xf0U) == 0x00U);
     } else {
@@ -95,6 +92,48 @@ static bool mazda_empty_radar_track_msg_valid(const CANPacket_t *msg) {
   return valid;
 }
 
+// The only occupied 5/6 pattern is the synthetic far object in slot 5 (mazdacan.py
+// SYNTHETIC_TRACK_CYCLE); slot 6 sends the empty template. Occupied frames are camera-facing
+// only; the body bus carries the empty templates through the empty pattern below.
+#define MAZDA_SYNTHETIC_TRACK_FRAMES 1
+
+static bool mazda_synthetic_track_frame_match(uint64_t capture, const CANPacket_t *msg) {
+  // byte 7's low nibble carries the live counter; every other bit must be exact
+  uint64_t dat = 0U;
+  for (int i = 0; i < 8; i++) {
+    dat = (dat << 8) | msg->data[i];
+  }
+  return ((dat ^ capture) & 0xFFFFFFFFFFFFFFF0ULL) == 0U;
+}
+
+static bool mazda_synthetic_track_msg_valid(const CANPacket_t *msg) {
+  // The occupied replay is camera-facing only; the body bus carries the empty templates
+  // through the empty pattern above.
+  bool valid = false;
+  if (msg->bus == (unsigned char)MAZDA_CAM) {
+    // block scope: the tables have a single consumer (misra-c2012-8.9)
+    static const uint64_t mazda_synthetic_track_5[MAZDA_SYNTHETIC_TRACK_FRAMES] = {
+      0xaf00a4001bff37c1ULL,
+    };
+    static const uint64_t mazda_synthetic_track_6[MAZDA_SYNTHETIC_TRACK_FRAMES] = {
+      0xfff7fe7ffbff3fc0ULL,
+    };
+    const uint64_t *table = NULL;
+    if (msg->addr == MAZDA_RADAR_TRACK_5) {
+      table = mazda_synthetic_track_5;
+    } else if (msg->addr == MAZDA_RADAR_TRACK_6) {
+      table = mazda_synthetic_track_6;
+    } else {
+    }
+    if (table != NULL) {
+      for (int i = 0; i < MAZDA_SYNTHETIC_TRACK_FRAMES; i++) {
+        valid = valid || mazda_synthetic_track_frame_match(table[i], msg);
+      }
+    }
+  }
+  return valid;
+}
+
 static bool mazda_synthetic_lead_radar_track_msg_valid(const CANPacket_t *msg) {
   // Permit only the distance and relative-velocity fields in the occupied-track template.
   return (msg->addr == MAZDA_RADAR_TRACK_4) &&
@@ -106,7 +145,8 @@ static bool mazda_synthetic_lead_radar_track_msg_valid(const CANPacket_t *msg) {
 static bool mazda_radar_track_msg_valid(const CANPacket_t *msg) {
   // Occupied tracks represent perception and remain valid while controls are disengaged.
   return mazda_empty_radar_track_msg_valid(msg) ||
-         mazda_synthetic_lead_radar_track_msg_valid(msg);
+         mazda_synthetic_lead_radar_track_msg_valid(msg) ||
+         mazda_synthetic_track_msg_valid(msg);
 }
 
 // track msgs coming from OP so that we know what CAM msgs to drop and what to forward