This commit is contained in:
firestar5683
2026-08-31 11:58:21 -05:00
parent 5d32cccaf1
commit 138e4cca06
2 changed files with 35 additions and 12 deletions
@@ -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