mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-28 00:53:45 +08:00
Subaru
Update Subaru vehicle messages and controller/native safety integration.
This commit is contained in:
firestar5683
@@ -1,21 +1,32 @@
|
||||
import numpy as np
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import Bus, make_tester_present_msg
|
||||
from opendbc.car.lateral import apply_driver_steer_torque_limits, common_fault_avoidance
|
||||
from opendbc.car import Bus, 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.values import DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags
|
||||
from opendbc.car.subaru.values import CAR, DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
|
||||
# FIXME: These limits aren't exact. The real limit is more than likely over a larger time period and
|
||||
# involves the total steering angle change rather than rate, but these limits work well for now
|
||||
MAX_STEER_RATE = 25 # deg/s
|
||||
MAX_STEER_RATE_FRAMES = 7 # tx control frames needed before torque can be cut
|
||||
GEN2_ANGLE_PORTS = frozenset((CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023))
|
||||
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_names, CP):
|
||||
super().__init__(dbc_names, CP)
|
||||
self.apply_torque_last = 0
|
||||
self.apply_angle_last = 0.
|
||||
self.angle_was_active = False
|
||||
self.angle_override_frames = 0
|
||||
self.ascent_angle_initialized = False
|
||||
self.ascent_lkas_alert_counter_last = None
|
||||
self.angle_driver_override = False
|
||||
self.angle_handoff_active = False
|
||||
self.VM = VehicleModel(CP) if CP.carFingerprint in GEN2_ANGLE_PORTS else None
|
||||
|
||||
self.cruise_button_prev = 0
|
||||
self.steer_rate_counter = 0
|
||||
@@ -23,6 +34,46 @@ class CarController(CarControllerBase):
|
||||
self.p = CarControllerParams(CP)
|
||||
self.packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
|
||||
|
||||
def angle_active(self, CS, available):
|
||||
if not available:
|
||||
self.angle_driver_override = False
|
||||
self.angle_override_frames = 0
|
||||
self.angle_handoff_active = False
|
||||
return False
|
||||
high, low = (150, 100) if self.CP.carFingerprint == CAR.SUBARU_CROSSTREK_2025 else (200, 150)
|
||||
torque = abs(CS.out.steeringTorque)
|
||||
if self.angle_driver_override:
|
||||
if torque < low:
|
||||
self.angle_driver_override = False
|
||||
elif torque > high:
|
||||
self.angle_override_frames += 1
|
||||
if self.angle_override_frames >= 2:
|
||||
self.angle_driver_override = True
|
||||
self.angle_override_frames = 0
|
||||
else:
|
||||
self.angle_override_frames = 0
|
||||
if self.angle_driver_override:
|
||||
self.angle_handoff_active = True
|
||||
return False
|
||||
if self.angle_handoff_active:
|
||||
if abs(CS.out.steeringRateDeg) <= 2.0:
|
||||
self.angle_handoff_active = False
|
||||
return False
|
||||
if not self.angle_was_active and abs(CS.out.steeringRateDeg) > 2.0:
|
||||
self.angle_handoff_active = True
|
||||
return False
|
||||
return True
|
||||
|
||||
def _ascent_lkas_alert(self, CC, CS):
|
||||
if self.CP.carFingerprint != CAR.SUBARU_ASCENT_2023 or not self.CP.flags & SubaruFlags.ASCENT_LKAS_ALERT:
|
||||
return None
|
||||
dat = CS.es_lkas_alert_dat
|
||||
msg = subarucan.create_ascent_lkas_alert(dat, self.angle_was_active and CC.enabled and CC.latActive, CC.hudControl.visualAlert)
|
||||
if msg is None or (dat[1] & 0xf) == self.ascent_lkas_alert_counter_last:
|
||||
return None
|
||||
self.ascent_lkas_alert_counter_last = dat[1] & 0xf
|
||||
return msg
|
||||
|
||||
def update(self, CC, CS, now_nanos):
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
@@ -32,30 +83,56 @@ class CarController(CarControllerBase):
|
||||
|
||||
# *** steering ***
|
||||
if (self.frame % self.p.STEER_STEP) == 0:
|
||||
apply_torque = int(round(actuators.torque * self.p.STEER_MAX))
|
||||
|
||||
# limits due to driver torque
|
||||
|
||||
new_torque = int(round(apply_torque))
|
||||
apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.p)
|
||||
|
||||
if not CC.latActive:
|
||||
apply_torque = 0
|
||||
|
||||
if self.CP.flags & SubaruFlags.PREGLOBAL:
|
||||
can_sends.append(subarucan.create_preglobal_steering_control(self.packer, self.frame // self.p.STEER_STEP, apply_torque, CC.latActive))
|
||||
if self.CP.carFingerprint in GEN2_ANGLE_PORTS:
|
||||
angle_available = (CC.enabled and CC.latActive and CS.out.cruiseState.available and CS.out.cruiseState.enabled and
|
||||
CS.out.gearShifter == structs.CarState.GearShifter.drive and
|
||||
not CS.out.standstill and not CS.out.brakePressed)
|
||||
angle_active = self.angle_active(CS, angle_available)
|
||||
if self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023 and not self.ascent_angle_initialized:
|
||||
self.apply_angle_last = CS.out.steeringAngleDeg
|
||||
self.ascent_angle_initialized = True
|
||||
angle_active = False
|
||||
if angle_active and not self.angle_was_active and self.CP.carFingerprint != CAR.SUBARU_ASCENT_2023:
|
||||
self.apply_angle_last = CS.out.steeringAngleDeg
|
||||
if self.CP.carFingerprint in (CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023):
|
||||
apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last,
|
||||
CS.out.vEgoRaw, CS.out.steeringAngleDeg, angle_active,
|
||||
self.p.FIXED_ANGLE_LIMITS)
|
||||
else:
|
||||
apply_angle = apply_steer_angle_limits_vm(actuators.steeringAngleDeg, self.apply_angle_last,
|
||||
CS.out.vEgoRaw, CS.out.steeringAngleDeg, angle_active,
|
||||
self.p, self.VM)
|
||||
if not angle_active and self.CP.carFingerprint != CAR.SUBARU_ASCENT_2023:
|
||||
apply_angle = CS.out.steeringAngleDeg
|
||||
self.apply_angle_last = apply_angle
|
||||
self.angle_was_active = angle_active
|
||||
angle_bus = CanBus.main if self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023 else CanBus.alt
|
||||
can_sends.append(subarucan.create_steering_control_angle(self.packer, apply_angle, angle_active, angle_bus))
|
||||
else:
|
||||
apply_steer_req = CC.latActive
|
||||
apply_torque = int(round(actuators.torque * self.p.STEER_MAX))
|
||||
|
||||
if self.CP.flags & SubaruFlags.STEER_RATE_LIMITED:
|
||||
# Steering rate fault prevention
|
||||
self.steer_rate_counter, apply_steer_req = \
|
||||
common_fault_avoidance(abs(CS.out.steeringRateDeg) > MAX_STEER_RATE, apply_steer_req,
|
||||
self.steer_rate_counter, MAX_STEER_RATE_FRAMES)
|
||||
# limits due to driver torque
|
||||
|
||||
can_sends.append(subarucan.create_steering_control(self.packer, apply_torque, apply_steer_req))
|
||||
new_torque = int(round(apply_torque))
|
||||
apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.p)
|
||||
|
||||
self.apply_torque_last = apply_torque
|
||||
if not CC.latActive:
|
||||
apply_torque = 0
|
||||
|
||||
if self.CP.flags & SubaruFlags.PREGLOBAL:
|
||||
can_sends.append(subarucan.create_preglobal_steering_control(self.packer, self.frame // self.p.STEER_STEP, apply_torque, CC.latActive))
|
||||
else:
|
||||
apply_steer_req = CC.latActive
|
||||
|
||||
if self.CP.flags & SubaruFlags.STEER_RATE_LIMITED:
|
||||
# Steering rate fault prevention
|
||||
self.steer_rate_counter, apply_steer_req = \
|
||||
common_fault_avoidance(abs(CS.out.steeringRateDeg) > MAX_STEER_RATE, apply_steer_req,
|
||||
self.steer_rate_counter, MAX_STEER_RATE_FRAMES)
|
||||
|
||||
can_sends.append(subarucan.create_steering_control(self.packer, apply_torque, apply_steer_req))
|
||||
|
||||
self.apply_torque_last = apply_torque
|
||||
|
||||
# *** longitudinal ***
|
||||
|
||||
@@ -74,6 +151,9 @@ class CarController(CarControllerBase):
|
||||
cruise_brake = CarControllerParams.BRAKE_MIN
|
||||
|
||||
# *** alerts and pcm cancel ***
|
||||
lkas_alert_msg = self._ascent_lkas_alert(CC, CS)
|
||||
if lkas_alert_msg is not None:
|
||||
can_sends.append(lkas_alert_msg)
|
||||
if self.CP.flags & SubaruFlags.PREGLOBAL:
|
||||
if self.frame % 5 == 0:
|
||||
# 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep
|
||||
@@ -139,6 +219,8 @@ class CarController(CarControllerBase):
|
||||
new_actuators = actuators.as_builder()
|
||||
new_actuators.torque = self.apply_torque_last / self.p.STEER_MAX
|
||||
new_actuators.torqueOutputCan = self.apply_torque_last
|
||||
if self.CP.carFingerprint in GEN2_ANGLE_PORTS:
|
||||
new_actuators.steeringAngleDeg = self.apply_angle_last
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
|
||||
@@ -3,7 +3,7 @@ from opendbc.can import CANDefine, CANParser
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from opendbc.car.subaru.values import DBC, CanBus, SubaruFlags
|
||||
from opendbc.car.subaru.values import CAR, DBC, CanBus, SubaruFlags
|
||||
from opendbc.car import CanSignalRateCalculator
|
||||
|
||||
|
||||
@@ -13,6 +13,7 @@ class CarState(CarStateBase):
|
||||
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
|
||||
self.shifter_values = can_define.dv["Transmission"]["Gear"]
|
||||
|
||||
self.es_lkas_alert_dat = b""
|
||||
self.angle_rate_calulator = CanSignalRateCalculator(50)
|
||||
|
||||
def update(self, can_parsers) -> structs.CarState:
|
||||
@@ -83,7 +84,10 @@ class CarState(CarStateBase):
|
||||
cp_cruise = cp_alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp
|
||||
cp_es_brake = cp_alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp_cam
|
||||
|
||||
if self.CP.flags & (SubaruFlags.HYBRID | SubaruFlags.LKAS_ANGLE):
|
||||
if self.CP.carFingerprint in (CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023):
|
||||
ret.cruiseState.enabled = cp_es_brake.vl["ES_Status"]["Cruise_Activated"] != 0
|
||||
ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]["Cruise_On"] != 0
|
||||
elif self.CP.flags & (SubaruFlags.HYBRID | SubaruFlags.LKAS_ANGLE):
|
||||
# ES_DashStatus->Cruise_Activated_Dash is likely intended for the dash display only, as it falls
|
||||
# during user gas override and at standstill. ES_Status is missing on hybrid, so we use ES_Brake instead
|
||||
|
||||
@@ -134,6 +138,10 @@ class CarState(CarStateBase):
|
||||
self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"])
|
||||
|
||||
self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"])
|
||||
if self.CP.flags & SubaruFlags.ASCENT_LKAS_ALERT:
|
||||
alert = cp_cam.vl["ES_LKAS_Alert"]
|
||||
self.es_lkas_alert_dat = (bytes((int(alert["CHECKSUM"]), int(alert["COUNTER"]) | (int(alert["Signal1"]) << 4))) +
|
||||
int(alert["Payload"]).to_bytes(6, "little"))
|
||||
if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
|
||||
self.es_infotainment_msg = copy.copy(cp_cam.vl["ES_Infotainment"])
|
||||
|
||||
@@ -143,6 +151,6 @@ class CarState(CarStateBase):
|
||||
def get_can_parsers(CP):
|
||||
return {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.main),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.camera),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_Alert", 0)] if CP.flags & SubaruFlags.ASCENT_LKAS_ALERT else [], CanBus.camera),
|
||||
Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.alt)
|
||||
}
|
||||
|
||||
@@ -3,7 +3,9 @@ from opendbc.car.disable_ecu import disable_ecu
|
||||
from opendbc.car.interfaces import CarInterfaceBase
|
||||
from opendbc.car.subaru.carcontroller import CarController
|
||||
from opendbc.car.subaru.carstate import CarState
|
||||
from opendbc.car.subaru.values import CAR, GLOBAL_ES_ADDR, SubaruFlags, SubaruSafetyFlags
|
||||
from opendbc.car.subaru.values import CAR, GLOBAL_ES_ADDR, CanBus, SubaruFlags, SubaruSafetyFlags
|
||||
|
||||
GEN2_ANGLE_PORTS = frozenset((CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023))
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@@ -18,13 +20,17 @@ class CarInterface(CarInterfaceBase):
|
||||
# - replacement for ES_Distance so we can cancel the cruise control
|
||||
# - to find the Cruise_Activated bit from the car
|
||||
# - proper panda safety setup (use the correct cruise_activated bit, throttle from Throttle_Hybrid, etc)
|
||||
ret.dashcamOnly = bool(ret.flags & (SubaruFlags.PREGLOBAL | SubaruFlags.LKAS_ANGLE | SubaruFlags.HYBRID))
|
||||
ret.dashcamOnly = bool(ret.flags & (SubaruFlags.PREGLOBAL | SubaruFlags.HYBRID) or
|
||||
(ret.flags & SubaruFlags.LKAS_ANGLE and candidate not in GEN2_ANGLE_PORTS))
|
||||
ret.autoResumeSng = False
|
||||
|
||||
# Detect infotainment message sent from the camera
|
||||
if not (ret.flags & SubaruFlags.PREGLOBAL) and 0x323 in fingerprint[2]:
|
||||
ret.flags |= SubaruFlags.SEND_INFOTAINMENT.value
|
||||
|
||||
if candidate == CAR.SUBARU_ASCENT_2023 and fingerprint[CanBus.camera].get(0x3C4) == 8:
|
||||
ret.flags |= SubaruFlags.ASCENT_LKAS_ALERT.value
|
||||
|
||||
if ret.flags & SubaruFlags.PREGLOBAL:
|
||||
if 0x25c in fingerprint[0]:
|
||||
ret.flags |= SubaruFlags.HAS_BSM.value
|
||||
@@ -35,6 +41,14 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.subaru)]
|
||||
if ret.flags & SubaruFlags.GLOBAL_GEN2:
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.GEN2.value
|
||||
if candidate in GEN2_ANGLE_PORTS:
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.LKAS_ANGLE.value
|
||||
if candidate in (CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023):
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.FIXED_ANGLE_LIMITS.value
|
||||
if candidate == CAR.SUBARU_ASCENT_2023:
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.ANGLE_MAIN_BUS.value
|
||||
if ret.flags & SubaruFlags.ASCENT_LKAS_ALERT:
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.ASCENT_LKAS_ALERT.value
|
||||
|
||||
ret.steerLimitTimer = 0.4
|
||||
ret.steerActuatorDelay = 0.1
|
||||
|
||||
@@ -13,13 +13,13 @@ def create_steering_control(packer, apply_torque, steer_req):
|
||||
return packer.make_can_msg("ES_LKAS", 0, values)
|
||||
|
||||
|
||||
def create_steering_control_angle(packer, apply_torque, steer_req):
|
||||
def create_steering_control_angle(packer, apply_torque, steer_req, bus=CanBus.main):
|
||||
values = {
|
||||
"LKAS_Output": apply_torque,
|
||||
"LKAS_Request": steer_req,
|
||||
"SET_3": 3
|
||||
}
|
||||
return packer.make_can_msg("ES_LKAS_ANGLE", 0, values)
|
||||
return packer.make_can_msg("ES_LKAS_ANGLE", bus, values)
|
||||
|
||||
|
||||
def create_steering_status(packer):
|
||||
@@ -257,6 +257,18 @@ def create_es_infotainment(packer, frame, es_infotainment_msg, visual_alert):
|
||||
return packer.make_can_msg("ES_Infotainment", CanBus.main, values)
|
||||
|
||||
|
||||
def create_ascent_lkas_alert(raw_dat: bytes, lateral_active: bool, visual_alert):
|
||||
address = 0x3C4
|
||||
if len(raw_dat) != 8 or raw_dat[0] != (((address & 0xff) + (address >> 8) + sum(raw_dat[1:])) & 0xff):
|
||||
return None
|
||||
|
||||
dat = bytearray(raw_dat)
|
||||
if lateral_active and visual_alert != VisualAlert.steerRequired and dat[2:] == b"\x00\x01\x04\x00\x00\x00":
|
||||
dat[3:5] = b"\x00\x00"
|
||||
dat[0] = ((address & 0xff) + (address >> 8) + sum(dat[1:])) & 0xff
|
||||
return address, bytes(dat), CanBus.main
|
||||
|
||||
|
||||
def create_es_highbeamassist(packer):
|
||||
values = {
|
||||
"HBA_Available": False,
|
||||
|
||||
@@ -0,0 +1,53 @@
|
||||
import unittest
|
||||
|
||||
from opendbc.car.subaru.tests.test_ascent_angle import setup_controller, step
|
||||
from opendbc.car.subaru.values import CAR
|
||||
|
||||
|
||||
class TestAngleHandoff(unittest.TestCase):
|
||||
def test_override_release_waits_for_low_torque_and_settled_wheel(self):
|
||||
for car, high, low in ((CAR.SUBARU_CROSSTREK_2025, 150, 100), (CAR.SUBARU_LEGACY_2025, 200, 150)):
|
||||
for sign in (-1, 1):
|
||||
with self.subTest(car=car, sign=sign):
|
||||
_, controller, command, state = setup_controller(car)
|
||||
state.out.steeringRateDeg = 0
|
||||
state.out.steeringAngleDeg = 10
|
||||
# One sample above the threshold must not hand control back and forth.
|
||||
for torque, rate, expected in ((high, 0, 1), (high + 1, 0, 1), (high + 1, 0, 0),
|
||||
(high, 0, 0), (low, 0, 0), (low - 1, 3, 0),
|
||||
(0, 2, 0), (0, 2, 1)):
|
||||
state.out.steeringTorque = torque * sign
|
||||
state.out.steeringRateDeg = rate * sign
|
||||
steer, decoded = step(controller, command, state)
|
||||
self.assertEqual(steer[2], 1)
|
||||
self.assertEqual(decoded['LKAS_Request'], expected)
|
||||
if not expected:
|
||||
self.assertEqual(decoded['LKAS_Output'], 10)
|
||||
|
||||
def test_engagement_waits_for_manual_turn_to_settle(self):
|
||||
for car in (CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_LEGACY_2025):
|
||||
with self.subTest(car=car):
|
||||
_, controller, command, state = setup_controller(car)
|
||||
state.out.steeringTorque = 0
|
||||
for rate, expected in ((3, 0), (2.1, 0), (2, 0), (2, 1)):
|
||||
state.out.steeringRateDeg = rate
|
||||
_, decoded = step(controller, command, state)
|
||||
self.assertEqual(decoded['LKAS_Request'], expected)
|
||||
|
||||
def test_unavailable_control_clears_override_without_sending_active_commands(self):
|
||||
for car in (CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_LEGACY_2025):
|
||||
with self.subTest(car=car):
|
||||
_, controller, command, state = setup_controller(car)
|
||||
state.out.steeringRateDeg = 0
|
||||
state.out.steeringTorque = 300
|
||||
step(controller, command, state)
|
||||
step(controller, command, state)
|
||||
command.latActive = False
|
||||
_, decoded = step(controller, command, state)
|
||||
self.assertEqual(decoded['LKAS_Request'], 0)
|
||||
self.assertFalse(controller.angle_driver_override)
|
||||
self.assertFalse(controller.angle_handoff_active)
|
||||
state.out.steeringTorque = 0
|
||||
command.latActive = True
|
||||
_, decoded = step(controller, command, state)
|
||||
self.assertEqual(decoded['LKAS_Request'], 1)
|
||||
@@ -0,0 +1,143 @@
|
||||
import unittest
|
||||
from math import isclose
|
||||
|
||||
from opendbc.can import CANPacker, CANParser
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.car.subaru import subarucan
|
||||
from opendbc.car.subaru.carcontroller import CarController
|
||||
from opendbc.car.subaru.carstate import CarState
|
||||
from opendbc.car.subaru.interface import CarInterface
|
||||
from opendbc.car.subaru.values import CAR, DBC, SubaruSafetyFlags
|
||||
from opendbc.safety.tests import test_subaru_gen2_angle_pair as pair
|
||||
|
||||
|
||||
CAR_ID = CAR.SUBARU_ASCENT_2023
|
||||
|
||||
|
||||
def setup_controller(car=CAR_ID):
|
||||
cp = CarInterface.get_params(car, {0: {}, 1: {}, 2: {}}, [], False, False, False)
|
||||
_, frames = pair.TestSubaruGen2AnglePair.sources(car)
|
||||
state = CarState(cp)
|
||||
parsers = state.get_can_parsers(cp)
|
||||
state.update(parsers)
|
||||
for parser in parsers.values():
|
||||
parser.update([(1_000_000_000, frames)])
|
||||
state.out = state.update(parsers)
|
||||
command = structs.CarControl()
|
||||
command.enabled = command.latActive = True
|
||||
command.actuators.steeringAngleDeg = -1.45
|
||||
return cp, CarController(DBC[car], cp), command, state
|
||||
|
||||
|
||||
def step(controller, command, state):
|
||||
_, sends = controller.update(command.as_reader(), state, 1_000_000_000 + controller.frame * 10_000_000)
|
||||
if controller.frame % 2 == 0:
|
||||
_, sends = controller.update(command.as_reader(), state, 1_000_000_000 + controller.frame * 10_000_000)
|
||||
steer = next(frame for frame in sends if frame[0] == 0x124)
|
||||
parser = CANParser(DBC[controller.CP.carFingerprint][Bus.pt], [('ES_LKAS_ANGLE', 0)], steer[2])
|
||||
parser.update([(1, [steer])])
|
||||
return steer, parser.vl['ES_LKAS_ANGLE']
|
||||
|
||||
|
||||
class TestSubaruAscentController(unittest.TestCase):
|
||||
def test_ascent_admission_and_outback_unchanged(self):
|
||||
cp, _, _, state = setup_controller()
|
||||
assert not cp.dashcamOnly
|
||||
assert not cp.openpilotLongitudinalControl
|
||||
assert not cp.alphaLongitudinalAvailable
|
||||
assert cp.steerControlType == structs.CarParams.SteerControlType.angle
|
||||
assert cp.safetyConfigs[0].safetyParam == (SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE |
|
||||
SubaruSafetyFlags.FIXED_ANGLE_LIMITS | SubaruSafetyFlags.ANGLE_MAIN_BUS)
|
||||
assert state.out.cruiseState.enabled
|
||||
assert state.out.cruiseState.available
|
||||
outback = CarInterface.get_non_essential_params(CAR.SUBARU_OUTBACK_2023)
|
||||
assert outback.dashcamOnly
|
||||
assert outback.safetyConfigs[0].safetyParam == SubaruSafetyFlags.GEN2
|
||||
_, controller, command, state = setup_controller(CAR.SUBARU_OUTBACK_2023)
|
||||
command.latActive = False
|
||||
_, sends = controller.update(command.as_reader(), state, 1_000_000_000)
|
||||
assert not any(frame[0] == 0x124 for frame in sends)
|
||||
torque = next(frame for frame in sends if frame[0] == 0x122)
|
||||
assert torque == subarucan.create_steering_control(CANPacker(DBC[CAR.SUBARU_OUTBACK_2023][Bus.pt]), 0, False)
|
||||
|
||||
def test_first_entry_and_reentry_use_original_transmitted_history(self):
|
||||
_, controller, command, state = setup_controller()
|
||||
state.out.vEgoRaw = 30.3
|
||||
state.out.steeringAngleDeg = 0.78
|
||||
state.out.steeringRateDeg = -1.5
|
||||
controller.angle_handoff_active = True
|
||||
steer, decoded = step(controller, command, state)
|
||||
assert steer[2] == 0
|
||||
assert decoded['LKAS_Request'] == 0
|
||||
assert decoded['LKAS_Output'] == 0.78
|
||||
state.out.steeringAngleDeg = 0.74
|
||||
state.out.steeringRateDeg = -1.99
|
||||
_, decoded = step(controller, command, state)
|
||||
assert decoded['LKAS_Request'] == 1
|
||||
assert isclose(decoded['LKAS_Output'], 0.53, abs_tol=0.01)
|
||||
|
||||
_, initial, command, state = setup_controller()
|
||||
state.out.vEgoRaw = 30.3
|
||||
state.out.steeringAngleDeg = 20.0
|
||||
state.out.steeringRateDeg = 0.0
|
||||
_, decoded = step(initial, command, state)
|
||||
assert decoded['LKAS_Request'] == 0
|
||||
assert decoded['LKAS_Output'] == 20.0
|
||||
_, decoded = step(initial, command, state)
|
||||
assert decoded['LKAS_Request'] == 1
|
||||
assert 19.7 < decoded['LKAS_Output'] < 20.0
|
||||
|
||||
def test_driver_override_hysteresis_and_settled_handoff(self):
|
||||
_, controller, command, state = setup_controller()
|
||||
state.out.vEgoRaw = 30.3
|
||||
state.out.steeringAngleDeg = -25.06
|
||||
state.out.steeringTorque = -250
|
||||
state.out.steeringRateDeg = 35
|
||||
for _ in range(2):
|
||||
_, decoded = step(controller, command, state)
|
||||
assert decoded['LKAS_Request'] == 0
|
||||
state.out.steeringTorque = -150
|
||||
state.out.steeringRateDeg = 0
|
||||
_, decoded = step(controller, command, state)
|
||||
assert decoded['LKAS_Request'] == 0
|
||||
state.out.steeringTorque = -149
|
||||
state.out.steeringAngleDeg = -17.91
|
||||
_, decoded = step(controller, command, state)
|
||||
assert decoded['LKAS_Request'] == 0
|
||||
assert decoded['LKAS_Output'] == -17.91
|
||||
_, decoded = step(controller, command, state)
|
||||
assert decoded['LKAS_Request'] == 1
|
||||
assert -17.91 < decoded['LKAS_Output'] < -17.6
|
||||
|
||||
def test_ascent_inactive_follows_measurement_and_requires_stock_permission(self):
|
||||
for revocation in ('brake', 'standstill', 'park', 'reverse', 'cruise', 'main', 'disabled', 'lateral'):
|
||||
with self.subTest(revocation=revocation):
|
||||
_, controller, command, state = setup_controller()
|
||||
state.out.steeringAngleDeg = 12.5
|
||||
if revocation == 'brake':
|
||||
state.out.brakePressed = True
|
||||
elif revocation == 'standstill':
|
||||
state.out.standstill = True
|
||||
elif revocation in ('park', 'reverse'):
|
||||
state.out.gearShifter = getattr(structs.CarState.GearShifter, revocation)
|
||||
elif revocation == 'cruise':
|
||||
state.out.cruiseState.enabled = False
|
||||
elif revocation == 'main':
|
||||
state.out.cruiseState.available = False
|
||||
elif revocation == 'disabled':
|
||||
command.enabled = False
|
||||
else:
|
||||
command.latActive = False
|
||||
_, decoded = step(controller, command, state)
|
||||
assert decoded['LKAS_Request'] == 0
|
||||
assert decoded['LKAS_Output'] == 12.5
|
||||
|
||||
def test_inactive_ascent_preserves_fixed_angle_bound(self):
|
||||
for measured in (-600.0, 600.0):
|
||||
with self.subTest(measured=measured):
|
||||
_, controller, command, state = setup_controller()
|
||||
state.out.steeringAngleDeg = measured
|
||||
command.latActive = False
|
||||
_, decoded = step(controller, command, state)
|
||||
assert decoded['LKAS_Request'] == 0
|
||||
assert decoded['LKAS_Output'] == (545.0 if measured > 0 else -545.0)
|
||||
@@ -0,0 +1,68 @@
|
||||
import unittest
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.subaru.carcontroller import CarController
|
||||
from opendbc.car.subaru.carstate import CarState
|
||||
from opendbc.car.subaru.interface import CarInterface
|
||||
from opendbc.car.subaru.values import CAR, DBC, SubaruFlags, SubaruSafetyFlags
|
||||
from opendbc.car.subaru import subarucan
|
||||
from opendbc.safety.tests.test_subaru_gen2_angle_pair import TestSubaruGen2AnglePair
|
||||
|
||||
|
||||
class TestAscentLkasAlert(unittest.TestCase):
|
||||
def params(self, car=CAR.SUBARU_ASCENT_2023, bus=2, length=8):
|
||||
fingerprint = {0: {}, 1: {}, 2: {}}
|
||||
fingerprint[bus][0x3C4] = length
|
||||
return CarInterface.get_params(car, fingerprint, [], False, False, False)
|
||||
|
||||
def test_platform_bus_length_gate_preserves_native_identity(self):
|
||||
for car in CAR:
|
||||
cp = self.params(car)
|
||||
self.assertEqual(bool(cp.flags & SubaruFlags.ASCENT_LKAS_ALERT), car == CAR.SUBARU_ASCENT_2023)
|
||||
self.assertEqual(bool(cp.safetyConfigs[0].safetyParam & SubaruSafetyFlags.ASCENT_LKAS_ALERT), car == CAR.SUBARU_ASCENT_2023)
|
||||
for bus, length in ((0, 8), (1, 8), (2, 4), (2, 16)):
|
||||
cp = self.params(bus=bus, length=length)
|
||||
self.assertFalse(cp.flags & SubaruFlags.ASCENT_LKAS_ALERT)
|
||||
self.assertEqual(cp.safetyConfigs[0].safetyParam, 0xB1)
|
||||
self.assertEqual(self.params().safetyConfigs[0].safetyParam, 0x8B1)
|
||||
|
||||
def test_production_parser_state_controller_and_actual_request(self):
|
||||
cp = self.params()
|
||||
state = CarState(cp)
|
||||
parsers = state.get_can_parsers(cp)
|
||||
state.update(parsers)
|
||||
_, frames = TestSubaruGen2AnglePair.sources(CAR.SUBARU_ASCENT_2023)
|
||||
raw = bytes.fromhex("ce02000104000000")
|
||||
frames.append((0x3C4, raw, 2))
|
||||
for parser in parsers.values():
|
||||
parser.update([(1_000_000_000, frames)])
|
||||
state.out = state.update(parsers)
|
||||
self.assertEqual(state.es_lkas_alert_dat, raw)
|
||||
controller = CarController(DBC[cp.carFingerprint], cp)
|
||||
cc = structs.CarControl()
|
||||
cc.enabled = cc.latActive = True
|
||||
_, sends = controller.update(cc.as_reader(), state, 1_000_000_000)
|
||||
self.assertEqual(next(msg for msg in sends if msg[0] == 0x3C4)[1], raw)
|
||||
self.assertIsNone(controller._ascent_lkas_alert(cc, state))
|
||||
controller.angle_was_active = True
|
||||
for counter in (3, 15, 0):
|
||||
dat = bytearray(raw)
|
||||
dat[1] = counter | 0xA0
|
||||
dat[0] = (0xC4 + 3 + sum(dat[1:])) & 0xFF
|
||||
state.es_lkas_alert_dat = bytes(dat)
|
||||
msg = controller._ascent_lkas_alert(cc, state)
|
||||
self.assertEqual(msg[1][1], dat[1])
|
||||
self.assertEqual(msg[1][2:], bytes(6))
|
||||
self.assertIsNone(controller._ascent_lkas_alert(cc, state))
|
||||
|
||||
def test_only_known_hands_on_pattern_masked(self):
|
||||
raw = bytes.fromhex("ce02000104000000")
|
||||
for active, alert, masked in ((True, 0, True), (False, 0, False), (True, structs.CarControl.HUDControl.VisualAlert.steerRequired, False)):
|
||||
msg = subarucan.create_ascent_lkas_alert(raw, active, alert)
|
||||
self.assertEqual(msg[1][2:], bytes(6) if masked else raw[2:])
|
||||
for payload in ("000000110000", "1e0030000000", "000704000000", "190000010000", "010104000000"):
|
||||
dat = bytearray(bytes.fromhex("0002" + payload))
|
||||
dat[0] = (0xC4 + 3 + sum(dat[1:])) & 0xFF
|
||||
self.assertEqual(subarucan.create_ascent_lkas_alert(bytes(dat), True, 0)[1], dat)
|
||||
for dat in (b"", bytes(7), bytes(9), bytes.fromhex("0002000104000000")):
|
||||
self.assertIsNone(subarucan.create_ascent_lkas_alert(dat, True, 0))
|
||||
@@ -1,15 +1,28 @@
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum, IntFlag
|
||||
|
||||
from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
|
||||
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column
|
||||
from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
|
||||
from opendbc.car.lateral import AngleSteeringLimits, AngleSteeringLimitsVM, ISO_LATERAL_ACCEL
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
|
||||
class CarControllerParams:
|
||||
ANGLE_LIMITS = AngleSteeringLimitsVM(
|
||||
650,
|
||||
MAX_LATERAL_ACCEL=ISO_LATERAL_ACCEL + ACCELERATION_DUE_TO_GRAVITY * 0.06,
|
||||
MAX_LATERAL_JERK=3.0 + ACCELERATION_DUE_TO_GRAVITY * 0.06,
|
||||
MAX_ANGLE_RATE=1,
|
||||
)
|
||||
FIXED_ANGLE_LIMITS = AngleSteeringLimits(
|
||||
545, ([0., 5., 35.], [5., .8, .15]), ([0., 5., 35.], [5., .8, .15]),
|
||||
)
|
||||
|
||||
LEGACY_2025_ANGLE_LIMITS = FIXED_ANGLE_LIMITS
|
||||
|
||||
def __init__(self, CP):
|
||||
self.STEER_STEP = 2 # how often we update the steer cmd
|
||||
self.STEER_DELTA_UP = 50 # torque increase per refresh, 0.8s to max
|
||||
@@ -57,6 +70,10 @@ class SubaruSafetyFlags(IntFlag):
|
||||
GEN2 = 1
|
||||
LONG = 2
|
||||
PREGLOBAL_REVERSED_DRIVER_TORQUE = 4
|
||||
LKAS_ANGLE = 16
|
||||
FIXED_ANGLE_LIMITS = 128
|
||||
ANGLE_MAIN_BUS = 32
|
||||
ASCENT_LKAS_ALERT = 2048
|
||||
|
||||
|
||||
class SubaruFlags(IntFlag):
|
||||
@@ -74,6 +91,7 @@ class SubaruFlags(IntFlag):
|
||||
PREGLOBAL = 16
|
||||
HYBRID = 32
|
||||
LKAS_ANGLE = 64
|
||||
ASCENT_LKAS_ALERT = 512
|
||||
|
||||
|
||||
GLOBAL_ES_ADDR = 0x787
|
||||
@@ -212,6 +230,16 @@ class CAR(Platforms):
|
||||
SUBARU_ASCENT.specs,
|
||||
flags=SubaruFlags.LKAS_ANGLE,
|
||||
)
|
||||
SUBARU_LEGACY_2025 = SubaruGen2PlatformConfig(
|
||||
[SubaruCarDocs("Subaru Legacy 2025", "All", car_parts=CarParts.common([CarHarness.subaru_d]))],
|
||||
SUBARU_OUTBACK.specs,
|
||||
flags=SubaruFlags.LKAS_ANGLE,
|
||||
)
|
||||
SUBARU_CROSSTREK_2025 = SubaruGen2PlatformConfig(
|
||||
[SubaruCarDocs("Subaru Crosstrek 2025", "All", car_parts=CarParts.common([CarHarness.subaru_d]))],
|
||||
CarSpecs(mass=1529, wheelbase=2.67, steerRatio=17),
|
||||
flags=SubaruFlags.LKAS_ANGLE,
|
||||
)
|
||||
|
||||
|
||||
SUBARU_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||
|
||||
@@ -252,6 +252,12 @@ BO_ 803 ES_Infotainment: 8 XXX
|
||||
SG_ LKAS_State_Infotainment : 22|3@0+ (1,0) [0|7] "" XXX
|
||||
SG_ Signal2 : 24|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 964 ES_LKAS_Alert: 8 XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ Signal1 : 12|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ Payload : 16|48@1+ (1,0) [0|281474976710655] "" XXX
|
||||
|
||||
BO_ 722 AC_State: 8 XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -23,13 +23,17 @@
|
||||
#define MSG_SUBARU_CruiseControl 0x240U
|
||||
#define MSG_SUBARU_Throttle 0x40U
|
||||
#define MSG_SUBARU_Steering_Torque 0x119U
|
||||
#define MSG_SUBARU_Steering_2 0x11aU
|
||||
#define MSG_SUBARU_Wheel_Speeds 0x13aU
|
||||
|
||||
#define MSG_SUBARU_ES_LKAS 0x122U
|
||||
#define MSG_SUBARU_ES_LKAS_ANGLE 0x124U
|
||||
#define MSG_SUBARU_ES_Distance 0x221U
|
||||
#define MSG_SUBARU_ES_Status 0x222U
|
||||
#define MSG_SUBARU_ES_DashStatus 0x321U
|
||||
#define MSG_SUBARU_ES_LKAS_State 0x322U
|
||||
#define MSG_SUBARU_ES_Infotainment 0x323U
|
||||
#define MSG_SUBARU_ES_LKAS_Alert 0x3C4U
|
||||
|
||||
#define SUBARU_MAIN_BUS 0U
|
||||
#define SUBARU_ALT_BUS 1U
|
||||
@@ -44,6 +48,13 @@
|
||||
#define SUBARU_COMMON_TX_MSGS(alt_bus) \
|
||||
{MSG_SUBARU_ES_Distance, alt_bus, 8, .check_relay = false}, \
|
||||
|
||||
#define SUBARU_GEN2_ANGLE_TX_MSGS(angle_bus) \
|
||||
{MSG_SUBARU_ES_LKAS_ANGLE, angle_bus, 8, .check_relay = true}, \
|
||||
{MSG_SUBARU_ES_DashStatus, SUBARU_MAIN_BUS, 8, .check_relay = true}, \
|
||||
{MSG_SUBARU_ES_LKAS_State, SUBARU_MAIN_BUS, 8, .check_relay = true}, \
|
||||
{MSG_SUBARU_ES_Infotainment, SUBARU_MAIN_BUS, 8, .check_relay = true}, \
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
|
||||
|
||||
#define SUBARU_COMMON_RX_CHECKS(alt_bus) \
|
||||
{.msg = {{MSG_SUBARU_Throttle, SUBARU_MAIN_BUS, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_MAIN_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
@@ -51,7 +62,20 @@
|
||||
{.msg = {{MSG_SUBARU_Brake_Status, alt_bus, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{MSG_SUBARU_CruiseControl, alt_bus, 8, 20U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
#define SUBARU_GEN2_ANGLE_RX_CHECKS() \
|
||||
{.msg = {{MSG_SUBARU_Throttle, SUBARU_MAIN_BUS, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_MAIN_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{MSG_SUBARU_Steering_2, SUBARU_MAIN_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{MSG_SUBARU_Wheel_Speeds, SUBARU_ALT_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{MSG_SUBARU_Brake_Status, SUBARU_ALT_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{MSG_SUBARU_ES_Status, SUBARU_ALT_BUS, 8, 20U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{MSG_SUBARU_ES_DashStatus, SUBARU_CAM_BUS, 8, 10U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
static bool subaru_gen2 = false;
|
||||
static bool subaru_lkas_angle = false;
|
||||
static bool subaru_fixed_angle_limits = false;
|
||||
static bool subaru_angle_main_bus = false;
|
||||
static bool subaru_angle_invalid = false;
|
||||
|
||||
static uint32_t subaru_get_checksum(const CANPacket_t *msg) {
|
||||
return (uint8_t)msg->data[0];
|
||||
@@ -80,8 +104,21 @@ static void subaru_rx_hook(const CANPacket_t *msg) {
|
||||
update_sample(&torque_driver, torque_driver_new);
|
||||
}
|
||||
|
||||
if (subaru_lkas_angle && (msg->addr == MSG_SUBARU_Steering_2) && (msg->bus == SUBARU_MAIN_BUS)) {
|
||||
int angle_meas_new = GET_BYTES_LE(msg, 3, 3) & 0x1FFFFU;
|
||||
angle_meas_new = -1 * to_signed(angle_meas_new, 17);
|
||||
update_sample(&angle_meas, angle_meas_new);
|
||||
}
|
||||
|
||||
if (subaru_lkas_angle && (msg->addr == MSG_SUBARU_ES_Status) && (msg->bus == SUBARU_ALT_BUS)) {
|
||||
pcm_cruise_check(GET_BIT(msg, 29U));
|
||||
}
|
||||
if (subaru_lkas_angle && (msg->addr == MSG_SUBARU_ES_DashStatus) && (msg->bus == SUBARU_CAM_BUS)) {
|
||||
acc_main_on = GET_BIT(msg, 49U);
|
||||
}
|
||||
|
||||
// enter controls on rising edge of ACC, exit controls on ACC off
|
||||
if (msg_matches(msg, MSG_SUBARU_CruiseControl, alt_main_bus)) {
|
||||
if (!subaru_lkas_angle && msg_matches(msg, MSG_SUBARU_CruiseControl, alt_main_bus)) {
|
||||
bool cruise_engaged = (msg->data[5] >> 1) & 1U;
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
}
|
||||
@@ -110,6 +147,25 @@ static void subaru_rx_hook(const CANPacket_t *msg) {
|
||||
static bool subaru_tx_hook(const CANPacket_t *msg) {
|
||||
const TorqueSteeringLimits SUBARU_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70);
|
||||
const TorqueSteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1000, 40, 40);
|
||||
const AngleSteeringLimits SUBARU_ANGLE_STEERING_LIMITS = {
|
||||
.max_angle = 650 * 100,
|
||||
.angle_deg_to_can = 100.,
|
||||
.frequency = 50U,
|
||||
};
|
||||
const AngleSteeringLimits SUBARU_FIXED_ANGLE_STEERING_LIMITS = {
|
||||
.max_angle = 545 * 100,
|
||||
.angle_deg_to_can = 100.,
|
||||
.angle_rate_up_lookup = {{0.0, 5.0, 35.0}, {5.0, 0.8, 0.15}},
|
||||
.angle_rate_down_lookup = {{0.0, 5.0, 35.0}, {5.0, 0.8, 0.15}},
|
||||
.frequency = 50U,
|
||||
};
|
||||
// Only Crosstrek 2025 selects this VM profile. Match the host CarParams
|
||||
// vehicle model instead of the frozen generic angle example's Ascent geometry.
|
||||
const AngleSteeringParams SUBARU_CROSSTREK_2025_ANGLE_PARAMS = {
|
||||
.slip_factor = -0.0006281955319491566,
|
||||
.steer_ratio = 17.0,
|
||||
.wheelbase = 2.6700000762939453,
|
||||
};
|
||||
|
||||
const LongitudinalLimits SUBARU_LONG_LIMITS = {
|
||||
.min_gas = 808, // appears to be engine braking
|
||||
@@ -121,33 +177,54 @@ static bool subaru_tx_hook(const CANPacket_t *msg) {
|
||||
.max_transmission_rpm = 3600,
|
||||
};
|
||||
|
||||
bool tx = true;
|
||||
bool violation = false;
|
||||
bool tx = false;
|
||||
if (!subaru_angle_invalid) {
|
||||
tx = true;
|
||||
bool violation = false;
|
||||
|
||||
// steer cmd checks
|
||||
if (msg->addr == MSG_SUBARU_ES_LKAS) {
|
||||
int desired_torque = ((GET_BYTES_LE(msg, 0, 4) >> 16) & 0x1FFFU);
|
||||
desired_torque = -1 * to_signed(desired_torque, 13);
|
||||
// steer cmd checks
|
||||
if (msg->addr == MSG_SUBARU_ES_LKAS) {
|
||||
int desired_torque = ((GET_BYTES_LE(msg, 0, 4) >> 16) & 0x1FFFU);
|
||||
desired_torque = -1 * to_signed(desired_torque, 13);
|
||||
|
||||
bool steer_req = (msg->data[3] >> 5) & 1U;
|
||||
bool steer_req = (msg->data[3] >> 5) & 1U;
|
||||
|
||||
const TorqueSteeringLimits limits = subaru_gen2 ? SUBARU_GEN2_STEERING_LIMITS : SUBARU_STEERING_LIMITS;
|
||||
violation |= steer_torque_cmd_checks(desired_torque, steer_req, limits);
|
||||
}
|
||||
const TorqueSteeringLimits limits = subaru_gen2 ? SUBARU_GEN2_STEERING_LIMITS : SUBARU_STEERING_LIMITS;
|
||||
violation |= steer_torque_cmd_checks(desired_torque, steer_req, limits);
|
||||
}
|
||||
if (msg->addr == MSG_SUBARU_ES_LKAS_ANGLE) {
|
||||
int desired_angle = GET_BYTES_LE(msg, 5, 3) & 0x1FFFFU;
|
||||
desired_angle = -1 * to_signed(desired_angle, 17);
|
||||
bool lkas_request = GET_BIT(msg, 12U);
|
||||
violation |= lkas_request && !acc_main_on;
|
||||
if (subaru_angle_main_bus) {
|
||||
violation |= SAFETY_ABS(desired_angle) > SUBARU_FIXED_ANGLE_STEERING_LIMITS.max_angle;
|
||||
}
|
||||
if (subaru_fixed_angle_limits) {
|
||||
violation |= steer_angle_cmd_checks(desired_angle, lkas_request, SUBARU_FIXED_ANGLE_STEERING_LIMITS);
|
||||
} else {
|
||||
violation |= steer_angle_cmd_checks_vm(desired_angle, lkas_request, SUBARU_ANGLE_STEERING_LIMITS, SUBARU_CROSSTREK_2025_ANGLE_PARAMS);
|
||||
}
|
||||
}
|
||||
|
||||
// check es_distance cruise_throttle limits
|
||||
if (msg->addr == MSG_SUBARU_ES_Distance) {
|
||||
int cruise_throttle = (GET_BYTES_LE(msg, 2, 2) & 0x1FFFU);
|
||||
bool cruise_cancel = (msg->data[7] >> 0) & 1U;
|
||||
if (msg->addr == MSG_SUBARU_ES_LKAS_Alert) {
|
||||
violation |= subaru_get_checksum(msg) != subaru_compute_checksum(msg);
|
||||
}
|
||||
|
||||
// If openpilot is not controlling long, only allow ES_Distance for cruise cancel requests,
|
||||
// (when Cruise_Cancel is true, and Cruise_Throttle is inactive)
|
||||
violation |= (cruise_throttle != SUBARU_LONG_LIMITS.inactive_gas);
|
||||
violation |= (!cruise_cancel);
|
||||
}
|
||||
// check es_distance cruise_throttle limits
|
||||
if (msg->addr == MSG_SUBARU_ES_Distance) {
|
||||
int cruise_throttle = (GET_BYTES_LE(msg, 2, 2) & 0x1FFFU);
|
||||
bool cruise_cancel = (msg->data[7] >> 0) & 1U;
|
||||
|
||||
if (violation) {
|
||||
tx = false;
|
||||
// If openpilot is not controlling long, only allow ES_Distance for cruise cancel requests,
|
||||
// (when Cruise_Cancel is true, and Cruise_Throttle is inactive)
|
||||
violation |= (cruise_throttle != SUBARU_LONG_LIMITS.inactive_gas);
|
||||
violation |= (!cruise_cancel);
|
||||
}
|
||||
|
||||
if (violation){
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
return tx;
|
||||
@@ -163,16 +240,48 @@ static safety_config subaru_init(uint16_t param) {
|
||||
SUBARU_BASE_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
|
||||
};
|
||||
static const CanMsg SUBARU_GEN2_ANGLE_TX_MSGS_LIST[] = {
|
||||
SUBARU_GEN2_ANGLE_TX_MSGS(SUBARU_ALT_BUS)
|
||||
};
|
||||
|
||||
static const CanMsg SUBARU_ASCENT_ANGLE_TX_MSGS_LIST[] = {
|
||||
SUBARU_GEN2_ANGLE_TX_MSGS(SUBARU_MAIN_BUS)
|
||||
};
|
||||
|
||||
static const CanMsg SUBARU_ASCENT_ALERT_TX_MSGS[] = {
|
||||
SUBARU_GEN2_ANGLE_TX_MSGS(SUBARU_MAIN_BUS)
|
||||
{MSG_SUBARU_ES_LKAS_Alert, SUBARU_MAIN_BUS, 8, .check_relay = true},
|
||||
};
|
||||
|
||||
const uint16_t SUBARU_PARAM_ASCENT_ALERT = 2048;
|
||||
const bool ascent_alert = param == (1U | 16U | 128U | 32U | SUBARU_PARAM_ASCENT_ALERT);
|
||||
const uint16_t SUBARU_PARAM_GEN2 = 1;
|
||||
const uint16_t SUBARU_PARAM_LKAS_ANGLE = 16;
|
||||
const uint16_t SUBARU_PARAM_FIXED_ANGLE = 128;
|
||||
|
||||
const uint16_t SUBARU_PARAM_ANGLE_MAIN_BUS = 32;
|
||||
subaru_angle_main_bus = GET_FLAG(param, SUBARU_PARAM_ANGLE_MAIN_BUS);
|
||||
|
||||
subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2);
|
||||
subaru_lkas_angle = GET_FLAG(param, SUBARU_PARAM_LKAS_ANGLE);
|
||||
subaru_fixed_angle_limits = GET_FLAG(param, SUBARU_PARAM_FIXED_ANGLE);
|
||||
subaru_angle_invalid = (subaru_lkas_angle && (param != (SUBARU_PARAM_GEN2 | SUBARU_PARAM_LKAS_ANGLE)) &&
|
||||
(param != (SUBARU_PARAM_GEN2 | SUBARU_PARAM_LKAS_ANGLE | SUBARU_PARAM_FIXED_ANGLE)) &&
|
||||
(param != (SUBARU_PARAM_GEN2 | SUBARU_PARAM_LKAS_ANGLE | SUBARU_PARAM_FIXED_ANGLE | SUBARU_PARAM_ANGLE_MAIN_BUS)) && !ascent_alert) ||
|
||||
((subaru_fixed_angle_limits || subaru_angle_main_bus) && !subaru_lkas_angle);
|
||||
|
||||
// TODO: re-enable once more work is done on the limits
|
||||
// revert this in the PR that re-enables Subaru longitudinal: https://github.com/commaai/opendbc/pull/3689
|
||||
|
||||
safety_config ret;
|
||||
if (subaru_gen2) {
|
||||
if (subaru_lkas_angle) {
|
||||
static RxCheck subaru_gen2_angle_rx_checks[] = {
|
||||
SUBARU_GEN2_ANGLE_RX_CHECKS()
|
||||
};
|
||||
ret = ascent_alert ? BUILD_SAFETY_CFG(subaru_gen2_angle_rx_checks, SUBARU_ASCENT_ALERT_TX_MSGS) :
|
||||
subaru_angle_main_bus ? BUILD_SAFETY_CFG(subaru_gen2_angle_rx_checks, SUBARU_ASCENT_ANGLE_TX_MSGS_LIST) :
|
||||
BUILD_SAFETY_CFG(subaru_gen2_angle_rx_checks, SUBARU_GEN2_ANGLE_TX_MSGS_LIST);
|
||||
} else if (subaru_gen2) {
|
||||
static RxCheck subaru_gen2_rx_checks[] = {
|
||||
SUBARU_COMMON_RX_CHECKS(SUBARU_ALT_BUS)
|
||||
};
|
||||
|
||||
@@ -0,0 +1,41 @@
|
||||
import unittest
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.safety.tests.libsafety import libsafety_py
|
||||
|
||||
DEFAULT_ALERT = bytes.fromhex("ce02000104000000")
|
||||
|
||||
|
||||
class TestSubaruAscentAlert(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.safety = libsafety_py.libsafety
|
||||
|
||||
def mode(self, flags):
|
||||
self.safety.set_safety_hooks(structs.CarParams.SafetyModel.subaru, flags)
|
||||
self.safety.init_tests()
|
||||
|
||||
def packet(self, data=DEFAULT_ALERT, bus=0):
|
||||
return libsafety_py.make_CANPacket(0x3C4, bus, data)
|
||||
|
||||
def test_exact_parameter_bus_checksum_length_and_forwarding(self):
|
||||
for flags in (0, 1, 0x11, 0x91, 0xB1, 0x800, 0x811, 0x891, 0x8B3, 0x9B1):
|
||||
self.mode(flags)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.packet()), hex(flags))
|
||||
self.assertEqual(self.safety.safety_fwd_hook(2, 0x3C4), 0)
|
||||
self.mode(0x8B1)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self.packet()))
|
||||
self.assertEqual(self.safety.safety_fwd_hook(2, 0x3C4), -1)
|
||||
self.assertEqual(self.safety.safety_fwd_hook(0, 0x3C4), 2)
|
||||
for bus in (1, 2):
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.packet(bus=bus)))
|
||||
for data in (bytes.fromhex("0002000104000000"), bytes(7), bytes(12)):
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.packet(data)))
|
||||
|
||||
def test_stock_main_frame_and_relay_malfunction_revoke(self):
|
||||
self.mode(0x8B1)
|
||||
self.safety.safety_rx_hook(self.packet(bus=2))
|
||||
self.assertFalse(self.safety.get_relay_malfunction())
|
||||
self.safety.safety_rx_hook(self.packet())
|
||||
self.assertTrue(self.safety.get_relay_malfunction())
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.packet()))
|
||||
self.assertEqual(self.safety.safety_fwd_hook(2, 0x3C4), -1)
|
||||
@@ -0,0 +1,162 @@
|
||||
import unittest
|
||||
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.car.subaru import subarucan
|
||||
from opendbc.car.subaru.values import CAR, DBC, SubaruSafetyFlags
|
||||
from opendbc.car.subaru.tests.test_ascent_angle import setup_controller, step
|
||||
from opendbc.safety.tests.libsafety import libsafety_py
|
||||
from opendbc.safety.tests import test_subaru_gen2_angle_pair as pair
|
||||
|
||||
|
||||
class TestSubaruAscentAngle(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.safety = libsafety_py.libsafety
|
||||
self.cp, self.controller, self.command, self.state = setup_controller()
|
||||
self.packer = CANPacker(DBC[CAR.SUBARU_ASCENT_2023][Bus.pt])
|
||||
|
||||
@staticmethod
|
||||
def packet(frame):
|
||||
return libsafety_py.make_CANPacket(frame[0], frame[2], frame[1])
|
||||
|
||||
def arm(self, **changes):
|
||||
cfg = self.cp.safetyConfigs[0]
|
||||
self.assertEqual(self.safety.set_safety_hooks(cfg.safetyModel.raw, cfg.safetyParam), 0)
|
||||
self.safety.init_tests()
|
||||
self.safety.set_timer(1_000_000)
|
||||
speed = changes.pop('speed', 30.3 * 3.6)
|
||||
_, frames = pair.TestSubaruGen2AnglePair.sources(CAR.SUBARU_ASCENT_2023, speed=speed, **changes)
|
||||
for frame in frames:
|
||||
if frame[0] in {0x40, 0x119, 0x11A, 0x13A, 0x13C, 0x222, 0x321}:
|
||||
self.assertTrue(self.safety.safety_rx_hook(self.packet(frame)))
|
||||
for counter in range(1, 6):
|
||||
wheel = self.packer.make_can_msg("Wheel_Speeds", 1, {"FL": speed, "FR": speed, "RL": speed, "RR": speed, "COUNTER": counter})
|
||||
self.assertTrue(self.safety.safety_rx_hook(self.packet(wheel)))
|
||||
self.safety.safety_tick()
|
||||
self.assertTrue(self.safety.safety_config_valid())
|
||||
|
||||
def angle(self, value, active=True, bus=0):
|
||||
return self.packet(subarucan.create_steering_control_angle(self.packer, value, active, bus))
|
||||
|
||||
def test_host_inactive_entry_and_reentry_match_native_history(self):
|
||||
self.arm()
|
||||
self.state.out.vEgoRaw = 30.3
|
||||
self.state.out.steeringAngleDeg = 0.78
|
||||
self.state.out.steeringRateDeg = -1.5
|
||||
self.controller.ascent_handoff_active = True
|
||||
self.safety.set_angle_meas(78, 78)
|
||||
steer, decoded = step(self.controller, self.command, self.state)
|
||||
self.assertFalse(decoded['LKAS_Request'])
|
||||
self.assertTrue(self.safety.safety_tx_hook(self.packet(steer)))
|
||||
self.state.out.steeringAngleDeg = 0.74
|
||||
self.state.out.steeringRateDeg = -1.99
|
||||
self.safety.set_angle_meas(74, 74)
|
||||
steer, decoded = step(self.controller, self.command, self.state)
|
||||
self.assertAlmostEqual(decoded['LKAS_Output'], 0.53, delta=0.01)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self.packet(steer)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.angle(-1.45)))
|
||||
|
||||
def test_first_nonzero_angle_primes_native_history_before_request(self):
|
||||
self.arm()
|
||||
self.state.out.vEgoRaw = 30.3
|
||||
self.state.out.steeringAngleDeg = 20.0
|
||||
self.state.out.steeringRateDeg = 0.0
|
||||
self.safety.set_angle_meas(2000, 2000)
|
||||
steer, decoded = step(self.controller, self.command, self.state)
|
||||
self.assertFalse(decoded['LKAS_Request'])
|
||||
self.assertTrue(self.safety.safety_tx_hook(self.packet(steer)))
|
||||
steer, decoded = step(self.controller, self.command, self.state)
|
||||
self.assertTrue(decoded['LKAS_Request'])
|
||||
self.assertTrue(self.safety.safety_tx_hook(self.packet(steer)))
|
||||
|
||||
def test_buses_angle_bounds_permission_and_rate_limits(self):
|
||||
for bad in (self.angle(0, bus=1), self.angle(0, bus=2), self.angle(546)):
|
||||
self.arm()
|
||||
self.assertFalse(self.safety.safety_tx_hook(bad))
|
||||
self.arm()
|
||||
self.safety.set_desired_angle_last(0)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self.angle(0.25)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.angle(1.0)))
|
||||
for changes in ({'main': False}, {'cruise': False}, {'brake': True}):
|
||||
self.arm(**changes)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.angle(0)))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self.angle(0, active=False)))
|
||||
for flag in (SubaruSafetyFlags.LONG, 0x100, 0x40):
|
||||
cfg = self.cp.safetyConfigs[0]
|
||||
self.safety.set_safety_hooks(cfg.safetyModel.raw, cfg.safetyParam | flag)
|
||||
self.safety.init_tests()
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.angle(0)))
|
||||
|
||||
def test_forwarding_and_exact_parameter_discriminator(self):
|
||||
self.arm()
|
||||
self.assertEqual(self.safety.safety_fwd_hook(2, 0x124), -1)
|
||||
self.assertEqual(self.safety.safety_fwd_hook(0, 0x124), 2)
|
||||
self.assertEqual(self.safety.safety_fwd_hook(1, 0x124), -1)
|
||||
self.assertEqual(self.safety.safety_fwd_hook(2, 0x221), 0)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.packet(self.packer.make_can_msg('ES_LKAS', 0, {}))))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.packet(self.packer.make_can_msg('ES_Brake', 1, {}))))
|
||||
self.safety.set_desired_angle_last(54500)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.angle(546)))
|
||||
self.arm()
|
||||
self.safety.set_desired_angle_last(-54500)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.angle(-546)))
|
||||
for invalid in (0x20, 0x21, 0x30, 0x31, 0xA0, 0xB0, 0xB3, 0x1B1):
|
||||
self.safety.set_safety_hooks(structs.CarParams.SafetyModel.subaru, invalid)
|
||||
self.safety.init_tests()
|
||||
self.safety.set_controls_allowed(True)
|
||||
for bus in (0, 1, 2):
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.angle(0, bus=bus)), hex(invalid))
|
||||
self.arm()
|
||||
self.safety.set_relay_malfunction(True)
|
||||
self.assertEqual(self.safety.safety_fwd_hook(0, 0x124), -1)
|
||||
self.assertEqual(self.safety.safety_fwd_hook(2, 0x221), -1)
|
||||
|
||||
def test_required_can_loss_and_relay_revoke(self):
|
||||
self.arm()
|
||||
self.assertTrue(self.safety.safety_tx_hook(self.angle(0)))
|
||||
self.safety.set_timer(10_000_000)
|
||||
self.safety.safety_tick()
|
||||
self.assertFalse(self.safety.safety_config_valid())
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.angle(0)))
|
||||
self.arm()
|
||||
self.safety.set_relay_malfunction(True)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.angle(0)))
|
||||
|
||||
def test_received_stock_angle_detects_relay(self):
|
||||
for bus in (1, 2):
|
||||
self.arm()
|
||||
self.safety.safety_rx_hook(self.angle(0, active=False, bus=bus))
|
||||
self.assertFalse(self.safety.get_relay_malfunction())
|
||||
self.arm()
|
||||
self.safety.safety_rx_hook(self.angle(0, active=False))
|
||||
self.assertTrue(self.safety.get_relay_malfunction())
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.angle(0)))
|
||||
self.assertEqual(self.safety.safety_fwd_hook(2, 0x221), -1)
|
||||
|
||||
def test_inactive_measured_beyond_fixed_bound_is_safely_clamped(self):
|
||||
for measured in (-600.0, 600.0):
|
||||
with self.subTest(measured=measured):
|
||||
self.arm()
|
||||
_, self.controller, self.command, self.state = setup_controller()
|
||||
self.command.latActive = False
|
||||
self.state.out.steeringAngleDeg = measured
|
||||
self.safety.set_angle_meas(int(measured * 100), int(measured * 100))
|
||||
steer, decoded = step(self.controller, self.command, self.state)
|
||||
self.assertEqual(abs(decoded['LKAS_Output']), 545.0)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self.packet(steer)))
|
||||
|
||||
def test_driver_override_handoff_frames_match_native(self):
|
||||
self.arm()
|
||||
self.state.out.vEgoRaw = 30.3
|
||||
for torque, rate, measured, active in ((0, 0, 0, False), (0, 0, 0, True),
|
||||
(250, 35, 0, True), (250, 35, 0, False),
|
||||
(150, 0, 0, False), (149, 3, 0, False),
|
||||
(149, 0, 0, False), (149, 0, 0, True), (0, 0, 0, True)):
|
||||
self.state.out.steeringTorque = torque
|
||||
self.state.out.steeringRateDeg = rate
|
||||
self.state.out.steeringAngleDeg = measured
|
||||
self.safety.set_angle_meas(int(measured * 100), int(measured * 100))
|
||||
steer, decoded = step(self.controller, self.command, self.state)
|
||||
self.assertEqual(bool(decoded['LKAS_Request']), active, (torque, rate))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self.packet(steer)))
|
||||
@@ -0,0 +1,289 @@
|
||||
import unittest
|
||||
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.car.fw_versions import match_fw_to_car
|
||||
from opendbc.car.subaru.carcontroller import CarController
|
||||
from opendbc.car.subaru.carstate import CarState
|
||||
from opendbc.car.subaru.interface import CarInterface
|
||||
from opendbc.car.subaru.values import CAR, DBC, SubaruSafetyFlags
|
||||
from opendbc.safety.tests.libsafety import libsafety_py
|
||||
|
||||
|
||||
CARS = (CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_LEGACY_2025)
|
||||
|
||||
|
||||
class TestSubaruGen2AnglePair(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.safety = libsafety_py.libsafety
|
||||
|
||||
@staticmethod
|
||||
def packet(frame):
|
||||
return libsafety_py.make_CANPacket(frame[0], frame[2], frame[1])
|
||||
|
||||
@staticmethod
|
||||
def params(car, *, alpha=False, release=False):
|
||||
return CarInterface.get_params(car, {0: {}, 1: {}, 2: {}}, [], alpha, release, False)
|
||||
|
||||
def mode(self, cp, *, extra_flags=0):
|
||||
cfg = cp.safetyConfigs[0]
|
||||
self.assertEqual(self.safety.set_safety_hooks(cfg.safetyModel.raw, cfg.safetyParam | extra_flags), 0)
|
||||
self.safety.init_tests()
|
||||
self.safety.set_timer(1_000_000)
|
||||
|
||||
@staticmethod
|
||||
def sources(car, *, brake=False, cruise=True, speed=30, main=True, torque=0):
|
||||
packer = CANPacker(DBC[car][Bus.pt])
|
||||
inputs = (
|
||||
('Throttle', 0, {}),
|
||||
('Steering_Torque', 0, {'Steer_Torque_Sensor': torque}),
|
||||
('Steering_2', 0, {'Steering_Angle': 0}),
|
||||
('Wheel_Speeds', 1, {'FL': speed, 'FR': speed, 'RL': speed, 'RR': speed}),
|
||||
('Brake_Status', 1, {'Brake': int(brake)}),
|
||||
('ES_Status', 1, {'Cruise_Activated': int(cruise)}),
|
||||
('ES_DashStatus', 2, {'Cruise_On': int(main)}),
|
||||
('Transmission', 0, {'Gear': 121}),
|
||||
('Dashlights', 0, {}),
|
||||
('BodyInfo', 0, {}),
|
||||
('ES_Distance', 1, {}),
|
||||
('ES_LKAS_State', 2, {}),
|
||||
('ES_Brake', 1, {}),
|
||||
)
|
||||
return packer, [packer.make_can_msg(name, bus, values) for name, bus, values in inputs]
|
||||
|
||||
def controller(self, car, cp, frames, *, active=True):
|
||||
state = CarState(cp)
|
||||
parsers = state.get_can_parsers(cp)
|
||||
state.update(parsers)
|
||||
for parser in parsers.values():
|
||||
parser.update([(1_000_000_000, frames)])
|
||||
state.out = state.update(parsers).as_reader()
|
||||
self.assertEqual(state.out.gearShifter, structs.CarState.GearShifter.drive)
|
||||
self.assertGreater(state.out.vEgoRaw, 0)
|
||||
|
||||
command = structs.CarControl()
|
||||
command.enabled = True
|
||||
command.latActive = active
|
||||
command.actuators.steeringAngleDeg = 1.0
|
||||
_, sends = CarController(DBC[car], cp).update(command.as_reader(), state, 1_100_000_000)
|
||||
steer = next(frame for frame in sends if frame[0] == 0x124)
|
||||
self.assertEqual(steer[2], 1)
|
||||
self.assertEqual(bool(steer[1][1] & 0x10), active and state.out.cruiseState.available and
|
||||
state.out.cruiseState.enabled and not state.out.brakePressed)
|
||||
return state, steer, sends
|
||||
|
||||
def arm(self, cp, frames):
|
||||
self.mode(cp)
|
||||
required = {0x40, 0x119, 0x11A, 0x13A, 0x13C, 0x222, 0x321}
|
||||
for frame in frames:
|
||||
if frame[0] in required:
|
||||
self.assertTrue(self.safety.safety_rx_hook(self.packet(frame)), hex(frame[0]))
|
||||
self.safety.safety_tick()
|
||||
self.assertTrue(self.safety.safety_config_valid())
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_stock_packed_host_to_native_and_inactive(self):
|
||||
for car in CARS:
|
||||
for release in (False, True):
|
||||
with self.subTest(car=car, release=release):
|
||||
cp = self.params(car, alpha=True, release=release)
|
||||
self.assertFalse(cp.dashcamOnly)
|
||||
self.assertFalse(cp.openpilotLongitudinalControl)
|
||||
self.assertFalse(cp.alphaLongitudinalAvailable)
|
||||
self.assertEqual(cp.steerControlType, structs.CarParams.SteerControlType.angle)
|
||||
packer, frames = self.sources(car)
|
||||
state, steer, sends = self.controller(car, cp, frames)
|
||||
self.assertTrue(state.out.cruiseState.enabled)
|
||||
self.assertNotEqual(steer[1][5:7], b'\x00\x00')
|
||||
self.arm(cp, frames)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self.packet(steer)))
|
||||
self.assertEqual({frame[0] for frame in sends}, {0x124, 0x321, 0x322})
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.packet((0x122, bytes(8), 0))))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.packet((0x124, steer[1], 0))))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.packet((0x124, steer[1][:-1], 1))))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.packet(packer.make_can_msg('ES_Distance', 1, {}))))
|
||||
cancel = structs.CarControl()
|
||||
cancel.cruiseControl.cancel = True
|
||||
_, cancel_sends = CarController(DBC[car], cp).update(cancel.as_reader(), state, 1_100_000_000)
|
||||
distance = next(frame for frame in cancel_sends if frame[0] == 0x221)
|
||||
self.assertEqual(distance[2], 1)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self.packet(distance)))
|
||||
|
||||
_, inactive, _ = self.controller(car, cp, frames, active=False)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self.packet(inactive)))
|
||||
brake_frames = [packer.make_can_msg('Brake_Status', 1, {'Brake': 1}) if f[0] == 0x13C else f for f in frames]
|
||||
brake_state, brake_steer, _ = self.controller(car, cp, brake_frames)
|
||||
self.assertTrue(brake_state.out.brakePressed)
|
||||
self.assertFalse(brake_steer[1][1] & 0x10)
|
||||
self.assertTrue(self.safety.safety_rx_hook(self.packet(brake_frames[4])))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.assertTrue(self.safety.safety_tx_hook(self.packet(brake_steer)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.packet(steer)))
|
||||
|
||||
def test_required_health_and_raw_long_conflict(self):
|
||||
for car in CARS:
|
||||
cp = self.params(car)
|
||||
_, frames = self.sources(car)
|
||||
required = [f for f in frames if f[0] in {0x40, 0x119, 0x11A, 0x13A, 0x13C, 0x222, 0x321}]
|
||||
for missing in required:
|
||||
with self.subTest(car=car, missing=hex(missing[0])):
|
||||
self.mode(cp)
|
||||
for frame in required:
|
||||
if frame is not missing:
|
||||
self.safety.safety_rx_hook(self.packet(frame))
|
||||
self.safety.safety_tick()
|
||||
self.assertFalse(self.safety.safety_config_valid())
|
||||
self.arm(cp, frames)
|
||||
_, steer, _ = self.controller(car, cp, frames)
|
||||
for flags in (SubaruSafetyFlags.LONG, 0x20, 0x100):
|
||||
with self.subTest(car=car, flags=flags):
|
||||
self.mode(cp, extra_flags=flags)
|
||||
for frame in required:
|
||||
self.safety.safety_rx_hook(self.packet(frame))
|
||||
self.safety.safety_tick()
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.packet(steer)))
|
||||
for raw_param in (0x10, 0x30, 0x31, 0x111):
|
||||
with self.subTest(car=car, raw_param=hex(raw_param)):
|
||||
self.assertEqual(self.safety.set_safety_hooks(structs.CarParams.SafetyModel.subaru, raw_param), 0)
|
||||
self.safety.init_tests()
|
||||
for frame in required:
|
||||
self.safety.safety_rx_hook(self.packet(frame))
|
||||
self.safety.safety_tick()
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.packet(steer)))
|
||||
self.mode(cp)
|
||||
for frame in required:
|
||||
if frame[0] == 0x222:
|
||||
frame = (frame[0], frame[1][:-1] + bytes([frame[1][-1] ^ 1]), frame[2])
|
||||
self.safety.safety_rx_hook(self.packet(frame))
|
||||
self.safety.safety_tick()
|
||||
self.assertFalse(self.safety.safety_config_valid())
|
||||
|
||||
for source in required:
|
||||
for bad in ((source[0], source[1], (source[2] + 1) % 3),
|
||||
(source[0], source[1][:-1], source[2])):
|
||||
with self.subTest(car=car, source=hex(source[0]), bad=bad[2:]):
|
||||
self.mode(cp)
|
||||
for frame in required:
|
||||
self.safety.safety_rx_hook(self.packet(bad if frame is source else frame))
|
||||
self.safety.safety_tick()
|
||||
self.assertFalse(self.safety.safety_config_valid())
|
||||
|
||||
self.arm(cp, frames)
|
||||
self.safety.set_timer(10_000_000)
|
||||
self.safety.safety_tick()
|
||||
self.assertFalse(self.safety.safety_config_valid())
|
||||
|
||||
self.mode(cp)
|
||||
for frame in required:
|
||||
self.safety.safety_rx_hook(self.packet(frame))
|
||||
for _ in range(6):
|
||||
self.safety.safety_rx_hook(self.packet(next(f for f in required if f[0] == 0x222)))
|
||||
self.safety.safety_tick()
|
||||
self.assertFalse(self.safety.safety_config_valid())
|
||||
|
||||
def test_stock_main_driver_and_cruise_handoff(self):
|
||||
for car in CARS:
|
||||
cp = self.params(car)
|
||||
for override in ({'main': False}, {'cruise': False}, {'brake': True}, {'torque': 250}):
|
||||
with self.subTest(car=car, override=override):
|
||||
_, frames = self.sources(car, **override)
|
||||
self.mode(cp)
|
||||
for frame in frames:
|
||||
self.safety.safety_rx_hook(self.packet(frame))
|
||||
state, steer, _ = self.controller(car, cp, frames)
|
||||
if override.get('torque'):
|
||||
# The host deliberately waits for a second steering period before
|
||||
# cutting a request on persistent driver input.
|
||||
command = structs.CarControl()
|
||||
command.enabled = command.latActive = True
|
||||
command.actuators.steeringAngleDeg = 1.0
|
||||
controller = CarController(DBC[car], cp)
|
||||
controller.update(command.as_reader(), state, 1_100_000_000)
|
||||
controller.update(command.as_reader(), state, 1_110_000_000)
|
||||
_, sends = controller.update(command.as_reader(), state, 1_120_000_000)
|
||||
steer = next(frame for frame in sends if frame[0] == 0x124)
|
||||
self.assertFalse(steer[1][1] & 0x10)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self.packet(steer)))
|
||||
|
||||
_, frames = self.sources(car)
|
||||
self.arm(cp, frames)
|
||||
_, steer, _ = self.controller(car, cp, frames)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self.packet(steer)))
|
||||
_, main_off = self.sources(car, main=False)
|
||||
dash = next(frame for frame in main_off if frame[0] == 0x321)
|
||||
self.assertTrue(self.safety.safety_rx_hook(self.packet(dash)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.packet(steer)))
|
||||
|
||||
def test_forwarding_and_relay(self):
|
||||
for car in CARS:
|
||||
cp = self.params(car)
|
||||
self.mode(cp)
|
||||
self.assertEqual(self.safety.safety_fwd_hook(0, 0x124), 2)
|
||||
self.assertEqual(self.safety.safety_fwd_hook(2, 0x124), 0)
|
||||
self.assertEqual(self.safety.safety_fwd_hook(2, 0x321), -1)
|
||||
self.assertEqual(self.safety.safety_fwd_hook(2, 0x322), -1)
|
||||
self.assertEqual(self.safety.safety_fwd_hook(2, 0x221), 0)
|
||||
self.assertEqual(self.safety.safety_fwd_hook(1, 0x124), -1)
|
||||
_, frames = self.sources(car)
|
||||
_, steer, _ = self.controller(car, cp, frames)
|
||||
self.arm(cp, frames)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self.packet(steer)))
|
||||
self.safety.set_relay_malfunction(True)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.packet(steer)))
|
||||
self.assertEqual(self.safety.safety_fwd_hook(0, 0x124), -1)
|
||||
self.assertEqual(self.safety.safety_fwd_hook(2, 0x221), -1)
|
||||
|
||||
def test_host_native_angle_rate_matrix(self):
|
||||
for car in CARS:
|
||||
for speed in (5, 30, 80):
|
||||
for target in (-650., 650.):
|
||||
with self.subTest(car=car, speed=speed, target=target):
|
||||
cp = self.params(car)
|
||||
_, frames = self.sources(car, speed=speed)
|
||||
state = CarState(cp)
|
||||
parsers = state.get_can_parsers(cp)
|
||||
state.update(parsers)
|
||||
for parser in parsers.values():
|
||||
parser.update([(1_000_000_000, frames)])
|
||||
state.out = state.update(parsers).as_reader()
|
||||
self.arm(cp, frames)
|
||||
controller = CarController(DBC[car], cp)
|
||||
command = structs.CarControl()
|
||||
command.enabled = command.latActive = True
|
||||
command.actuators.steeringAngleDeg = target
|
||||
seen = []
|
||||
for i in range(120):
|
||||
self.safety.set_timer(1_000_000 + i * 10_000)
|
||||
_, sends = controller.update(command.as_reader(), state, 1_000_000_000 + i * 10_000_000)
|
||||
for frame in sends:
|
||||
if frame[0] == 0x124:
|
||||
self.assertTrue(self.safety.safety_tx_hook(self.packet(frame)),
|
||||
f'{car.value} {speed=} {target=} {i=} {frame[1].hex()}')
|
||||
seen.append(frame)
|
||||
self.assertGreater(len(seen), 50)
|
||||
self.assertNotEqual(seen[-1][1][5:7], b'\x00\x00')
|
||||
|
||||
def test_firmware_complete_partial_and_mixed(self):
|
||||
# The two identities stay manual-only: the production exact matcher treats
|
||||
# multiple observed versions at one ECU address as alternatives. A mixed
|
||||
# ABS observation can otherwise select a sibling with false confidence.
|
||||
e = structs.CarParams.Ecu
|
||||
versions = {
|
||||
CARS[0]: [(e.abs, 0x7b0, b'\xa2 $\x15\x05'), (e.fwdCamera, 0x787, b'\x1d!\x08\x00F\x14!\x08\x00='),
|
||||
(e.engine, 0x7a2, b'\x04"cP\x07')],
|
||||
CARS[1]: [(e.abs, 0x7b0, b'\xa1 $\x11\x00'), (e.eps, 0x746, b'[\xc0\xd1\x10\x00'),
|
||||
(e.fwdCamera, 0x787, b'\x1a!\x08\x00C\x0e!\x08\x018'),
|
||||
(e.engine, 0x7a2, b'\x08,\xa0p\x07'), (e.transmission, 0x7a3, b'\xeb\x17U!r')],
|
||||
}
|
||||
|
||||
def records(entries):
|
||||
return [structs.CarParams.CarFw(ecu=ecu, address=addr, fwVersion=version, brand='subaru')
|
||||
for ecu, addr, version in entries]
|
||||
|
||||
for car in CARS:
|
||||
for entries in (versions[car], versions[car][:1], versions[car][1:2]):
|
||||
_, matches = match_fw_to_car(records(entries), '00000000000000000', log=False)
|
||||
self.assertNotIn(car, matches)
|
||||
mixed = versions[CARS[0]][:1] + versions[CARS[1]]
|
||||
_, matches = match_fw_to_car(records(mixed), '00000000000000000', log=False)
|
||||
self.assertFalse(set(CARS) & matches)
|
||||
Reference in New Issue
Block a user