mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-25 08:53:45 +08:00
Flight Delayed
This commit is contained in:
firestar5683
@@ -702,6 +702,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"StandbyMode", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
|
||||
{"StandbyWakeButton", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"StandbyButtonPressTime", {CLEAR_ON_MANAGER_START | DONT_LOG, INT, "0", "0"}},
|
||||
{"ScreenOffToggleCounter", {CLEAR_ON_MANAGER_START | DONT_LOG, INT, "0", "0"}},
|
||||
{"StandbyWakeEngage", {PERSISTENT, BOOL, "1", "1", 2, SETTINGS_SIMPLE}},
|
||||
{"StandbyWakeDisengage", {PERSISTENT, BOOL, "1", "1", 2, SETTINGS_SIMPLE}},
|
||||
{"StandbyWakeInfoAlert", {PERSISTENT, BOOL, "1", "1", 2, SETTINGS_SIMPLE}},
|
||||
@@ -740,6 +741,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"SubaruSNG", {PERSISTENT, BOOL, "1", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"SubaruSNGManualParkingBrake", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"SubaruStopStartOff", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"SubaruAvhStartup", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"SubaruRedneckCruise", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"TacoTune", {PERSISTENT, BOOL, "0", "0", 2}},
|
||||
{"TeslaCoopSteering", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
|
||||
@@ -811,7 +811,7 @@ class CarController(CarControllerBase):
|
||||
if not self.long_active_ecu:
|
||||
if self.cancel_counter > CANCEL_BUTTON_DELAY_FRAMES:
|
||||
can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP))
|
||||
elif self._ray_pedal and CC.longActive and CS.out.cruiseState.enabled:
|
||||
elif self._ray_pedal and CC.enabled and CS.out.cruiseState.enabled:
|
||||
if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
|
||||
can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP))
|
||||
self.last_button_frame = self.frame
|
||||
|
||||
@@ -168,3 +168,49 @@ def test_ray_controller_heartbeats_and_only_actuates_when_ready():
|
||||
hud, actuators, CS, CC, 2, 0)
|
||||
assert next(dat for addr, dat, bus in messages if addr == 0x200 and bus == 0)[:4] == bytes(4)
|
||||
assert any(addr == 0x4F1 and bus == 0 for addr, _, bus in messages) # cancel stock CC
|
||||
|
||||
|
||||
@pytest.mark.parametrize("candidate", [CAR.KIA_RAY_EV, CAR.HYUNDAI_KONA_EV_NON_SCC])
|
||||
def test_ray_stock_cruise_cancellation_survives_accelerator_override(candidate):
|
||||
CP = CarInterface.get_params(candidate, ray_fingerprint(), [], False, False, False, None)
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS11", 0), ("CLU11", 0)], 0)
|
||||
CS = SimpleNamespace(
|
||||
lkas11=parser.vl["LKAS11"], clu11=parser.vl["CLU11"],
|
||||
out=SimpleNamespace(vEgo=12.0, gasPressed=True, brakePressed=False,
|
||||
cruiseState=SimpleNamespace(enabled=True)),
|
||||
ray_pedal_valid=True, ray_pedal_state=0, is_metric=True,
|
||||
)
|
||||
CC = SimpleNamespace(
|
||||
enabled=True, longActive=False, latActive=True,
|
||||
cruiseControl=SimpleNamespace(cancel=False, resume=False, override=True),
|
||||
)
|
||||
hud = SimpleNamespace(
|
||||
visualAlert=CarControl.HUDControl.VisualAlert.none,
|
||||
leftLaneVisible=True, rightLaneVisible=True, leftLaneDepart=False, rightLaneDepart=False,
|
||||
)
|
||||
actuators = SimpleNamespace(longControlState=CarControl.Actuators.LongControlState.off)
|
||||
controller._create_can_redneck_button_messages = lambda _: []
|
||||
|
||||
def messages(frame):
|
||||
controller.frame = frame
|
||||
return controller.create_can_msgs(True, 0, False, 0.0, 2.0, False,
|
||||
hud, actuators, CS, CC, 2, 0)
|
||||
|
||||
def cancel_frames(msgs):
|
||||
return [dat for addr, dat, bus in msgs if addr == 0x4F1 and bus == 0 and dat[0] & 7 == 4]
|
||||
|
||||
msgs = messages(20)
|
||||
assert bool(cancel_frames(msgs)) is (candidate == CAR.KIA_RAY_EV)
|
||||
if candidate == CAR.KIA_RAY_EV:
|
||||
pedal = next(dat for addr, dat, bus in msgs if addr == 0x200 and bus == 0)
|
||||
assert pedal[:4] == bytes(4)
|
||||
assert not (pedal[4] & 0x80)
|
||||
assert not cancel_frames(messages(24)) # retain the existing cancellation rate limit
|
||||
assert cancel_frames(messages(32))
|
||||
|
||||
CS.out.cruiseState.enabled = False
|
||||
assert not cancel_frames(messages(44))
|
||||
CS.out.cruiseState.enabled = True
|
||||
CC.enabled = False
|
||||
assert not cancel_frames(messages(56)) # AOL alone must not cancel native cruise
|
||||
|
||||
@@ -0,0 +1,95 @@
|
||||
"""One bounded Legacy AVH ON request; 0x32B is status, never a TX command."""
|
||||
|
||||
AVH_REQUEST = 0x6BB
|
||||
AVH_STATUS = 0x32B
|
||||
INPUTS = (AVH_REQUEST, AVH_STATUS, 0x40, 0x48, 0x13A, 0x174)
|
||||
|
||||
|
||||
def checksum(address, data):
|
||||
return ((address & 0xFF) + (address >> 8) + sum(data[1:])) & 0xFF
|
||||
|
||||
|
||||
def avh_request(template, step):
|
||||
if len(template) != 8 or checksum(AVH_REQUEST, template) != template[0] or template[2] & 3 or step not in (1, 2):
|
||||
raise ValueError("Invalid AVH template or counter step")
|
||||
data = bytearray(template)
|
||||
data[1] = (data[1] & 0xF0) | ((data[1] + step) & 0xF)
|
||||
data[2] |= 2
|
||||
data[0] = checksum(AVH_REQUEST, data)
|
||||
return AVH_REQUEST, bytes(data), 1
|
||||
|
||||
|
||||
class AvhStartup:
|
||||
def __init__(self):
|
||||
self.started = None
|
||||
self.last_time = None
|
||||
self.stable_since = None
|
||||
self.frames = {}
|
||||
self.done = False
|
||||
self.followup = None
|
||||
|
||||
def update(self, now, frames, enabled, can_valid, controls_active):
|
||||
if self.started is None:
|
||||
self.started = now
|
||||
if self.last_time is not None and now < self.last_time:
|
||||
self.done = True
|
||||
self.last_time = now
|
||||
if self.done:
|
||||
return []
|
||||
if now - self.started > 30 or controls_active:
|
||||
self.done = True
|
||||
return []
|
||||
|
||||
for address, (timestamp, data) in frames.items():
|
||||
if address not in INPUTS or timestamp <= 0:
|
||||
continue
|
||||
previous = self.frames.get(address)
|
||||
if previous and timestamp == previous[0]:
|
||||
continue
|
||||
if len(data) != 8 or checksum(address, data) != data[0] or timestamp > now or (previous and timestamp < previous[0]):
|
||||
self.done = True
|
||||
return []
|
||||
if (address == AVH_REQUEST and data[2] & 3) or (address == AVH_STATUS and data[5] & 0x20) or \
|
||||
(address == 0x48 and data[3] != 4) or (address == 0x40 and data[4]) or \
|
||||
(address == 0x13A and any((int.from_bytes(data, 'little') >> bit) & 0x1FFF for bit in (12, 25, 38, 51))):
|
||||
self.done = True
|
||||
return []
|
||||
if previous and (data[1] & 15) == (previous[1][1] & 15):
|
||||
continue # duplicate counters cannot refresh freshness
|
||||
# Controller snapshots can skip 50/100 Hz samples between updates. Panda
|
||||
# checks their full counter stream; require consecutive head-unit frames here.
|
||||
sequential = bool(previous and (address not in (AVH_REQUEST, AVH_STATUS) or
|
||||
(data[1] & 15) == ((previous[1][1] + 1) & 15)))
|
||||
self.frames[address] = (timestamp, data, sequential)
|
||||
|
||||
fresh = all(a in self.frames and self.frames[a][2] and
|
||||
0 <= now - self.frames[a][0] <= (1.5 if a == AVH_REQUEST else 0.3) for a in INPUTS)
|
||||
if not enabled or not can_valid or not fresh:
|
||||
self.stable_since = None
|
||||
if self.followup is not None:
|
||||
self.done = True
|
||||
return []
|
||||
throttle = self.frames[0x40][1]
|
||||
rpm = int.from_bytes(throttle[2:4], 'little') & 0x1FFF
|
||||
if rpm < 400 or not self.frames[0x174][1][2] & 8:
|
||||
self.stable_since = None
|
||||
if self.followup is not None:
|
||||
self.done = True
|
||||
return []
|
||||
if self.stable_since is None:
|
||||
self.stable_since = now
|
||||
if self.followup is not None:
|
||||
sent, timestamp, template = self.followup
|
||||
if now - sent > 0.075 or self.frames[AVH_REQUEST][0] != timestamp:
|
||||
self.done = True
|
||||
elif now - sent >= 0.05:
|
||||
self.done = True
|
||||
return [avh_request(template, 2)]
|
||||
return []
|
||||
if now - self.started < 10 or now - self.stable_since < 3:
|
||||
return []
|
||||
timestamp, template, _ = self.frames[AVH_REQUEST]
|
||||
if now - timestamp > 0.010:
|
||||
return []
|
||||
self.followup = (now, timestamp, template)
|
||||
return [avh_request(template, 1)]
|
||||
@@ -4,6 +4,7 @@ from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, structs
|
||||
from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_std_steer_angle_limits, apply_steer_angle_limits_vm, common_fault_avoidance
|
||||
from opendbc.car.interfaces import CarControllerBase
|
||||
from opendbc.car.subaru import subarucan
|
||||
from opendbc.car.subaru.avh import AvhStartup
|
||||
from opendbc.car.subaru.values import CAR, DBC, GLOBAL_ES_ADDR, SUBARU_STOP_START_CARS, CanBus, CarControllerParams, SubaruFlags
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
|
||||
@@ -69,6 +70,7 @@ class CarController(CarControllerBase):
|
||||
self.stop_start_counter = 0
|
||||
self.stop_start_acknowledged = False
|
||||
self.last_redneck_button_frame = 0
|
||||
self.avh_startup = AvhStartup()
|
||||
|
||||
def _stop_start_off_request(self, CC, CS, starpilot_toggles):
|
||||
"""Send one bounded Subaru Stop/Start OFF request after ignition.
|
||||
@@ -215,7 +217,8 @@ class CarController(CarControllerBase):
|
||||
self.ascent_aol_arm_frames = _ASCENT_AOL_ARM_FRAMES if lkas_available else 0
|
||||
|
||||
if self.CP.carFingerprint == CAR.SUBARU_OUTBACK_2023:
|
||||
manual_handoff = False
|
||||
manual_handoff = not self.angle_lkas_active and \
|
||||
abs(getattr(CS.out, "steeringRateDeg", 0.0)) > _ANGLE_REENGAGE_MAX_STEER_RATE
|
||||
else:
|
||||
manual_handoff = self._angle_manual_handoff(CS, lkas_available)
|
||||
lkas_active = lkas_available and not manual_handoff
|
||||
@@ -307,6 +310,13 @@ class CarController(CarControllerBase):
|
||||
if stop_start_msg is not None:
|
||||
can_sends.append(stop_start_msg)
|
||||
|
||||
if self.CP.carFingerprint == CAR.SUBARU_LEGACY_2025:
|
||||
can_sends.extend(self.avh_startup.update(
|
||||
now_nanos / 1e9, getattr(CS, "avh_frames", {}),
|
||||
getattr(starpilot_toggles, "subaru_avh_on", False), getattr(CS.out, "canValid", False),
|
||||
CC.enabled or CC.latActive or CC.longActive,
|
||||
))
|
||||
|
||||
# *** steering ***
|
||||
if (self.frame % self.p.STEER_STEP) == 0:
|
||||
if self.CP.flags & SubaruFlags.LKAS_ANGLE:
|
||||
|
||||
@@ -4,8 +4,9 @@ from opendbc.can import CANDefine, CANParser
|
||||
from opendbc.car import Bus, create_button_events, structs
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from opendbc.car.subaru.values import DBC, CanBus, SUBARU_REDNECK_CRUISE_CARS, SUBARU_STOP_START_CARS, SubaruFlags
|
||||
from opendbc.car.subaru.values import CAR, DBC, CanBus, SUBARU_REDNECK_CRUISE_CARS, SUBARU_STOP_START_CARS, SubaruFlags
|
||||
from opendbc.car import CanSignalRateCalculator
|
||||
from opendbc.car.subaru.avh import INPUTS as AVH_INPUTS
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
|
||||
@@ -26,6 +27,7 @@ class CarState(CarStateBase):
|
||||
self.dashlights_msg = {}
|
||||
self.dashlights_dat = b""
|
||||
self.stop_start_state = 0
|
||||
self.avh_frames = {}
|
||||
self.cruise_buttons_msg = {}
|
||||
self.cruise_buttons = {button: 0 for button in SUBARU_CRUISE_BUTTONS}
|
||||
|
||||
@@ -37,6 +39,9 @@ class CarState(CarStateBase):
|
||||
cp_angle = cp_main if self.CP.flags & SubaruFlags.D_PLATFORM else cp
|
||||
ret = structs.CarState()
|
||||
|
||||
if self.CP.carFingerprint == CAR.SUBARU_LEGACY_2025:
|
||||
self.avh_frames = {a: (cp_alt.ts_nanos[a]["CHECKSUM"] / 1e9, cp_alt.vl_raw[a]) for a in AVH_INPUTS}
|
||||
|
||||
if self.CP.carFingerprint in SUBARU_STOP_START_CARS:
|
||||
stop_start_cp = cp_alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp
|
||||
self.dashlights_msg = copy.copy(stop_start_cp.vl["Dashlights"])
|
||||
@@ -177,11 +182,15 @@ class CarState(CarStateBase):
|
||||
|
||||
@staticmethod
|
||||
def get_can_parsers(CP):
|
||||
avh_messages = [(a, 0) for a in (0x6BB, 0x32B, 0x40, 0x48)] if CP.carFingerprint == CAR.SUBARU_LEGACY_2025 else []
|
||||
parsers = {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.main_for_cp(CP)),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.camera),
|
||||
Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.alt_for_cp(CP))
|
||||
Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], avh_messages, CanBus.alt_for_cp(CP))
|
||||
}
|
||||
if CP.flags & SubaruFlags.D_PLATFORM:
|
||||
parsers[Bus.main] = CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.main)
|
||||
if CP.carFingerprint == CAR.SUBARU_LEGACY_2025:
|
||||
for address in AVH_INPUTS:
|
||||
parsers[Bus.alt].vl[address]
|
||||
return parsers
|
||||
|
||||
@@ -42,6 +42,8 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM_CAMERA.value
|
||||
if candidate in SUBARU_STOP_START_CARS:
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.STOP_START_BUTTON.value
|
||||
if candidate == CAR.SUBARU_LEGACY_2025:
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.AVH_STARTUP.value
|
||||
if candidate in (CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023):
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.FIXED_ANGLE_LIMITS.value
|
||||
|
||||
|
||||
@@ -0,0 +1,112 @@
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
from opendbc.car.subaru.avh import AVH_REQUEST, AVH_STATUS, INPUTS, AvhStartup, avh_request, checksum
|
||||
from opendbc.car.subaru.carcontroller import CarController
|
||||
from opendbc.car.subaru.interface import CarInterface
|
||||
from opendbc.car.subaru.values import CAR, SubaruSafetyFlags
|
||||
from opendbc.car import Bus
|
||||
|
||||
|
||||
def sample(address, counter):
|
||||
data = bytearray(8)
|
||||
data[1] = counter & 15
|
||||
if address == AVH_REQUEST:
|
||||
data[3], data[5], data[6] = 1, 0x80, 0x0E # captured Legacy payload, not Outback constants
|
||||
elif address == 0x40:
|
||||
data[2:4] = (800).to_bytes(2, 'little')
|
||||
elif address == 0x48:
|
||||
data[3] = 4
|
||||
elif address == 0x174:
|
||||
data[2] = 8
|
||||
data[0] = checksum(address, data)
|
||||
return bytes(data)
|
||||
|
||||
|
||||
def prepare(fast_counter_step=1):
|
||||
policy = AvhStartup()
|
||||
frames = {}
|
||||
for tick in range(101):
|
||||
now = 100 + tick / 10
|
||||
for address in INPUTS:
|
||||
if address != AVH_REQUEST or tick % 10 == 0:
|
||||
counter = tick // 10 if address == AVH_REQUEST else tick * (1 if address == AVH_STATUS else fast_counter_step)
|
||||
frames[address] = (now, sample(address, counter))
|
||||
sent = policy.update(now, frames, True, True, False)
|
||||
if tick < 100:
|
||||
assert sent == []
|
||||
assert sent == [avh_request(frames[AVH_REQUEST][1], 1)]
|
||||
return policy, frames
|
||||
|
||||
|
||||
def test_captured_legacy_press_bytes():
|
||||
template = bytes.fromhex('5b0b000100800e00')
|
||||
assert avh_request(template, 1) == (0x6BB, bytes.fromhex('5e0c020100800e00'), 1)
|
||||
assert avh_request(template, 2) == (0x6BB, bytes.fromhex('5f0d020100800e00'), 1)
|
||||
wrap = sample(AVH_REQUEST, 15)
|
||||
assert avh_request(wrap, 1)[1][1] == 0
|
||||
assert avh_request(wrap, 2)[1][1] == 1
|
||||
|
||||
|
||||
def test_two_frames_only_and_no_retry():
|
||||
policy, frames = prepare()
|
||||
assert policy.update(110.04, frames, True, True, False) == []
|
||||
assert policy.update(110.06, frames, True, True, False) == [avh_request(frames[AVH_REQUEST][1], 2)]
|
||||
assert policy.update(110.07, frames, True, True, False) == []
|
||||
assert policy.update(111, frames, True, True, False) == []
|
||||
|
||||
|
||||
def test_controller_snapshots_may_skip_fast_can_samples():
|
||||
policy, frames = prepare(fast_counter_step=2)
|
||||
assert policy.update(110.06, frames, True, True, False) == [avh_request(frames[AVH_REQUEST][1], 2)]
|
||||
|
||||
|
||||
@pytest.mark.parametrize('reason', ['late', 'new_template', 'manual', 'ack', 'moving', 'gas', 'gear', 'invalid', 'disabled', 'engaged', 'stale'])
|
||||
def test_followup_aborts_permanently(reason):
|
||||
policy, frames = prepare()
|
||||
address, offset, value = {
|
||||
'manual': (AVH_REQUEST, 2, 1), 'ack': (AVH_STATUS, 5, 32),
|
||||
'moving': (0x13A, 2, 1), 'gas': (0x40, 4, 1), 'gear': (0x48, 3, 3),
|
||||
'new_template': (AVH_REQUEST, 1, 11),
|
||||
}.get(reason, (None, None, None))
|
||||
if address is not None:
|
||||
data = bytearray(frames[address][1])
|
||||
data[1] = (data[1] + 1) & 15
|
||||
data[offset] = value
|
||||
data[0] = checksum(address, data)
|
||||
frames[address] = (110.05, bytes(data))
|
||||
if reason == 'stale':
|
||||
frames[0x40] = (109, frames[0x40][1])
|
||||
now = 110.08 if reason == 'late' else 110.06
|
||||
assert policy.update(now, frames, reason != 'disabled', reason != 'invalid', reason == 'engaged') == []
|
||||
assert policy.done
|
||||
assert policy.update(111, frames, True, True, False) == []
|
||||
|
||||
|
||||
def test_only_legacy_has_avh_safety_permission():
|
||||
for car in CAR:
|
||||
cp = CarInterface.get_non_essential_params(car)
|
||||
assert bool(cp.safetyConfigs[0].safetyParam & SubaruSafetyFlags.AVH_STARTUP) == (car == CAR.SUBARU_LEGACY_2025)
|
||||
|
||||
|
||||
def test_existing_required_messages_keep_alive_checks():
|
||||
cp = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
|
||||
parser = CarInterface.CarState.get_can_parsers(cp)[Bus.alt]
|
||||
assert not parser.message_states[0x13A].ignore_alive
|
||||
assert not parser.message_states[0x174].ignore_alive
|
||||
assert parser.message_states[AVH_REQUEST].ignore_alive
|
||||
assert parser.message_states[AVH_STATUS].ignore_alive
|
||||
|
||||
|
||||
def test_controller_sends_only_when_opted_in():
|
||||
cp = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
|
||||
controller = CarController({}, cp)
|
||||
cc = SimpleNamespace(enabled=False, latActive=False, longActive=False,
|
||||
actuators=SimpleNamespace(as_builder=lambda: SimpleNamespace(steeringAngleDeg=0)),
|
||||
hudControl=SimpleNamespace(leadVisible=False), cruiseControl=SimpleNamespace(cancel=False))
|
||||
cs = SimpleNamespace(out=SimpleNamespace(canValid=True), avh_frames={})
|
||||
toggles = SimpleNamespace(subaru_stop_start_off=False, subaru_avh_on=False, subaru_sng=False)
|
||||
controller.frame = 1
|
||||
_, sent = controller.update(cc, cs, 100_000_000_000, toggles)
|
||||
assert not any(m[0] in (AVH_REQUEST, AVH_STATUS) for m in sent)
|
||||
@@ -806,7 +806,7 @@ def test_outback_manual_steering_keeps_cooperative_angle_request():
|
||||
CS = SimpleNamespace(out=SimpleNamespace(
|
||||
vEgoRaw=0.9,
|
||||
steeringAngleDeg=-57.0,
|
||||
steeringRateDeg=-45.0,
|
||||
steeringRateDeg=0.0,
|
||||
steeringTorque=0.0,
|
||||
steeringPressed=True,
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
@@ -815,6 +815,7 @@ def test_outback_manual_steering_keeps_cooperative_angle_request():
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
|
||||
|
||||
for frame, steering_torque in enumerate((-79.0, -81.0, -170.0, -250.0, -250.0, 79.0, 81.0, 170.0, 250.0, 250.0), start=1):
|
||||
CS.out.steeringRateDeg = 0.0 if frame == 1 else -45.0
|
||||
CS.out.steeringTorque = steering_torque
|
||||
CS.out.steeringPressed = abs(steering_torque) > 80.0
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
@@ -825,6 +826,27 @@ def test_outback_manual_steering_keeps_cooperative_angle_request():
|
||||
assert controller._lkas_status_active(CC)
|
||||
|
||||
|
||||
def test_outback_waits_for_manual_turn_to_settle_before_reentry():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_OUTBACK_2023)
|
||||
controller = CarController({}, CP)
|
||||
CC = SimpleNamespace(enabled=False, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-80.0))
|
||||
CS = SimpleNamespace(out=SimpleNamespace(
|
||||
vEgoRaw=7.3, steeringAngleDeg=-121.47, steeringRateDeg=126.5,
|
||||
gearShifter=structs.CarState.GearShifter.drive, standstill=False,
|
||||
))
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
|
||||
for frame, (angle, rate, active) in enumerate([
|
||||
(-121.47, 126.5, False), (-117.96, 122.5, False), (-88.65, 112.0, False),
|
||||
(-0.24, 0.0, True),
|
||||
], start=1):
|
||||
CS.out.steeringAngleDeg = angle
|
||||
CS.out.steeringRateDeg = rate
|
||||
parser.update([(frame, [controller.lateral_angle(CC, CS)])])
|
||||
assert bool(parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"]) == active
|
||||
if not active:
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(angle, abs=0.01)
|
||||
|
||||
|
||||
def test_ascent_hud_waits_for_angle_request():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
|
||||
controller = CarController({}, CP)
|
||||
|
||||
@@ -90,6 +90,7 @@ class SubaruSafetyFlags(IntFlag):
|
||||
FIXED_ANGLE_LIMITS = 128
|
||||
STOP_START_BUTTON = 256
|
||||
REDNECK_CRUISE = 512
|
||||
AVH_STARTUP = 1024
|
||||
LEGACY_2025_ANGLE_LIMITS = FIXED_ANGLE_LIMITS
|
||||
|
||||
|
||||
|
||||
@@ -153,7 +153,7 @@ class CarController(CarControllerBase):
|
||||
def _update_preap(self, CC, CS):
|
||||
actuators = CC.actuators
|
||||
can_sends = []
|
||||
lat_active = CC.latActive and CS.hands_on_level < 3
|
||||
lat_active = CC.latActive and CS.hands_on_level < 3 and getattr(CS, "preap_lateral_authorized", False)
|
||||
|
||||
if CC.cruiseControl.cancel and CS.cruiseEnabled:
|
||||
CS.cruiseEnabled = False
|
||||
|
||||
@@ -13,6 +13,7 @@ class PreAPEngagement:
|
||||
self.enableDoublePull = double_pull_enabled
|
||||
self.double_pull_window_ms = double_pull_window_ms
|
||||
self.cruiseEnabled = False
|
||||
self.lateralEnabled = False
|
||||
self.enableLongControl = False
|
||||
self.enableJustCC = False
|
||||
self.pending_enable = False
|
||||
@@ -28,6 +29,7 @@ class PreAPEngagement:
|
||||
|
||||
def handle_steering_disengage(self, steering_disengage: bool) -> None:
|
||||
if steering_disengage and not self.prev_steering_disengage:
|
||||
self.lateralEnabled = False
|
||||
self.cruiseEnabled = False
|
||||
self.enableLongControl = False
|
||||
self.enableJustCC = False
|
||||
@@ -45,6 +47,7 @@ class PreAPEngagement:
|
||||
button_events: list[structs.CarState.ButtonEvent] = []
|
||||
|
||||
if cruise_buttons == CruiseButtons.MAIN and prev_cruise_buttons != CruiseButtons.MAIN:
|
||||
self.lateralEnabled = True
|
||||
if self.enableDoublePull:
|
||||
self._handle_double_pull(curr_time_ms, v_ego, speed_units, use_pedal, pedal_long_allowed, long_control_allowed, di_cruise_state)
|
||||
else:
|
||||
@@ -75,6 +78,7 @@ class PreAPEngagement:
|
||||
def check_can_engage(self, door_open: bool, gear_shifter, seatbelt_unlatched: bool) -> bool:
|
||||
can_engage = not door_open and gear_shifter == structs.CarState.GearShifter.drive and not seatbelt_unlatched
|
||||
if not can_engage:
|
||||
self.lateralEnabled = False
|
||||
self.cruiseEnabled = False
|
||||
self.enableLongControl = False
|
||||
self.enableJustCC = False
|
||||
@@ -118,6 +122,7 @@ class PreAPEngagement:
|
||||
((curr_time_ms - self.preap_last_cc_spoof_ms) < SPOOF_ECHO_WINDOW_MS)
|
||||
be.type = ButtonType.unknown if is_echo else ButtonType.cancel
|
||||
if not is_echo:
|
||||
self.lateralEnabled = False
|
||||
self.cruiseEnabled = False
|
||||
self.enableLongControl = False
|
||||
self.enableJustCC = False
|
||||
@@ -145,4 +150,3 @@ class PreAPEngagement:
|
||||
def _capture_target_speed(v_ego: float, speed_units: str) -> float:
|
||||
speed_uom_kph = CV.MPH_TO_KPH if speed_units == "MPH" else 1.0
|
||||
return max(int(v_ego * CV.MS_TO_KPH / speed_uom_kph + 0.5) * speed_uom_kph, 0.0)
|
||||
|
||||
|
||||
@@ -0,0 +1,19 @@
|
||||
from opendbc.car import structs
|
||||
from opendbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
|
||||
|
||||
def preap_lateral_authorized(CP, CS, panda_states, panda_states_valid: bool) -> bool:
|
||||
"""Match Pre-AP's existing safety authorization without treating software CC availability as ACC main."""
|
||||
if not panda_states_valid or CS.out.gearShifter != structs.CarState.GearShifter.drive or CS.out.doorOpen or CS.out.steeringDisengage:
|
||||
return False
|
||||
config = CP.safetyConfigs[0]
|
||||
matching = [p for p in panda_states if p.safetyModel == config.safetyModel and p.safetyParam == config.safetyParam]
|
||||
if len(matching) != 1 or matching[0].safetyRxChecksInvalid:
|
||||
return False
|
||||
panda = matching[0]
|
||||
# Physical cancel/override/gear changes clear this latch immediately, whereas
|
||||
# Panda telemetry can lag. Longitudinal software cancellation leaves it intact.
|
||||
stalk_authorized = CS.engagement.lateralEnabled and panda.controlsAllowed
|
||||
stock_main = CS.di_cruise_state in ("STANDBY", "ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL")
|
||||
aol_authorized = bool(panda.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL) and stock_main
|
||||
return bool(stalk_authorized or aol_authorized)
|
||||
@@ -77,12 +77,8 @@ def should_bypass_toyota_long_pid(CP, starpilot_toggles=None) -> bool:
|
||||
) or highlander_sdsu)
|
||||
|
||||
|
||||
def get_toyota_lat_active(car_fingerprint, requested_active: bool, steering_torque: float,
|
||||
steering_pressed: bool) -> bool:
|
||||
if not requested_active or abs(steering_torque) >= MAX_USER_TORQUE:
|
||||
return False
|
||||
|
||||
return not (car_fingerprint == CAR.TOYOTA_COROLLA_TSS2 and steering_pressed)
|
||||
def get_toyota_lat_active(requested_active: bool, steering_torque: float) -> bool:
|
||||
return requested_active and abs(steering_torque) < MAX_USER_TORQUE
|
||||
|
||||
|
||||
def supports_toyota_auto_hold(CP, auto_hold_enabled: bool) -> bool:
|
||||
@@ -343,8 +339,7 @@ class CarController(CarControllerBase):
|
||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
||||
hud_control = CC.hudControl
|
||||
pcm_cancel_cmd = CC.cruiseControl.cancel
|
||||
lat_active = get_toyota_lat_active(self.CP.carFingerprint, CC.latActive,
|
||||
CS.out.steeringTorque, CS.out.steeringPressed)
|
||||
lat_active = get_toyota_lat_active(CC.latActive, CS.out.steeringTorque)
|
||||
|
||||
if len(CC.orientationNED) == 3:
|
||||
self.pitch.update(CC.orientationNED[1])
|
||||
|
||||
@@ -6,12 +6,14 @@ from hypothesis import given, settings, strategies as st
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.can import CANPacker, CANParser
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.lateral import common_fault_avoidance
|
||||
from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car
|
||||
from opendbc.car.toyota import toyotacan
|
||||
from opendbc.car.toyota.carcontroller import CarController, get_camry_hybrid_feedforward, get_long_tune, get_prius_feedforward, \
|
||||
get_prius_positive_feedforward_scale, \
|
||||
get_rav4_interceptor_pedal_scale, \
|
||||
get_toyota_lat_active, \
|
||||
MAX_STEER_RATE, MAX_STEER_RATE_FRAMES, MAX_USER_TORQUE, \
|
||||
limit_interceptor_pcm_accel, \
|
||||
limit_interceptor_stopping_accel, limit_no_lead_cruise_sign_flip, \
|
||||
limit_prius_stopping_accel, should_bypass_toyota_long_pid, supports_toyota_auto_hold, \
|
||||
@@ -735,14 +737,26 @@ class TestToyotaFingerprint:
|
||||
|
||||
|
||||
class TestToyotaCarController:
|
||||
def test_corolla_tss2_hands_off_immediately_when_driver_is_steering(self):
|
||||
assert not get_toyota_lat_active(CAR.TOYOTA_COROLLA_TSS2, True, 117, True)
|
||||
@pytest.mark.parametrize("driver_torque", [-191, -117, -99, 99, 117, 191])
|
||||
def test_toyota_assisting_driver_keeps_lateral_active(self, driver_torque):
|
||||
assert get_toyota_lat_active(True, driver_torque)
|
||||
|
||||
def test_corolla_tss2_stays_active_without_driver_input(self):
|
||||
assert get_toyota_lat_active(CAR.TOYOTA_COROLLA_TSS2, True, 99, False)
|
||||
@pytest.mark.parametrize("driver_torque", [-MAX_USER_TORQUE, MAX_USER_TORQUE, MAX_USER_TORQUE + 1])
|
||||
def test_toyota_high_driver_torque_still_disables_lateral(self, driver_torque):
|
||||
assert not get_toyota_lat_active(True, driver_torque)
|
||||
|
||||
def test_toyota_driver_handoff_behavior_is_corolla_only(self):
|
||||
assert get_toyota_lat_active(CAR.TOYOTA_RAV4_TSS2, True, 117, True)
|
||||
def test_toyota_inactive_request_stays_inactive(self):
|
||||
assert not get_toyota_lat_active(False, 0)
|
||||
|
||||
def test_toyota_assisting_driver_retains_rate_fault_protection(self):
|
||||
counter = 0
|
||||
requests = []
|
||||
for _ in range(36):
|
||||
counter, request = common_fault_avoidance(
|
||||
150 >= MAX_STEER_RATE, get_toyota_lat_active(True, 117), counter, MAX_STEER_RATE_FRAMES,
|
||||
)
|
||||
requests.append(request)
|
||||
assert requests == ([True] * 17 + [False]) * 2
|
||||
|
||||
@staticmethod
|
||||
def _make_controller(*, standstill_req=False, last_standstill=False):
|
||||
|
||||
@@ -170,8 +170,6 @@ class CarController(CarControllerBase):
|
||||
# convention = driver pushing right → yields right authority
|
||||
# (LOOSELY/+ arm), retains left (INV/- arm).
|
||||
# Yield arm scales with |drv| above OVERRIDE_THRESH — strong presses
|
||||
# (potholes, hard corrections) cross past zero so EPS hands the wheel
|
||||
# to the driver in their direction.
|
||||
excess = max(0.0, self.lca_auth_drv_mag_filt - float(P.LCA_AUTH_OVERRIDE_ENTER))
|
||||
yield_signed = float(P.LCA_AUTH_YIELD_BASE) - P.LCA_AUTH_YIELD_SLOPE * excess
|
||||
yield_signed = max(float(P.LCA_AUTH_YIELD_MIN), min(yield_signed, float(P.LCA_AUTH_YIELD_BASE)))
|
||||
|
||||
@@ -1,11 +1,14 @@
|
||||
from collections import defaultdict
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
from opendbc.car.volvo.carcontroller import CarController
|
||||
from opendbc.car.volvo.helpers import checksum_lca_5_message
|
||||
from opendbc.car.volvo.interface import CarInterface
|
||||
from opendbc.car.volvo.values import CAR, DBC
|
||||
from opendbc.car.volvo.volvocan import create_c1_checksum
|
||||
from opendbc.safety.tests.libsafety import libsafety_py
|
||||
|
||||
|
||||
def _zero_message():
|
||||
@@ -70,6 +73,35 @@ def test_controller_relays_stock_lca5_angle_when_inactive():
|
||||
assert abs(raw * 0.05596 - 12.0) < 0.1
|
||||
|
||||
|
||||
@pytest.mark.parametrize("fingerprint", [CAR.POLESTAR_2, CAR.VOLVO_XC40_RECHARGE])
|
||||
@pytest.mark.parametrize("driver_torque", [20.0, -20.0, 128.0, -127.0])
|
||||
def test_override_lca_stream_passes_safety_and_recovers(fingerprint, driver_torque):
|
||||
cp = CarInterface.get_non_essential_params(fingerprint)
|
||||
controller = CarController(DBC[cp.carFingerprint], cp)
|
||||
cs = _state()
|
||||
cc = SimpleNamespace(latActive=True, actuators=_Actuators())
|
||||
safety = libsafety_py.libsafety
|
||||
config = cp.safetyConfigs[0]
|
||||
assert safety.set_safety_hooks(config.safetyModel.raw, config.safetyParam) == 0
|
||||
safety.init_tests()
|
||||
safety.set_controls_allowed(True)
|
||||
|
||||
for active, torque in [(True, 0), (True, driver_torque), (True, -driver_torque),
|
||||
(True, 0), (False, 0), (True, 0)]:
|
||||
cc.latActive = active
|
||||
cs.out.steeringTorque = torque
|
||||
for _ in range(350):
|
||||
_, messages = controller.update(cc, cs, 0, None)
|
||||
address, data, bus = next(msg for msg in messages if msg[0] == 0x58)
|
||||
assert safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)), (
|
||||
active, torque, controller.frame, controller.lca_auth_pos, controller.lca_auth_neg)
|
||||
if active and torque == 0:
|
||||
assert controller.lca_auth_pos == 614
|
||||
assert controller.lca_auth_neg == -614
|
||||
elif active:
|
||||
assert min(abs(controller.lca_auth_pos), abs(controller.lca_auth_neg)) == 0
|
||||
|
||||
|
||||
def _c1_state():
|
||||
return SimpleNamespace(
|
||||
out=SimpleNamespace(steeringAngleDeg=10.0, vEgo=12.0, vEgoRaw=12.0),
|
||||
|
||||
@@ -71,14 +71,12 @@ class CarControllerParams:
|
||||
# (potholes, lane corrections) get full yield while light sustained pressure
|
||||
# only gets a soft yield. yield_signed = YIELD_BASE − YIELD_SLOPE *
|
||||
# max(0, drv_mag_filt − OVERRIDE_ENTER), clamped to [YIELD_MIN, YIELD_BASE].
|
||||
# At |drv|=7 (just over threshold): yield = +60 (light resistance).
|
||||
# At |drv|=14: yield ≈ -4 (crosses past zero — EPS hands wheel to driver).
|
||||
# drv_mag_filt is a low-pass of |drv| (alpha=0.04, ~250 ms time constant) —
|
||||
# without it, 1-2 unit driver-torque jitter became ~10 unit yield-arm jitter
|
||||
# which PSCM converted to felt ripple at sustained co-steering pressure.
|
||||
LCA_AUTH_YIELD_BASE = 60 # yield-arm magnitude at the override threshold
|
||||
LCA_AUTH_YIELD_SLOPE = 8 # counts of yield reduction per unit |drv torque| above threshold
|
||||
LCA_AUTH_YIELD_MIN = -30 # cap how far past zero the yield arm can go (full hand-over)
|
||||
LCA_AUTH_YIELD_MIN = 0 # yield authority without crossing the safety sign boundary
|
||||
LCA_AUTH_YIELD_LP_ALPHA = 0.04 # LP-filter coefficient on |drv| for yield calc (~250 ms tau)
|
||||
LCA_AUTH_SPLIT = 200 # symmetric → asymmetric handover
|
||||
LCA_AUTH_REBUILD_RATE = 230 # counts/s (≈ 2.7 s rebuild from 0 to 614)
|
||||
|
||||
@@ -1,5 +1,15 @@
|
||||
CM_ "IMPORT _subaru_global.dbc";
|
||||
|
||||
BO_ 1723 AVH_Request: 8 XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ REQUEST : 16|2@1+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 811 AVH_Status: 8 XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ ENABLED : 45|1@1+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 72 Transmission: 8 XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -307,6 +307,16 @@ VAL_ 544 AEB_Status 12 "AEB related" 8 "AEB actuation" 4 "AEB related" 0 "No AEB
|
||||
|
||||
CM_ "subaru_global_2017.dbc starts here";
|
||||
|
||||
BO_ 1723 AVH_Request: 8 XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ REQUEST : 16|2@1+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 811 AVH_Status: 8 XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ ENABLED : 45|1@1+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 72 Transmission: 8 XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -136,6 +136,8 @@ static uint32_t subaru_compute_checksum(const CANPacket_t *msg) {
|
||||
return checksum;
|
||||
}
|
||||
|
||||
#include "opendbc/safety/modes/subaru_avh.h"
|
||||
|
||||
static void subaru_rx_hook(const CANPacket_t *msg) {
|
||||
const unsigned int alt_main_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS;
|
||||
const unsigned int status_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_CAM_BUS;
|
||||
@@ -308,6 +310,10 @@ static bool subaru_tx_hook(const CANPacket_t *msg) {
|
||||
violation |= subaru_get_checksum(msg) != subaru_compute_checksum(msg);
|
||||
}
|
||||
|
||||
if (msg->addr == 0x6BBU) {
|
||||
violation |= !subaru_avh_tx(msg);
|
||||
}
|
||||
|
||||
if (violation){
|
||||
tx = false;
|
||||
}
|
||||
@@ -315,6 +321,12 @@ static bool subaru_tx_hook(const CANPacket_t *msg) {
|
||||
}
|
||||
|
||||
static safety_config subaru_init(uint16_t param) {
|
||||
static const CanMsg SUBARU_LEGACY_AVH_TX_MSGS[] = {
|
||||
SUBARU_BASE_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS_ANGLE)
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
|
||||
SUBARU_STOP_START_TX_MSGS(SUBARU_ALT_BUS)
|
||||
{0x6BBU, SUBARU_ALT_BUS, 8, .check_relay = false},
|
||||
};
|
||||
static const CanMsg SUBARU_TX_MSGS[] = {
|
||||
SUBARU_BASE_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS)
|
||||
@@ -455,12 +467,22 @@ static safety_config subaru_init(uint16_t param) {
|
||||
subaru_stop_and_go ? BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_STOP_AND_GO_TX_MSGS) : \
|
||||
BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_TX_MSGS);
|
||||
}
|
||||
bool avh_enabled = false;
|
||||
#ifdef ALLOW_DEBUG
|
||||
avh_enabled = GET_FLAG(param, 1024U) && subaru_gen2 && subaru_lkas_angle && subaru_fixed_angle_limits &&
|
||||
subaru_stop_start_button && !subaru_d_platform && !GET_FLAG(param, 2U) && !subaru_redneck_cruise;
|
||||
#endif
|
||||
subaru_avh_init(avh_enabled);
|
||||
if (avh_enabled) {
|
||||
ret = BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_LEGACY_AVH_TX_MSGS);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
const safety_hooks subaru_hooks = {
|
||||
.init = subaru_init,
|
||||
.rx = subaru_rx_hook,
|
||||
.rx_all = subaru_avh_rx,
|
||||
.tx = subaru_tx_hook,
|
||||
.get_counter = subaru_get_counter,
|
||||
.get_checksum = subaru_get_checksum,
|
||||
|
||||
@@ -0,0 +1,140 @@
|
||||
#pragma once
|
||||
|
||||
// Legacy startup AVH only. Never transmit the 0x32B status message.
|
||||
static const unsigned int SUBARU_AVH_INPUTS[] = {0x6BBU, 0x32BU, 0x40U, 0x48U, 0x13AU, 0x174U};
|
||||
static uint8_t subaru_avh_data[6][8];
|
||||
static uint32_t subaru_avh_ts[6];
|
||||
static bool subaru_avh_seen[6];
|
||||
static bool subaru_avh_seq[6];
|
||||
static bool subaru_avh_enabled;
|
||||
static bool subaru_avh_done;
|
||||
static unsigned int subaru_avh_count;
|
||||
static uint32_t subaru_avh_start;
|
||||
static uint32_t subaru_avh_sent;
|
||||
static uint32_t subaru_avh_template_ts;
|
||||
static uint32_t subaru_avh_stable_since;
|
||||
static bool subaru_avh_stable;
|
||||
|
||||
static void subaru_avh_init(bool enabled) {
|
||||
subaru_avh_enabled = enabled;
|
||||
subaru_avh_done = false;
|
||||
subaru_avh_count = 0U;
|
||||
subaru_avh_start = microsecond_timer_get();
|
||||
subaru_avh_sent = 0U;
|
||||
subaru_avh_template_ts = 0U;
|
||||
subaru_avh_stable_since = 0U;
|
||||
subaru_avh_stable = false;
|
||||
for (int i = 0; i < 6; i++) {
|
||||
subaru_avh_seen[i] = false;
|
||||
subaru_avh_seq[i] = false;
|
||||
subaru_avh_ts[i] = 0U;
|
||||
for (int j = 0; j < 8; j++) {
|
||||
subaru_avh_data[i][j] = 0U;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool subaru_avh_ready(uint32_t now) {
|
||||
bool ready = true;
|
||||
for (int i = 0; i < 6; i++) {
|
||||
ready &= subaru_avh_seen[i] && subaru_avh_seq[i] &&
|
||||
(safety_get_ts_elapsed(now, subaru_avh_ts[i]) <= ((i == 0) ? 1500000U : 300000U));
|
||||
}
|
||||
const unsigned int rpm = ((unsigned int)subaru_avh_data[2][2] | ((unsigned int)subaru_avh_data[2][3] << 8U)) & 0x1FFFU;
|
||||
ready &= (rpm >= 400U) && (subaru_avh_data[2][4] == 0U) && (subaru_avh_data[3][3] == 4U);
|
||||
ready &= (subaru_avh_data[5][2] & 8U) != 0U;
|
||||
ready &= !vehicle_moving && !controls_allowed;
|
||||
return ready;
|
||||
}
|
||||
|
||||
static void subaru_avh_rx(const CANPacket_t *msg) {
|
||||
if (subaru_avh_enabled && !subaru_avh_done && (msg->bus == 1U)) {
|
||||
const uint32_t now = microsecond_timer_get();
|
||||
for (int i = 0; i < 6; i++) {
|
||||
if (msg->addr == SUBARU_AVH_INPUTS[i]) {
|
||||
if ((GET_LEN(msg) != 8U) || (subaru_get_checksum(msg) != subaru_compute_checksum(msg))) {
|
||||
subaru_avh_done = true;
|
||||
} else {
|
||||
const uint8_t old_counter = subaru_avh_data[i][1] & 0xFU;
|
||||
const uint8_t counter = msg->data[1] & 0xFU;
|
||||
if (!subaru_avh_seen[i] || (counter != old_counter)) {
|
||||
subaru_avh_seq[i] = subaru_avh_seen[i] && (counter == ((old_counter + 1U) & 0xFU));
|
||||
subaru_avh_seen[i] = true;
|
||||
subaru_avh_ts[i] = now;
|
||||
for (int j = 0; j < 8; j++) {
|
||||
subaru_avh_data[i][j] = msg->data[j];
|
||||
}
|
||||
}
|
||||
if (((i == 0) && ((msg->data[2] & 3U) != 0U)) ||
|
||||
((i == 1) && ((msg->data[5] & 0x20U) != 0U)) ||
|
||||
((i == 2) && (msg->data[4] != 0U)) || ((i == 3) && (msg->data[3] != 4U)) ||
|
||||
((i == 4) && (((GET_BYTES(msg, 1, 3) >> 4) & 0x1FFFU) != 0U ||
|
||||
((GET_BYTES(msg, 3, 3) >> 1) & 0x1FFFU) != 0U ||
|
||||
((GET_BYTES(msg, 4, 3) >> 6) & 0x1FFFU) != 0U ||
|
||||
((GET_BYTES(msg, 6, 2) >> 3) & 0x1FFFU) != 0U))) {
|
||||
subaru_avh_done = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (controls_allowed || (safety_get_ts_elapsed(now, subaru_avh_start) > 30000000U)) {
|
||||
subaru_avh_done = true;
|
||||
}
|
||||
if (!subaru_avh_ready(now)) {
|
||||
subaru_avh_stable = false;
|
||||
if (subaru_avh_count > 0U) {
|
||||
subaru_avh_done = true;
|
||||
}
|
||||
} else if (!subaru_avh_stable) {
|
||||
subaru_avh_stable = true;
|
||||
subaru_avh_stable_since = now;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool subaru_avh_tx(const CANPacket_t *msg) {
|
||||
const uint32_t now = microsecond_timer_get();
|
||||
const uint32_t elapsed = safety_get_ts_elapsed(now, subaru_avh_start);
|
||||
const bool second = subaru_avh_count == 1U;
|
||||
bool allowed = subaru_avh_enabled && !subaru_avh_done && (subaru_avh_count < 2U) &&
|
||||
(msg->bus == 1U) && (GET_LEN(msg) == 8U) && !safety_rx_checks_invalid &&
|
||||
(elapsed >= 10000000U) && (elapsed <= 30000000U) && subaru_avh_ready(now) &&
|
||||
subaru_avh_stable && (safety_get_ts_elapsed(now, subaru_avh_stable_since) >= 3000000U);
|
||||
// Rejected generic RX frames may not reach our hook; invalidate their cached inputs too.
|
||||
for (int i = 0; i < current_safety_config.rx_checks_len; i++) {
|
||||
const RxCheck *check = ¤t_safety_config.rx_checks[i];
|
||||
for (int j = 0; j < 6; j++) {
|
||||
if (((unsigned int)check->msg[check->status.index].addr == SUBARU_AVH_INPUTS[j]) && (check->msg[check->status.index].bus == 1U)) {
|
||||
allowed &= check->status.valid_checksum && (check->status.wrong_counters < MAX_WRONG_COUNTERS);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (second) {
|
||||
const uint32_t spacing = safety_get_ts_elapsed(now, subaru_avh_sent);
|
||||
allowed &= (spacing >= 45000U) && (spacing <= 80000U) && (subaru_avh_ts[0] == subaru_avh_template_ts) &&
|
||||
(safety_get_ts_elapsed(now, subaru_avh_ts[0]) <= 110000U);
|
||||
} else {
|
||||
allowed &= safety_get_ts_elapsed(now, subaru_avh_ts[0]) <= 30000U;
|
||||
}
|
||||
uint8_t sum = (uint8_t)(0xBBU + 6U);
|
||||
for (int i = 1; i < 8; i++) {
|
||||
uint8_t expected = subaru_avh_data[0][i];
|
||||
if (i == 1) {
|
||||
expected = (expected & 0xF0U) | ((expected + (second ? 2U : 1U)) & 0xFU);
|
||||
} else if (i == 2) {
|
||||
expected |= 2U;
|
||||
} else {
|
||||
// Preserve every unrelated payload bit.
|
||||
}
|
||||
allowed &= msg->data[i] == expected;
|
||||
sum += expected;
|
||||
}
|
||||
allowed &= msg->data[0] == sum;
|
||||
if (allowed) {
|
||||
subaru_avh_count++;
|
||||
subaru_avh_sent = now;
|
||||
subaru_avh_template_ts = subaru_avh_ts[0];
|
||||
subaru_avh_done = second;
|
||||
}
|
||||
return allowed;
|
||||
}
|
||||
@@ -82,3 +82,19 @@ def test_non_ray_hyundai_ev_keeps_native_driver_gas_detection():
|
||||
native_gas = bytes.fromhex("004e008000ae0700")
|
||||
assert safety.safety_rx_hook(libsafety_py.make_CANPacket(0x371, 0, native_gas))
|
||||
assert safety.get_gas_pressed_prev()
|
||||
|
||||
|
||||
@pytest.mark.parametrize("controls_allowed", [False, True])
|
||||
def test_ray_native_cruise_cancel_allowed_during_pedal_override(controls_allowed):
|
||||
safety = libsafety_py.libsafety
|
||||
safety.set_safety_hooks(CarParams.SafetyModel.hyundai, 0x9405)
|
||||
safety.init_tests()
|
||||
safety.set_controls_allowed(controls_allowed)
|
||||
safety.set_gas_pressed_prev(True)
|
||||
packer = CANPacker("hyundai_can_refresh_generated")
|
||||
addr, dat, bus = packer.make_can_msg("CLU11", 0, {"CF_Clu_CruiseSwState": 4})
|
||||
assert safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, dat))
|
||||
|
||||
pedal_packer = CANPacker("hyundai_kia_ray_pedal")
|
||||
addr, dat, bus = create_gas_interceptor_command(pedal_packer, 0.1, 3)
|
||||
assert not safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, dat))
|
||||
|
||||
@@ -0,0 +1,107 @@
|
||||
import pytest
|
||||
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.subaru.avh import AVH_REQUEST, AVH_STATUS, INPUTS, avh_request, checksum
|
||||
from opendbc.car.subaru.tests.test_avh import sample
|
||||
from opendbc.car.subaru.values import SubaruSafetyFlags
|
||||
from opendbc.safety.tests.libsafety import libsafety_py
|
||||
|
||||
FLAGS = int(SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.FIXED_ANGLE_LIMITS |
|
||||
SubaruSafetyFlags.STOP_START_BUTTON | SubaruSafetyFlags.AVH_STARTUP)
|
||||
|
||||
|
||||
def packet(address, data, bus=1):
|
||||
return libsafety_py.make_CANPacket(address, bus, data)
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def safety():
|
||||
s = libsafety_py.libsafety
|
||||
s.set_timer(0)
|
||||
assert s.set_safety_hooks(CarParams.SafetyModel.subaru, FLAGS) == 0
|
||||
s.set_controls_allowed(False)
|
||||
for tick in range(101):
|
||||
s.set_timer(tick * 100_000)
|
||||
for address in INPUTS:
|
||||
if address != AVH_REQUEST or tick % 10 == 0:
|
||||
assert s.safety_rx_hook(packet(address, sample(address, tick // 10 if address == AVH_REQUEST else tick)))
|
||||
return s
|
||||
|
||||
|
||||
def request(step=1):
|
||||
return avh_request(sample(AVH_REQUEST, 10), step)[1]
|
||||
|
||||
|
||||
def test_pair_and_third_frame_blocked(safety):
|
||||
assert safety.safety_tx_hook(packet(AVH_REQUEST, request()))
|
||||
safety.set_timer(10_050_000)
|
||||
assert safety.safety_tx_hook(packet(AVH_REQUEST, request(2)))
|
||||
safety.set_timer(10_100_000)
|
||||
assert not safety.safety_tx_hook(packet(AVH_REQUEST, request(2)))
|
||||
|
||||
|
||||
@pytest.mark.parametrize('byte', range(8))
|
||||
def test_payload_mutation_blocked(safety, byte):
|
||||
data = bytearray(request())
|
||||
data[byte] ^= 4
|
||||
if byte:
|
||||
data[0] = checksum(AVH_REQUEST, data)
|
||||
assert not safety.safety_tx_hook(packet(AVH_REQUEST, data))
|
||||
|
||||
|
||||
@pytest.mark.parametrize('bus', [0, 2])
|
||||
def test_wrong_bus_blocked(safety, bus):
|
||||
assert not safety.safety_tx_hook(packet(AVH_REQUEST, request(), bus))
|
||||
|
||||
|
||||
@pytest.mark.parametrize('delay', [44_999, 80_001])
|
||||
def test_followup_timing(safety, delay):
|
||||
assert safety.safety_tx_hook(packet(AVH_REQUEST, request()))
|
||||
safety.set_timer(10_000_000 + delay)
|
||||
assert not safety.safety_tx_hook(packet(AVH_REQUEST, request(2)))
|
||||
|
||||
|
||||
@pytest.mark.parametrize('address,offset,value', [(AVH_REQUEST, 2, 1), (AVH_STATUS, 5, 32),
|
||||
(0x40, 4, 1), (0x48, 3, 3), (0x13A, 2, 1)])
|
||||
def test_abort_on_manual_ack_or_movement(safety, address, offset, value):
|
||||
assert safety.safety_tx_hook(packet(AVH_REQUEST, request()))
|
||||
safety.set_timer(10_050_000)
|
||||
data = bytearray(sample(address, 11 if address == AVH_REQUEST else 101))
|
||||
data[offset] = value
|
||||
data[0] = checksum(address, data)
|
||||
assert safety.safety_rx_hook(packet(address, data))
|
||||
assert not safety.safety_tx_hook(packet(AVH_REQUEST, request(2)))
|
||||
|
||||
|
||||
def test_stale_template_and_status_tx_blocked(safety):
|
||||
assert not safety.safety_tx_hook(packet(AVH_STATUS, sample(AVH_STATUS, 1)))
|
||||
safety.set_timer(10_030_001)
|
||||
assert not safety.safety_tx_hook(packet(AVH_REQUEST, request()))
|
||||
|
||||
|
||||
@pytest.mark.parametrize('flags', [FLAGS & ~1024, FLAGS | 32, FLAGS | 2, FLAGS & ~16, FLAGS | 512])
|
||||
def test_permission_gates(safety, flags):
|
||||
assert safety.set_safety_hooks(CarParams.SafetyModel.subaru, flags) == 0
|
||||
assert not safety.safety_tx_hook(packet(AVH_REQUEST, request()))
|
||||
|
||||
|
||||
@pytest.mark.parametrize('reason', ['new_template', 'corrupt', 'engaged', 'expired', 'duplicate'])
|
||||
def test_extra_failure_gates(safety, reason):
|
||||
if reason == 'engaged':
|
||||
safety.set_controls_allowed(True)
|
||||
elif reason == 'expired':
|
||||
safety.set_timer(30_000_001)
|
||||
elif reason == 'duplicate':
|
||||
safety.set_timer(10_040_000)
|
||||
assert safety.safety_rx_hook(packet(AVH_REQUEST, sample(AVH_REQUEST, 10)))
|
||||
elif reason == 'corrupt':
|
||||
data = bytearray(sample(AVH_REQUEST, 11))
|
||||
data[0] ^= 1
|
||||
safety.safety_rx_hook(packet(AVH_REQUEST, data))
|
||||
else:
|
||||
assert safety.safety_tx_hook(packet(AVH_REQUEST, request()))
|
||||
safety.set_timer(10_050_000)
|
||||
assert safety.safety_rx_hook(packet(AVH_REQUEST, sample(AVH_REQUEST, 11)))
|
||||
assert not safety.safety_tx_hook(packet(AVH_REQUEST, request(2)))
|
||||
return
|
||||
assert not safety.safety_tx_hook(packet(AVH_REQUEST, request()))
|
||||
@@ -363,7 +363,14 @@ class Car:
|
||||
elif any(be.type in (ButtonType.decelCruise, ButtonType.setCruise) for be in CS.buttonEvents):
|
||||
self.resume_prev_button = False
|
||||
|
||||
FPCS = self.starpilot_card.update(CS, FPCS, self.sm, self.starpilot_toggles)
|
||||
preap_authorized = False
|
||||
if self.CP.carFingerprint == "TESLA_MODEL_S_PREAP":
|
||||
from opendbc.car.tesla.preap.lateral import preap_lateral_authorized
|
||||
preap_authorized = preap_lateral_authorized(
|
||||
self.CP, self.CI.CS, self.sm['pandaStates'], self.sm.all_checks(['pandaStates']),
|
||||
)
|
||||
self.CI.CS.preap_lateral_authorized = preap_authorized
|
||||
FPCS = self.starpilot_card.update(CS, FPCS, self.sm, self.starpilot_toggles, preap_authorized=preap_authorized)
|
||||
return CS, RD, FPCS
|
||||
|
||||
def state_publish(self, CS: car.CarState, RD: structs.RadarDataT | None, FPCS: custom.StarPilotCarState):
|
||||
|
||||
@@ -17,6 +17,7 @@ from openpilot.starpilot.common.car_params_capability import capability_car_para
|
||||
from openpilot.system.hardware import HARDWARE, PC
|
||||
from openpilot.starpilot.common.screen_settings import (
|
||||
alert_wake_key, brightness_preferences, calculate_screen_brightness, enabled_wake_keys, standby_button_press_time,
|
||||
screen_off_toggle_counter,
|
||||
)
|
||||
|
||||
BACKLIGHT_OFFROAD = 65 if HARDWARE.get_device_type() == "mici" else 50
|
||||
@@ -308,6 +309,9 @@ class Device:
|
||||
|
||||
def __init__(self):
|
||||
self._ignition = False
|
||||
self._screen_off = False
|
||||
self._screen_off_started = ui_state.started
|
||||
self._screen_off_counter = screen_off_toggle_counter(ui_state.params_memory)
|
||||
self._last_button_press = standby_button_press_time(ui_state.params_memory)
|
||||
self._last_car_button_frame = int(time.monotonic() * 1e9)
|
||||
self._last_turn_signal = None
|
||||
@@ -444,6 +448,8 @@ class Device:
|
||||
self._last_brightness = brightness
|
||||
|
||||
def _calculate_brightness(self) -> int:
|
||||
if self._screen_off:
|
||||
return 0
|
||||
clipped_brightness = self._offroad_brightness
|
||||
|
||||
if ui_state.started and ui_state.light_sensor >= 0:
|
||||
@@ -484,7 +490,22 @@ class Device:
|
||||
wake_for_onroad_event = (ui_state.started and self._standby_mode and self._screen_brightness_onroad != 0 and
|
||||
(selected_status_change or self._visible_onroad_alert() or selected_turn_signal))
|
||||
|
||||
if ignition_state_changed or any(ev.left_down for ev in gui_app.mouse_events) or button_pressed or wake_for_onroad_event:
|
||||
counter = screen_off_toggle_counter(ui_state.params_memory)
|
||||
presses = counter - self._screen_off_counter
|
||||
self._screen_off_counter = counter
|
||||
road_changed = ui_state.started != self._screen_off_started
|
||||
self._screen_off_started = ui_state.started
|
||||
touched = any(ev.left_down for ev in gui_app.mouse_events)
|
||||
was_screen_off = self._screen_off
|
||||
if not ui_state.started or road_changed or ignition_state_changed:
|
||||
self._screen_off = False
|
||||
elif presses > 0:
|
||||
if presses % 2:
|
||||
self._screen_off = not self._screen_off
|
||||
elif touched or wake_for_onroad_event or "StandbyWakeCriticalAlert" in self._active_standby_alerts():
|
||||
self._screen_off = False
|
||||
|
||||
if ignition_state_changed or touched or button_pressed or wake_for_onroad_event or presses > 0 or (was_screen_off and not self._screen_off):
|
||||
self._reset_interactive_timeout()
|
||||
|
||||
interaction_timeout = time.monotonic() > self._interaction_time
|
||||
@@ -496,7 +517,7 @@ class Device:
|
||||
standby_active = ui_state.started and self._standby_mode
|
||||
keep_display_awake = not interaction_timeout or PC
|
||||
keep_display_awake |= ui_state.ignition and not standby_active
|
||||
self._set_awake(keep_display_awake)
|
||||
self._set_awake(keep_display_awake and not self._screen_off)
|
||||
|
||||
@staticmethod
|
||||
def _fresh_message(name):
|
||||
|
||||
@@ -486,7 +486,10 @@ class FordLateralController:
|
||||
curvature_rate = 0.0
|
||||
|
||||
self.curvature_last = float(np.clip(applied, -0.02, 0.02))
|
||||
curvature_rate = float(np.clip(curvature_rate, -0.001024, 0.001023))
|
||||
min_curvature_rate = -0.001024
|
||||
if self.CP.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and self.CP.flags & FordFlags.CANFD:
|
||||
min_curvature_rate = -0.001023
|
||||
curvature_rate = float(np.clip(curvature_rate, min_curvature_rate, 0.001023))
|
||||
return FordLateralResult(
|
||||
curvature=self.curvature_last,
|
||||
curvature_rate=curvature_rate,
|
||||
|
||||
@@ -7,7 +7,7 @@ from opendbc.can import CANPacker
|
||||
from opendbc.car.ford.fordcan import CanBus
|
||||
from opendbc.car.ford.values import CAR, FordFlags
|
||||
from .. import fordcan
|
||||
from ..lateral import FordLateralController, HumanTurnDetector
|
||||
from ..lateral import FordLateralController, HumanTurnDetector, STEER_DT
|
||||
|
||||
|
||||
class FakeSubMaster(dict):
|
||||
@@ -77,6 +77,51 @@ def test_extended_messages_are_curvature_only(canfd):
|
||||
assert raw_path_offset == 512
|
||||
|
||||
|
||||
@pytest.mark.parametrize("requested_rate", (-0.002, -0.001024, -0.001023, -0.0005, 0.0, 0.0005, 0.001023, 0.002))
|
||||
def test_mach_e_canfd_curvature_rate_survives_wire_sign_conversion(controller, monkeypatch, requested_rate):
|
||||
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
speed = 8.0
|
||||
predicted = -0.012
|
||||
controller.curvature_last = predicted
|
||||
controller.desired_curvature_last = predicted
|
||||
controller.curvature_samples.append(predicted - requested_rate * STEER_DT * speed)
|
||||
monkeypatch.setattr(controller, "_predicted_curvature", lambda *_args: predicted)
|
||||
|
||||
result = controller.update(
|
||||
SimpleNamespace(latActive=True), car_state(speed=speed, curvature=predicted),
|
||||
SimpleNamespace(curvature=predicted))
|
||||
expected = max(-0.001023, min(0.001023, requested_rate))
|
||||
assert result.curvature == pytest.approx(predicted)
|
||||
assert result.curvature_rate == pytest.approx(expected)
|
||||
|
||||
packer = CANPacker("ford_lincoln_base_pt")
|
||||
can_bus = CanBus(SimpleNamespace(flags=FordFlags.CANFD, safetyConfigs=[SimpleNamespace()]))
|
||||
_, data, _ = fordcan.create_lat_ctl2_msg(
|
||||
packer, can_bus, 1, result.ramp_type, result.precision_type,
|
||||
-result.curvature, -result.curvature_rate, 0)
|
||||
decoded_rate = ((data[6] << 3) | (data[7] >> 5)) * 1e-6 - 0.001024
|
||||
assert -decoded_rate == pytest.approx(expected, abs=0.5e-6)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("fingerprint,flags", (
|
||||
(CAR.FORD_MUSTANG_MACH_E_MK1, 0),
|
||||
(CAR.FORD_EXPLORER_MK6, FordFlags.CANFD),
|
||||
(CAR.FORD_EDGE_MK2, 0),
|
||||
))
|
||||
def test_mach_e_canfd_rate_bound_preserves_other_paths(controller, monkeypatch, fingerprint, flags):
|
||||
controller.CP.carFingerprint = fingerprint
|
||||
controller.CP.flags = flags
|
||||
controller.desired_curvature_last = -0.012
|
||||
controller.curvature_samples.append(0.0)
|
||||
monkeypatch.setattr(controller, "_predicted_curvature", lambda *_args: -0.012)
|
||||
|
||||
result = controller.update(
|
||||
SimpleNamespace(latActive=True), car_state(speed=8.0), SimpleNamespace(curvature=-0.012))
|
||||
|
||||
assert result.curvature_rate == pytest.approx(-0.001024)
|
||||
|
||||
|
||||
def test_curvature_strategy_uses_polynomial_signals(controller):
|
||||
result = controller.update(
|
||||
SimpleNamespace(latActive=True), car_state(), SimpleNamespace(curvature=0.001))
|
||||
|
||||
@@ -3609,6 +3609,16 @@
|
||||
"ui_type": "toggle",
|
||||
"settings_tier": "simple"
|
||||
},
|
||||
{
|
||||
"key": "SubaruAvhStartup",
|
||||
"label": "AVH On at Startup (Experimental)",
|
||||
"description": "For the 2025 Subaru Legacy, request Auto Vehicle Hold once at startup while stopped in Park with the engine running. Manual choices are preserved. Enable before the next drive.",
|
||||
"picker_description": "Requests AVH ON at startup on the 2025 Legacy.",
|
||||
"data_type": "bool",
|
||||
"ui_type": "toggle",
|
||||
"galaxy_only": true,
|
||||
"settings_tier": "simple"
|
||||
},
|
||||
{
|
||||
"key": "SubaruStopStartOff",
|
||||
"label": "Stop/Start Off at Startup",
|
||||
|
||||
@@ -8,7 +8,9 @@ CONTROLLER_ACTION_FORCE_COAST = "__starpilot_controller_action__:force_coast"
|
||||
CONTROLLER_ACTION_TOGGLE_AOL = "__starpilot_controller_action__:toggle_aol"
|
||||
CONTROLLER_ACTION_ENGAGE = "__starpilot_controller_action__:engage_openpilot"
|
||||
CONTROLLER_ACTION_DISENGAGE = "__starpilot_controller_action__:disengage_openpilot"
|
||||
CONTROLLER_ACTION_SCREEN_OFF = "__starpilot_controller_action__:toggle_screen_off"
|
||||
CONTROLLER_ACTION_COUNTERS = {
|
||||
CONTROLLER_ACTION_SCREEN_OFF: "ScreenOffToggleCounter",
|
||||
CONTROLLER_ACTION_BOOKMARK: "WheelButtonBookmarkCounter",
|
||||
CONTROLLER_ACTION_PULSE_AND_GLIDE: "WheelControlPulseGlideCounter",
|
||||
CONTROLLER_ACTION_FORCE_COAST: "WheelControlForceCoastCounter",
|
||||
@@ -17,6 +19,12 @@ CONTROLLER_ACTION_COUNTERS = {
|
||||
CONTROLLER_ACTION_DISENGAGE: "WheelControlDisengageCounter",
|
||||
}
|
||||
CONTROLLER_ACTION_OPTIONS = (
|
||||
{
|
||||
"key": CONTROLLER_ACTION_SCREEN_OFF,
|
||||
"label": "Toggle Screen Off",
|
||||
"description": "Turns only the display off while driving. Press again, touch the screen, or go offroad to wake it. Driving controls stay active.",
|
||||
"section": "Controller Actions",
|
||||
},
|
||||
{
|
||||
"key": CONTROLLER_ACTION_CYCLE_PERSONALITY,
|
||||
"label": "Cycle Driving Personality",
|
||||
|
||||
@@ -211,6 +211,7 @@ SAFE_MODE_MANAGED_KEYS = (
|
||||
"SubaruSNG",
|
||||
"SubaruSNGManualParkingBrake",
|
||||
"SubaruStopStartOff",
|
||||
"SubaruAvhStartup",
|
||||
"SubaruRedneckCruise",
|
||||
"VoltSNG",
|
||||
"JeepBrakeHold",
|
||||
@@ -230,6 +231,7 @@ SAFE_MODE_FIXED_VALUES = {
|
||||
"LongitudinalPersonality": int(log.LongitudinalPersonality.relaxed),
|
||||
"UseAutoSteerDelay": True,
|
||||
"SubaruStopStartOff": False,
|
||||
"SubaruAvhStartup": False,
|
||||
"SubaruRedneckCruise": False,
|
||||
PERSONALITY_PROFILES_PARAM: profile_document(default_personality_profiles(False), enabled=False),
|
||||
}
|
||||
|
||||
@@ -15,6 +15,9 @@ from openpilot.common.params import UnknownKeyName
|
||||
BRIGHTNESS_KEYS = ('ScreenBrightness', 'ScreenBrightnessOnroad')
|
||||
SCREEN_INT_KEYS = frozenset(key + suffix for key in BRIGHTNESS_KEYS for suffix in ('', 'Manual', 'Offset'))
|
||||
STANDBY_BUTTON_PRESS_PARAM = 'StandbyButtonPressTime'
|
||||
SCREEN_OFF_TOGGLE_PARAM = 'ScreenOffToggleCounter'
|
||||
|
||||
|
||||
SCREEN_WAKE_OPTIONS = (
|
||||
('StandbyWakeEngage', 'Engagement', True),
|
||||
('StandbyWakeDisengage', 'Disengagement', True),
|
||||
@@ -192,6 +195,13 @@ def alert_wake_key(alert):
|
||||
return None
|
||||
|
||||
|
||||
def screen_off_toggle_counter(params):
|
||||
try:
|
||||
return int(_raw(params, SCREEN_OFF_TOGGLE_PARAM) or 0)
|
||||
except (TypeError, ValueError, OverflowError):
|
||||
return 0
|
||||
|
||||
|
||||
def standby_button_press_time(params):
|
||||
try:
|
||||
return max(0, int(_raw(params, STANDBY_BUTTON_PRESS_PARAM) or 0))
|
||||
|
||||
@@ -1591,6 +1591,7 @@ class StarPilotVariables:
|
||||
toggle.subaru_stop_start_off = self.get_value(
|
||||
"SubaruStopStartOff", condition=toggle.car_model in SUBARU_STOP_START_CARS,
|
||||
)
|
||||
toggle.subaru_avh_on = self.get_value("SubaruAvhStartup", condition=toggle.car_model == "SUBARU_LEGACY_2025")
|
||||
toggle.jeep_brake_hold = self.get_value(
|
||||
"JeepBrakeHold",
|
||||
condition=toggle.car_make == "chrysler" and toggle.car_model in CHRYSLER_JEEPS,
|
||||
|
||||
@@ -0,0 +1,110 @@
|
||||
"""Regress StarPilot's route51 AOL-to-stalk transition against real Panda hooks."""
|
||||
from types import SimpleNamespace
|
||||
import pytest
|
||||
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.tesla.carcontroller import CarController
|
||||
from opendbc.car.tesla.interface import CarInterface
|
||||
from opendbc.car.tesla.values import CAR, DBC
|
||||
from opendbc.safety.tests.libsafety import libsafety_py
|
||||
from opendbc.car.tesla.preap.lateral import preap_lateral_authorized
|
||||
from opendbc.car.tesla.preap.engagement import PreAPEngagement
|
||||
|
||||
|
||||
@pytest.mark.parametrize('stock_main', [False, True])
|
||||
def test_preap_aol_stalk_cancel_and_reengagement(stock_main):
|
||||
cp = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_PREAP)
|
||||
controller = CarController(DBC[CAR.TESLA_MODEL_S_PREAP], cp)
|
||||
controller.stock_cc = None
|
||||
safety = libsafety_py.libsafety
|
||||
assert safety.set_safety_hooks(cp.safetyConfigs[0].safetyModel.raw, 0) == 0
|
||||
safety.init_tests()
|
||||
safety.set_alternative_experience(32)
|
||||
packer = CANPacker('tesla_can')
|
||||
state = structs.CarState.new_message()
|
||||
state.vEgoRaw = state.vEgo = 18.8
|
||||
state.gearShifter = structs.CarState.GearShifter.drive
|
||||
cs = SimpleNamespace(out=state.as_reader(), hands_on_level=0, cruiseEnabled=False,
|
||||
di_cruise_state='STANDBY' if stock_main else 'OFF', engagement=PreAPEngagement(False, 500))
|
||||
panda = SimpleNamespace(safetyModel=cp.safetyConfigs[0].safetyModel, safetyParam=0,
|
||||
safetyRxChecksInvalid=False, alternativeExperience=32, controlsAllowed=False)
|
||||
active_commands = 0
|
||||
|
||||
for frame in range(600):
|
||||
safety.set_timer(frame * 10000)
|
||||
for name, values in (
|
||||
('EPAS_sysStatus', {'EPAS_internalSAS': 0, 'EPAS_eacStatus': 1}),
|
||||
('ESP_B', {'ESP_vehicleSpeed': 18.8 * 3.6}),
|
||||
('DI_torque2', {'DI_gear': 4}),
|
||||
('GTW_carState', {}),
|
||||
('DI_state', {'DI_cruiseState': 1 if stock_main else 0}),
|
||||
('STW_ACTN_RQ', {'SpdCtrlLvr_Stat': 2 if frame in (200, 500) else 1 if frame == 400 else 0}),
|
||||
):
|
||||
addr, dat, bus = packer.make_can_msg(name, 0, values)
|
||||
assert safety.safety_rx_hook(libsafety_py.make_CANPacket(addr, bus, dat))
|
||||
|
||||
button = 2 if frame in (200, 500) else 1 if frame == 400 else 0
|
||||
previous = 2 if frame in (201, 501) else 1 if frame == 401 else 0
|
||||
cs.engagement.process_buttons(button, previous, 10000 + frame * 10, 18.8, 'KPH', False, False, True, False)
|
||||
cs.engagement.check_can_engage(False, state.gearShifter, False)
|
||||
cs.cruiseEnabled = cs.engagement.cruiseEnabled
|
||||
panda.controlsAllowed = safety.get_controls_allowed()
|
||||
cs.preap_lateral_authorized = preap_lateral_authorized(cp, cs, [panda], not 300 <= frame < 320)
|
||||
cc = structs.CarControl.new_message()
|
||||
# AOL only, with no normal engagement. A delayed control packet continues
|
||||
# requesting steering even during cancel and telemetry loss.
|
||||
cc.enabled = False
|
||||
cc.latActive = True
|
||||
cc.cruiseControl.cancel = cs.cruiseEnabled
|
||||
cc.actuators.steeringAngleDeg = -12.2
|
||||
_, sends = controller.update(cc.as_reader(), cs, frame * 10000000, None)
|
||||
for addr, dat, bus in sends:
|
||||
assert safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, dat)), (frame, hex(addr), dat.hex())
|
||||
if addr == 0x488 and dat[2] >> 6 == 1:
|
||||
active_commands += 1
|
||||
assert active_commands == (290 if stock_main else 140)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('failure', ['stale', 'wrong_mode', 'wrong_param', 'rx_invalid', 'park', 'door', 'override', 'cancel'])
|
||||
def test_preap_authorization_fails_closed(failure):
|
||||
cp = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_PREAP)
|
||||
out = SimpleNamespace(gearShifter=structs.CarState.GearShifter.drive, doorOpen=False, steeringDisengage=False)
|
||||
cs = SimpleNamespace(out=out, engagement=SimpleNamespace(lateralEnabled=True), di_cruise_state='OFF')
|
||||
panda = SimpleNamespace(safetyModel=cp.safetyConfigs[0].safetyModel, safetyParam=0,
|
||||
safetyRxChecksInvalid=False, alternativeExperience=32, controlsAllowed=True)
|
||||
assert preap_lateral_authorized(cp, cs, [panda], True)
|
||||
if failure == 'wrong_mode':
|
||||
panda.safetyModel = structs.CarParams.SafetyModel.noOutput
|
||||
if failure == 'wrong_param':
|
||||
panda.safetyParam = 1
|
||||
if failure == 'rx_invalid':
|
||||
panda.safetyRxChecksInvalid = True
|
||||
if failure == 'park':
|
||||
out.gearShifter = structs.CarState.GearShifter.park
|
||||
if failure == 'door':
|
||||
out.doorOpen = True
|
||||
if failure == 'override':
|
||||
out.steeringDisengage = True
|
||||
if failure == 'cancel':
|
||||
cs.engagement.lateralEnabled = False
|
||||
assert not preap_lateral_authorized(cp, cs, [panda], failure != 'stale')
|
||||
|
||||
|
||||
@pytest.mark.parametrize('reset', ['cancel', 'override', 'door', 'park'])
|
||||
def test_preap_physical_lateral_session_requires_new_pull_after_reset(reset):
|
||||
engagement = PreAPEngagement(False, 500)
|
||||
engagement.process_buttons(2, 0, 10000, 10., 'KPH', False, False, True, False)
|
||||
assert engagement.lateralEnabled
|
||||
if reset == 'cancel':
|
||||
engagement.process_buttons(1, 0, 11000, 10., 'KPH', False, False, True, False)
|
||||
elif reset == 'override':
|
||||
engagement.handle_steering_disengage(True)
|
||||
engagement.handle_steering_disengage(False)
|
||||
else:
|
||||
gear = structs.CarState.GearShifter.park if reset == 'park' else structs.CarState.GearShifter.drive
|
||||
engagement.check_can_engage(reset == 'door', gear, False)
|
||||
engagement.check_can_engage(False, structs.CarState.GearShifter.drive, False)
|
||||
assert not engagement.lateralEnabled
|
||||
engagement.process_buttons(2, 0, 12000, 10., 'KPH', False, False, True, False)
|
||||
assert engagement.lateralEnabled
|
||||
@@ -64,6 +64,49 @@ def test_driving_and_parked_offsets_apply_independently():
|
||||
assert device._calculate_brightness() == 52
|
||||
|
||||
|
||||
@pytest.mark.parametrize('wake', ['button', 'touch', 'offroad', 'ignition', 'critical'])
|
||||
@pytest.mark.parametrize('management', [False, True])
|
||||
def test_manual_screen_off_and_wake_only_change_display(wake, management):
|
||||
device, state, app = make_device(ScreenManagement=management, StandbyMode=False)
|
||||
original_settings = dict(state.ui_params.values)
|
||||
state.params_memory.values[screen.SCREEN_OFF_TOGGLE_PARAM] = 1
|
||||
device._update_wakefulness()
|
||||
assert not device.awake
|
||||
assert device._calculate_brightness() == 0
|
||||
device._update_wakefulness()
|
||||
assert not device.awake
|
||||
if wake == 'button':
|
||||
state.params_memory.values[screen.SCREEN_OFF_TOGGLE_PARAM] = 2
|
||||
elif wake == 'touch':
|
||||
app.mouse_events = [SimpleNamespace(left_down=True)]
|
||||
elif wake == 'offroad':
|
||||
state.started = False
|
||||
elif wake == 'ignition':
|
||||
state.ignition = False
|
||||
else:
|
||||
state.sm['selfdriveState'].alertStatus = 'critical'
|
||||
state.sm['selfdriveState'].alertSize = 'full'
|
||||
device._update_wakefulness()
|
||||
assert device.awake
|
||||
assert device._calculate_brightness() > 0
|
||||
assert state.ui_params.values == original_settings
|
||||
|
||||
|
||||
def test_screen_off_action_ignores_offroad_and_old_requests():
|
||||
device, state, app = make_device(StandbyMode=False)
|
||||
state.started = False
|
||||
state.params_memory.values[screen.SCREEN_OFF_TOGGLE_PARAM] = 1
|
||||
device._update_wakefulness()
|
||||
assert device.awake
|
||||
state.started = True
|
||||
device._update_wakefulness()
|
||||
assert device.awake
|
||||
app.mouse_events = [SimpleNamespace(left_down=True)]
|
||||
state.params_memory.values[screen.SCREEN_OFF_TOGGLE_PARAM] = 2
|
||||
device._update_wakefulness()
|
||||
assert not device.awake
|
||||
|
||||
|
||||
def test_offset_is_ignored_in_manual_and_when_screen_settings_disabled():
|
||||
device, _, _ = make_device(StandbyMode=False, ScreenBrightnessOnroad=22, ScreenBrightnessOnroadOffset=50)
|
||||
assert device._calculate_brightness() == 22
|
||||
|
||||
@@ -30,6 +30,11 @@ def test_volvo_aol_is_held_off_until_pscm_sequence_is_validated():
|
||||
assert spv.always_on_lateral_available(SimpleNamespace(brand="honda")) is True
|
||||
|
||||
|
||||
def test_tesla_aol_capability_includes_preap():
|
||||
for fingerprint in ("TESLA_MODEL_S_PREAP", "TESLA_MODEL_S_HW1", "TESLA_MODEL_X_HW1", "TESLA_MODEL_3", "TESLA_MODEL_Y"):
|
||||
assert spv.always_on_lateral_available(SimpleNamespace(brand="tesla", carFingerprint=fingerprint))
|
||||
|
||||
|
||||
def test_explicit_main_cruise_aol_mapping_is_not_disabled_by_longitudinal_gate():
|
||||
aol_button = spv.BUTTON_FUNCTIONS["AOL_TOGGLE"]
|
||||
|
||||
|
||||
@@ -213,7 +213,7 @@ class StarPilotCard:
|
||||
else:
|
||||
self.params.put_bool_nonblocking("ExperimentalMode", not sm["selfdriveState"].experimentalMode)
|
||||
|
||||
def update(self, carState, starpilotCarState, sm, starpilot_toggles):
|
||||
def update(self, carState, starpilotCarState, sm, starpilot_toggles, *, preap_authorized=False):
|
||||
self.switchback_mode_enabled = self.params_memory.get_bool("SwitchbackModeEnabled")
|
||||
self._handle_favorite_traffic_mode_action(sm)
|
||||
|
||||
@@ -348,6 +348,8 @@ class StarPilotCard:
|
||||
self.always_on_lateral_allowed = False
|
||||
|
||||
self.always_on_lateral_enabled = self.always_on_lateral_allowed and self.always_on_lateral_set
|
||||
if getattr(self.CP, "carFingerprint", None) == "TESLA_MODEL_S_PREAP":
|
||||
self.always_on_lateral_enabled &= preap_authorized
|
||||
self.always_on_lateral_enabled &= carState.gearShifter not in NON_DRIVING_GEARS
|
||||
self.always_on_lateral_enabled &= not hyundai_aol_needs_engagement or self.hyundai_aol_ready
|
||||
self.always_on_lateral_enabled &= sm["starpilotPlan"].lateralCheck
|
||||
|
||||
@@ -102,6 +102,21 @@ def make_toggles(**overrides):
|
||||
return SimpleNamespace(**defaults)
|
||||
|
||||
|
||||
def test_preap_aol_stays_available_but_waits_for_authorization(monkeypatch, tmp_path):
|
||||
monkeypatch.setattr(spc, "Params", FakeParams)
|
||||
monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
|
||||
card = spc.StarPilotCard(SimpleNamespace(brand="tesla", carFingerprint="TESLA_MODEL_S_PREAP"),
|
||||
SimpleNamespace(alternativeExperience=32))
|
||||
toggles = make_toggles(always_on_lateral=True, always_on_lateral_main=True)
|
||||
sm = make_sm()
|
||||
for authorized in (False, True, False, True):
|
||||
ret = card.update(make_car_state(available=True, enabled=False), SimpleNamespace(distancePressed=False),
|
||||
sm, toggles, preap_authorized=authorized)
|
||||
assert card.always_on_lateral_supported
|
||||
assert ret.alwaysOnLateralAllowed
|
||||
assert ret.alwaysOnLateralEnabled == authorized
|
||||
|
||||
|
||||
def test_pulse_and_glide_requires_developer_access_and_active_longitudinal(monkeypatch, tmp_path):
|
||||
monkeypatch.setattr(spc, "Params", FakeParams)
|
||||
monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
|
||||
|
||||
@@ -83,6 +83,7 @@ const VEHICLE_SETTING_MAKES = {
|
||||
SubaruSNG: ["Subaru"],
|
||||
SubaruSNGManualParkingBrake: ["Subaru"],
|
||||
SubaruStopStartOff: ["Subaru"],
|
||||
SubaruAvhStartup: ["Subaru"],
|
||||
SubaruRedneckCruise: ["Subaru"],
|
||||
SNGHack: ["Lexus", "Toyota"],
|
||||
ToyotaAutoHold: ["Lexus", "Toyota"],
|
||||
|
||||
@@ -69,6 +69,7 @@ const VEHICLE_SETTING_MAKES = {
|
||||
SubaruSNG: ["Subaru"],
|
||||
SubaruSNGManualParkingBrake: ["Subaru"],
|
||||
SubaruStopStartOff: ["Subaru"],
|
||||
SubaruAvhStartup: ["Subaru"],
|
||||
SubaruRedneckCruise: ["Subaru"],
|
||||
SNGHack: ["Lexus", "Toyota"],
|
||||
ToyotaAutoHold: ["Lexus", "Toyota"],
|
||||
|
||||
@@ -40,6 +40,24 @@ def source(name="Macro Pad"):
|
||||
return wheel_controlsd.InputSource("/dev/input/event9", "stable-device", name, 3, 0x1234, 0x5678)
|
||||
|
||||
|
||||
def test_screen_off_action_from_favorite_and_controller_changes_only_display_request():
|
||||
from openpilot.starpilot.common.controller_actions import CONTROLLER_ACTION_SCREEN_OFF
|
||||
from openpilot.starpilot.common.favorite_slots import FAVORITE_ACTION_KEYS, execute_favorite_key
|
||||
from openpilot.starpilot.common.screen_settings import SCREEN_OFF_TOGGLE_PARAM
|
||||
|
||||
params = FakeParams({'IsOnroad': True, 'ScreenBrightnessOnroad': 70})
|
||||
memory = FakeParams()
|
||||
assert CONTROLLER_ACTION_SCREEN_OFF in FAVORITE_ACTION_KEYS
|
||||
assert execute_favorite_key(CONTROLLER_ACTION_SCREEN_OFF, params, memory)
|
||||
assert memory.values == {SCREEN_OFF_TOGGLE_PARAM: 1}
|
||||
wheel_controlsd.set_controller_action_slot(0, CONTROLLER_ACTION_SCREEN_OFF, 'Toggle Screen Off', params,
|
||||
eligible_keys={CONTROLLER_ACTION_SCREEN_OFF})
|
||||
before = dict(params.values)
|
||||
assert wheel_controlsd.execute_mapping_slot(3, params, memory)
|
||||
assert memory.values == {SCREEN_OFF_TOGGLE_PARAM: 2}
|
||||
assert params.values == before
|
||||
|
||||
|
||||
def test_mapping_round_trip_and_reassignment():
|
||||
params = FakeParams()
|
||||
|
||||
|
||||
Reference in New Issue
Block a user