From 178c8694f2e748afc60edad42c5128fe9ab793ea Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 22 Oct 2026 14:00:06 -0500 Subject: [PATCH] Restore Mazda camera stop-sign observations --- opendbc_repo/opendbc/car/mazda/carstate.py | 33 ++++- .../mazda/tests/test_dashboard_stop_sign.py | 118 ++++++++++++++++++ openpilot/selfdrive/car/card.py | 3 + openpilot/starpilot/car/mazda/dashboard.py | 35 ++++++ prebuilt.json | 2 +- tools/ci/run_vehicle_tests.py | 1 + 6 files changed, 189 insertions(+), 3 deletions(-) create mode 100644 opendbc_repo/opendbc/car/mazda/tests/test_dashboard_stop_sign.py create mode 100644 openpilot/starpilot/car/mazda/dashboard.py diff --git a/opendbc_repo/opendbc/car/mazda/carstate.py b/opendbc_repo/opendbc/car/mazda/carstate.py index eae22f768d..b0e5d66c3d 100644 --- a/opendbc_repo/opendbc/car/mazda/carstate.py +++ b/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)), } diff --git a/opendbc_repo/opendbc/car/mazda/tests/test_dashboard_stop_sign.py b/opendbc_repo/opendbc/car/mazda/tests/test_dashboard_stop_sign.py new file mode 100644 index 0000000000..f417a70a4e --- /dev/null +++ b/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 diff --git a/openpilot/selfdrive/car/card.py b/openpilot/selfdrive/car/card.py index 032e747cd2..f8361073d6 100644 --- a/openpilot/selfdrive/car/card.py +++ b/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: diff --git a/openpilot/starpilot/car/mazda/dashboard.py b/openpilot/starpilot/car/mazda/dashboard.py new file mode 100644 index 0000000000..60ffc31c7f --- /dev/null +++ b/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 diff --git a/prebuilt.json b/prebuilt.json index cdd8f687d6..28340136ac 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "66c65be741a8b1e5726aab053897f5bea235dd48697dfd7ec7f908cbb2dee514", "schema_version": 1} +{"protected_inventory_sha256": "68477cd60a2523b67edcbdb8eabdadc32b3e686aa3e4429987b40b903a570322", "schema_version": 1} diff --git a/tools/ci/run_vehicle_tests.py b/tools/ci/run_vehicle_tests.py index 890cd71442..ceac0f8bc2 100644 --- a/tools/ci/run_vehicle_tests.py +++ b/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",