Restore Mazda camera stop-sign observations

opendbc_repo/opendbc/car/mazda/carstate.py

@@ -2,11 +2,29 @@ from opendbc.can import CANDefine, CANParser
 from opendbc.car import Bus, create_button_events, structs
 from opendbc.car.common.conversions import Conversions as CV
 from opendbc.car.interfaces import CarStateBase
-from opendbc.car.mazda.values import DBC, LKAS_LIMITS
+from opendbc.car.mazda.values import CAR, DBC, LKAS_LIMITS, MazdaFlags
 from opendbc.car.mazda.stock_aol import qualified
 
 ButtonType = structs.CarState.ButtonEvent.Type
 
+DASHBOARD_CARS = frozenset(CAR)
+
+
+class DashboardCameraParser(CANParser):
+  def __init__(self, dbc_name):
+    super().__init__(dbc_name, [], 2)
+    self.dashboard_parser = CANParser(dbc_name, [("CAM_TRAFFIC_SIGNS", float('nan'))], 2)
+
+  def update(self, strings, sendcan=False):
+    packets = strings
+    if packets and not isinstance(packets[0], list | tuple):
+      packets = [packets]
+    result = super().update(packets, sendcan)
+    admitted = [(stamp, [(address, data, bus) for address, data, bus in frames
+                         if address == 0x35F and bus == 2 and len(data) == 8]) for stamp, frames in packets]
+    self.dashboard_parser.update(admitted, sendcan)
+    return result
+
 
 class CarState(CarStateBase):
   def __init__(self, CP):
@@ -23,6 +41,7 @@ class CarState(CarStateBase):
     self.accel_button = 0
     self.decel_button = 0
     self.cancel_button = 0
+    self.dashboard_stop_sign = None
 
   def update(self, can_parsers) -> structs.CarState:
     cp = can_parsers[Bus.pt]
@@ -128,11 +147,21 @@ class CarState(CarStateBase):
       *create_button_events(self.decel_button, prev_decel_button, {1: ButtonType.decelCruise}),
     ]
 
+    self.dashboard_stop_sign = None
+    if isinstance(cp_cam, DashboardCameraParser):
+      dashboard = cp_cam.dashboard_parser
+      stamp = dashboard.ts_nanos["CAM_TRAFFIC_SIGNS"]["STOP_SIGN"]
+      if stamp > 0:
+        self.dashboard_stop_sign = (dashboard.vl["CAM_TRAFFIC_SIGNS"]["STOP_SIGN"] == 9, stamp)
+
     return ret
 
   @staticmethod
   def get_can_parsers(CP):
     return {
       Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 0),
-      Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2),
+      Bus.cam: (DashboardCameraParser(DBC[CP.carFingerprint][Bus.pt])
+                if CP.brand == 'mazda' and CP.carFingerprint in DASHBOARD_CARS and
+                int(CP.flags) == int(MazdaFlags.GEN1) and not CP.notCar
+                else CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2)),
     }

opendbc_repo/opendbc/car/mazda/tests/test_dashboard_stop_sign.py

@@ -0,0 +1,118 @@
+"""Factory/parser/Card regressions for root's coordinated native runtime."""
+from types import SimpleNamespace
+import pytest
+from opendbc.can import CANPacker
+from opendbc.car import Bus, gen_empty_fingerprint
+from opendbc.car.mazda.interface import CarInterface
+from opendbc.car.mazda.values import CAR, DBC
+from opendbc.car.mazda.tests.test_stock_startup import complete_frames, exercise_startup
+from openpilot.starpilot.car.mazda import dashboard
+
+SUPPORTED = (CAR.MAZDA_CX5_2022, CAR.MAZDA_CX9_2021)
+
+
+@pytest.mark.parametrize('identity', list(CAR))
+@pytest.mark.parametrize('alpha,release', ((False, False), (True, False), (False, True), (True, True)))
+def test_all_factory_variants_preserve_stock_ownership(identity, alpha, release):
+  cp = CarInterface.get_params(identity, gen_empty_fingerprint(), [], alpha, release, False)
+  assert cp.dashcamOnly is (identity not in SUPPORTED)
+  assert cp.pcmCruise and not cp.openpilotLongitudinalControl and cp.mass > 1000
+  assert cp.steerActuatorDelay == pytest.approx(0.1) and cp.steerLimitTimer == pytest.approx(0.8)
+  assert cp.safetyConfigs[0].safetyParam == 0 and len(cp.safetyConfigs) == 1
+  assert type(CarInterface(cp).CC).__module__ == 'opendbc.car.mazda.carcontroller'
+
+
+@pytest.mark.parametrize('identity', SUPPORTED)
+@pytest.mark.parametrize('aol', (False, True))
+def test_actual_card_composes_original_camera_observation(identity, aol, monkeypatch):
+  from openpilot.cereal import messaging
+
+  def probe(card, ci, controls, saved):
+    mono, offset = [100_000_000_000], 2_000_000_000
+    monkeypatch.setattr(dashboard.time, 'monotonic_ns', lambda: mono[0])
+    monkeypatch.setattr(dashboard.time, 'clock_gettime_ns', lambda clock: mono[0] + offset)
+    create = messaging.new_message
+    def new_message(service=None, size=None, **kwargs):
+      kwargs.setdefault('logMonoTime', mono[0])
+      return create(service, size, **kwargs)
+    monkeypatch.setattr(messaging, 'new_message', new_message)
+    sent, send = [], card.pm.send
+    def capture(service, event):
+      if service == 'starpilotCarState':
+        event = event.to_bytes()
+        sent.append(messaging.log_from_bytes(event).as_builder())
+      return send(service, event)
+    monkeypatch.setattr(card.pm, 'send', capture)
+    packer, frozen = CANPacker(DBC[identity][Bus.pt]), card.CP.to_dict()
+    value = 0
+    for tick in range(1, 80):
+      mono[0] += 10_000_000
+      rx = complete_frames(packer, card.CP, tick, main=False)
+      if 41 <= tick <= 58:
+        value = (list(range(16)) + [9, 0])[tick - 41]
+        rx.append((0x35F, bytes((0, 0, 0, value << 4, 0, 0, 0, 0)), 2))
+      state = ci.update([(mono[0] + offset, rx)])
+      if tick < 41:
+        continue
+      assert state.canValid and not state.canTimeout
+      sent.clear()
+      card.state_publish(state, None)
+      assert len(sent) == 1
+      event = sent[0]
+      assert event.valid and event.logMonoTime == mono[0]
+      payload = event.starpilotCarState
+      assert payload.dashboardStopSign == int(value == 9)
+    assert card.CP.to_dict() == frozen == controls.CP.to_dict()
+  exercise_startup(identity, False, False, aol, monkeypatch, probe=probe)
+
+
+@pytest.mark.parametrize('bus,length', ((0, 8), (1, 8), (2, 4), (2, 7), (2, 9), (2, 64)))
+def test_wrong_bus_and_dlc_cannot_create_camera_evidence(bus, length):
+  cp = CarInterface.get_params(SUPPORTED[0], gen_empty_fingerprint(), [], False, False, False)
+  ci = CarInterface(cp)
+  ci.update([(100_000_000_000, [(0x35F, bytes(length), bus)])])
+  assert ci.CS.dashboard_stop_sign is None
+
+
+@pytest.mark.parametrize('age', (-1, 0, 150_000_000, 150_000_001))
+@pytest.mark.parametrize('health', ('healthy', 'invalid', 'timeout'))
+def test_composition_preserves_other_evidence(age, health, monkeypatch):
+  from openpilot.cereal import messaging
+  cp = CarInterface.get_params(SUPPORTED[0], gen_empty_fingerprint(), [], False, False, False)
+  now, offset = 100_000_000_000, 2_000_000_000
+  monkeypatch.setattr(dashboard.time, 'monotonic_ns', lambda: now)
+  monkeypatch.setattr(dashboard.time, 'clock_gettime_ns', lambda clock: now + offset)
+  message = messaging.new_message('starpilotCarState', valid=True, logMonoTime=now)
+  message.starpilotCarState.sourceCarStateMonoTime = now - 1
+  message.starpilotCarState.learningApplication.profileSequence = 7
+  state = SimpleNamespace(canValid=health != 'invalid', canTimeout=health == 'timeout')
+  parsed = SimpleNamespace(dashboard_stop_sign=(True, now + offset - age))
+  assert dashboard.compose(cp, parsed, state, message, car_state_stamp=now) is message
+  assert message.valid and message.logMonoTime == now
+  assert message.starpilotCarState.sourceCarStateMonoTime == now - 1
+  assert message.starpilotCarState.learningApplication.profileSequence == 7
+  valid = health == 'healthy' and 0 <= age <= 150_000_000
+  assert message.starpilotCarState.dashboardStopSign == int(valid)
+
+
+@pytest.mark.parametrize('identity', list(CAR))
+def test_all_six_dom_dashboard_identities_observe_without_new_actuation(identity, monkeypatch):
+  cp = CarInterface.get_params(identity, gen_empty_fingerprint(), [], False, False, False)
+  ci = CarInterface(cp)
+  before = cp.to_dict()
+  now, offset = 100_000_000_000, 2_000_000_000
+  monkeypatch.setattr(dashboard.time, 'monotonic_ns', lambda: now)
+  monkeypatch.setattr(dashboard.time, 'clock_gettime_ns', lambda clock: now + offset)
+  packer = CANPacker(DBC[identity][Bus.pt])
+  for tick in range(1, 41):
+    source = now + offset - (40 - tick) * 10_000_000
+    rx = complete_frames(packer, cp, tick, main=False)
+    rx.append((0x35F, bytes((0, 0, 0, 0x90, 0, 0, 0, 0)), 2))
+    state = ci.update([(source, rx)])
+  assert state.canValid and not state.canTimeout
+  assert ci.CS.dashboard_stop_sign == (True, now + offset)
+  message = dashboard.compose(cp, ci.CS, state, None, car_state_stamp=now)
+  assert message.starpilotCarState.dashboardStopSign == 1
+  assert cp.to_dict() == before
+  assert cp.dashcamOnly is (identity not in SUPPORTED)
+  assert cp.pcmCruise and not cp.openpilotLongitudinalControl

openpilot/selfdrive/car/card.py

@@ -1054,6 +1054,9 @@ class Car:
         companion = feedback_companion
       except Exception:
         self.learning_feedback_publication = None
+    if self.CP.brand == 'mazda':
+      from openpilot.starpilot.car.mazda.dashboard import compose
+      companion = compose(self.CP, self.CI.CS, CS, companion, car_state_stamp=int(cs_send.logMonoTime))
     self.car_gps_publisher.update(self.CI.CS, self.pm, companion)
     self.pm.send('carState', cs_send)
     if getattr(self, 'learning_stamp_observer', None) is not None:

openpilot/starpilot/car/mazda/dashboard.py

@@ -0,0 +1,35 @@
+"""Optional Mazda camera observation; never a longitudinal control grant."""
+import time
+
+from opendbc.car.mazda.values import CAR, MazdaFlags
+from openpilot.cereal import messaging
+
+MAX_AGE_NS = 150_000_000
+DASHBOARD_CARS = frozenset(CAR)
+
+
+def capable(cp):
+  # Read-only observation scope is separate from active-control/AOL admission.
+  # Dom's intentional dashboard commit explicitly covers all six GEN1 IDs.
+  return (cp.brand == 'mazda' and cp.carFingerprint in DASHBOARD_CARS and int(cp.flags) == int(MazdaFlags.GEN1) and not cp.notCar)
+
+
+def compose(cp, parsed, state, message, *, car_state_stamp):
+  if not capable(cp):
+    return message
+  if message is None:
+    message = messaging.new_message('starpilotCarState', valid=bool(state.canValid), logMonoTime=car_state_stamp)
+    message.starpilotCarState.sourceCarStateMonoTime = car_state_stamp
+  # Missing or stale optional camera data must not invalidate unrelated evidence.
+  message.starpilotCarState.dashboardStopSign = 0
+  before = time.monotonic_ns()
+  boot = time.clock_gettime_ns(getattr(time, 'CLOCK_BOOTTIME', time.CLOCK_MONOTONIC))
+  after = time.monotonic_ns()
+  observation = getattr(parsed, 'dashboard_stop_sign', None)
+  if (type(observation) is tuple and len(observation) == 2 and type(observation[0]) is bool and
+      type(observation[1]) is int and 0 <= after - before <= 1_000_000 and state.canValid and not state.canTimeout):
+    age = boot - observation[1]
+    stamp = before - age
+    if 0 <= age <= MAX_AGE_NS and 0 < stamp <= car_state_stamp:
+      message.starpilotCarState.dashboardStopSign = int(observation[0])
+  return message

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "66c65be741a8b1e5726aab053897f5bea235dd48697dfd7ec7f908cbb2dee514", "schema_version": 1}
+{"protected_inventory_sha256": "68477cd60a2523b67edcbdb8eabdadc32b3e686aa3e4429987b40b903a570322", "schema_version": 1}

tools/ci/run_vehicle_tests.py

@@ -41,6 +41,7 @@ AOL_LIFECYCLE_TARGETS = (
   "openpilot/starpilot/aol/tests/test_honda_recovery_lifecycle.py",
   "openpilot/starpilot/aol/tests/test_fresh_inactive_transport.py",
   "opendbc_repo/opendbc/car/mazda/tests/test_native_engagement_callers.py",
+  "opendbc_repo/opendbc/car/mazda/tests/test_dashboard_stop_sign.py",
   "opendbc_repo/opendbc/safety/tests/test_mazda_stock_aol.py",
   "openpilot/starpilot/aol/tests/test_temporary_lag_intent.py",
   "openpilot/starpilot/aol/tests/test_stock_location_recovery.py",