This commit is contained in:
firestar5683
2026-08-29 15:21:35 -05:00
parent 83c3ee8e26
commit b6964f0142
5 changed files with 28 additions and 3 deletions
+5
View File
@@ -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
@@ -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
@@ -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"]
+14 -2
View File
@@ -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
@@ -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