🧪 mazda: relay stock LKAS and lane alerts

opendbc/car/mazda/carcontroller.py

@@ -1,7 +1,7 @@
 import numpy as np
 
 from opendbc.can import CANPacker
-from opendbc.car import Bus, make_tester_present_msg, rate_limit, structs, uds
+from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, rate_limit, structs, uds
 from opendbc.car.lateral import apply_driver_steer_torque_limits
 from opendbc.car.interfaces import CarControllerBase
 from opendbc.car.mazda import mazdacan
@@ -18,7 +18,8 @@
 # received frames between those buses, not our own transmissions.
 LONG_BUSES = (0, 2)
 
-
+# a quiet camera longer than this drops the HUD relay to the 2 Hz hold on the last frame
+LANEINFO_STALE_FRAMES = int(1.0 / DT_CTRL)
 class CarController(CarControllerBase, IntelligentCruiseButtonManagementInterface):
   def __init__(self, dbc_names, CP, CP_SP):
     CarControllerBase.__init__(self, dbc_names, CP, CP_SP)
@@ -32,6 +33,10 @@ def __init__(self, dbc_names, CP, CP_SP):
     self.radar_counter = 0
     self.radar_session = RadarSessionManager()
     self.accel_last = 0.
+    self.ctr_offset = 0
+    self.last_lat_active = False
+    self.last_laneinfo_ts = None
+    self.laneinfo_age_frames = 0
 
   def update(self, CC, CC_SP, CS, now_nanos):
     can_sends = []
@@ -71,17 +76,28 @@ def update(self, CC, CC_SP, CS, now_nanos):
     if self.CP.openpilotLongitudinalControl:
       can_sends.extend(self.update_longitudinal(CC, CC_SP, CS))
 
-    # send HUD alerts
-    if self.frame % 50 == 0:
-      ldw = CC.hudControl.visualAlert == VisualAlert.ldw
-      steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired
-      # TODO: find a way to silence audible warnings so we can add more hud alerts
-      steer_required = steer_required and CS.lkas_allowed_speed
-      can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required))
-
-    # send steering command
+    # while openpilot steers it drives the steering-assist indicator (the orange wheel
+    # stock lights while the EPS corrects) as its alert channel, and blanks the lines
+    cam_ts = CS.cam_laneinfo_ts
+    new_frame = cam_ts > 0 and cam_ts != self.last_laneinfo_ts
+    if new_frame or (self.laneinfo_age_frames >= LANEINFO_STALE_FRAMES and self.laneinfo_age_frames % 50 == 0):
+      steer_indicator = None
+      if CC.latActive:
+        steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired
+        steer_indicator = steer_required and CS.lkas_allowed_speed
+      can_sends.append(mazdacan.create_laneinfo_relay(CS.cam_laneinfo_raw if cam_ts > 0 else None,
+                                                      steer_indicator,
+                                                      steer_indicator is not None and not steer_indicator))
+      self.last_laneinfo_ts = cam_ts
+    self.laneinfo_age_frames = 0 if new_frame else self.laneinfo_age_frames + 1
+
+    # send steering command; the counter continues the camera's sequence across an engage
+    if CC.latActive and not self.last_lat_active:
+      self.ctr_offset = (int(CS.cam_lkas["CTR"]) + 1 - self.frame) % 16
+    self.last_lat_active = CC.latActive
     can_sends.append(mazdacan.create_steering_control(self.packer, self.CP,
-                                                      self.frame, apply_torque, CS.cam_lkas))
+                                                      self.frame + self.ctr_offset,
+                                                      apply_torque, CS.cam_lkas, CS.cam_lkas_raw))
 
     # Intelligent Cruise Button Management
     # Suppress ICBM CRZ_BTNS spam while cancel/resume are in flight or while the driver is

opendbc/car/mazda/carstate.py

@@ -39,6 +39,9 @@ def __init__(self, CP, CP_SP):
     self.radar_was_silenced = False
     self.cancel_context_frames = 0
     self.cam_laneinfo_seen = False
+    self.cam_laneinfo_raw = 0
+    self.cam_laneinfo_ts = 0
+    self.cam_lkas_raw = None
     self.fsc_settled_frames = 0
     # the body ECU has taken the standstill hold over and is holding the brakes itself
     self.brake_hold = False
@@ -108,6 +111,8 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]:
     else:
       self.lkas_allowed_speed = True
 
+    laneinfo = cp_cam.vl["CAM_LANEINFO"]
+
     if self.CP.openpilotLongitudinalControl:
       # The radar teardown silences the radar-owned CRZ_CTRL frame, so cruise state comes
       # from PEDALS: ACC_OFF means MRCC is armed but idle, ACC_ACTIVE means it is engaged.
@@ -171,7 +176,6 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]:
       # timer at zero, so the radar was never silenced and the two-master guard held
       # accFaulted for the entire drive with nothing to tell the driver why.
       self.cam_laneinfo_seen |= len(cp_cam.vl_all["CAM_LANEINFO"]["LANE_LINES"]) > 0
-      laneinfo = cp_cam.vl["CAM_LANEINFO"]
       settled = self.cam_laneinfo_seen and not any(laneinfo[s] for s in ("NO_ERR_BIT", "ERR_BIT"))
       self.fsc_settled_frames = self.fsc_settled_frames + 1 if settled else 0
     else:
@@ -210,7 +214,14 @@ def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]:
 
     # camera signals
     self.cam_lkas = cp_cam.vl["CAM_LKAS"]
-    self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"]
+    # exact frame bytes: both relays must carry bits the DBC doesn't describe
+    if cp_cam.ts_nanos["CAM_LKAS"]["CTR"] > 0:
+      lkas_raw = cp_cam.vl["CAM_LKAS"]
+      self.cam_lkas_raw = (int(lkas_raw["FRAME_RAW_HI"]) << 32) | int(lkas_raw["FRAME_RAW_LO"])
+    else:
+      self.cam_lkas_raw = None
+    self.cam_laneinfo_raw = (int(laneinfo["FRAME_RAW_HI"]) << 32) | int(laneinfo["FRAME_RAW_LO"])
+    self.cam_laneinfo_ts = cp_cam.ts_nanos["CAM_LANEINFO"]["FRAME_RAW_HI"]
     ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
 
     # cruise control button events: distance, inc, dec, resume, cancel, and main

opendbc/car/mazda/mazdacan.py

@@ -103,7 +103,43 @@ def create_radar_frames(bus, counter, lead):
   return frames
 
 
-def create_steering_control(packer, CP, frame, apply_torque, lkas):
+# CAM_LKAS bits the controller owns, probed against the packer: CTR owns byte 0's high
+# nibble, the torque field byte 0's low nibble plus byte 1, the angle fields bytes 4-6.
+# LINE_NOT_VISIBLE is forced off (the EPS gates torque on it); every other bit rides through.
+LKAS_WRITE_MASKS = {0: 0xFF, 1: 0xFF, 2: 0x08, 4: 0x03, 5: 0xFF, 6: 0xD0}
+LKAS_LNV_MASK_B2 = 0x08
+
+
+def _angle_checksum_terms(steering_angle: int, angle_enabled: int) -> int:
+  # the checksum contribution the curated formula assigns to the angle fields
+  tmp = steering_angle + 2048
+  ahi = tmp >> 10
+  amd = (tmp & 0x3FF) >> 2
+  amd = (amd >> 4) | ((amd & 0xF) << 4)
+  alo = (tmp & 0x3) << 2
+  return ahi + amd + alo + angle_enabled - (15 if ahi == 1 else 0)
+
+
+def _overlay_steering_control(ours: bytes, cam_raw: int, lkas) -> bytes:
+  dat = bytearray(cam_raw.to_bytes(8, "big"))
+
+  # checksum delta over exactly the fields written below; the camera's own checksum
+  # already covers every other bit, defined or not
+  csum = dat[7]
+  csum -= (ours[0] >> 4) - (dat[0] >> 4)
+  csum -= (ours[0] & 0x0F) - (dat[0] & 0x0F)
+  csum -= ours[1] - dat[1]
+  csum += dat[2] & LKAS_LNV_MASK_B2   # visibility bit forced off; removing it adds back
+  csum += _angle_checksum_terms(int(lkas["STEERING_ANGLE"]), int(lkas["ANGLE_ENABLED"]))
+  csum -= _angle_checksum_terms(0, 0)
+
+  for i, mask in LKAS_WRITE_MASKS.items():
+    dat[i] = (dat[i] & (0xFF ^ mask)) | (ours[i] & mask)
+  dat[7] = csum % 256
+  return bytes(dat)
+
+
+def create_steering_control(packer, CP, ctr, apply_torque, lkas, cam_raw: int | None = None):
 
   tmp = apply_torque + 2048
 
@@ -113,6 +149,7 @@ def create_steering_control(packer, CP, frame, apply_torque, lkas):
   # copy values from camera
   b1 = int(lkas["BIT_1"])
   er1 = int(lkas["ERR_BIT_1"])
+  # LDW stays zero: the overlay carries the camera's alert bit from its raw bytes
   lnv = 0
   ldw = 0
   er2 = int(lkas["ERR_BIT_2"])
@@ -128,7 +165,7 @@ def create_steering_control(packer, CP, frame, apply_torque, lkas):
   amd = (amd >> 4) | ((amd & 0xF) << 4)
   alo = (tmp & 0x3) << 2
 
-  ctr = frame % 16
+  ctr = ctr % 16
   # bytes:     [    1  ] [ 2 ] [             3               ]  [           4         ]
   csum = 249 - ctr - hi - lo - (lnv << 3) - er1 - (ldw << 7) - (er2 << 4) - (b1 << 5)
 
@@ -161,33 +198,37 @@ def create_steering_control(packer, CP, frame, apply_torque, lkas):
       "CHKSUM": csum
     }
 
-  return packer.make_can_msg("CAM_LKAS", 0, values)
-
-
-def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool):
-  values = {s: cam_msg[s] for s in [
-    "LINE_VISIBLE",
-    "LINE_NOT_VISIBLE",
-    "LANE_LINES",
-    "BIT1",
-    "BIT2",
-    "BIT3",
-    "NO_ERR_BIT",
-    "S1",
-    "S1_HBEAM",
-  ]}
-  values.update({
-    # TODO: what's the difference between all these? do we need to send all?
-    "HANDS_WARN_3_BITS": 0b111 if steer_required else 0,
-    "HANDS_ON_STEER_WARN": steer_required,
-    "HANDS_ON_STEER_WARN_2": steer_required,
-
-    # TODO: right lane works, left doesn't
-    # TODO: need to do something about L/R
-    "LDW_WARN_LL": 0,
-    "LDW_WARN_RL": 0,
-  })
-  return packer.make_can_msg("CAM_LANEINFO", 0, values)
+  if cam_raw is None:
+    return packer.make_can_msg("CAM_LKAS", 0, values)
+  # overlay: the controller's torque/counter/angle bits written into the camera's exact frame
+  ours = packer.make_can_msg("CAM_LKAS", 0, values)[1]
+  return CanData(0x243, _overlay_steering_control(ours, cam_raw, lkas), 0)
+
+
+CAM_LANEINFO_ADDR = 0x440
+# Steering-assist indicator bits: the orange wheel the dash lights while the EPS applies
+# corrective torque (the DBC's HANDS_* names are misleading). Byte positions match the
+# packer mapping: the three-bit field in byte 6, the single bits in byte 7
+STEER_IND_B6 = 0x0E
+STEER_IND_B7 = 0x09
+LANE_LINES_MASK_B1 = 0x07   # LANE_LINES, 0 = LKAS disabled
+
+
+def create_laneinfo_relay(cam_raw: int | None, steer_indicator: bool | None = None, suppress_lines: bool = False):
+  # byte-for-byte: bits the DBC does not describe must reach the dash as the camera sent
+  # them. steer_indicator None relays the camera's own indicator state, True/False light
+  # or clear it for openpilot's hold-the-wheel alerts
+  dat = bytearray(8 if cam_raw is None else cam_raw.to_bytes(8, "big"))
+  if steer_indicator is not None:
+    if steer_indicator:
+      dat[6] |= STEER_IND_B6
+      dat[7] |= STEER_IND_B7
+    else:
+      dat[6] &= 0xFF ^ STEER_IND_B6
+      dat[7] &= 0xFF ^ STEER_IND_B7
+  if suppress_lines:
+    dat[1] &= 0xFF ^ LANE_LINES_MASK_B1
+  return CanData(CAM_LANEINFO_ADDR, bytes(dat), 0)
 
 
 def create_button_cmd(packer, CP, counter, button):

opendbc/car/mazda/tests/test_mazda_carstate.py

@@ -6,6 +6,7 @@
 from opendbc.car.mazda.values import CAR, CarControllerParams
 
 CAM_LANEINFO = 0x440
+CAM_LKAS = 0x243
 
 # Real CAM_LANEINFO prefixes, captured on two CX-5 2022s running the same FSC firmware
 # (GSH7-67XK2-U). Only byte 1 differs: bit 5 is BIT2, bit 6 is NO_ERR_BIT.
@@ -78,6 +79,52 @@ def test_gate_starts_closed_before_any_camera_frame(self):
     assert not CI.CS.fsc_settled
 
 
+class TestCamRelaySources:
+  """CS.cam_lkas (decoded bits) and CS.cam_lkas_raw / CS.cam_laneinfo_raw (exact frame
+  bytes) are the relay sources: the controller overlays its steering command onto the
+  camera's 0x243 bytes and re-sends the 0x440 bytes verbatim."""
+
+  @staticmethod
+  def _feed_cam(CI, addr, values, frames=2):
+    # CANParser registers a message lazily on first access, so the first frame only arms it
+    from opendbc.can import CANPacker
+    packer = CANPacker("mazda_2017")
+    msg = packer.make_can_msg("CAM_LKAS" if addr == CAM_LKAS else "CAM_LANEINFO", 2, values)
+    for i in range(frames):
+      CI.update([(int(i * DT_CTRL * 1e9), [(addr, msg[1], 2)])])
+
+  def test_cam_lkas_decodes_the_camera_bits(self):
+    values = {"BIT_1": 1, "ERR_BIT_1": 1, "ERR_BIT_2": 1, "LDW": 1, "LINE_NOT_VISIBLE": 1}
+    CI = _interface()
+    self._feed_cam(CI, CAM_LKAS, values)
+    for k, v in values.items():
+      assert CI.CS.cam_lkas[k] == v
+    assert CI.CS.out.steerFaultPermanent
+    assert CI.CS.cam_lkas_raw is not None
+
+  def test_cam_lkas_raw_carries_undefined_bits(self):
+    # byte 2's bits 1,2,4,5,6 and byte 3's low five bits carry no DBC signal
+    payload = bytes([0x35, 0xC7, 0xF6, 0x3B, 0xA8, 0x51, 0x7E, 0x42])
+    CI = _interface()
+    for i in range(2):
+      CI.update([(int(i * DT_CTRL * 1e9), [(CAM_LKAS, payload, 2)])])
+    assert CI.CS.cam_lkas_raw == int.from_bytes(payload, "big")
+
+  def test_steer_fault_follows_the_err_bit(self):
+    CI = _interface()
+    self._feed_cam(CI, CAM_LKAS, {"BIT_1": 1, "ERR_BIT_1": 0, "ERR_BIT_2": 1})
+    assert not CI.CS.out.steerFaultPermanent
+
+  def test_cam_laneinfo_raw_carries_undefined_bits(self):
+    # bytes 2 and 5 carry no DBC signal at all, but the dash reads bits there
+    payload = bytes([0x42, 0x41, 0xAB, 0x00, 0x00, 0xCD, 0x71, 0x3C])
+    CI = _interface()
+    for i in range(2):
+      CI.update([(int(i * DT_CTRL * 1e9), [(CAM_LANEINFO, payload, 2)])])
+    assert CI.CS.cam_laneinfo_raw == int.from_bytes(payload, "big")
+    assert CI.CS.cam_laneinfo_ts > 0
+
+
 class TestBrakeHold:
   """GEAR.BRAKE_HOLD is the body ECU reporting that it owns the standstill hold. Stock relaxes
   its own command the instant this sets, so the payloads below come straight off the two logs