diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py index 9c668988c0..44c2303fec 100644 --- a/opendbc_repo/opendbc/car/subaru/carcontroller.py +++ b/opendbc_repo/opendbc/car/subaru/carcontroller.py @@ -5,6 +5,7 @@ from opendbc.car.lateral import (apply_driver_steer_torque_limits, apply_std_ste apply_steer_angle_limits_vm, common_fault_avoidance) from opendbc.car.interfaces import CarControllerBase from opendbc.car.subaru import subarucan +from opendbc.car.subaru.stock_aol import qualified as subaru_aol_qualified from opendbc.car.subaru.values import CAR, DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags from opendbc.car.vehicle_model import VehicleModel @@ -69,7 +70,8 @@ class CarController(CarControllerBase): 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) + lateral_active = self.angle_was_active and CC.latActive and (CC.enabled or subaru_aol_qualified(self.CP, marked_only=True)) + msg = subarucan.create_ascent_lkas_alert(dat, lateral_active, 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 @@ -88,9 +90,11 @@ class CarController(CarControllerBase): # *** steering *** if (self.frame % self.p.STEER_STEP) == 0: 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 + independent_lateral = subaru_aol_qualified(self.CP, marked_only=True) + angle_available = (CC.latActive and CS.out.cruiseState.available and + (independent_lateral or CC.enabled and CS.out.cruiseState.enabled and not CS.out.brakePressed) and CS.out.gearShifter == structs.CarState.GearShifter.drive and - not CS.out.standstill and not CS.out.brakePressed) + not CS.out.standstill and not CS.out.steerFaultTemporary and not CS.out.steerFaultPermanent) 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 diff --git a/opendbc_repo/opendbc/car/subaru/stock_aol.py b/opendbc_repo/opendbc/car/subaru/stock_aol.py new file mode 100644 index 0000000000..e4d8789189 --- /dev/null +++ b/opendbc_repo/opendbc/car/subaru/stock_aol.py @@ -0,0 +1,40 @@ +"""Exact stock-cruise Subaru independent-lateral configuration contract.""" +from opendbc.car import structs +from opendbc.car.subaru.values import CAR, SubaruFlags, SubaruSafetyFlags + +AOL_EXPERIENCE = 32 +TORQUE_CARS = frozenset((CAR.SUBARU_ASCENT, CAR.SUBARU_OUTBACK, CAR.SUBARU_LEGACY, + CAR.SUBARU_IMPREZA, CAR.SUBARU_IMPREZA_2020, CAR.SUBARU_FORESTER)) +ANGLE_CARS = frozenset((CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023)) + + +def qualified(cp, *, marked_only=False): + if (cp is None or cp.brand != 'subaru' or cp.carFingerprint not in TORQUE_CARS | ANGLE_CARS or + cp.notCar or cp.passive or cp.dashcamOnly or not cp.pcmCruise or cp.openpilotLongitudinalControl or + cp.transmissionType == structs.CarParams.TransmissionType.manual or len(cp.safetyConfigs) != 1 or + cp.alternativeExperience not in ((AOL_EXPERIENCE,) if marked_only else (0, AOL_EXPERIENCE))): + return False + static = int(CAR[cp.carFingerprint].config.flags) + allowed = int(SubaruFlags.HAS_BSM | SubaruFlags.SEND_INFOTAINMENT) + expected = int(SubaruSafetyFlags.GEN2) if static & SubaruFlags.GLOBAL_GEN2 else 0 + angle = cp.carFingerprint in ANGLE_CARS + if angle: + expected |= int(SubaruSafetyFlags.LKAS_ANGLE) + if cp.carFingerprint != CAR.SUBARU_CROSSTREK_2025: + expected |= int(SubaruSafetyFlags.FIXED_ANGLE_LIMITS) + if cp.carFingerprint == CAR.SUBARU_ASCENT_2023: + expected |= int(SubaruSafetyFlags.ANGLE_MAIN_BUS) + allowed |= int(SubaruFlags.ASCENT_LKAS_ALERT) + if cp.flags & SubaruFlags.ASCENT_LKAS_ALERT: + expected |= int(SubaruSafetyFlags.ASCENT_LKAS_ALERT) + if (int(cp.flags) & ~allowed != static or + cp.steerControlType != (structs.CarParams.SteerControlType.angle if angle else structs.CarParams.SteerControlType.torque)): + return False + config = cp.safetyConfigs[0] + return config.safetyModel == structs.CarParams.SafetyModel.subaru and config.safetyParam == expected + + +def temporary_restriction(cp, state): + return bool(state is not None and qualified(cp, marked_only=True) and state.canValid and not state.canTimeout and + not state.steerFaultPermanent and not state.accFaulted and state.cruiseState.available and + (state.gearShifter != structs.CarState.GearShifter.drive or state.steerFaultTemporary or state.standstill)) diff --git a/opendbc_repo/opendbc/safety/modes/subaru.h b/opendbc_repo/opendbc/safety/modes/subaru.h index 9635d63f89..78a8e7be3b 100644 --- a/opendbc_repo/opendbc/safety/modes/subaru.h +++ b/opendbc_repo/opendbc/safety/modes/subaru.h @@ -1,6 +1,7 @@ #pragma once #include "opendbc/safety/declarations.h" +#include "opendbc/safety/modes/subaru_stock_aol.h" #define SUBARU_STEERING_LIMITS_GENERATOR(steer_max, rate_up, rate_down) \ { \ @@ -102,6 +103,13 @@ static void subaru_rx_hook(const CANPacket_t *msg) { torque_driver_new = ((GET_BYTES_LE(msg, 0, 4) >> 16) & 0x7FFU); torque_driver_new = -1 * to_signed(torque_driver_new, 11); update_sample(&torque_driver, torque_driver_new); + if (subaru_aol_enabled) { + subaru_aol_permanent = GET_BIT(msg, 12U); + subaru_aol_temporary = GET_BIT(msg, 29U); + if (subaru_aol_permanent) { + subaru_aol_clear(); + } + } } if (subaru_lkas_angle && (msg->addr == MSG_SUBARU_Steering_2) && (msg->bus == SUBARU_MAIN_BUS)) { @@ -112,15 +120,31 @@ static void subaru_rx_hook(const CANPacket_t *msg) { if (subaru_lkas_angle && (msg->addr == MSG_SUBARU_ES_Status) && (msg->bus == SUBARU_ALT_BUS)) { pcm_cruise_check(GET_BIT(msg, 29U)); + subaru_aol_grant(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); + subaru_aol_main = acc_main_on; + subaru_aol_main_seen = true; + subaru_aol_main_ts = microsecond_timer_get(); + if (!acc_main_on) { + subaru_aol_clear(); + } } // enter controls on rising edge of ACC, exit controls on ACC off 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); + if (subaru_aol_enabled) { + subaru_aol_grant(cruise_engaged); + subaru_aol_main = GET_BIT(msg, 40U); + subaru_aol_main_seen = true; + subaru_aol_main_ts = microsecond_timer_get(); + if (!subaru_aol_main) { + subaru_aol_clear(); + } + } } // update vehicle moving with any non-zero wheel speed @@ -273,6 +297,21 @@ static safety_config subaru_init(uint16_t param) { // 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 + subaru_aol_reset(); + subaru_aol_enabled = ((unsigned int)alternative_experience == 32U) && + ((param == 0U) || (param == 1U) || (param == 17U) || (param == 145U) || + (param == 177U) || (param == 2225U)); + if (subaru_aol_enabled) { + static const AolSafetyPolicy policy = { + .reset = subaru_aol_reset, + .host_request = subaru_aol_host_request, + .request_mask = subaru_aol_request, + .permission_mask = subaru_aol_permission, + .rx_invalid = subaru_aol_clear, + }; + aol_policy = &policy; + } + safety_config ret; if (subaru_lkas_angle) { static RxCheck subaru_gen2_angle_rx_checks[] = { @@ -292,12 +331,17 @@ static safety_config subaru_init(uint16_t param) { }; ret = BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_TX_MSGS); } + if (((unsigned int)alternative_experience == 32U) && !subaru_aol_enabled) { + ret.tx_msgs = NULL; + ret.tx_msgs_len = 0; + } return ret; } const safety_hooks subaru_hooks = { .init = subaru_init, .rx = subaru_rx_hook, + .optional_rx = subaru_aol_optional_rx, .tx = subaru_tx_hook, .get_counter = subaru_get_counter, .get_checksum = subaru_get_checksum, diff --git a/opendbc_repo/opendbc/safety/modes/subaru_stock_aol.h b/opendbc_repo/opendbc/safety/modes/subaru_stock_aol.h new file mode 100644 index 0000000000..f63d837f1f --- /dev/null +++ b/opendbc_repo/opendbc/safety/modes/subaru_stock_aol.h @@ -0,0 +1,154 @@ +#pragma once + +// The existing axis transport owns host request/heartbeat leases. This owner +// grants lateral only after ordinary engagement, with current Subaru sources. +static bool subaru_aol_enabled = false; +static uint8_t subaru_aol_gear_bus = 0U; +static bool subaru_aol_armed = false; +static bool subaru_aol_token = false; +static bool subaru_aol_pending = false; +static bool subaru_aol_session = false; +static bool subaru_aol_requested = false; +static uint32_t subaru_aol_grant_ts = 0U; +static bool subaru_aol_main = false; +static bool subaru_aol_cruise = false; +static bool subaru_aol_permanent = false; +static bool subaru_aol_temporary = false; +static bool subaru_aol_drive = false; +static bool subaru_aol_gear_seen = false; +static bool subaru_aol_gear_qualified = false; +static uint8_t subaru_aol_gear_counter = 0U; +static uint32_t subaru_aol_gear_ts = 0U; +static uint32_t subaru_aol_main_ts = 0U; +static bool subaru_aol_main_seen = false; + +static void subaru_aol_clear(void) { + subaru_aol_armed = false; + subaru_aol_token = false; + subaru_aol_pending = false; + subaru_aol_session = false; +} + +static void subaru_aol_reset(void) { + subaru_aol_clear(); + subaru_aol_enabled = false; + subaru_aol_requested = false; + subaru_aol_grant_ts = 0U; + subaru_aol_gear_bus = 0U; + subaru_aol_main = false; + subaru_aol_cruise = false; + subaru_aol_permanent = false; + subaru_aol_temporary = false; + subaru_aol_drive = false; + subaru_aol_gear_seen = false; + subaru_aol_gear_qualified = false; + subaru_aol_main_seen = false; + subaru_aol_gear_counter = 0U; + subaru_aol_gear_ts = 0U; + subaru_aol_main_ts = 0U; +} + +// A successful physical stock engagement precedes the host/Panda join. +// The token cannot grant output until the existing fresh request and heartbeat +// acknowledge it. A fault-clear while cruise remains held cannot mint a token. +static void subaru_aol_grant(bool cruise) { + if (subaru_aol_enabled && cruise && !subaru_aol_cruise && controls_allowed && + subaru_aol_main && subaru_aol_gear_qualified && subaru_aol_drive && + (safety_get_ts_elapsed(microsecond_timer_get(), subaru_aol_gear_ts) <= 100000U) && + !subaru_aol_permanent && !subaru_aol_temporary && !brake_pressed && + !relay_malfunction && !safety_rx_checks_invalid && aol_rx_healthy()) { + subaru_aol_token = true; + subaru_aol_pending = !heartbeat_engaged; + subaru_aol_session = heartbeat_engaged; + subaru_aol_grant_ts = microsecond_timer_get(); + } + subaru_aol_cruise = cruise; +} + +static void subaru_aol_host_request(uint8_t mask) { + subaru_aol_requested |= mask != 0U; +} + +static uint8_t subaru_aol_request(void) { + uint8_t request = 0U; + const uint32_t now = microsecond_timer_get(); + const uint32_t grant_age = safety_get_ts_elapsed(now, subaru_aol_grant_ts); + const bool pending_expired = subaru_aol_pending && (grant_age > AOL_HOST_REQUEST_TIMEOUT_US); + const bool request_expired = subaru_aol_requested && + (safety_get_ts_elapsed(now, aol_host_request_ts) > AOL_HOST_REQUEST_TIMEOUT_US); + if (relay_malfunction || safety_rx_checks_invalid || !aol_rx_healthy() || pending_expired || request_expired || + (subaru_aol_session && !heartbeat_engaged)) { + subaru_aol_clear(); + } else if (heartbeat_engaged) { + const bool acknowledged = !subaru_aol_pending || ((aol_host_axis_mask != 0U) && + (safety_get_ts_elapsed(now, aol_host_request_ts) < grant_age)); + if (acknowledged) { + subaru_aol_pending = false; + subaru_aol_session = true; + request = aol_host_axis_mask & 1U; + } + } else { + } + return request; +} + +static uint8_t subaru_aol_permission(void) { + const uint8_t request = subaru_aol_request(); + const uint32_t now = microsecond_timer_get(); + // Conservative additional-source bounds do not extend common RX/host leases. + const bool current = subaru_aol_gear_qualified && subaru_aol_main_seen && + (safety_get_ts_elapsed(now, subaru_aol_gear_ts) <= 100000U); + // MAIN is already a required, checksum/counter/frequency-qualified RX source. + // Its original lease stays owned by aol_rx_healthy(); an extra one-period + // deadline incorrectly revoked intent at ordinary 10Hz publication jitter. + if (!subaru_aol_main || subaru_aol_permanent) { + subaru_aol_clear(); + } + uint8_t permission = 0U; + if (current && subaru_aol_main && subaru_aol_drive && vehicle_moving && + !subaru_aol_permanent && !subaru_aol_temporary) { + if ((request != 0U) && controls_allowed && subaru_aol_cruise && subaru_aol_token) { + subaru_aol_armed = true; + } + if (subaru_aol_armed) { + permission = request; + } + } + return permission; +} + +// Called only for the non-required Transmission frame, after its own exact +// checksum/counter checks. Required control RX remains independently mandatory. +static void subaru_aol_optional_rx(const CANPacket_t *msg) { + if (subaru_aol_enabled && (msg->addr == 0x48U) && (msg->bus == subaru_aol_gear_bus)) { + if (GET_LEN(msg) != 8U) { + subaru_aol_gear_qualified = false; + subaru_aol_gear_seen = false; + } else { + uint8_t checksum = 0x48U; + for (int i = 1; i < 8; i++) { + checksum += msg->data[i]; + } + const uint8_t counter = msg->data[1] & 15U; + const bool valid = msg->data[0] == checksum; + if (!valid) { + subaru_aol_gear_qualified = false; + subaru_aol_gear_seen = false; + } else if (!subaru_aol_gear_seen || (counter != subaru_aol_gear_counter)) { + subaru_aol_gear_qualified = subaru_aol_gear_seen && + (counter == ((subaru_aol_gear_counter + 1U) & 15U)); + subaru_aol_gear_seen = true; + subaru_aol_gear_counter = counter; + subaru_aol_gear_ts = microsecond_timer_get(); + subaru_aol_drive = msg->data[3] == 121U; + } else { + // A contradictory duplicate must withdraw Drive immediately; it may + // neither renew the source receipt nor retain an old Drive permission. + if (((msg->data[3] == 121U) && !subaru_aol_drive) || ((msg->data[3] != 121U) && subaru_aol_drive)) { + subaru_aol_gear_qualified = false; + subaru_aol_gear_seen = false; + } + } + } + } +} diff --git a/opendbc_repo/opendbc/safety/tests/test_subaru_stock_aol.py b/opendbc_repo/opendbc/safety/tests/test_subaru_stock_aol.py new file mode 100644 index 0000000000..e81260b508 --- /dev/null +++ b/opendbc_repo/opendbc/safety/tests/test_subaru_stock_aol.py @@ -0,0 +1,207 @@ +"""Real Subaru native permission, without forcing controlsAllowed.""" +import unittest + +from opendbc.car import structs +from opendbc.safety.tests.common import CANPackerSafety +from opendbc.safety.tests.libsafety import libsafety_py + + +class TestSubaruStockAol(unittest.TestCase): + def setUp(self): + self.safety = libsafety_py.libsafety + + def configure(self, param=177, experience=32): + self.safety = libsafety_py.libsafety + self.safety.init_tests() + self.safety.set_alternative_experience(experience) + self.assertEqual(self.safety.set_safety_hooks(structs.CarParams.SafetyModel.subaru, param), 0) + self.safety.set_aol_test_heartbeat(True) + self.packer = CANPackerSafety('subaru_global_2017_generated') + self.param = param + self.now = 1_000_000 + self.safety.set_timer(self.now) + + def tearDown(self): + self.safety.init_tests() + self.safety.set_alternative_experience(0) + self.safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0) + + def frame(self, name, values, bus=0): + return self.packer.make_can_msg_safety(name, bus, values) + + def feed(self, *, main=True, cruise=False, brake=False, gear=121, temporary=False, permanent=False, + gear_bus=None, omit_gear=False, host_mask=1, omit_main=False): + self.now += 10_000 + self.safety.set_timer(self.now) + alt = 1 if self.param & 1 else 0 + main_bus = 1 if self.param == 209 else 0 + if gear_bus is None: + gear_bus = main_bus + frames = [ + ('Throttle', {}, main_bus), + ('Steering_Torque', {'Steer_Error_1': int(permanent), 'Steer_Warning': int(temporary)}, 0), + ('Steering_2', {}, 0), + ('Wheel_Speeds', dict.fromkeys(('FL', 'FR', 'RL', 'RR'), 54), alt), + ('Brake_Status', {'Brake': int(brake)}, alt), + ('CruiseControl', {'Cruise_On': int(main), 'Cruise_Activated': int(cruise)}, alt), + ('ES_Status', {'Cruise_Activated': int(cruise)}, 2 if self.param == 16 else 1), + ('ES_DashStatus', {'Cruise_On': int(main)}, 2), + ] + if not omit_gear: + frames.append(('Transmission', {'Gear': gear}, gear_bus)) + for name, fields, bus in frames: + if omit_main and name == 'ES_DashStatus': + continue + self.safety.safety_rx_hook(self.frame(name, fields, bus)) + self.safety.safety_tick() + self.safety.aol_set_host_request(host_mask) + return self.safety.aol_get_permission_mask() + + def arm(self): + for _ in range(12): + self.feed(cruise=False) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + self.assertEqual(self.feed(cruise=True), 1) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_all_stock_profiles_full_engagement_brake_reverse_drive_main_off(self): + for param in (0, 1, 17, 145, 177, 2225): + with self.subTest(param=param): + self.configure(param) + self.arm() + self.assertEqual(self.feed(brake=True), 1) + self.assertFalse(self.safety.get_controls_allowed()) + self.assertEqual(self.feed(gear=3), 0) + self.assertEqual(self.feed(gear=121), 1) + self.assertFalse(self.safety.get_controls_allowed()) + self.assertEqual(self.feed(temporary=True), 0) + self.assertEqual(self.feed(), 1) + self.assertEqual(self.feed(main=False), 0) + self.assertEqual(self.feed(main=True), 0) + self.assertEqual(self.feed(cruise=True), 1) + + def test_permanent_fault_and_rx_fault_require_fresh_ordinary_engagement(self): + self.configure() + self.arm() + self.assertEqual(self.feed(permanent=True, brake=True), 0) + self.assertEqual(self.feed(), 0) + self.assertEqual(self.feed(cruise=True), 1) + bad = self.frame('Steering_Torque', {}) + # Mutate the wire checksum without recomputing it. + raw = bytearray(bytes(bad.data)) + raw[0] ^= 1 + malformed = libsafety_py.make_CANPacket(0x119, 0, bytes(raw[:8])) + self.assertFalse(self.safety.safety_rx_hook(malformed)) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + + def test_additional_gear_counter_checksum_bus_and_freshness(self): + self.configure() + for _ in range(12): + self.feed(cruise=True, gear_bus=1) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + self.assertEqual(self.feed(cruise=True), 0) + self.assertEqual(self.feed(cruise=True), 0) + # Correct gear alone cannot restore the lost physical engagement token. + self.assertEqual(self.feed(cruise=False), 0) + self.assertEqual(self.feed(cruise=True), 1) + gear = self.frame('Transmission', {'Gear': 121}) + self.safety.safety_rx_hook(gear) + for _ in range(12): + self.feed(cruise=False, omit_gear=True) + self.safety.safety_rx_hook(gear) # a replay must not renew its original receipt + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + bad = self.frame('Transmission', {'Gear': 121}) + raw = bytearray(bytes(bad.data)) + raw[0] ^= 1 + self.safety.safety_rx_hook(libsafety_py.make_CANPacket(0x48, 0, bytes(raw[:8]))) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + + def test_stock_off_host_timeout_heartbeat_and_no_longitudinal_grant(self): + self.configure() + self.arm() + self.safety.aol_set_host_request(3) + self.assertEqual(self.safety.aol_get_permission_mask(), 1) + self.safety.aol_set_host_request(0) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + self.safety.set_timer(self.now + 301_000) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + self.safety.set_aol_test_heartbeat(False) + self.assertEqual(self.feed(cruise=True), 0) + self.configure(experience=0) + self.assertEqual(self.feed(cruise=True), 0) # ordinary controls is separate from AOL permission + + def test_real_angle_and_torque_tx_keep_existing_limits(self): + for param in (0, 1, 17, 145, 177): + self.configure(param) + self.arm() + self.feed(brake=True) + angle = bool(param & 16) + bus = 0 if param & 32 or param in (16, 209) or not angle else 1 + command = (self.frame('ES_LKAS_ANGLE', {'LKAS_Output': 0, 'LKAS_Request': 1}, bus) if angle else + self.frame('ES_LKAS', {'LKAS_Output': 1, 'LKAS_Request': 1}, bus)) + self.assertTrue(self.safety.safety_tx_hook(command)) + self.assertFalse(self.safety.safety_tx_hook(self.frame('ES_Distance', {'Cruise_Throttle': 2123}, 1 if param & 1 else 0))) + self.feed(main=False) + self.assertFalse(self.safety.safety_tx_hook(command)) + + def test_main_uses_original_required_rx_lease(self): + self.configure(145) + self.arm() + for _ in range(11): + self.assertEqual(self.feed(omit_main=True), 1) + # Required health still fails on the original native boundary; no MAIN lease renewal. + self.now += 1_000_001 + self.safety.set_timer(self.now) + self.safety.aol_set_host_request(1) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + + def test_conflicting_gear_duplicate_withdraws_without_renewing(self): + self.configure(145) + self.arm() + drive = self.frame('Transmission', {'Gear': 121}) + self.safety.safety_rx_hook(drive) + self.safety.aol_set_host_request(1) + self.assertEqual(self.safety.aol_get_permission_mask(), 1) + raw = bytearray(bytes(drive.data)[:8]) + raw[3] = 3 # Same counter, contradictory Reverse payload. + raw[0] = (0x48 + sum(raw[1:])) & 255 + self.safety.safety_rx_hook(libsafety_py.make_CANPacket(0x48, 0, bytes(raw))) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + + def test_permanent_fault_cannot_rearm_with_cruise_held_on(self): + self.configure() + self.arm() + self.assertEqual(self.feed(permanent=True, cruise=True), 0) + self.assertEqual(self.feed(permanent=False, cruise=True), 0) + self.assertEqual(self.feed(cruise=False), 0) + self.assertEqual(self.feed(cruise=True), 1) + + def test_unrecognized_words_cannot_acquire_independent_permission(self): + for param in (2, 3, 16, 209, 65535): + self.configure(param) + for _ in range(12): + self.feed(cruise=False) + self.feed(cruise=True) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + + def test_physical_engagement_pending_host_join_and_expiry(self): + self.configure() + self.safety.set_aol_test_heartbeat(False) + for _ in range(12): + self.feed(cruise=False) + self.assertEqual(self.feed(cruise=True), 0) + for _ in range(8): + self.assertEqual(self.feed(cruise=True), 0) + self.safety.set_aol_test_heartbeat(True) + self.assertEqual(self.feed(cruise=True), 1) + self.configure() + self.safety.set_aol_test_heartbeat(False) + for _ in range(12): + self.feed(cruise=False) + self.assertEqual(self.feed(cruise=True), 0) + for _ in range(31): + self.assertEqual(self.feed(cruise=True), 0) + self.safety.set_aol_test_heartbeat(True) + self.assertEqual(self.feed(cruise=True), 0) + self.assertEqual(self.feed(cruise=False), 0) + self.assertEqual(self.feed(cruise=True), 1) diff --git a/openpilot/selfdrive/pandad/pandad.cc b/openpilot/selfdrive/pandad/pandad.cc index b52ba47f37..8b03af8e94 100644 --- a/openpilot/selfdrive/pandad/pandad.cc +++ b/openpilot/selfdrive/pandad/pandad.cc @@ -7,6 +7,7 @@ #include "openpilot/starpilot/car/mazda/aol_policy.h" #include "openpilot/starpilot/car/volkswagen/aol_policy.h" #include "openpilot/starpilot/car/chrysler/aol_policy.h" +#include "openpilot/starpilot/car/subaru/aol_policy.h" #include "openpilot/starpilot/car/toyota/aol_policy.h" #include "openpilot/starpilot/car/tesla/aol_policy.h" #include "openpilot/starpilot/car/hyundai/aol_policy.h" @@ -478,7 +479,7 @@ void pandad_run(std::vector &pandas) { Panda *previous_aol_owner = nullptr; bool engaged = false; bool is_onroad = false; - constexpr AolSafetyProfile aol_profiles[] = {HONDA_AOL_PROFILE, HONDA_STOCK_AOL_PROFILE, HONDA_NIDEC_AOL_PROFILE, HYUNDAI_AOL_PROFILE, HYUNDAI_CLASSIC_AOL_PROFILE, HYUNDAI_LEGACY_AOL_PROFILE, GM_AOL_PROFILE, FORD_AOL_PROFILE, MAZDA_AOL_PROFILE, CHRYSLER_AOL_PROFILE, TESLA_PREAP_AOL_PROFILE, TESLA_SCREEN_AOL_PROFILE, TOYOTA_AOL_PROFILE, NISSAN_AOL_PROFILE, VOLKSWAGEN_STOCK_AOL_PROFILE, VOLKSWAGEN_LONG_AOL_PROFILE}; + constexpr AolSafetyProfile aol_profiles[] = {HONDA_AOL_PROFILE, HONDA_STOCK_AOL_PROFILE, HONDA_NIDEC_AOL_PROFILE, HYUNDAI_AOL_PROFILE, HYUNDAI_CLASSIC_AOL_PROFILE, HYUNDAI_LEGACY_AOL_PROFILE, GM_AOL_PROFILE, FORD_AOL_PROFILE, MAZDA_AOL_PROFILE, CHRYSLER_AOL_PROFILE, TESLA_PREAP_AOL_PROFILE, TESLA_SCREEN_AOL_PROFILE, TOYOTA_AOL_PROFILE, NISSAN_AOL_PROFILE, VOLKSWAGEN_STOCK_AOL_PROFILE, VOLKSWAGEN_LONG_AOL_PROFILE, SUBARU_AOL_PROFILE}; const AolProfileRegistry aol_registry{aol_profiles, std::size(aol_profiles)}; AolAxisNegotiator aol_negotiator(aol_registry); // Main loop: receive CAN first, then process lower priority panda and peripheral state. diff --git a/openpilot/selfdrive/pandad/tests/test_aol_protocol.cc b/openpilot/selfdrive/pandad/tests/test_aol_protocol.cc index da71d793fe..c5ed8a0d88 100644 --- a/openpilot/selfdrive/pandad/tests/test_aol_protocol.cc +++ b/openpilot/selfdrive/pandad/tests/test_aol_protocol.cc @@ -5,6 +5,7 @@ #include "openpilot/starpilot/car/mazda/aol_policy.h" #include "openpilot/starpilot/car/volkswagen/aol_policy.h" #include "openpilot/starpilot/car/chrysler/aol_policy.h" +#include "openpilot/starpilot/car/subaru/aol_policy.h" #include "openpilot/starpilot/car/toyota/aol_policy.h" #include "openpilot/starpilot/car/tesla/aol_policy.h" #include "openpilot/starpilot/car/hyundai/aol_policy.h" @@ -27,7 +28,7 @@ constexpr AolSafetyProfile TEST_PROFILES[] = { {TEST_MODE, test_param, false}, {TEST_ALT_MODE, test_alt_param, false}, }; const AolProfileRegistry TEST_REGISTRY{TEST_PROFILES, std::size(TEST_PROFILES)}; -constexpr AolSafetyProfile VEHICLE_PROFILES[] = {HONDA_AOL_PROFILE, HONDA_STOCK_AOL_PROFILE, HONDA_NIDEC_AOL_PROFILE, HYUNDAI_AOL_PROFILE, HYUNDAI_CLASSIC_AOL_PROFILE, HYUNDAI_LEGACY_AOL_PROFILE, GM_AOL_PROFILE, FORD_AOL_PROFILE, MAZDA_AOL_PROFILE, CHRYSLER_AOL_PROFILE, TESLA_PREAP_AOL_PROFILE, TESLA_SCREEN_AOL_PROFILE, TOYOTA_AOL_PROFILE, NISSAN_AOL_PROFILE, VOLKSWAGEN_STOCK_AOL_PROFILE, VOLKSWAGEN_LONG_AOL_PROFILE}; +constexpr AolSafetyProfile VEHICLE_PROFILES[] = {HONDA_AOL_PROFILE, HONDA_STOCK_AOL_PROFILE, HONDA_NIDEC_AOL_PROFILE, HYUNDAI_AOL_PROFILE, HYUNDAI_CLASSIC_AOL_PROFILE, HYUNDAI_LEGACY_AOL_PROFILE, GM_AOL_PROFILE, FORD_AOL_PROFILE, MAZDA_AOL_PROFILE, CHRYSLER_AOL_PROFILE, TESLA_PREAP_AOL_PROFILE, TESLA_SCREEN_AOL_PROFILE, TOYOTA_AOL_PROFILE, NISSAN_AOL_PROFILE, VOLKSWAGEN_STOCK_AOL_PROFILE, VOLKSWAGEN_LONG_AOL_PROFILE, SUBARU_AOL_PROFILE}; const AolProfileRegistry VEHICLE_REGISTRY{VEHICLE_PROFILES, std::size(VEHICLE_PROFILES)}; aol_safety_health_t status(uint8_t request = 0U, uint8_t permission = 0U) { @@ -88,6 +89,18 @@ void queue(FakeTransport &transport, const aol_safety_health_t &before, } // namespace int main() { + for (uint32_t word = 0U; word <= 65535U; word++) { + auto subaru = status(); + subaru.safety_mode = 11U; + subaru.safety_param = static_cast(word); + subaru.capability_flags = 1U; + const bool expected = (word == 0U) || (word == 1U) || (word == 17U) || + (word == 145U) || (word == 177U) || (word == 2225U); + assert(aol_capable(subaru, 11U, VEHICLE_REGISTRY) == expected); + subaru.capability_flags = 0U; + assert(!aol_capable(subaru, 11U, VEHICLE_REGISTRY)); + } + for (uint32_t word = 0U; word <= 65535U; ++word) { const bool stock = (word == 8U) || (word == 24U); const bool longitudinal = (word == 9U) || (word == 25U); diff --git a/openpilot/starpilot/aol/transport_pause.py b/openpilot/starpilot/aol/transport_pause.py index 8f32694c1c..a7e1408eb7 100644 --- a/openpilot/starpilot/aol/transport_pause.py +++ b/openpilot/starpilot/aol/transport_pause.py @@ -361,6 +361,7 @@ class TransportPauseFeedback: from opendbc.car.chrysler.aol_contract import qualified as chrysler_qualified from openpilot.starpilot.car.nissan.aol import qualified as nissan_qualified from opendbc.car.ford.aol import qualified as ford_qualified + from opendbc.car.subaru.stock_aol import qualified as subaru_qualified from opendbc.car.hyundai.canfd_angle_aol import qualified as angle_qualified from opendbc.car.hyundai.canfd_stock_aol import qualified as stock_canfd_qualified from opendbc.car.hyundai.classic_scc_aol import qualified as stock_classic_qualified @@ -374,7 +375,8 @@ class TransportPauseFeedback: stock_hkg = stock_angle or stock_canfd_qualified(cp, marked_only=True) or stock_classic_qualified(cp, marked_only=True) stock_user = stock_lateral_user_event(cp) stock_temporary_user = bool((location_fault or eps_recovery) and (stock_hkg or chrysler_qualified(cp, marked_only=True) or - ford_qualified(cp, marked_only=True) or nissan_qualified(cp, marked_only=True)) and users and + ford_qualified(cp, marked_only=True) or nissan_qualified(cp, marked_only=True) + or subaru_qualified(cp, marked_only=True)) and users and not axis.desiredLongitudinal and not axis.longitudinalActive and cs.cruiseState.available and not cs.cruiseState.enabled and cs.gearShifter in (gears.drive, gears.low, gears.sport, gears.brake) and @@ -1176,6 +1178,14 @@ class TransportPauseFeedback: qualified_cp=nissan_qualified(cp, marked_only=True), informational_events=(log.OnroadEvent.EventName.userBookmarkNotPaired,), allow_withdrawal_join=True): return False + if cp.brand == 'subaru' and current and continuity: + from opendbc.car.subaru.stock_aol import qualified as subaru_qualified + from openpilot.starpilot.aol.reverse_intent import retain_reverse_intent + + if retain_reverse_intent(self, sm, cp, cs, axis, now_ns, latched=latched, producer=intent_producer_session, + qualified_cp=subaru_qualified(cp, marked_only=True), + informational_events=(log.OnroadEvent.EventName.userBookmarkNotPaired,), allow_withdrawal_join=True): + return False if cp.brand == 'ford' and current and continuity: from openpilot.starpilot.car.ford.reverse_intent import retain_reverse_intent from openpilot.starpilot.car.ford.physical_pause import retain_physical_pause_intent diff --git a/openpilot/starpilot/aol/vehicle.py b/openpilot/starpilot/aol/vehicle.py index fe3a18f0a3..6d2b0d0da8 100644 --- a/openpilot/starpilot/aol/vehicle.py +++ b/openpilot/starpilot/aol/vehicle.py @@ -13,13 +13,14 @@ from openpilot.starpilot.car.gm import aol as gm from openpilot.starpilot.car.ford import aol as ford from openpilot.starpilot.car.mazda import aol as mazda from openpilot.starpilot.car.chrysler import aol as chrysler +from openpilot.starpilot.car.subaru import aol as subaru from openpilot.starpilot.car.tesla import aol as tesla from openpilot.starpilot.car.toyota import aol as toyota from openpilot.starpilot.car.volkswagen import aol as volkswagen from openpilot.starpilot.car.nissan import aol as nissan _PORTS = {'honda': honda, 'hyundai': hyundai, 'gm': gm, 'ford': ford, 'mazda': mazda, - 'tesla': tesla, 'toyota': toyota, 'chrysler': chrysler, 'nissan': nissan, 'volkswagen': volkswagen} + 'tesla': tesla, 'toyota': toyota, 'chrysler': chrysler, 'nissan': nissan, 'volkswagen': volkswagen, 'subaru': subaru} _POLICY_CACHE_SIZE = 8 _POLICY_CACHE_MAX_BYTES = 64 * 1024 diff --git a/openpilot/starpilot/car/subaru/__init__.py b/openpilot/starpilot/car/subaru/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/starpilot/car/subaru/aol.py b/openpilot/starpilot/car/subaru/aol.py new file mode 100644 index 0000000000..d1ada081b8 --- /dev/null +++ b/openpilot/starpilot/car/subaru/aol.py @@ -0,0 +1,53 @@ +"""Subaru stock ACC retains independent lateral intent after full engagement.""" +from opendbc.car import structs +from opendbc.car.subaru.stock_aol import qualified, temporary_restriction +from openpilot.starpilot.aol.intent import AolCardIntent +from openpilot.starpilot.aol.policy import AolVehiclePolicy + + +class SubaruStockCardIntent(AolCardIntent): + observe_stock_engagement = True + + def __init__(self, settings): + super().__init__(settings, explicit_latch=True, lkas_button_supported=False) + + def update(self, CS, **kwargs): + super().update(CS, **kwargs) + if not CS.cruiseState.available: + self._record_latch_clear('subaru_main_off') + self.allowed_latch = False + self._withdraw_full_engagement() + + +def policy_for(cp): + admitted = qualified(cp) + return AolVehiclePolicy(intent_supported=admitted, settings_supported=admitted, runtime_supported=admitted, + normal_runtime_supported=admitted, full_axis_runtime_required=qualified(cp, marked_only=True), + ordinary_axis_ack_required=qualified(cp, marked_only=True), explicit_latch=admitted, + fixed_cruise_buttons=admitted, lkas_button_supported=False, + alternative_experience_addition=32 if admitted else 0) + + +def native_profile_supported(model, param): + return model == int(structs.CarParams.SafetyModel.subaru) and param in (0, 1, 17, 145, 177, 2225) + + +def native_accepts_cp(cp, model, param): + return qualified(cp, marked_only=True) and native_profile_supported(model, param) + + +def native_latch_rejected(cp, native): + return qualified(cp, marked_only=True) and native.requestedLateral and not native.lateralAllowed + + +def retain_on_native_denial(cp, native, state): + return temporary_restriction(cp, state) + + +def ordinary_axis_request_allowed(cp, state): + return (qualified(cp, marked_only=True) and state.canValid and not state.canTimeout and + not state.steerFaultPermanent and not state.accFaulted) + + +def create_intent(cp, settings): + return SubaruStockCardIntent(settings) diff --git a/openpilot/starpilot/car/subaru/aol_policy.h b/openpilot/starpilot/car/subaru/aol_policy.h new file mode 100644 index 0000000000..ec14f30d69 --- /dev/null +++ b/openpilot/starpilot/car/subaru/aol_policy.h @@ -0,0 +1,10 @@ +#pragma once + +#include "selfdrive/pandad/aol_protocol.h" + +inline bool subaru_aol_param(uint16_t param) { + return (param == 0U) || (param == 1U) || (param == 17U) || (param == 145U) || + (param == 177U) || (param == 2225U); +} + +inline constexpr AolSafetyProfile SUBARU_AOL_PROFILE{11U, subaru_aol_param, true}; diff --git a/openpilot/starpilot/car/subaru/tests/caller_helpers.py b/openpilot/starpilot/car/subaru/tests/caller_helpers.py new file mode 100644 index 0000000000..31c820b8dc --- /dev/null +++ b/openpilot/starpilot/car/subaru/tests/caller_helpers.py @@ -0,0 +1,116 @@ +"""Synthetic acquisition and perception around the real Subaru production callers.""" + +import os +from contextlib import ExitStack, contextmanager +import json +from unittest.mock import patch + +from opendbc.can import CANPacker +from opendbc.car import structs +from opendbc.car import car_helpers +from opendbc.car.fw_versions import match_fw_to_car +from openpilot.cereal import messaging +from openpilot.common.params import Params +from openpilot.common.prefix import OpenpilotPrefix +from openpilot.selfdrive.car.card import Car +from openpilot.selfdrive.selfdrived import selfdrived as selfdrived_module +from openpilot.starpilot.lateral.tests.test_lane_runtime import feed +from openpilot.starpilot.vehicle_selection import encode + + +CONTROLS_PERCEPTION_SERVICES = frozenset(('deviceState', 'vehicleParameters', 'lateralDelay', 'modelV2')) +CONTROLS_OTHER_OWNERS = frozenset(('carState', 'carOutput', 'selfdriveState', 'onroadEvents', 'longitudinalPlan', 'aolAxisState', 'aolSafetyWire')) + + +def feed_perception_only(controls, timestamp, tick, **kwargs): + """Use shared perception fixtures without submitting synthetic actual-caller publications.""" + pending, observed = [], [] + + def capture(cur_time, events): + for event in events: + service = event.which() + assert service in CONTROLS_PERCEPTION_SERVICES | CONTROLS_OTHER_OWNERS, service + delivered = service in CONTROLS_PERCEPTION_SERVICES + observed.append({'service': service, 'source_ns': int(event.logMonoTime), 'valid': bool(event.valid), 'delivered': delivered}) + if delivered: + pending.append(event) + + with patch.object(controls.sm, 'update_msgs', side_effect=capture): + feed(controls, timestamp, tick, **kwargs) + controls.sm.update_msgs(timestamp / 1e9, pending) + return {'observed': observed, 'delivered': [event.which() for event in pending]} + + +def firmware_records(candidate, versions, brand): + return [ + structs.CarParams.CarFw(ecu=ecu, address=address, subAddress=0 if sub is None else sub, fwVersion=values[0], brand=brand) + for (ecu, address, sub), values in versions[candidate].items() + ] + + +@contextmanager +def actual_factory(candidate, fingerprint, fw, vin, *, manual, alpha=False, release=False, aol=True): + with ( + OpenpilotPrefix(), + patch.object(selfdrived_module, 'SIMULATION', True), + patch.object(selfdrived_module, 'REPLAY', False), + patch('openpilot.selfdrive.car.card.REPLAY', False), + patch.dict(os.environ, {'SIMULATION': '1', 'AOL_REPLAY_RUNTIME': '0', 'SKIP_FW_QUERY': '', 'FINGERPRINT': '', 'DISABLE_FW_CACHE': '1'}), + ): + os.environ.pop('REPLAY', None) + params = Params() + for key, value in [ + ('OpenpilotEnabledToggle', True), + ('AlphaLongitudinalEnabled', alpha), + ('IsReleaseBranch', release), + ('AlwaysOnLateral', aol), + ('SafeMode', False), + ('IsOffroad', False), + ('ControlsReady', False), + ]: + params.put_bool(key, value, block=True) + params.put('VehicleSelection', json.loads(encode(str(candidate) if manual else None)), block=True) + _, matches = match_fw_to_car(fw, vin, log=False) + if not manual: + assert matches == {candidate}, (candidate, matches) + with ExitStack() as io: + io.enter_context(patch('openpilot.selfdrive.car.card.messaging.recv_one_retry', return_value=messaging.new_message('can', 1))) + io.enter_context(patch('openpilot.selfdrive.car.card.can_comm_callbacks', return_value=(lambda *a, **kw: [], lambda *a, **kw: None))) + io.enter_context(patch('openpilot.selfdrive.car.card.obd_callback', return_value=lambda _: None)) + io.enter_context(patch('openpilot.selfdrive.car.card.messaging.recv_one_or_none', return_value=None)) + io.enter_context(patch.object(car_helpers, 'get_vin', return_value=(-1, -1, vin))) + io.enter_context(patch.object(car_helpers, 'get_present_ecus', return_value=set())) + io.enter_context(patch.object(car_helpers, 'get_fw_versions_ordered', return_value=fw)) + # Acquisition boundary only: retain production fingerprint FW matcher and get_car. + io.enter_context(patch.object(car_helpers, 'can_fingerprint', return_value=(None, fingerprint))) + io.enter_context(patch.object(car_helpers, 'normal_bus_topology', wraps=car_helpers.normal_bus_topology)) + card = Car() + cp = card.CP + assert cp.carFingerprint == candidate, (cp.carFingerprint, candidate) + assert cp.fingerprintSource == (structs.CarParams.FingerprintSource.fixed if manual else structs.CarParams.FingerprintSource.fw) + with structs.CarParams.from_bytes(params.get('CarParams')) as published: + assert published.to_dict() == cp.to_dict() + assert card.aol_qualified and card.aol_card_intent is not None + assert cp.alternativeExperience == 32 + yield card, params, matches + + +class CompleteCan: + """Pack every subscribed physical source; counters/checksums use real packer.""" + + def __init__(self, ci): + self.ci = ci + ci.update([]) + self.packers = {bus: CANPacker(parser.dbc.name) for bus, parser in ci.can_parsers.items()} + + def frames(self, values, *, tick, slow=(), omit=()): + result = [] + for logical_bus, parser in self.ci.can_parsers.items(): + for address in sorted(parser.addresses): + name = parser.dbc.addr_to_msg[address].name + if (logical_bus, name) in omit or (name in slow and tick % 5): + continue + intended = values.get(name, {}) + assert intended.keys() <= parser.dbc.name_to_msg[name].sigs.keys(), (name, intended) + result.append(self.packers[logical_bus].make_can_msg(name, parser.bus, intended)) + return result diff --git a/openpilot/starpilot/car/subaru/tests/native_fixture.py b/openpilot/starpilot/car/subaru/tests/native_fixture.py new file mode 100644 index 0000000000..b80a2b20d3 --- /dev/null +++ b/openpilot/starpilot/car/subaru/tests/native_fixture.py @@ -0,0 +1,90 @@ +"""Compile current native safety and C++ negotiation for Subaru caller tests.""" + +import subprocess +from pathlib import Path + +import capnproto +import pytest + +from opendbc.safety.tests.libsafety import libsafety_py + + +@pytest.fixture(scope='module') +def subaru_native_pair(tmp_path_factory): + root = Path(__file__).resolve().parents[5] + temporary = tmp_path_factory.mktemp('subaru-native') + library = temporary / 'libsafety.so' + bridge = temporary / 'pandad-order' + subprocess.run( + [ + 'cc', + '-fPIC', + '-Wall', + '-Wextra', + '-Werror', + '-nostdlib', + '-fno-builtin', + '-std=gnu11', + '-Wfatal-errors', + '-Wno-pointer-to-int-cast', + '-g', + '-O0', + '-DALLOW_DEBUG', + '-fsanitize=undefined', + '-fno-sanitize-recover=undefined', + '-I', + str(root / 'opendbc_repo'), + '-c', + str(root / 'opendbc_repo/opendbc/safety/tests/libsafety/safety.c'), + '-o', + str(temporary / "libsafety.o"), + ], + check=True, + timeout=35, + ) + subprocess.run( + ['cc', '-shared', str(temporary / 'libsafety.o'), '-o', str(library), '-fsanitize=undefined', '-fno-sanitize-recover=undefined'], check=True, timeout=35 + ) + generated = [root / ('openpilot/cereal/gen/cpp/' + name + '.capnp.c++') for name in ('log', 'car', 'custom', 'deprecated')] + subprocess.run( + [ + 'c++', + '-std=c++17', + '-Wall', + '-Wextra', + '-Werror', + '-O0', + '-g', + '-I', + str(root), + '-I', + str(root / 'openpilot'), + '-I', + str(root / 'openpilot/cereal/gen/cpp'), + '-I', + capnproto.INCLUDE_DIR, + str(Path(__file__).with_name('pandad_order.cc')), + *map(str, generated), + '-L', + capnproto.LIB_DIR, + '-lcapnp', + '-lkj', + '-o', + str(bridge), + ], + check=True, + timeout=35, + ) + original = {key: vars(libsafety_py).get(key) for key in ('libsafety', 'libpath')} + with pytest.MonkeyPatch.context() as environment: + environment.setenv('SUBARU_NATIVE_LIBRARY', str(library)) + environment.setenv('SUBARU_PANDAD_BRIDGE', str(bridge)) + libsafety_py.load(library) + try: + yield library, bridge + finally: + for key, value in original.items(): + if value is None: + vars(libsafety_py).pop(key, None) + else: + setattr(libsafety_py, key, value) diff --git a/openpilot/starpilot/car/subaru/tests/pandad_order.cc b/openpilot/starpilot/car/subaru/tests/pandad_order.cc new file mode 100644 index 0000000000..2883a6d259 --- /dev/null +++ b/openpilot/starpilot/car/subaru/tests/pandad_order.cc @@ -0,0 +1,51 @@ +// Test transport around unchanged production negotiation/armed-wire code. +#include "openpilot/selfdrive/pandad/aol_protocol.h" +#include "openpilot/selfdrive/pandad/aol_wire.h" +#include "openpilot/starpilot/car/subaru/aol_policy.h" +#include +#include +#include + +int main() { + constexpr AolSafetyProfile profiles[] = {SUBARU_AOL_PROFILE}; + AolAxisNegotiator owner({profiles, std::size(profiles)}); + AolWritePlan plan; + std::string line; + while (std::getline(std::cin, line)) { + std::istringstream in(line); + std::string operation; + unsigned request, permission, mode, word, capable; + in >> operation >> request >> permission >> mode >> word >> capable; + aol_safety_health_t status{AOL_SAFETY_PROTOCOL_MAGIC, AOL_SAFETY_PROTOCOL_VERSION, + static_cast(request), static_cast(permission), static_cast(mode), + static_cast(word), static_cast(capable)}; + if (operation == "PREP") { + AolAxisInput axis; + uint64_t now, car_state_ns, intent_message_ns; + std::string raw; + bool onroad; + in >> now >> axis.alive_valid >> axis.qualified >> axis.session >> axis.observed_ns >> axis.valid_until_ns + >> axis.message_ns >> axis.desired_lateral >> axis.desired_longitudinal >> onroad + >> car_state_ns >> intent_message_ns >> raw; + std::vector bytes; + if (raw != "-") { + if (raw.size() % 2U != 0U) return 2; + for (size_t pos = 0; pos < raw.size(); pos += 2U) { + bytes.push_back(static_cast(std::stoul(raw.substr(pos, 2U), nullptr, 16))); + } + } + axis.retain_lateral_arm = onroad && aol_armed_intent({bytes.data(), bytes.size()}, intent_message_ns, car_state_ns, now); + plan = owner.prepare(aol_runtime_enabled(false, status, {profiles, std::size(profiles)}) ? + std::optional(status) : std::nullopt, axis, now, static_cast(mode)); + std::cout << plan.capable << ' ' << unsigned(plan.request_mask) << ' ' << plan.retain_lateral_arm << std::endl; + } else if (operation == "DONE") { + bool write_ok; + in >> write_ok; + const auto outcome = owner.complete(plan, write_ok, status); + std::cout << outcome.compatible << ' ' << (outcome.session.empty() ? "-" : outcome.session) << std::endl; + } else { + return 3; + } + if (in.fail()) return 4; + } +} diff --git a/openpilot/starpilot/car/subaru/tests/pandad_order.py b/openpilot/starpilot/car/subaru/tests/pandad_order.py new file mode 100644 index 0000000000..faa011baa0 --- /dev/null +++ b/openpilot/starpilot/car/subaru/tests/pandad_order.py @@ -0,0 +1,100 @@ +"""Actual exported native status + actual production C++ negotiate/write/HB/read order.""" + +import ctypes +import subprocess +from openpilot.cereal import messaging +from openpilot.starpilot.aol.wire import PandaSlot, SafetyState, encode_safety + + +class PandadOrder: + def __init__(self, safety, library_path, executable): + self.safety = safety + self.lib = ctypes.CDLL(str(library_path)) + self.process = subprocess.Popen([str(executable)], stdin=subprocess.PIPE, stdout=subprocess.PIPE, text=True) + self.heartbeat = False + + def status(self): + # Board USB0xd5 initializer reads request first, permission second. + request = int(self.safety.aol_get_request_mask()) + permission = int(self.safety.aol_get_permission_mask()) + mode = ctypes.c_uint16.in_dll(self.lib, 'current_safety_mode').value + word = ctypes.c_uint16.in_dll(self.lib, 'current_safety_param').value + capability = int(ctypes.c_void_p.in_dll(self.lib, 'aol_policy').value is not None) + return request, permission, mode, word, capability + + def exchange(self, line): + assert self.process.poll() is None + self.process.stdin.write(line + '\n') + self.process.stdin.flush() + result = self.process.stdout.readline().strip().split() + assert result, self.process.poll() + return result + + def step(self, driven, published, now): + before = self.status() + axis_event = driven['aolAxisState'] + axis = axis_event.aolAxisState + intent_event = published['aolIntentWire'] + fields = [ + now, + 1, + int(axis.qualified), + axis.sessionId, + axis.observedMonoTime, + axis.validUntilMonoTime, + axis_event.logMonoTime, + int(axis.desiredLateral), + int(axis.desiredLongitudinal), + 1, + axis.sourceCarStateMonoTime, + intent_event.logMonoTime, + bytes(intent_event.aolIntentWire).hex(), + ] + capable, request, retain = map(int, self.exchange('PREP ' + ' '.join(map(str, (*before, *fields))))) + if capable: + self.safety.aol_set_host_request(request) + # Actual pandad OR happens only after successful write. + self.heartbeat = bool(driven['selfdriveState'].selfdriveState.enabled or (capable and (request or retain))) + self.safety.set_aol_test_heartbeat(self.heartbeat) + after = self.status() + compatible, session = self.exchange('DONE ' + ' '.join(map(str, (*after, capable)))) + request_after, permission, mode, word, _ = after + wire = messaging.new_message('aolSafetyWire', 0, valid=bool(int(compatible)), logMonoTime=now) + wire.aolSafetyWire = encode_safety( + SafetyState( + 1, + bool(int(compatible)), + now, + now + 200_000_000, + mode, + word, + bool(permission & 1), + bool(permission & 2), + bool(request_after & 1), + bool(request_after & 2), + 'synthetic-subaru', + '' if session == '-' else session, + sourcePandaStatesMonoTime=now, + pandaInventory=( + PandaSlot( + 0, + 'synthetic-subaru', + mode, + word, + 32, + bool(self.safety.get_controls_allowed()), + not bool(self.safety.safety_config_valid()), + False, + int(bool(self.safety.get_relay_malfunction())), + ), + ), + ) + ) + return wire, {'before': before, 'plan': (capable, request, retain), 'heartbeat': self.heartbeat, 'after': after, 'session': session} + + def close(self): + self.process.stdin.close() + try: + assert self.process.wait(timeout=10) == 0 + finally: + self.process.stdout.close() diff --git a/openpilot/starpilot/car/subaru/tests/selfdrive_step.py b/openpilot/starpilot/car/subaru/tests/selfdrive_step.py new file mode 100644 index 0000000000..c31ebecabf --- /dev/null +++ b/openpilot/starpilot/car/subaru/tests/selfdrive_step.py @@ -0,0 +1,68 @@ +"""Current-schema peers for the retained actual SelfdriveD step; no Params shim.""" + +from unittest.mock import patch +from openpilot.cereal import messaging + + +def selfdrive_step(sd, published, native, previous_controls, cp, safety, now, *, calibration): + peers = [] + for service in sd.sm.data: + if service in ('alertDebug', 'lateralManeuverPlan', 'laneChangeAssistWire', 'starpilotLongitudinalPlan'): + continue + if service == 'aolIntentWire': + peers.append(published[service]) + continue + if service == 'aolSafetyWire': + if native is not None: + peers.append(native.as_reader()) + continue + if service == 'pandaStates': + if service in published: + peers.append(published[service]) + continue + if service in published: + peers.append(published[service]) + continue + if service in ('carControl', 'controlsState'): + if service in previous_controls: + peers.append(previous_controls[service]) + continue + size = 1 if service == 'pandaStates' else None + event = messaging.new_message(service, size, valid=True, logMonoTime=now) + value = getattr(event, service) + if service == 'extrinsicsCalibration': + value.calStatus, value.rpyCalib = calibration, [0.0, 0.0, 0.0] + elif service == 'vehicleParameters': + value.stiffnessFactor, value.steerRatio, value.valid = 1.0, cp.steerRatio, True + elif service == 'deviceMotion': + value.inputsOK = value.posenetOK = True + value.velocityDevice.x = published['carState'].carState.vEgo + elif service == 'deviceState': + value.freeSpacePercent = 50.0 + value.started, value.startedMonoTime = True, 500_000_000 + peers.append(event.as_reader()) + output = {} + + def capture(service, message): + # Snapshot only a real producer send, once; retain its source timestamp. + wire = message if isinstance(message, bytes) else message.to_bytes() + output[service] = messaging.log_from_bytes(wire) + + real_new_message = messaging.new_message + + def stamped_message(service=None, size=None, **kwargs): + return real_new_message(service, size, **({'logMonoTime': now} | kwargs)) + + with ( + patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.new_message', side_effect=stamped_message), + patch('openpilot.selfdrive.selfdrived.selfdrived.REPLAY', False), + patch('openpilot.selfdrive.selfdrived.selfdrived.time.monotonic_ns', return_value=now), + patch('openpilot.selfdrive.selfdrived.selfdrived.time.monotonic', return_value=now / 1e9), + patch('openpilot.selfdrive.selfdrived.selfdrived.time.clock_gettime_ns', return_value=now), + patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.recv_one_or_none', return_value=None), + patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.recv_one', return_value=published['carState']), + patch.object(sd.sm, 'update', side_effect=lambda _: sd.sm.update_msgs(now / 1e9, peers)), + patch.object(sd.pm, 'send', side_effect=capture), + ): + sd.step() + return output diff --git a/openpilot/starpilot/car/subaru/tests/test_aol.py b/openpilot/starpilot/car/subaru/tests/test_aol.py new file mode 100644 index 0000000000..1a2ed8b9b5 --- /dev/null +++ b/openpilot/starpilot/car/subaru/tests/test_aol.py @@ -0,0 +1,94 @@ +import unittest +from dataclasses import replace + +from opendbc.car import gen_empty_fingerprint, structs +from opendbc.car.subaru.interface import CarInterface +from opendbc.car.subaru.stock_aol import ANGLE_CARS, TORQUE_CARS, qualified +from opendbc.car.subaru.tests.test_ascent_angle import setup_controller, step +from opendbc.car.subaru.values import CAR +from openpilot.starpilot.aol.intent import AolSettings +from openpilot.starpilot.aol.vehicle import policy_for, native_matches_cp +from openpilot.starpilot.car.subaru.aol import SubaruStockCardIntent + + +class TestSubaruStockAol(unittest.TestCase): + def test_exact_factory_profiles_and_stock_off_choices(self): + for car in CAR: + for alpha in (False, True): + for source in ('fixed', 'fw', 'can'): + cp = CarInterface.get_params(car, gen_empty_fingerprint(), [], alpha, False, False) + cp.fingerprintSource = source + admitted = car in ANGLE_CARS | TORQUE_CARS + self.assertEqual(qualified(cp), admitted) + policy = policy_for(cp) + self.assertFalse(policy.full_axis_runtime_required) + self.assertFalse(cp.openpilotLongitudinalControl) + if admitted: + cp.alternativeExperience |= policy.alternative_experience_addition + self.assertTrue(qualified(cp, marked_only=True)) + self.assertTrue(policy_for(cp).full_axis_runtime_required) + self.assertTrue(native_matches_cp(cp, int(cp.safetyConfigs[0].safetyModel.raw), cp.safetyConfigs[0].safetyParam)) + cp.safetyConfigs[0].safetyParam |= 2 + self.assertFalse(qualified(cp)) + + def test_successful_full_engagement_brake_reverse_fault_main_and_off(self): + settings = AolSettings(True, 0., 0, 0, (0, 0, 0), (0, 0, 0)) + intent = SubaruStockCardIntent(settings) + cs = structs.CarState(canValid=True, vEgo=15, gearShifter=structs.CarState.GearShifter.drive) + cs.cruiseState.available = cs.cruiseState.enabled = True + def observe(enabled, active, stamp): + intent.observe_full_engagement(cs.as_reader(), enabled=enabled, active=active, source_ns=stamp, + observation_valid=True, now_ns=stamp, fault_active=False) + intent.update(cs.as_reader(), now_ns=1) + self.assertFalse(intent.allowed_latch) + observe(False, False, 10) + observe(True, False, 20) + self.assertFalse(intent.allowed_latch) + observe(True, True, 30) + self.assertTrue(intent.allowed_latch) + cs.brakePressed = True + cs.cruiseState.enabled = False + intent.update(cs.as_reader(), now_ns=40) + self.assertTrue(intent.allowed_latch) + cs.gearShifter = structs.CarState.GearShifter.reverse + intent.update(cs.as_reader(), now_ns=50) + self.assertTrue(intent.allowed_latch) + cs.steerFaultTemporary = True + intent.update(cs.as_reader(), now_ns=60) + self.assertTrue(intent.allowed_latch) + cs.gearShifter = structs.CarState.GearShifter.drive + cs.steerFaultTemporary = False + cs.steerFaultPermanent = True + intent.update(cs.as_reader(), now_ns=70) + self.assertFalse(intent.allowed_latch) + cs.steerFaultPermanent = False + intent.update(cs.as_reader(), fault_active=False, now_ns=80) + observe(False, False, 90) + observe(True, True, 100) + self.assertTrue(intent.allowed_latch) + cs.cruiseState.available = False + intent.update(cs.as_reader(), now_ns=110) + self.assertFalse(intent.allowed_latch) + cs.cruiseState.available = True + intent.update(cs.as_reader(), now_ns=120) + self.assertFalse(intent.allowed_latch) + intent.settings = replace(settings, enabled=False) + observe(False, False, 130) + observe(True, True, 140) + self.assertFalse(intent.allowed_latch) + + def test_angle_controller_only_marked_profile_accepts_independent_lateral(self): + for car in ANGLE_CARS: + for enrolled in (False, True): + cp, controller, command, state = setup_controller(car) + cp.alternativeExperience = 32 if enrolled else 0 + command.enabled = False + command.latActive = True + state.out.brakePressed = True + state.out.cruiseState.enabled = False + step(controller, command, state) + _, decoded = step(controller, command, state) + self.assertEqual(bool(decoded['LKAS_Request']), enrolled) + state.out.gearShifter = structs.CarState.GearShifter.reverse + _, decoded = step(controller, command, state) + self.assertEqual(decoded['LKAS_Request'], 0) diff --git a/openpilot/starpilot/car/subaru/tests/test_reverse_join.py b/openpilot/starpilot/car/subaru/tests/test_reverse_join.py new file mode 100644 index 0000000000..7f85698805 --- /dev/null +++ b/openpilot/starpilot/car/subaru/tests/test_reverse_join.py @@ -0,0 +1,85 @@ +"""Exact stock Subaru profiles share the guarded physical Reverse join.""" + +import pytest + +from opendbc.car import gen_empty_fingerprint, structs +from opendbc.car.subaru.interface import CarInterface +from opendbc.car.subaru.stock_aol import qualified +from opendbc.car.subaru.values import CAR +from openpilot.cereal import log +from openpilot.common.prefix import OpenpilotPrefix +from openpilot.starpilot.aol.reverse_intent import retain_reverse_intent +from openpilot.starpilot.tests import test_ford_reverse_intent as vectors + + +@pytest.fixture(autouse=True, params=(CAR.SUBARU_ASCENT_2023, CAR.SUBARU_FORESTER)) +def stock_subaru_vectors(request, monkeypatch): + def cp_state(): + candidate = request.param + fp = gen_empty_fingerprint() + with OpenpilotPrefix(): + cp = CarInterface.get_params(candidate, fp, [], False, False, False) + cp.alternativeExperience = 32 + assert qualified(cp, marked_only=True) + cs = structs.CarState(canValid=True, canTimeout=False, vEgo=15., gearShifter=structs.CarState.GearShifter.drive) + cs.cruiseState.available = True + return cp, cs + + def retain(feedback, sm, cp, cs, axis, now_ns, *, latched, producer): + return retain_reverse_intent(feedback, sm, cp, cs, axis, now_ns, latched=latched, producer=producer, + qualified_cp=qualified(cp, marked_only=True), + informational_events=(log.OnroadEvent.EventName.userBookmarkNotPaired,), allow_withdrawal_join=True) + + original_sample = vectors.sample + + def initial_epoch_sample(*args, **kwargs): + sm, native = original_sample(*args, **kwargs) + vectors.change(sm, 'aolAxisState', lambda axis: setattr(axis, 'intentIntegrityEpoch', 0)) + return sm, native + + monkeypatch.setattr(vectors, 'sample', initial_epoch_sample) + monkeypatch.setattr(vectors, 'cp_state', cp_state) + monkeypatch.setattr(vectors, 'retain_reverse_intent', retain) + + +def test_reverse_retains_only_existing_intent_without_renewing_leases(): + vectors.test_first_reverse_join_retains_only_intent_without_renewing_prior_leases() + + +def test_off_proof_and_drive_need_fresh_native_admission(): + vectors.test_actual_off_proof_and_drive_recovery_use_unchanged_native_admission() + + +@pytest.mark.parametrize('fault', [fault for fault in vectors.NEGATIVES if fault != 'long-cp']) +def test_reverse_preserves_fault_source_identity_and_long_vetoes(fault): + vectors.test_reverse_join_never_bypasses_current_fault_source_identity_or_long_veto(fault) + + +def test_first_unacknowledged_off_joins_fresh_native_off_without_renewing_intent(): + cp, cs, feedback = vectors.baseline() + cs.gearShifter = structs.CarState.GearShifter.reverse + now = vectors.NOW + 20_000_000 + sm, _ = vectors.sample(cp, vectors.NOW + 10_000_000, now, 2, active=False, requested=False, allowed=False) + vectors.change(sm, 'aolAxisState', lambda axis: setattr(axis, 'nativeAcknowledged', False)) + before = dict(vars(feedback)) + assert vectors.retain_reverse_intent(feedback, sm, cp, cs, sm['aolAxisState'], now, + latched=True, producer=vectors.PRODUCER) + assert vars(feedback) == before + assert not feedback.observe(sm, cp, cs, now, latched=True, intent_producer_session=vectors.PRODUCER) + assert vars(feedback) == before + + +@pytest.mark.parametrize('fault', [fault for fault in vectors.NEGATIVES if fault not in ('long-cp', 'axis-unacknowledged')]) +def test_off_withdrawal_join_keeps_original_fault_source_identity_and_long_vetoes(fault, monkeypatch): + original_sample = vectors.sample + + def withdrawing_sample(cp, source, observed, sequence, **kwargs): + if source > vectors.NOW: + kwargs.update(active=False, requested=False, allowed=False) + sm, native = original_sample(cp, source, observed, sequence, **kwargs) + if source > vectors.NOW: + vectors.change(sm, 'aolAxisState', lambda axis: setattr(axis, 'nativeAcknowledged', False)) + return sm, native + + monkeypatch.setattr(vectors, 'sample', withdrawing_sample) + vectors.test_reverse_join_never_bypasses_current_fault_source_identity_or_long_veto(fault) diff --git a/openpilot/starpilot/car/subaru/tests/test_stock_aol_callers.py b/openpilot/starpilot/car/subaru/tests/test_stock_aol_callers.py new file mode 100644 index 0000000000..51543e39c3 --- /dev/null +++ b/openpilot/starpilot/car/subaru/tests/test_stock_aol_callers.py @@ -0,0 +1,209 @@ +"""Actual automatic Subaru factory/parser/Card/SD/Controls/native/CPP negotiation.""" + +import json +import time +from unittest.mock import patch +import pytest +from opendbc.can import CANParser +from opendbc.car import Bus, gen_empty_fingerprint, structs +from opendbc.car.subaru.values import CAR, DBC +from opendbc.car.subaru.fingerprints import FW_VERSIONS +from opendbc.safety.tests.libsafety import libsafety_py +from openpilot.cereal import messaging +from openpilot.selfdrive.selfdrived.selfdrived import SelfdriveD +from openpilot.selfdrive.controls.controlsd import Controls +from openpilot.selfdrive.pandad import can_capnp_to_list +from openpilot.starpilot.aol.wire import decode_intent +from openpilot.starpilot.car.subaru.tests.caller_helpers import actual_factory, firmware_records, CompleteCan, feed_perception_only +from openpilot.starpilot.car.subaru.tests.selfdrive_step import selfdrive_step +from openpilot.starpilot.car.subaru.tests.pandad_order import PandadOrder + +from openpilot.starpilot.car.subaru.tests.native_fixture import subaru_native_pair as subaru_native_pair + + +@pytest.mark.parametrize('candidate', [CAR.SUBARU_ASCENT_2023, CAR.SUBARU_LEGACY_2025, CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_FORESTER, CAR.SUBARU_OUTBACK]) +def test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(candidate, subaru_native_pair, tmp_path): + aol = True + fw = firmware_records(candidate, FW_VERSIONS, 'subaru') + rows = [] + with actual_factory(candidate, gen_empty_fingerprint(), fw, '0' * 17, manual=False, aol=aol) as (card, params, matches): + cp = card.CP + ci = card.CI + safety = libsafety_py.libsafety + assert cp.pcmCruise and not cp.openpilotLongitudinalControl and not cp.alphaLongitudinalAvailable + assert cp.alternativeExperience == 32 and not cp.dashcamOnly + sd, controls = SelfdriveD(CP=cp), Controls() + stream = CompleteCan(ci) + safety.init_tests() + safety.set_alternative_experience(32) + assert safety.set_safety_hooks(cp.safetyConfigs[0].safetyModel.raw, cp.safetyConfigs[0].safetyParam) == 0 + safety.set_aol_test_heartbeat(False) + library, bridge = subaru_native_pair + pandad = PandadOrder(safety, library, bridge) + previous_controls = {} + latest_controls = {} + driven = {} + driven_new = {} + receipt = None + steering_name = 'ES_LKAS_ANGLE' if cp.steerControlType == structs.CarParams.SteerControlType.angle else 'ES_LKAS' + steering_address = ci.CC.packer.dbc.name_to_msg[steering_name].address + steering_bus = 0 if candidate in (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_FORESTER, CAR.SUBARU_OUTBACK) else 1 + decoder = CANParser(DBC[candidate][Bus.pt], [(steering_name, 0)], steering_bus) + active_emissions = [] + base = time.monotonic_ns() + 100_000_000 + try: + for tick in range(460): + now = base + tick * 10_000_000 + cruise = 30 <= tick < 110 or tick >= 410 + brake = 90 <= tick < 110 + reverse = 150 <= tick < 185 + temporary = 225 <= tick < 260 + main = not 310 <= tick < 350 + values = { + 'Steering_Torque': {'Steer_Warning': int(temporary)}, + 'Steering_2': {'Steering_Angle': 0}, + 'Wheel_Speeds': dict.fromkeys(('FL', 'FR', 'RL', 'RR'), 54), + 'Brake_Status': {'Brake': int(brake)}, + 'CruiseControl': {'Cruise_On': int(main), 'Cruise_Activated': int(cruise)}, + 'ES_Status': {'Cruise_Activated': int(cruise)}, + 'ES_DashStatus': {'Cruise_On': int(main), 'Cruise_Set_Speed': 100}, + 'Transmission': {'Gear': 3 if reverse else 121}, + } + incoming = stream.frames(values, tick=tick) + safety.set_timer((now // 1000) & 0xFFFFFFFF) + for addr, data, bus in incoming: + safety.safety_rx_hook(libsafety_py.make_CANPacket(addr, bus, data)) + safety.safety_tick() + panda = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=now) + panda.pandaStates[0].safetyModel = cp.safetyConfigs[0].safetyModel + panda.pandaStates[0].safetyParam = cp.safetyConfigs[0].safetyParam + panda.pandaStates[0].alternativeExperience = 32 + panda.pandaStates[0].ignitionLine = True + panda.pandaStates[0].controlsAllowed = bool(safety.get_controls_allowed()) + panda.pandaStates[0].safetyRxChecksInvalid = not bool(safety.safety_config_valid()) + device = messaging.new_message('deviceState', valid=True, logMonoTime=now) + device.deviceState.started = True + device.deviceState.startedMonoTime = 500_000_000 + peers = [device.as_reader()] + if tick % 10 == 0: + peers.append(panda.as_reader()) + if 'carControl' in latest_controls: + peers.append(latest_controls['carControl']) + peers.extend(driven_new[s] for s in ('onroadEvents', 'selfdriveState', 'aolAxisState') if s in driven_new) + if receipt is not None and tick % 10 == 1: + peers.append(receipt.as_reader()) + card.sm.update_msgs((now + 1000) / 1e9, peers) + published = {} + real_new = messaging.new_message + + def stamped(service=None, size=None, *, _real_new=real_new, _now=now, **kwargs): + return _real_new(service, size, **({'logMonoTime': _now} | kwargs)) + + def capture(service, msg, _published=published): + wire = msg if isinstance(msg, bytes) else msg.to_bytes() + _published[service] = messaging.log_from_bytes(wire) + + with ( + patch.object(card.sm, 'update'), + patch('openpilot.selfdrive.car.card.messaging.drain_sock_raw', return_value=[b'can']), + patch('openpilot.selfdrive.car.card.can_capnp_to_list', return_value=[(now, incoming)]), + patch('openpilot.selfdrive.car.card.REPLAY', False), + patch('openpilot.selfdrive.car.card.time.monotonic_ns', return_value=now + 2000), + patch('openpilot.selfdrive.car.card.time.monotonic', return_value=(now + 2000) / 1e9), + patch('openpilot.selfdrive.car.card.time.clock_gettime_ns', return_value=now + 2000), + patch('openpilot.selfdrive.car.card.messaging.new_message', side_effect=stamped), + patch.object(card.pm, 'send', side_effect=capture), + ): + card.step() + if tick % 10 == 0: + published['pandaStates'] = panda.as_reader() + cs = published['carState'].carState + assert cs.brakePressed == brake and cs.gearShifter == (structs.CarState.GearShifter.reverse if reverse else structs.CarState.GearShifter.drive) + if tick >= 10: + assert cs.canValid and safety.safety_config_valid() + # Deliver each genuine10Hz native receipt once; do not refresh receive time/frequency from a cached snapshot. + new_native = receipt if receipt is not None and tick % 10 == 1 else None + driven_new = selfdrive_step(sd, published, new_native, previous_controls, cp, safety, now, calibration='calibrated') + driven.update(driven_new) + order = None + if tick % 10 == 0: + receipt, order = pandad.step(driven, published, now) + feed_perception_only(controls, now, tick, enabled=sd.enabled, active=sd.active) + plan = messaging.new_message('longitudinalPlan', valid=True, logMonoTime=now) + plan.longitudinalPlan.aTarget = 0.5 + control_peers = [published['carState'], published['carOutput'], plan.as_reader()] + control_peers.extend(driven_new[s] for s in ('selfdriveState', 'onroadEvents', 'aolAxisState') if s in driven_new) + if receipt is not None and tick % 10 == 0: + control_peers.append(receipt.as_reader()) + controls.sm.update_msgs((now + 1000) / 1e9, control_peers) + previous_controls = {} + + def capture_control(service, msg, _controls=previous_controls): + wire = msg if isinstance(msg, bytes) else msg.to_bytes() + _controls[service] = messaging.log_from_bytes(wire) + + with ( + patch('openpilot.selfdrive.controls.controlsd.time.monotonic_ns', return_value=now + 2000), + patch('openpilot.cereal.messaging.time.monotonic', return_value=(now + 2000) / 1e9), + patch.object(controls.pm, 'send', side_effect=capture_control), + ): + command, lateral_log = controls.state_control() + controls.publish(command, lateral_log) + latest_controls.update(previous_controls) + command = previous_controls['carControl'].carControl + outgoing = can_capnp_to_list([published['sendcan'].as_builder().to_bytes()], msgtype='sendcan')[0][1] if 'sendcan' in published else [] + tx = [(a, d.hex(), b, bool(safety.safety_tx_hook(libsafety_py.make_CANPacket(a, b, d)))) for a, d, b in outgoing] + decoder.update([(now, outgoing)]) + decoded_steering = [] + for address, _data, _bus, accepted in tx: + if address == steering_address: + request = bool(decoder.vl[steering_name]['LKAS_Request']) + output = decoder.vl[steering_name]['LKAS_Output'] + decoded_steering.append({'request': request, 'output': output, 'nativeAccepted': accepted}) + if 65 <= tick < 90 or aol and (125 <= tick < 145 or 205 <= tick < 220 or 280 <= tick < 300): + assert request and accepted, (tick, decoded_steering) + if output != 0: + active_emissions.append(tick) + if 165 <= tick < 180 or 240 <= tick < 255 or not aol and (125 <= tick < 145 or 205 <= tick < 220 or 280 <= tick < 300): + assert not request and output == 0, (tick, decoded_steering) + intent = decode_intent(published['aolIntentWire'].aolIntentWire) + row = { + 'tick': tick, + 'intent': intent.allowedLatch, + 'armed': intent.lateralArmed, + 'cardLatch': card.aol_card_intent.allowed_latch, + 'lat': command.latActive, + 'long': command.longActive, + 'enabled': sd.enabled, + 'active': sd.active, + 'permission': int(safety.aol_get_permission_mask()), + 'controls': bool(safety.get_controls_allowed()), + 'nativeNewToSD': new_native is not None, + 'pandaNew': tick % 10 == 0, + 'nativeSource': None if receipt is None else int(receipt.logMonoTime), + 'order': order, + 'tx': tx, + 'steering': decoded_steering, + 'fault': str(driven['aolAxisState'].aolAxisState.faultReason), + 'events': [str(e.name) for e in driven['onroadEvents'].onroadEvents], + } + rows.append(row) + assert not command.longActive, row + if 10 <= tick < 30: + assert not command.latActive and not intent.allowedLatch and row['permission'] == 0, row + if 65 <= tick < 90: + assert command.latActive and intent.allowedLatch == aol and row['permission'] == 1, row + if 125 <= tick < 145 or 205 <= tick < 220 or 280 <= tick < 300: + assert command.latActive == aol and intent.allowedLatch == aol and row['permission'] == int(aol) and not row['controls'], row + if 165 <= tick < 180: + assert not command.latActive and not intent.allowedLatch and intent.lateralArmed == aol and row['cardLatch'] == aol and row['permission'] == 0, row + if 240 <= tick < 255: + assert not command.latActive and intent.lateralArmed == aol and row['cardLatch'] == aol and row['permission'] == 0, row + if 330 <= tick < 400: + assert not command.latActive and not intent.allowedLatch, row + if tick >= 440: + assert command.latActive and intent.allowedLatch == aol and row['permission'] == 1, row + assert len(active_emissions) >= 4, rows[65:90] + finally: + pandad.close() + (tmp_path / (str(candidate) + '-' + str(aol) + '-actual.json')).write_text(json.dumps(rows, indent=2) + '\n') diff --git a/tools/ci/run_vehicle_tests.py b/tools/ci/run_vehicle_tests.py index e6f37a4d56..866488db9e 100644 --- a/tools/ci/run_vehicle_tests.py +++ b/tools/ci/run_vehicle_tests.py @@ -51,9 +51,14 @@ AOL_LIFECYCLE_TARGETS = ( "opendbc_repo/opendbc/safety/tests/test_volkswagen_stock_aol.py", "opendbc_repo/opendbc/safety/tests/test_volkswagen_engagement_arm.py", "opendbc_repo/opendbc/safety/tests/test_volkswagen_long_aol.py", + "openpilot/starpilot/car/subaru/tests/test_aol.py", + "openpilot/starpilot/car/subaru/tests/test_reverse_join.py", + "openpilot/starpilot/car/subaru/tests/test_stock_aol_callers.py", + "opendbc_repo/opendbc/safety/tests/test_subaru_stock_aol.py", ) AOL_LIFECYCLE_SCOPE = { + "subaru": "nine stock global identities; five actual factory-to-native lifecycles, brake, Reverse/Drive, EPS recovery, MAIN-off and refusal guards", "volkswagen": "stock MQB auto/manual, gateway/camera factory acquisition, MAIN/engagement/brake/Reverse joins, permanent EPS native rearm", "gm": "Volt/Bolt factory profiles, MAIN boot, stock/Alpha, BE/C9, pedal/regen, Cancel mappings, RES, lag withdrawal/recovery", "honda": "stock Civic source/native vetoes, native-first Reverse joins, and EPS-recovery/Reverse/Drive native TX lifecycle", @@ -305,6 +310,7 @@ RELEASE_TARGETS = ( "opendbc_repo/opendbc/safety/tests/test_subaru_ascent_angle.py", "opendbc_repo/opendbc/safety/tests/test_ford_three.py", "opendbc_repo/opendbc/safety/tests/test_hyundai_canfd_carnival.py", + "opendbc_repo/opendbc/safety/tests/test_subaru_stock_aol.py", ) # Vehicle tests import both the vendored car code and these host control owners.