mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-03 06:33:50 +08:00
fix
This commit is contained in:
@@ -39,8 +39,7 @@ _STOP_START_PULSE_FRAMES = 30
|
||||
_STOP_START_PULSE_PERIOD_FRAMES = 5
|
||||
_AVH_STARTUP_DELAY_FRAMES = _STOP_START_STARTUP_DELAY_FRAMES
|
||||
_AVH_STARTUP_DEADLINE_FRAMES = _STOP_START_STARTUP_DEADLINE_FRAMES
|
||||
_AVH_PULSE_FRAMES = _STOP_START_PULSE_FRAMES
|
||||
_AVH_PULSE_PERIOD_FRAMES = _STOP_START_PULSE_PERIOD_FRAMES
|
||||
_AVH_PULSE_MESSAGES = 10 # Match the native 10 Hz AVH frame for roughly one second
|
||||
|
||||
|
||||
def get_safety_CP():
|
||||
@@ -93,8 +92,8 @@ class CarController(CarControllerBase):
|
||||
self.stop_start_acknowledged = False
|
||||
self.avh_attempted = False
|
||||
self.avh_request_started = False
|
||||
self.avh_request_frame = 0
|
||||
self.avh_counter = 0
|
||||
self.avh_last_counter = None
|
||||
self.avh_messages_sent = 0
|
||||
|
||||
def _stop_start_off_request(self, CC, CS, starpilot_toggles):
|
||||
"""Send one bounded Subaru Stop/Start OFF request after ignition.
|
||||
@@ -182,22 +181,22 @@ class CarController(CarControllerBase):
|
||||
|
||||
if not self.avh_request_started:
|
||||
self.avh_request_started = True
|
||||
self.avh_request_frame = self.frame
|
||||
self.avh_counter = (int(avh_msg.get("COUNTER", 0)) + 1) % 0x10
|
||||
self.avh_last_counter = int(avh_msg.get("COUNTER", 0)) % 0x10
|
||||
|
||||
elapsed = self.frame - self.avh_request_frame
|
||||
if elapsed >= _AVH_PULSE_FRAMES:
|
||||
if self.avh_messages_sent >= _AVH_PULSE_MESSAGES:
|
||||
self.avh_attempted = True
|
||||
return None
|
||||
|
||||
if elapsed % _AVH_PULSE_PERIOD_FRAMES != 0:
|
||||
counter = int(avh_msg.get("COUNTER", 0)) % 0x10
|
||||
if counter == self.avh_last_counter:
|
||||
return None
|
||||
|
||||
msg = subarucan.create_avh_control(
|
||||
self.packer, avh_msg, raw_dat=avh_dat,
|
||||
counter=self.avh_counter, bus=CanBus.alt_for_cp(self.CP),
|
||||
counter=counter, bus=CanBus.alt_for_cp(self.CP),
|
||||
)
|
||||
self.avh_counter = (self.avh_counter + 1) % 0x10
|
||||
self.avh_last_counter = counter
|
||||
self.avh_messages_sent += 1
|
||||
return msg
|
||||
|
||||
def _reset_legacy_2025_handoff(self):
|
||||
|
||||
@@ -322,6 +322,16 @@ def test_avh_request_sets_observed_bit_and_is_bounded():
|
||||
)
|
||||
toggles = SimpleNamespace(subaru_stop_start_off=False, subaru_avh_on=True, subaru_sng=False)
|
||||
|
||||
# Start the request from the current live counter. AVH is a native 10 Hz
|
||||
# frame, so the controller waits for each next live counter before sending
|
||||
# its matching button frame.
|
||||
_, can_sends = controller.update(CC, CS, 0, toggles)
|
||||
avh_msgs = [msg for msg in can_sends if msg[0] == 0x32b]
|
||||
assert not avh_msgs
|
||||
|
||||
CS.avh_msg["COUNTER"] = 0
|
||||
CS.avh_dat = bytes.fromhex("14001c4208800000")
|
||||
controller.frame = 103
|
||||
_, can_sends = controller.update(CC, CS, 0, toggles)
|
||||
avh_msgs = [msg for msg in can_sends if msg[0] == 0x32b]
|
||||
assert avh_msgs == [(0x32b, bytes.fromhex("34001c4208a00000"), CanBus.alt)]
|
||||
@@ -331,7 +341,21 @@ def test_avh_request_sets_observed_bit_and_is_bounded():
|
||||
assert parser.vl["AVH"]["AVH"] == 1
|
||||
assert parser.vl["AVH"]["COUNTER"] == 0
|
||||
|
||||
controller.frame = 131
|
||||
for counter in range(1, 10):
|
||||
CS.avh_msg["COUNTER"] = counter
|
||||
raw_dat = bytearray.fromhex("14001c4208800000")
|
||||
raw_dat[1] = counter
|
||||
raw_dat[0] = ((0x32B & 0xFF) + ((0x32B >> 8) & 0xFF) + sum(raw_dat[1:])) & 0xFF
|
||||
CS.avh_dat = bytes(raw_dat)
|
||||
controller.frame = 103 + (counter * 10)
|
||||
_, can_sends = controller.update(CC, CS, 0, toggles)
|
||||
avh_msgs.extend(msg for msg in can_sends if msg[0] == 0x32b)
|
||||
|
||||
assert len(avh_msgs) == 10
|
||||
assert [msg[1][1] & 0x0F for msg in avh_msgs] == list(range(10))
|
||||
assert all(msg[1][5] & 0x20 for msg in avh_msgs)
|
||||
|
||||
controller.frame = 203
|
||||
_, can_sends = controller.update(CC, CS, 0, toggles)
|
||||
assert not any(msg[0] == 0x32b for msg in can_sends)
|
||||
assert controller.avh_attempted
|
||||
|
||||
Reference in New Issue
Block a user