mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-03 06:33:50 +08:00
fix
This commit is contained in:
@@ -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"]
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user