diff --git a/opendbc_repo/opendbc/can/parser.py b/opendbc_repo/opendbc/can/parser.py index 543c8f855..67dc58d00 100644 --- a/opendbc_repo/opendbc/can/parser.py +++ b/opendbc_repo/opendbc/can/parser.py @@ -132,6 +132,7 @@ class CANParser: self.dbc: DBC = DBC(dbc_name) self.vl: dict[int | str, dict[str, float]] = VLDict(self) + self.vl_raw: dict[int | str, bytes] = {} self.vl_all: dict[int | str, dict[str, list[float]]] = {} self.ts_nanos: dict[int | str, dict[str, int]] = {} self.addresses: set[int] = set() @@ -166,6 +167,8 @@ class CANParser: signals_dict = {s: 0.0 for s in signal_names} dict.__setitem__(self.vl, msg.address, signals_dict) dict.__setitem__(self.vl, msg.name, signals_dict) + self.vl_raw[msg.address] = bytes(msg.size) + self.vl_raw[msg.name] = bytes(msg.size) self.vl_all[msg.address] = defaultdict(list) self.vl_all[msg.name] = self.vl_all[msg.address] self.ts_nanos[msg.address] = {s: 0 for s in signal_names} @@ -247,6 +250,8 @@ class CANParser: vl_addr[sig.name] = state.vals[i] vl_all_addr[sig.name] = state.all_vals[i] ts_addr[sig.name] = state.timestamps[-1] + self.vl_raw[address] = bytes(dat) + self.vl_raw[state.name] = bytes(dat) if not bus_empty: self.last_nonempty_nanos = t diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py index 615b4b1c3..19339d46e 100644 --- a/opendbc_repo/opendbc/car/subaru/carcontroller.py +++ b/opendbc_repo/opendbc/car/subaru/carcontroller.py @@ -137,7 +137,8 @@ class CarController(CarControllerBase): return None msg = subarucan.create_stop_start_control( - self.packer, dashlights_msg, counter=self.stop_start_counter, bus=self.main_bus, + self.packer, dashlights_msg, raw_dat=getattr(CS, "dashlights_dat", None), + counter=self.stop_start_counter, bus=self.main_bus, ) self.stop_start_counter = (self.stop_start_counter + 1) % 0x10 return msg diff --git a/opendbc_repo/opendbc/car/subaru/carstate.py b/opendbc_repo/opendbc/car/subaru/carstate.py index b91767d67..9d7c1787a 100644 --- a/opendbc_repo/opendbc/car/subaru/carstate.py +++ b/opendbc_repo/opendbc/car/subaru/carstate.py @@ -16,6 +16,7 @@ class CarState(CarStateBase): self.angle_rate_calulator = CanSignalRateCalculator(50) self.dashlights_msg = {} + self.dashlights_dat = b"" self.stop_start_state = 0 def update(self, can_parsers, starpilot_toggles) -> structs.CarState: @@ -28,6 +29,7 @@ class CarState(CarStateBase): if self.CP.carFingerprint in SUBARU_STOP_START_CARS: self.dashlights_msg = copy.copy(cp.vl["Dashlights"]) + self.dashlights_dat = cp.vl_raw["Dashlights"] self.stop_start_state = cp.vl["Engine_Stop_Start"]["STOP_START_STATE"] throttle_msg = cp.vl["Throttle"] if not (self.CP.flags & SubaruFlags.HYBRID) else cp_alt.vl["Throttle_Hybrid"] diff --git a/opendbc_repo/opendbc/car/subaru/subarucan.py b/opendbc_repo/opendbc/car/subaru/subarucan.py index 9b8db0d6d..1d2d04d2e 100644 --- a/opendbc_repo/opendbc/car/subaru/subarucan.py +++ b/opendbc_repo/opendbc/car/subaru/subarucan.py @@ -182,12 +182,24 @@ def create_es_dashstatus(packer, frame, dashstatus_msg, enabled, long_enabled, l return packer.make_can_msg("ES_DashStatus", bus, values) -def create_stop_start_control(packer, dashlights_msg, counter=None, bus=CanBus.alt): +def create_stop_start_control(packer, dashlights_msg, raw_dat=None, counter=None, bus=CanBus.alt): """Create the supported Subaru momentary Stop/Start button request. Dashlights is a stock periodic message, so preserve the live frame and only - change the event bit. CANPacker calculates the Subaru checksum for us. + change the counter, event bit, and checksum. The raw frame is needed because + the DBC does not describe every byte in this message. """ + if raw_dat: + dat = bytearray(raw_dat) + if len(dat) != 8: + raise ValueError(f"Dashlights frame must be 8 bytes, got {len(dat)}") + if counter is None: + counter = (int(dashlights_msg.get("COUNTER", 0)) + 1) % 0x10 + dat[1] = (dat[1] & 0xF0) | (counter % 0x10) + dat[6] |= 0x40 # STOP_START, big-endian bit 54 + dat[0] = ((0x390 & 0xFF) + ((0x390 >> 8) & 0xFF) + sum(dat[1:])) & 0xFF + return 0x390, bytes(dat), bus + values = dict(dashlights_msg) if counter is None: counter = (int(values.get("COUNTER", 0)) + 1) % 0x10 diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py index 585a14c0f..4bad73809 100644 --- a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py +++ b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py @@ -211,13 +211,16 @@ def test_stop_start_inputs_are_captured_for_supported_models(platform): CP = CarInterface.get_non_essential_params(platform) car_state = CarState(CP, None) parsers = car_state.get_can_parsers(CP) + raw_dashlights = bytes.fromhex("13031407875a8100") parsers[Bus.pt].vl["Dashlights"]["COUNTER"] = 6 parsers[Bus.pt].vl["Dashlights"]["STOP_START"] = 0 parsers[Bus.pt].vl["Engine_Stop_Start"]["STOP_START_STATE"] = 3 + parsers[Bus.pt].vl_raw["Dashlights"] = raw_dashlights car_state.update(parsers, SimpleNamespace(subaru_sng=False)) assert car_state.dashlights_msg["COUNTER"] == 6 + assert car_state.dashlights_dat == raw_dashlights assert car_state.stop_start_state == 3 @@ -247,6 +250,7 @@ def test_stop_start_request_is_bounded_and_uses_live_dashlights(platform, expect CS = SimpleNamespace( canValid=True, dashlights_msg={"COUNTER": 6, "STOP_START": 0}, + dashlights_dat=bytes.fromhex("13061407875a8100"), stop_start_state=0, out=SimpleNamespace( standstill=True, @@ -259,6 +263,7 @@ def test_stop_start_request_is_bounded_and_uses_live_dashlights(platform, expect stop_start_msgs = [msg for msg in can_sends if msg[0] == 0x390] assert len(stop_start_msgs) == 1 assert stop_start_msgs[0][2] == expected_bus + assert stop_start_msgs[0][1] == bytes.fromhex("57071407875ac100") parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("Dashlights", 0)], expected_bus) parser.update([(expected_bus, [stop_start_msgs[0]])]) assert parser.vl["Dashlights"]["STOP_START"] == 1