From 45de4e2929e02ead8ea4d3f81dde63f7c2b0e061 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 9 Oct 2026 08:36:42 -0500 Subject: [PATCH] Lateral authority --- opendbc_repo/opendbc/car/gm/carcontroller.py | 3 +- opendbc_repo/opendbc/car/gm/carstate.py | 2 + opendbc_repo/opendbc/car/gm/interface.py | 7 +- .../opendbc/car/gm/steering_authority.py | 137 ++++++++++ .../car/gm/tests/test_ascm_intercept.py | 181 ++++++++++++- openpilot/cereal/custom.capnp | 2 + openpilot/selfdrive/car/card.py | 18 +- openpilot/selfdrive/selfdrived/selfdrived.py | 107 ++++++-- openpilot/starpilot/aol/tests/test_gm.py | 68 +++-- openpilot/starpilot/aol/tests/test_runtime.py | 242 ++++++++++++++++++ openpilot/starpilot/gps/publisher.py | 42 +-- openpilot/starpilot/gps/tests/test_car_gps.py | 114 +++++++++ 12 files changed, 855 insertions(+), 68 deletions(-) create mode 100644 opendbc_repo/opendbc/car/gm/steering_authority.py diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index cf0c5959d8..4d3e855969 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -568,7 +568,7 @@ class CarController(CarControllerBase): if self.CP.openpilotLongitudinalControl: stock_steer_ready = (stock_steer_ready and CS.pedal_sensor_healthy and 0 < CS.pedal_sensor_ts_nanos <= now_nanos <= CS.pedal_sensor_ts_nanos + PEDAL_SENSOR_TIMEOUT_NS) - lat_active = CC.latActive and stock_steer_ready + lat_active = CC.latActive and stock_steer_ready and not CS.steering_authority.latched # Steering (Active: 50Hz, inactive: 10Hz) steer_step = self.params.STEER_STEP if lat_active else self.params.INACTIVE_STEER_STEP @@ -605,6 +605,7 @@ class CarController(CarControllerBase): self.apply_torque_last = apply_torque idx = self.lka_steering_cmd_counter % 4 can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_torque, idx, lat_active)) + CS.steering_authority.command(now_nanos, lat_active) status_bolt = self.CP.carFingerprint in NO_ACC_BOLT_CAR and ( is_bolt_present_no_acc_pedal_profile(self.CP) or is_bolt_pedal_removed_profile(self.CP)) diff --git a/opendbc_repo/opendbc/car/gm/carstate.py b/opendbc_repo/opendbc/car/gm/carstate.py index 1615e1503f..8af0f97056 100644 --- a/opendbc_repo/opendbc/car/gm/carstate.py +++ b/opendbc_repo/opendbc/car/gm/carstate.py @@ -1,3 +1,4 @@ +from opendbc.car.gm.steering_authority import SteeringAuthority from opendbc.car.gps import CarGpsTracker, get_car_gps_config from opendbc.car.gm.values import gm_control_word, is_volt_one_pedal, camera_acc_pedal_profile, BrakeSource, volt_cc_pedal_profile from opendbc.car.gm.values import is_volt_longitudinal, is_gm_auto_hold @@ -35,6 +36,7 @@ BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.D class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) + self.steering_authority = SteeringAuthority(CP) self.car_gps_tracker = CarGpsTracker(CP) self.car_gps_supported = self.car_gps_tracker.config is not None self.hybrid_profile = malibu_hybrid_profile(CP) diff --git a/opendbc_repo/opendbc/car/gm/interface.py b/opendbc_repo/opendbc/car/gm/interface.py index 7f3fee98a8..f9cb2ef143 100755 --- a/opendbc_repo/opendbc/car/gm/interface.py +++ b/opendbc_repo/opendbc/car/gm/interface.py @@ -2,7 +2,7 @@ from math import fabs, exp import numpy as np -from opendbc.car import get_safety_config, structs +from opendbc.car import Bus, get_safety_config, structs from opendbc.car.pedal import supported_pedal_detected from opendbc.car.gm.pedal_capability import pedal_candidate from opendbc.car.common.conversions import Conversions as CV @@ -50,6 +50,7 @@ class CarInterface(CarInterfaceBase): return super().apply(c, now_nanos) def update(self, can_packets): + self.CS.steering_authority.observe(can_packets) if malibu_hybrid_profile(self.CP) is not None: self.CS.hybrid_buttons.observe_packets(can_packets) if (is_bolt_pedal_removed_profile(self.CP) or is_bolt_pedal_removed_profile(self.CP, stock_only=True)): @@ -63,7 +64,9 @@ class CarInterface(CarInterfaceBase): if is_conventional_cc_pedal_profile(self.CP): self.CS.conventional_cancel_credit.observe(can_packets) if not is_bolt_cc_profile(self.CP): - return super().update(can_packets) + ret = super().update(can_packets) + self.CS.steering_authority.update(ret, self.can_parsers[Bus.pt]._last_update_nanos) + return ret adaptive = (self.CP.carFingerprint == CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL and self.CP.safetyConfigs[0].safetyParam == 0xC140) camera_source = getattr(self, "bolt_cc_camera_observed", (0, b"")) diff --git a/opendbc_repo/opendbc/car/gm/steering_authority.py b/opendbc_repo/opendbc/car/gm/steering_authority.py new file mode 100644 index 0000000000..6e65962702 --- /dev/null +++ b/opendbc_repo/opendbc/car/gm/steering_authority.py @@ -0,0 +1,137 @@ +from opendbc.car import structs +from opendbc.car.gm.values import GMFlags, camera_acc_pedal_profile, is_volt_ascm_longitudinal + +# PSCMStatus is expected at 10 Hz. Three periods bound first acquisition and missing feedback. +EPS_STATUS_TIMEOUT_NS = 300_000_000 +STEERING_COMMAND_TIMEOUT_NS = 100_000_000 +ButtonType = structs.CarState.ButtonEvent.Type + + +def monitored_profile(CP: structs.CarParams) -> bool: + return (is_volt_ascm_longitudinal(CP) and camera_acc_pedal_profile(CP) is None and + not CP.flags & GMFlags.PEDAL_LONG) + + +class SteeringAuthority: + def __init__(self, CP: structs.CarParams): + self.enabled = monitored_profile(CP) + self.min_speed = CP.minSteerSpeed + self.latched = False + self.command_active = False + self.seen_active = False + self.last_active_ns = 0 + self.active_since_ns = 0 + self.status_ns = 0 + self.status = -1 + self.bad_since_ns = 0 + self.bad_samples = 0 + self.last_sample_ns = 0 + self.disable_ns = 0 + self.neutral_ns = 0 + self.main_off = False + self.source_ns: dict[int, int] = {} + + def observe(self, can_packets): + if not self.enabled: + return + self.source_ns.clear() + malformed = any(bus == 0 and address == 0x184 and (stamp <= 0 or len(data) != 8) + for stamp, packets in can_packets for address, data, bus in packets) + lengths = {0xC9: 8, 0xBE: 6, 0xBD: 7, 0x1E1: 7} + invalid = {address for stamp, packets in can_packets for address, data, bus in packets + if bus == 0 and address in lengths and (stamp <= 0 or len(data) != lengths[address])} + for address in invalid: + self.source_ns.pop(address, None) + for stamp, packets in can_packets: + for address, data, bus in packets: + if bus != 0 or stamp <= 0: + continue + if address in lengths and address not in invalid: + self.source_ns[address] = max(stamp, self.source_ns.get(address, 0)) + if address == 0x184 and stamp > self.status_ns: + if self.active_since_ns and stamp - max(self.status_ns, self.active_since_ns) > EPS_STATUS_TIMEOUT_NS: + self.latched = True + self.status_ns = stamp + self.status = -1 if malformed else (data[0] >> 3) & 7 + + def command(self, now_ns: int, active: bool): + if not self.enabled: + return + self.command_active = active + if active: + self.last_active_ns = now_ns + if not self.active_since_ns: + self.active_since_ns = now_ns + elif self.disable_ns and now_ns > self.disable_ns: + self.neutral_ns = now_ns + + def update(self, CS: structs.CarState, now_ns: int): + if not self.enabled: + return + healthy = CS.canValid and not CS.canTimeout + + def fresh(address): + stamp = self.source_ns.get(address, 0) + return 0 < stamp <= now_ns and now_ns - stamp <= STEERING_COMMAND_TIMEOUT_NS + + main_fresh = fresh(0xC9) + buttons_fresh = fresh(0x1E1) + pedal_fresh = (CS.brakePressed and (main_fresh or fresh(0xBE)) or + CS.regenBraking and fresh(0xBD)) + cancel = buttons_fresh and any(b.type == ButtonType.cancel and b.pressed for b in CS.buttonEvents) + if self.latched and healthy: + if pedal_fresh or cancel or main_fresh and not CS.cruiseState.available: + if not self.disable_ns: + self.disable_ns = now_ns + self.main_off |= main_fresh and not CS.cruiseState.available + physical_enable = buttons_fresh and self.source_ns[0x1E1] > self.neutral_ns and CS.buttonEnable + main_enable = self.main_off and main_fresh and self.source_ns[0xC9] > self.neutral_ns and CS.cruiseState.available + if (self.neutral_ns > self.disable_ns > 0 and (physical_enable or main_enable) and + CS.cruiseState.available and not CS.brakePressed and not CS.regenBraking): + self.latched = False + self.disable_ns = self.neutral_ns = 0 + self.main_off = False + self.last_active_ns = self.active_since_ns = 0 + self.seen_active = False + self.bad_since_ns = self.bad_samples = 0 + + deliberate_pause = (pedal_fresh or cancel or main_fresh and not CS.cruiseState.available or + CS.standstill or CS.vEgo < self.min_speed or CS.steeringPressed or + self.status in (2, 3) and 0 < self.status_ns <= now_ns and + now_ns - self.status_ns <= EPS_STATUS_TIMEOUT_NS) + if healthy and deliberate_pause and not self.latched: + self.seen_active = False + self.command_active = False + self.last_active_ns = self.active_since_ns = 0 + self.bad_since_ns = self.bad_samples = 0 + self.last_sample_ns = self.status_ns + # A missing EPS feed must retain a fault even after host health gates emit neutral. + if (self.active_since_ns and not self.latched and + now_ns - max(self.status_ns, self.active_since_ns) > EPS_STATUS_TIMEOUT_NS): + self.latched = True + + recent_command = 0 < self.last_active_ns <= now_ns and now_ns - self.last_active_ns <= STEERING_COMMAND_TIMEOUT_NS + eligible = ((healthy or self.status == -1) and self.command_active and recent_command and CS.cruiseState.available and + not CS.brakePressed and not CS.regenBraking and not cancel and + not CS.standstill and CS.vEgo >= self.min_speed and not CS.steeringPressed) + fresh_status = 0 < self.status_ns <= now_ns and now_ns - self.status_ns <= EPS_STATUS_TIMEOUT_NS + if eligible and fresh_status and self.status_ns > self.active_since_ns and self.status_ns != self.last_sample_ns: + self.last_sample_ns = self.status_ns + if self.status == 1: + self.seen_active = True + self.bad_since_ns = self.bad_samples = 0 + elif self.status not in (2, 3): + if not self.bad_since_ns: + self.bad_since_ns = self.status_ns + self.bad_samples += 1 + if self.seen_active: + self.latched = True + else: + self.bad_since_ns = self.bad_samples = 0 + if (eligible and self.bad_samples >= 2 and self.bad_since_ns and + now_ns - self.active_since_ns >= EPS_STATUS_TIMEOUT_NS): + self.latched = True + if not recent_command or CS.steeringPressed or CS.standstill or CS.vEgo < self.min_speed: + self.bad_since_ns = self.bad_samples = 0 + if self.latched: + CS.steerFaultTemporary = True diff --git a/opendbc_repo/opendbc/car/gm/tests/test_ascm_intercept.py b/opendbc_repo/opendbc/car/gm/tests/test_ascm_intercept.py index d5743a1962..043919332c 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_ascm_intercept.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_ascm_intercept.py @@ -1,9 +1,11 @@ import unittest from opendbc.can import CANPacker -from opendbc.car import Bus +from opendbc.car import Bus, structs from opendbc.car import gen_empty_fingerprint from opendbc.car.gm.carstate import CarState +from opendbc.car.gm import gmcan +from opendbc.car.gm.values import CruiseButtons from opendbc.car.gm.interface import CarInterface from opendbc.car.gm.fingerprints import FINGERPRINTS, FW_VERSIONS from opendbc.car.gm.values import ASCM_INTERCEPT_CAR, CAR, DBC, GMSafetyFlags, CarControllerParams @@ -22,6 +24,183 @@ def params(car, *, sascm=False, accelerator=True, radar=False, alpha=False, rele class TestAscmIntercept(unittest.TestCase): + def authority_frame(self, ci, packer, tick, *, status=1, active=True, main=True, + regen=False, brake=False, button=CruiseButtons.UNPRESS, driver=0., speed=8., torque=0.02, + malformed=False, physical_delay_ns=0, eps_missing=False): + now = 1_000_000_000 + tick * 10_000_000 + values = { + 'ECMCruiseControl': {'CruiseActive': 0}, + 'ECMEngineStatus': {'CruiseMainOn': int(main), 'BrakePressed': int(brake)}, + 'AcceleratorPedal2': {'CruiseState': 0}, + 'ECMPRDNL2': {'PRNDL2': 4}, + 'EBCMWheelSpdRear': {'RLWheelSpd': speed * 3.6, 'RRWheelSpd': speed * 3.6, 'RLWheelDir': 1, 'RRWheelDir': 1}, + 'EBCMWheelSpdFront': {'FLWheelSpd': speed * 3.6, 'FRWheelSpd': speed * 3.6}, + 'ESPStatus': {'TractionControlOn': 1}, + 'BCMDoorBeltStatus': {'LeftSeatBelt': 1}, + 'BCMGeneralPlatformStatus': {}, 'BCMTurnSignals': {}, 'PSCMSteeringAngle': {}, + 'ECMAcceleratorPos': {'BrakePedalPos': 30 if brake else 0}, + 'EBCMFrictionBrakeStatus': {}, 'EBCMRegenPaddle': {'RegenPaddle': int(regen)}, + } + packets = [packer.make_can_msg(name, 0, signals) for name, signals in values.items()] + packets += [gmcan.create_buttons(packer, 0, tick % 4, button), + packer.make_can_msg('ASCMLKASteeringCmd', 2, {}), + packer.make_can_msg('ASCMActiveCruiseControlStatus', 2, {}), + packer.make_can_msg('AEBCmd', 2, {})] + if tick % 10 == 0 and not eps_missing: + pscm = packer.make_can_msg('PSCMStatus', 0, {'LKATorqueDeliveredStatus': status, + 'LKADriverAppldTrq': driver, 'LKATorqueDelivered': 0}) + if malformed: + packets.append((0x184, b'\x00', 0)) + packets.append(pscm) + physical = [packet for packet in packets if packet[0] in (0xC9, 0xBE, 0xBD, 0x1E1)] + other = [packet for packet in packets if packet[0] not in (0xC9, 0xBE, 0xBD, 0x1E1)] + cs = ci.update([(now - physical_delay_ns, physical), (now, other)]) + if tick >= 10 and not eps_missing: + self.assertTrue(cs.canValid, (tick, status)) + self.assertFalse(cs.canTimeout, tick) + cc = structs.CarControl() + cc.enabled = active + cc.latActive = active + cc.actuators.torque = torque + cc.hudControl.setSpeed = 30 + _, sends = ci.apply(cc.as_reader(), now) + steering = [data for address, data, bus in sends if address == 0x180 and bus == 0] + if steering: + self.assertEqual(bool((steering[-1][0] >> 3) & 1), active and not ci.CS.steering_authority.latched, tick) + return cs + + def test_volt_steering_loss_requires_driver_rearm(self): + for reset in ('regen', 'brake', 'cancel', 'set', 'main'): + with self.subTest(reset=reset): + cp = params(CAR.CHEVROLET_VOLT_ASCM, sascm=True, alpha=True, radar=True) + ci, packer = CarInterface(cp), CANPacker(DBC[cp.carFingerprint][Bus.pt]) + for tick in range(30): + self.authority_frame(ci, packer, tick) + self.assertTrue(ci.CS.steering_authority.seen_active) + # One emitted neutral frame must not erase the confirmed EPS episode. + for tick in range(30, 34): + self.authority_frame(ci, packer, tick, active=False) + for tick in range(34, 41): + cs = self.authority_frame(ci, packer, tick, status=0) + self.assertTrue(cs.steerFaultTemporary) + self.assertTrue(ci.CS.steering_authority.latched) + for tick in range(41, 60): + cs = self.authority_frame(ci, packer, tick, active=False) + self.assertTrue(cs.steerFaultTemporary) # EPS recovery/automatic CC off cannot rearm. + for tick in range(60, 70): + self.authority_frame(ci, packer, tick, active=False, main=reset != 'main', + regen=reset == 'regen', brake=reset == 'brake', + button=CruiseButtons.CANCEL if reset == 'cancel' else CruiseButtons.UNPRESS) + cs = self.authority_frame(ci, packer, 70, active=False, physical_delay_ns=200_000_000 if reset == 'main' else 1_000_000, + button=CruiseButtons.DECEL_SET if reset == 'set' else + CruiseButtons.RES_ACCEL if reset != 'main' else CruiseButtons.UNPRESS) + if reset in ('set', 'main'): + self.assertTrue(cs.steerFaultTemporary, reset) + cs = self.authority_frame(ci, packer, 71, active=False) + resume_tick = 72 + if reset == 'set': + # SET alone is not the physical disable required to reset a loss latch. + self.assertTrue(cs.steerFaultTemporary, reset) + self.assertTrue(ci.CS.steering_authority.latched, reset) + self.assertEqual(ci.CS.steering_authority.disable_ns, 0, reset) + for tick in range(72, 82): + self.authority_frame(ci, packer, tick, active=False, regen=True) + self.assertGreater(ci.CS.steering_authority.neutral_ns, ci.CS.steering_authority.disable_ns, reset) + cs = self.authority_frame(ci, packer, 82, active=False, button=CruiseButtons.DECEL_SET) + self.assertTrue(cs.steerFaultTemporary, reset) + cs = self.authority_frame(ci, packer, 83, active=False) + resume_tick = 84 + context = (reset, cs.buttonEnable, cs.brakePressed, cs.regenBraking, + ci.CS.steering_authority.disable_ns, ci.CS.steering_authority.neutral_ns) + self.assertFalse(cs.steerFaultTemporary, context) + self.assertFalse(ci.CS.steering_authority.latched, context) + for tick in range(resume_tick, resume_tick + 20): + self.authority_frame(ci, packer, tick) + self.assertTrue(ci.CS.steering_authority.seen_active, context) + + def test_volt_steering_acquisition_and_invalid_status(self): + for status, malformed in ((0, False), (4, False), (1, True)): + with self.subTest(status=status, malformed=malformed): + cp = params(CAR.CHEVROLET_VOLT_ASCM, sascm=True, alpha=True, radar=True) + ci, packer = CarInterface(cp), CANPacker(DBC[cp.carFingerprint][Bus.pt]) + for tick in range(30): + cs = self.authority_frame(ci, packer, tick, status=status, malformed=malformed) + self.assertFalse(cs.steerFaultTemporary, tick) + for tick in range(30, 41): + cs = self.authority_frame(ci, packer, tick, status=status, malformed=malformed) + self.assertTrue(cs.steerFaultTemporary) + self.assertTrue(ci.CS.steering_authority.latched) + + cp = params(CAR.CHEVROLET_VOLT_ASCM, sascm=True, alpha=True, radar=True) + ci, packer = CarInterface(cp), CANPacker(DBC[cp.carFingerprint][Bus.pt]) + for tick in range(30): + self.authority_frame(ci, packer, tick) + for tick in range(30, 70): + cs = self.authority_frame(ci, packer, tick, active=tick < 40, eps_missing=True) + self.assertTrue(cs.steerFaultTemporary) + for tick in range(70, 90): + cs = self.authority_frame(ci, packer, tick, active=False) + self.assertTrue(cs.steerFaultTemporary) # Fresh EPS does not erase a missing-feed latch. + + # A total receive gap cannot conceal a stale EPS episode on the first fresh return. + cp = params(CAR.CHEVROLET_VOLT_ASCM, sascm=True, alpha=True, radar=True) + ci, packer = CarInterface(cp), CANPacker(DBC[cp.carFingerprint][Bus.pt]) + for tick in range(30): + self.authority_frame(ci, packer, tick) + cs = self.authority_frame(ci, packer, 70) + self.assertTrue(cs.steerFaultTemporary) + self.assertTrue(ci.CS.steering_authority.latched) + + def test_volt_steering_healthy_zero_and_existing_faults(self): + for status, active, speed, driver in ((1, True, 8., 0.), (0, False, 8., 0.), + (0, True, 0., 0.), (0, True, 8., 3.), + (2, True, 8., 0.), (3, True, 8., 0.)): + with self.subTest(status=status, active=active, speed=speed, driver=driver): + cp = params(CAR.CHEVROLET_VOLT_ASCM, sascm=True, alpha=True, radar=True) + ci, packer = CarInterface(cp), CANPacker(DBC[cp.carFingerprint][Bus.pt]) + for tick in range(60): + cs = self.authority_frame(ci, packer, tick, status=status, active=active, speed=speed, driver=driver, + torque=0. if status == 1 else 0.02) + self.assertFalse(ci.CS.steering_authority.latched) + self.assertEqual(cs.steerFaultTemporary, status == 2) + self.assertEqual(cs.steerFaultPermanent, status == 3) + # A genuine driver pause starts a new acquisition window, unlike an unexplained brief CC drop. + for pause in ('regen', 'brake', 'cancel', 'main', 'override', 'standstill'): + with self.subTest(pause=pause): + cp = params(CAR.CHEVROLET_VOLT_ASCM, sascm=True, alpha=True, radar=True) + ci, packer = CarInterface(cp), CANPacker(DBC[cp.carFingerprint][Bus.pt]) + for tick in range(30): + self.authority_frame(ci, packer, tick) + for tick in range(30, 50): + cs = self.authority_frame(ci, packer, tick, status=0, active=False, + regen=pause == 'regen', brake=pause == 'brake', main=pause != 'main', + button=CruiseButtons.CANCEL if pause == 'cancel' else CruiseButtons.UNPRESS, + driver=3. if pause == 'override' else 0., speed=0. if pause == 'standstill' else 8.) + self.assertFalse(cs.steerFaultTemporary) + for tick in range(50, 70): + cs = self.authority_frame(ci, packer, tick, status=0 if tick < 65 else 1) + self.assertFalse(cs.steerFaultTemporary, tick) + for tick in range(70, 80): + cs = self.authority_frame(ci, packer, tick) + self.assertFalse(cs.steerFaultTemporary, tick) + self.assertTrue(ci.CS.steering_authority.seen_active) + # Ordinary temporary EPS faults retain their normal recovery/acquisition behavior. + cp = params(CAR.CHEVROLET_VOLT_ASCM, sascm=True, alpha=True, radar=True) + ci, packer = CarInterface(cp), CANPacker(DBC[cp.carFingerprint][Bus.pt]) + for tick in range(30): + self.authority_frame(ci, packer, tick) + for tick in range(30, 50): + cs = self.authority_frame(ci, packer, tick, status=2, active=False) + self.assertTrue(cs.steerFaultTemporary) + self.assertFalse(ci.CS.steering_authority.latched) + for tick in range(50, 80): + cs = self.authority_frame(ci, packer, tick, status=0 if tick < 65 else 1) + self.assertFalse(cs.steerFaultTemporary, tick) + self.assertTrue(ci.CS.steering_authority.seen_active) + for cp in (params(CAR.CHEVROLET_VOLT_ASCM), params(CAR.CHEVROLET_VOLT), + params(CAR.CHEVROLET_BOLT_EUV)): + self.assertFalse(CarState(cp).steering_authority.enabled) + def test_eight_manual_ids_have_stock_acc_default(self): self.assertEqual(len(ASCM_INTERCEPT_CAR), 8) for car in ASCM_INTERCEPT_CAR: diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index 590d692f47..5a58395940 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -127,6 +127,8 @@ struct StarPilotCarState @0xf35cc4560bbf6ec2 { decelHardCruise @29 :Bool; # current/releasing decel cruise button came from GM hard-press signal pulseAndGlide @30 :Bool; # developer-only wheel-button pulse-and-glide mode is enabled gps @31 :Gps; + lateralAuthorityUnavailable @32 :Bool; + sourceCarStateMonoTime @33 :UInt64; struct Gps { sourceMonoTime @0 :UInt64; # CAN observation converted to CLOCK_MONOTONIC diff --git a/openpilot/selfdrive/car/card.py b/openpilot/selfdrive/car/card.py index 75e10370e9..01dd035151 100755 --- a/openpilot/selfdrive/car/card.py +++ b/openpilot/selfdrive/car/card.py @@ -440,7 +440,6 @@ class Car: # Update carState from CAN CS = self.CI.update(can_list) - self.car_gps_publisher.update(self.CI.CS, self.pm) self.observe_ioniq6_long_authority(can_list, CS) media_owner = getattr(self, 'ioniq6_media', None) media_observation = media_owner.update(can_list) if media_owner is not None else None @@ -775,6 +774,13 @@ class Car: publish_stock_status(self.pm, self.CP, internal, session=self.slc_producer_session, sequence=self.slc_cruise_event_id, car_state_stamp=int(cs_send.logMonoTime), valid=bool(cs_send.valid and not CS.canTimeout)) + companion = None + steering_authority = getattr(self.CI.CS, 'steering_authority', None) + if self.CP.brand == 'gm' and steering_authority is not None and steering_authority.enabled: + companion = messaging.new_message('starpilotCarState', valid=bool(cs_send.valid), logMonoTime=int(cs_send.logMonoTime)) + companion.starpilotCarState.lateralAuthorityUnavailable = bool(steering_authority.latched) + companion.starpilotCarState.sourceCarStateMonoTime = int(cs_send.logMonoTime) + self.car_gps_publisher.update(self.CI.CS, self.pm, companion) self.pm.send('carState', cs_send) wheel = getattr(self, 'wheel_publisher', None) @@ -1010,13 +1016,16 @@ class Car: for ps, cfg in zip(pandas, self.CP.safetyConfigs, strict=True))) def publish_sendcan(self, frames, valid=True): + steering_authority = getattr(getattr(getattr(self, 'CI', None), 'CS', None), 'steering_authority', None) + monitored_steering = (getattr(getattr(self, 'CP', None), 'brand', None) == 'gm' and + steering_authority is not None and steering_authority.enabled) packet = can_list_to_can_capnp(frames, msgtype='sendcan', valid=valid) if getattr(self, 'volt_cc_selected', False): # CAN producer/parser and pandad send age use BOOTTIME; Python controls/device envelopes use MONOTONIC. builder = messaging.log_from_bytes(packet).as_builder() builder.logMonoTime = int(getattr(self, 'volt_cc_now_boot_ns', 0)) packet = builder.to_bytes() - elif self.volt_startup_keepalive(): + elif self.volt_startup_keepalive() or monitored_steering: builder = messaging.log_from_bytes(packet).as_builder() builder.logMonoTime = time.clock_gettime_ns(time.CLOCK_BOOTTIME) packet = builder.to_bytes() @@ -1195,10 +1204,13 @@ class Car: if self.ioniq6_long_lost: self.publish_sendcan([], valid=False) return + steering_authority = getattr(self.CI.CS, 'steering_authority', None) + monitored_steering = self.CP.brand == 'gm' and steering_authority is not None and steering_authority.enabled # send car controls over can now_nanos = (self.volt_cc_now_boot_ns if getattr(self, 'volt_cc_selected', False) else self.can_log_mono_time if REPLAY else - time.clock_gettime_ns(time.CLOCK_BOOTTIME) if self.volt_startup_keepalive() else int(time.monotonic() * 1e9)) + time.clock_gettime_ns(time.CLOCK_BOOTTIME) if self.volt_startup_keepalive() or monitored_steering else + int(time.monotonic() * 1e9)) self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos) self.publish_sendcan(can_sends, valid=CS.canValid) diff --git a/openpilot/selfdrive/selfdrived/selfdrived.py b/openpilot/selfdrive/selfdrived/selfdrived.py index 773e28449b..cfa6a85358 100755 --- a/openpilot/selfdrive/selfdrived/selfdrived.py +++ b/openpilot/selfdrive/selfdrived/selfdrived.py @@ -29,6 +29,7 @@ from openpilot.selfdrive.selfdrived.helpers import ExcessiveActuationCheck from openpilot.system.manager.process_health import driving_process_failures from openpilot.selfdrive.selfdrived.state import StateMachine from opendbc.car.gm.aol import native_bootstrap_supported +from opendbc.car.gm.steering_authority import monitored_profile from opendbc.car.gm.values import GMFlags, is_volt_ascm_longitudinal from openpilot.starpilot.aol.intent import read_settings from openpilot.starpilot.aol.runtime import (INTENT_MAX_AGE_NS, AxisDecision, current_intent, current_native, decide_axes, @@ -95,6 +96,9 @@ class SelfdriveD: self.aol_sequence = 0 self.aol_axis_decision = AxisDecision() self.aol_dm_lateral_inhibit = False + self.aol_authority_lost = False + self.steering_authority_unavailable = False + self.steering_authority_pair = None self.force_stop_hold_alert = HoldAlertState() self.aol_car_state_log_ns = 0 self.aol_last_intent = None @@ -262,10 +266,44 @@ class SelfdriveD: self.big_model_chestnut = False return big_failed + def _update_steering_authority(self, CS): + if not monitored_profile(self.CP): + self.steering_authority_unavailable = False + return + now_ns = self.aol_car_state_log_ns if REPLAY and self.aol_car_state_log_ns else time.monotonic_ns() + service = 'starpilotCarState' + source_ns = self.aol_car_state_log_ns + source_current = bool(getattr(self, 'conditional_car_state_valid', False) and CS.canValid and not CS.canTimeout and + 0 < source_ns <= now_ns and now_ns - source_ns <= INTENT_MAX_AGE_NS) + envelope_current = bool(self.sm.seen.get(service, False) and self.sm.valid.get(service, False) and + self.sm.alive.get(service, False)) + pair = None + if source_current and envelope_current: + message_ns = int(self.sm.logMonoTime[service]) + companion = self.sm[service] + if (int(companion.sourceCarStateMonoTime) == message_ns and 0 < message_ns <= now_ns and + now_ns - message_ns <= INTENT_MAX_AGE_NS): + if message_ns == source_ns: + pair = (source_ns, bool(companion.lateralAuthorityUnavailable)) + self.steering_authority_pair = pair + elif message_ns > source_ns: + previous = getattr(self, 'steering_authority_pair', None) + if previous is not None and previous[0] == source_ns: + pair = previous + previous_axes = self.aol_axis_decision + expected_active = self.active or previous_axes.lateral_active or previous_axes.longitudinal_active + if pair is not None: + self.steering_authority_unavailable = pair[1] + elif expected_active: + self.steering_authority_unavailable = True + if getattr(self, 'steering_authority_unavailable', False): + self.events.add(EventName.controlsMismatch) + def update_events(self, CS): """Compute onroadEvents from carState""" self.events.clear() + self._update_steering_authority(CS) self.tesla_stock_alerts = [] self.nostalgia_paddle_cancel = False action_owner = getattr(self, 'controller_action_owner', None) @@ -842,21 +880,37 @@ class SelfdriveD: self.conditional_status = result.status self.conditional_result = result + def _decide_aol_axes(self, CS, intent, native): + return decide_axes( + standard_lateral=self.active, standard_longitudinal=self.enabled and self.CP.openpilotLongitudinalControl, + intent=intent, native=native, car_state=CS, initialized=self.initialized, + model_ready=bool(self.sm.all_checks(['modelV2', 'extrinsicsCalibration']) and + self.sm['extrinsicsCalibration'].calStatus == log.ExtrinsicsCalibration.Status.calibrated), + no_entry=aol_no_entry(self.events.names, CS, paddle_only_cancel=self.nostalgia_paddle_cancel, + allow_lateral_cancel=bool(intent is not None and intent.allowedLatch and intent.optionalSetRelease + and ev6_aol_qualified(self.CP, marked_only=True)), + cruise_main_required=getattr(self, 'aol_cruise_main_required', True), + allow_below_engage_speed=bool(is_volt_ascm_longitudinal(self.CP) and + not self.CP.flags & GMFlags.PEDAL_LONG)), + immediate_disable=self.events.contains(ET.IMMEDIATE_DISABLE), + dm_lockout=bool(not self.sm.all_checks(['driverMonitoringState']) or + self.sm['driverMonitoringState'].lockout or self.sm['driverMonitoringState'].alwaysOnLockout or + self.sm['driverMonitoringState'].alertLevel == AlertLevel.three), + pause_brake_mps=self.aol_settings.pause_brake_mps if self.aol_settings is not None else 0.0, + lateral_inhibit=self.aol_dm_lateral_inhibit) + def step(self): CS = self.data_sample() self.update_events(CS) native = None - lost_active_aol = False + lost_active_aol = bool(self.aol_replay and getattr(self, 'steering_authority_unavailable', False) and + (self.aol_axis_decision.lateral_active or self.aol_axis_decision.longitudinal_active)) if self.aol_replay or getattr(self, 'ordinary_axis_ack_required', False): now_ns = self.aol_car_state_log_ns if REPLAY and self.aol_car_state_log_ns else time.monotonic_ns() native = current_native(self.sm, self.CP, now_ns=now_ns, axis_session_id=self.aol_session_id) if native is None and self.initialized: lost_active_aol = self.aol_axis_decision.lateral_active or self.aol_axis_decision.longitudinal_active self.events.add(EventName.controlsMismatch) - if not self.CP.passive and self.initialized: - self.enabled, self.active = self.state_machine.update(self.events) - if lost_active_aol and ET.IMMEDIATE_DISABLE not in self.state_machine.current_alert_types: - self.state_machine.current_alert_types.append(ET.IMMEDIATE_DISABLE) if self.aol_replay: intent = current_intent(self.sm, car_state_ns=self.aol_car_state_log_ns, now_ns=now_ns, previous=getattr(self, 'aol_last_intent', None)) @@ -865,23 +919,32 @@ class SelfdriveD: self.aol_dm_lateral_inhibit = True elif intent is not None and not intent.allowedLatch: self.aol_dm_lateral_inhibit = False - self.aol_axis_decision = decide_axes( - standard_lateral=self.active, standard_longitudinal=self.enabled and self.CP.openpilotLongitudinalControl, - intent=intent, native=native, car_state=CS, initialized=self.initialized, - model_ready=bool(self.sm.all_checks(['modelV2', 'extrinsicsCalibration']) and - self.sm['extrinsicsCalibration'].calStatus == log.ExtrinsicsCalibration.Status.calibrated), - no_entry=aol_no_entry(self.events.names, CS, paddle_only_cancel=self.nostalgia_paddle_cancel, - allow_lateral_cancel=bool(intent is not None and intent.allowedLatch and intent.optionalSetRelease - and ev6_aol_qualified(self.CP, marked_only=True)), - cruise_main_required=getattr(self, 'aol_cruise_main_required', True), - allow_below_engage_speed=bool(is_volt_ascm_longitudinal(self.CP) and - not self.CP.flags & GMFlags.PEDAL_LONG)), - immediate_disable=self.events.contains(ET.IMMEDIATE_DISABLE), - dm_lockout=bool(not self.sm.all_checks(['driverMonitoringState']) or - self.sm['driverMonitoringState'].lockout or self.sm['driverMonitoringState'].alwaysOnLockout or - self.sm['driverMonitoringState'].alertLevel == AlertLevel.three), - pause_brake_mps=self.aol_settings.pause_brake_mps if self.aol_settings is not None else 0.0, - lateral_inhibit=self.aol_dm_lateral_inhibit) + previous = self.aol_axis_decision + requested = self._decide_aol_axes(CS, intent, native) + source_current = bool(intent is not None and getattr(self, 'conditional_car_state_valid', False) and + CS.canValid and not CS.canTimeout) + driver_override = bool(CS.brakePressed or CS.regenBraking or CS.gasPressed or + self.events.contains(ET.USER_DISABLE)) + lost_companion = bool((previous.lateral_active or previous.longitudinal_active) and + not source_current and not self.events.contains(ET.USER_DISABLE)) + lost_permission = bool(native is not None and not driver_override and ( + previous.lateral_active and requested.desired_lateral and native.requestedLateral and + not native.lateralAllowed and not CS.steeringPressed and not self.events.contains(ET.OVERRIDE_LATERAL) or + previous.longitudinal_active and requested.desired_longitudinal and native.requestedLongitudinal and + not native.longitudinalAllowed and not self.events.contains(ET.OVERRIDE_LONGITUDINAL))) + if self.initialized and (lost_active_aol or lost_companion or lost_permission): + lost_active_aol = True + self.aol_authority_lost = True + elif source_current and not intent.allowedLatch: + self.aol_authority_lost = False + if getattr(self, 'aol_authority_lost', False): + self.events.add(EventName.controlsMismatch) + if not self.CP.passive and self.initialized: + self.enabled, self.active = self.state_machine.update(self.events) + if lost_active_aol and ET.IMMEDIATE_DISABLE not in self.state_machine.current_alert_types: + self.state_machine.current_alert_types.append(ET.IMMEDIATE_DISABLE) + if self.aol_replay: + self.aol_axis_decision = self._decide_aol_axes(CS, intent, native) elif getattr(self, 'ordinary_axis_ack_required', False): self.aol_axis_decision = decide_ordinary_axis( requested=bool(self.initialized and self.conditional_car_state_valid and diff --git a/openpilot/starpilot/aol/tests/test_gm.py b/openpilot/starpilot/aol/tests/test_gm.py index 108230f3cd..1e7f28de4e 100644 --- a/openpilot/starpilot/aol/tests/test_gm.py +++ b/openpilot/starpilot/aol/tests/test_gm.py @@ -1162,6 +1162,7 @@ class TestGmAol(unittest.TestCase): self.assertEqual(controls.CP.to_dict(), cp.to_dict(), msg=ctx) sd = SelfdriveD.__new__(SelfdriveD) sd.CP, sd.initialized, sd.aol_replay = cp, True, True + sd.enabled = sd.active = False sd.aol_session_id, sd.aol_axis_decision = 'bolt-cancel', AxisDecision() sd.aol_dm_lateral_inhibit, sd.aol_settings = False, selected.aol_settings sd.nostalgia_paddle_cancel = False @@ -1322,6 +1323,7 @@ class TestGmAol(unittest.TestCase): sd.sm.update_msgs(now / 1e9, [intent.as_reader(), receipt.as_reader(), model.as_reader(), calibration.as_reader(), monitoring.as_reader()]) sd.aol_car_state_log_ns = now + sd.conditional_car_state_valid = cs.canValid and not cs.canTimeout sd.events = events with patch('openpilot.selfdrive.selfdrived.selfdrived.REPLAY', True), \ patch.object(sd, 'data_sample', return_value=cs), patch.object(sd, 'update_events'), \ @@ -1471,6 +1473,7 @@ class TestGmAol(unittest.TestCase): self.assertEqual(controls.CP.to_dict(), cp.to_dict()) sd = SelfdriveD.__new__(SelfdriveD) sd.CP, sd.initialized, sd.aol_replay = cp, True, True + sd.enabled = sd.active = False sd.aol_session_id, sd.aol_axis_decision = 'volt-brake', AxisDecision() sd.aol_dm_lateral_inhibit, sd.aol_settings = False, selected.aol_settings sd.nostalgia_paddle_cancel = False @@ -1484,6 +1487,7 @@ class TestGmAol(unittest.TestCase): self.assertEqual(safety.set_safety_hooks(structs.CarParams.SafetyModel.gm, 0xD114), 0) safety.init_tests() resumed = paused = 0 + previous_sd_onroad = None ctx = {'identity': str(cp.carFingerprint), 'word': '0xd114', 'phase': 'prime'} try: # Warm all selected parser/native sources, with stock cruise inactive. @@ -1491,6 +1495,8 @@ class TestGmAol(unittest.TestCase): stamp = 800_000_000 + warmup * 10_000_000 _, frames = feed_car(SimpleNamespace(update=lambda _: None), packer, stamp, counter=warmup % 4, active=False, speed=8., camera=True) + frames = [frame for frame in frames if frame[0] != 0x184] + frames.append(packer.make_can_msg('PSCMStatus', 0, {'LKATorqueDeliveredStatus': 1})) frames.append(packer.make_can_msg('ASCMActiveCruiseControlStatus', 2, {'ACCCruiseState': 0})) previous = ci.update([(stamp, frames)]) for frame in frames: @@ -1499,23 +1505,25 @@ class TestGmAol(unittest.TestCase): self.assertTrue(previous.canValid, msg=ctx) self.assertTrue(safety.safety_config_valid(), msg=ctx) self.assertFalse(safety.get_controls_allowed(), msg=ctx) - for tick in range(140): + for tick in range(146): now = 1_000_000_000 + tick * 10_000_000 brake = any(start <= tick < start + 12 for start in (20, 60, 100)) fault = tick == 138 - ctx.update(phase='scenario', tick=tick, brake=brake, fault=fault) + main = tick not in (140, 141) + ctx.update(phase='scenario', tick=tick, brake=brake, fault=fault, main=main) _, packets = feed_car(SimpleNamespace(update=lambda _: None), packer, now, counter=tick % 4, active=False, speed=8., camera=True) - packets = [frame for frame in packets if frame[0] not in (0xC9, 0x1C4, 0xBE)] - packets += [packer.make_can_msg('ASCMActiveCruiseControlStatus', 2, {'ACCCruiseState': 0}), - packer.make_can_msg('ECMEngineStatus', 0, {'CruiseMainOn': 1}), + packets = [frame for frame in packets if frame[0] not in (0xC9, 0x1C4, 0xBE, 0x184)] + packets += [packer.make_can_msg('PSCMStatus', 0, {'LKATorqueDeliveredStatus': 1}), + packer.make_can_msg('ASCMActiveCruiseControlStatus', 2, {'ACCCruiseState': 0}), + packer.make_can_msg('ECMEngineStatus', 0, {'CruiseMainOn': int(main)}), packer.make_can_msg('AcceleratorPedal2', 0, {'CruiseState': 0}), packer.make_can_msg('ECMAcceleratorPos', 0, {'BrakePedalPos': 30 if brake else 0})] cs = ci.update([(now, packets)]) self.assertTrue(cs.canValid, msg=ctx) self.assertEqual(cs.brakePressed, brake, msg=ctx) self.assertFalse(cs.cruiseState.enabled, msg=ctx) - self.assertTrue(cs.cruiseState.available, msg=ctx) + self.assertEqual(cs.cruiseState.available, main, msg=ctx) for frame in packets: self.assertTrue(native('rx', frame, now // 1000), (frame, ctx)) safety.safety_tick() @@ -1531,27 +1539,30 @@ class TestGmAol(unittest.TestCase): self.assertEqual(events.contains(ET.IMMEDIATE_DISABLE), fault, msg=ctx) onroad = messaging.new_message('onroadEvents', 0, valid=True, logMonoTime=now) onroad.onroadEvents = events.to_msg() - selected.sm.update_msgs(now / 1e9, [onroad.as_reader()]) - disarm = selected.aol_disarming_fault(cs, now, now) - self.assertEqual(disarm, fault, msg=ctx) + card_onroad = onroad.as_reader() if fault or previous_sd_onroad is None else previous_sd_onroad + selected.sm.update_msgs(now / 1e9, [card_onroad]) + disarm = selected.aol_disarming_fault(cs, int(card_onroad.logMonoTime), now) + self.assertEqual(disarm, tick in (136, 138, 139), msg=ctx) selected.aol_card_intent.update(cs, fault_active=disarm, now_ns=now, standard_enabled=False) - self.assertEqual(selected.aol_card_intent.allowed_latch, tick < 138, msg=ctx) - self.assertEqual(selected.aol_card_intent._main_cycle_required, tick >= 138, msg=ctx) + expected_latch = main and (tick < 136 or tick >= 142) + self.assertEqual(selected.aol_card_intent.allowed_latch, expected_latch, msg=ctx) + self.assertEqual(selected.aol_card_intent._main_cycle_required, 136 <= tick < 140, msg=ctx) allowed, pause_lat, pause_long = selected.aol_card_intent.output(cs) intent_value = IntentState('card', tick + 1, now, now, now + 200_000_000, allowed, pause_lat, pause_long, selected.aol_qualified, selected.aol_card_intent.allowed_latch) intent = decode_intent(encode_intent(intent_value)) self.assertIsNotNone(intent, msg=ctx) - self.assertEqual(intent.allowedLatch, tick < 138, msg=ctx) + self.assertEqual(intent.allowedLatch, expected_latch, msg=ctx) self.assertFalse(intent.pauseLateral, msg=ctx) self.assertFalse(intent.pauseLongitudinal, msg=ctx) - args = dict(standard_lateral=False, standard_longitudinal=False, intent=intent, car_state=cs, - initialized=True, model_ready=True, - no_entry=aol_no_entry(events.names, cs, paddle_only_cancel=False, allow_below_engage_speed=True), - immediate_disable=events.contains(ET.IMMEDIATE_DISABLE), dm_lockout=False, pause_brake_mps=10.) - wanted = decide_axes(native=None, **args) - expected_lat = not brake and tick < 138 + no_entry = aol_no_entry(events.names, cs, paddle_only_cancel=False, allow_below_engage_speed=True) + immediate_disable = events.contains(ET.IMMEDIATE_DISABLE) + wanted = decide_axes(native=None, standard_lateral=False, standard_longitudinal=False, + intent=intent, car_state=cs, initialized=True, model_ready=True, + no_entry=no_entry, immediate_disable=immediate_disable, + dm_lockout=False, pause_brake_mps=10.) + expected_lat = not brake and expected_latch self.assertEqual(wanted.desired_lateral, expected_lat, msg=ctx) self.assertFalse(wanted.desired_longitudinal, msg=ctx) safety.aol_set_host_request(int(wanted.desired_lateral)) @@ -1573,6 +1584,7 @@ class TestGmAol(unittest.TestCase): sd.sm.update_msgs(now / 1e9, [intent_message.as_reader(), receipt.as_reader(), model.as_reader(), calibration.as_reader(), monitoring.as_reader()]) sd.aol_car_state_log_ns = now + sd.conditional_car_state_valid = cs.canValid and not cs.canTimeout sd.events = events with patch('openpilot.selfdrive.selfdrived.selfdrived.REPLAY', True), \ patch.object(sd, 'data_sample', return_value=cs), patch.object(sd, 'update_events'), \ @@ -1580,12 +1592,22 @@ class TestGmAol(unittest.TestCase): patch.object(sd, 'publish_selfdriveState'): sd.step() actual_lat = expected_lat and tick != 135 + actual_onroad = messaging.new_message('onroadEvents', 0, valid=True, logMonoTime=now) + actual_onroad.onroadEvents = sd.events.to_msg() + previous_sd_onroad = actual_onroad.as_reader() self.assertFalse(sd.enabled or sd.active, msg=ctx) self.assertEqual(sd.aol_axis_decision.lateral_active, actual_lat, msg=ctx) self.assertFalse(sd.aol_axis_decision.longitudinal_active, msg=ctx) if tick == 135: + self.assertTrue(sd.aol_authority_lost, msg=ctx) self.assertIn(EventName.controlsMismatch, sd.events.names, msg=ctx) self.assertFalse(sd.aol_axis_decision.native_acknowledged, msg=ctx) + if tick in (136, 137): + self.assertFalse(sd.aol_axis_decision.lateral_active, msg=ctx) + self.assertFalse(selected.aol_card_intent.allowed_latch, msg=ctx) + if tick == 142: + self.assertTrue(sd.aol_axis_decision.lateral_active, msg=ctx) + self.assertFalse(selected.aol_card_intent._main_cycle_required, msg=ctx) feed(controls, now, tick, active=sd.active, enabled=sd.enabled) axis = messaging.new_message('aolAxisState', valid=True, logMonoTime=now) axis.aolAxisState.qualified = True @@ -1618,7 +1640,10 @@ class TestGmAol(unittest.TestCase): probe.brakePressed = True for speed in (9.99, 10., 10.01): probe.vEgo = speed - self.assertEqual(decide_axes(native=receipt_state, **dict(args, car_state=probe)).desired_lateral, + self.assertEqual(decide_axes(native=receipt_state, standard_lateral=False, standard_longitudinal=False, + intent=intent, car_state=probe, initialized=True, model_ready=True, + no_entry=no_entry, immediate_disable=immediate_disable, + dm_lockout=False, pause_brake_mps=10.).desired_lateral, speed >= 10., (speed, ctx)) previous = cs self.assertEqual(resumed, 3) @@ -1664,6 +1689,7 @@ class TestGmAol(unittest.TestCase): self.assertEqual(controls.CP.to_dict(), cp.to_dict()) sd = SelfdriveD.__new__(SelfdriveD) sd.CP, sd.initialized, sd.aol_replay = cp, True, True + sd.enabled = sd.active = False sd.aol_session_id, sd.aol_axis_decision = 'ascm-acc-fault', AxisDecision() sd.aol_dm_lateral_inhibit, sd.aol_settings = False, selected.aol_settings sd.nostalgia_paddle_cancel = False @@ -1707,10 +1733,9 @@ class TestGmAol(unittest.TestCase): packets = [packet for packet in packets if packet[0] not in (0x1C4, 0x3D1, 0x184)] packets += [packer.make_can_msg('AcceleratorPedal2', 0, {'CruiseState': AccState.FAULTED if fault else AccState.ACTIVE if ordinary else 0}), packer.make_can_msg('ECMEngineStatus', 0, {'CruiseMainOn': int(main)}), - packer.make_can_msg('PSCMStatus', 0, {'LKATorqueDeliveredStatus': 3 if critical == 'permanentEPS' and fault else 2 if tick == 20 else 0}), + packer.make_can_msg('PSCMStatus', 0, {'LKATorqueDeliveredStatus': 3 if critical == 'permanentEPS' and fault else 2 if tick == 20 else 1}), packer.make_can_msg('ASCMActiveCruiseControlStatus', 2, {'ACCCruiseState': 0})] self.assertFalse(any(address == 0x201 for address, _, _ in packets)) - ci.update([(now - 1_000_000, packets)]) cs = ci.update([(now, packets)]) self.assertTrue(cs.canValid, tick) self.assertEqual(cs.accFaulted, fault) @@ -1758,6 +1783,7 @@ class TestGmAol(unittest.TestCase): sd.sm.update_msgs(now / 1e9, [intent.as_reader(), receipt.as_reader(), model.as_reader(), calibration.as_reader(), monitoring.as_reader()]) sd.aol_car_state_log_ns = now + sd.conditional_car_state_valid = cs.canValid and not cs.canTimeout sd.events = events with patch('openpilot.selfdrive.selfdrived.selfdrived.REPLAY', True), \ patch.object(sd, 'data_sample', return_value=cs), patch.object(sd, 'update_events'), \ diff --git a/openpilot/starpilot/aol/tests/test_runtime.py b/openpilot/starpilot/aol/tests/test_runtime.py index 081977bde9..ab9de76278 100644 --- a/openpilot/starpilot/aol/tests/test_runtime.py +++ b/openpilot/starpilot/aol/tests/test_runtime.py @@ -646,6 +646,245 @@ class CardIntentTests(unittest.TestCase): class IpcAxisContractTests(unittest.TestCase): + def _authority_callers(self, full_op): + self.enterContext(mock.patch('openpilot.selfdrive.selfdrived.selfdrived.REPLAY', False)) + from opendbc.car.gm.tests.test_bolt_volt_configurations import ordinary_params + from opendbc.car.gm.values import CAR + from openpilot.selfdrive.controls.controlsd import Controls + from openpilot.starpilot.aol.vehicle import policy_for + from openpilot.starpilot.controllers.mode_actions import SwitchbackCooldown + from openpilot.starpilot.lateral.tests.test_lane_runtime import feed + + params = Params() + params.put_bool('OpenpilotEnabledToggle', True, block=True) + params.put_bool('SafeMode', False, block=True) + params.put_bool('AlwaysOnLateral', True, block=True) + cp = ordinary_params(CAR.CHEVROLET_VOLT_ASCM, alpha=True, sascm=True, radar=True) + policy = policy_for(cp) + cp.safetyConfigs[-1].safetyParam |= policy.safety_param_addition + cp.alternativeExperience |= policy.alternative_experience_addition + params.put('CarParams', cp.to_bytes(), block=True) + controls = Controls() + sd = SelfdriveD.__new__(SelfdriveD) + sd.params = params + sd.CP = cp.as_reader() + services = ['deviceState', 'pandaStates', 'driverMonitoringState', 'extrinsicsCalibration', 'modelV2', + 'carState', 'aolIntentWire', 'aolSafetyWire', 'starpilotCarState'] + self.enterContext(mock.patch.object(sd, 'sm', messaging.SubMaster(services, ignore_avg_freq=services), create=True)) + sd.sm.update = mock.Mock() + sd.events, sd.state_machine, sd.AM = Events(), StateMachine(), AlertManager() + sd.state_machine.state = log.SelfdriveState.OpenpilotState.enabled if full_op else log.SelfdriveState.OpenpilotState.disabled + sd.enabled = sd.active = full_op + sd.mismatch_counter = 0 + sd.events_prev = [] + sd.experimental_mode = sd.requested_experimental_mode = False + sd.initialized = sd.aol_replay = True + sd.aol_authority_lost = sd.aol_dm_lateral_inhibit = False + sd.aol_axis_decision = AxisDecision() + sd.aol_settings = None + sd.aol_car_state_log_ns = 0 + sd.conditional_car_state_valid = False + sd.aol_last_intent = None + sd.aol_session_id, sd.aol_sequence = 'authority-session', 0 + sd.conditional_replay = False + sd.nostalgia_paddle_cancel = False + sd.switchback_capable = False + sd.switchback_cooldown = SwitchbackCooldown() + sd.switchback_setting_ns, sd.switchback_cooldown_ns = 0, 300_000_000_000 + sd.personality, sd.is_metric = 1, False + sd.CS_prev = car_state() + sd.car_state_sock = object() + sd.update_conditional_mode = mock.Mock() + self.enterContext(mock.patch.object(sd, 'update_events', lambda cs: (sd.events.clear(), sd._update_steering_authority(cs)))) + + class Publisher: + def __init__(self): + self.messages = {} + def send(self, service, message): + self.messages[service] = messaging.log_from_bytes(message.to_bytes()) + self.enterContext(mock.patch.object(sd, 'pm', Publisher(), create=True)) + self.enterContext(mock.patch.object(controls, 'pm', Publisher())) + clock = self.enterContext(mock.patch('openpilot.selfdrive.selfdrived.selfdrived.time.monotonic_ns')) + recv = self.enterContext(mock.patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.recv_one')) + base = 10_000_000_000 + original_message = messaging.new_message + def clocked_message(*args, **kwargs): + message = original_message(*args, **kwargs) + if 'logMonoTime' not in kwargs: + message.logMonoTime = clock.return_value + return message + self.enterContext(mock.patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.new_message', clocked_message)) + + def step(tick, *, timeout=False, old_intent=False, allowed=True, pause=False, latched=True, + brake=False, steering=False, gas=False, user_disable=False, observed_state=None, authority_unavailable=False, companion=True, + long_allowed=True, pause_long=False): + now = base + tick * 10_000_000 + clock.return_value = now + state = car_state(brake=brake) if observed_state is None else observed_state + state.cruiseState.available = True + state.steeringPressed, state.gasPressed = steering, gas + state_event = messaging.new_message('carState', valid=True, logMonoTime=now) + state_event.carState = state + recv.return_value = None if timeout else state_event.as_reader() + frames = [] + for service in ('deviceState', 'driverMonitoringState', 'extrinsicsCalibration', 'modelV2'): + message = messaging.new_message(service, valid=True, logMonoTime=now) + if service == 'deviceState': + message.deviceState.started, message.deviceState.startedMonoTime = True, base - 1_000_000_000 + elif service == 'extrinsicsCalibration': + message.extrinsicsCalibration.calStatus = 'calibrated' + frames.append(message.as_reader()) + intent_ns = base if old_intent else now + intent = messaging.new_message('aolIntentWire', 0, valid=True, logMonoTime=intent_ns) + intent.aolIntentWire = encode_intent(IntentState('authority-card', tick + 1, intent_ns, intent_ns, + intent_ns + 200_000_000, latched, pause, pause_long, True, latched)) + native = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=now) + native.aolSafetyWire = encode_safety(SafetyState(1, True, now, now + 200_000_000, + int(car.CarParams.SafetyModel.gm), cp.safetyConfigs[-1].safetyParam, + allowed, full_op and long_allowed, True, full_op, 'authority-panda', sd.aol_session_id)) + if companion: + authority = messaging.new_message('starpilotCarState', valid=companion != 'invalid', + logMonoTime=now - 1 if companion == 'wrong_stamp' else now) + authority.starpilotCarState.sourceCarStateMonoTime = now - 1 if companion == 'wrong_source' else now + authority.starpilotCarState.lateralAuthorityUnavailable = authority_unavailable + frames.append(authority.as_reader()) + sd.sm.update_msgs(now / 1e9, frames + [intent.as_reader(), native.as_reader()]) + if user_disable: + self.enterContext(mock.patch.object(sd, 'update_events', lambda cs: ( + sd.events.clear(), sd._update_steering_authority(cs), sd.events.add(log.OnroadEvent.EventName.pedalPressed)))) + sd.step() + control_frames = [] + with mock.patch.object(controls.sm, 'update_msgs', side_effect=lambda _timestamp, messages: control_frames.extend(messages)): + feed(controls, now, tick * 5, active=False, enabled=False) + model = messaging.new_message('modelV2', valid=True, logMonoTime=now) + model.modelV2 = next(message.modelV2 for message in control_frames if message.which() == 'modelV2') + model.modelV2.action.desiredCurvature = 0. + control_state = messaging.new_message('carState', valid=sd.conditional_car_state_valid, + logMonoTime=sd.aol_car_state_log_ns) + control_state.carState = sd.CS_prev + replaced = ('carState', 'aolSafetyWire', 'modelV2', 'aolAxisState', 'selfdriveState') + controls.sm.update_msgs(now / 1e9, [message for message in control_frames if message.which() not in replaced] + + [control_state.as_reader(), native.as_reader(), model.as_reader(), + sd.pm.messages['aolAxisState'], sd.pm.messages['selfdriveState']]) + command, lac_log = controls.state_control() + controls.publish(command, lac_log) + return sd.pm.messages['aolAxisState'].aolAxisState, sd.pm.messages['selfdriveState'].selfdriveState, command + return sd, controls, step + + def _assert_withdrawal_alert(self, controls, message): + from openpilot.starpilot.audio.axis_alerts import AxisAlerts + from openpilot.starpilot.ui.runtime_snapshot import current_alert + now = int(message.logMonoTime) + alert = current_alert(controls.sm, now, after_frame=0) + self.assertTrue(alert.critical) + self.assertEqual(alert.size, 'full') + self.assertEqual(alert.text1, 'TAKE CONTROL IMMEDIATELY') + self.assertEqual(AxisAlerts().update(controls.sm, int(message.selfdriveState.alertSound.raw), now), + log.SelfdriveState.AudibleAlert.warningImmediate) + + def test_actual_callers_companion_loss_disables_and_requires_intent_withdrawal(self): + for full_op in (False, True): + for loss in ('car_state', 'intent', 'authority_companion', 'wrong_stamp', 'wrong_source', 'invalid'): + with self.subTest(full_op=full_op, loss=loss), OpenpilotPrefix(): + sd, controls, step = self._authority_callers(full_op) + axis, _, command = step(0) + self.assertTrue(axis.lateralActive and axis.nativeAcknowledged and command.latActive) + self.assertAlmostEqual(command.actuators.torque, 0.) + retained, _, command = step(2, timeout=True) + self.assertTrue(retained.lateralActive and command.latActive) + self.assertNotIn(log.OnroadEvent.EventName.controlsMismatch, sd.events.names) + self.assertEqual(retained.sourceCarStateMonoTime, 10_000_000_000) + axis, state, command = step(7, timeout=loss == 'car_state', old_intent=loss == 'intent', companion=False if loss == 'authority_companion' else loss) + self.assertFalse(sd.enabled or sd.active or axis.lateralActive or axis.longitudinalActive or command.latActive or command.longActive) + self.assertIn(log.OnroadEvent.EventName.controlsMismatch, sd.events.names) + self.assertEqual(state.alertText1, 'TAKE CONTROL IMMEDIATELY') + self.assertEqual(state.alertSound, log.SelfdriveState.AudibleAlert.warningImmediate) + self.assertFalse(controls.pm.messages['carControl'].carControl.latActive) + self._assert_withdrawal_alert(controls, sd.pm.messages['selfdriveState']) + self.assertEqual(axis.sourceCarStateMonoTime, 0 if loss == 'car_state' else 10_070_000_000) + self.assertFalse(sd.pm.messages['aolAxisState'].valid if loss == 'car_state' else axis.lateralActive) + self.assertFalse(step(8)[0].lateralActive) # Fresh input alone cannot restart the latched axis. + self.assertFalse(step(9, latched=False)[0].lateralActive) + self.assertTrue(step(10)[0].lateralActive) # Intent owner supplies its fresh rearm after withdrawal. + + def test_actual_callers_fresh_native_denial_preserves_deliberate_pauses(self): + for full_op in (False, True): + for reason in ('denial', 'pause', 'brake', 'steering', 'gas', 'user_disable', 'startup', + *(('long_denial', 'long_pause') if full_op else ())): + with self.subTest(full_op=full_op, reason=reason), OpenpilotPrefix(): + sd, controls, step = self._authority_callers(full_op) + if reason != 'startup': + self.assertTrue(step(0)[0].lateralActive) + axis, state, command = step(1, allowed=reason in ('long_denial', 'long_pause'), pause=reason == 'pause', + long_allowed=reason not in ('long_denial', 'long_pause'), pause_long=reason == 'long_pause', + brake=reason == 'brake', steering=reason == 'steering', + gas=reason == 'gas', user_disable=reason == 'user_disable') + self.assertEqual(bool(axis.lateralActive), reason == 'long_pause') + self.assertEqual(bool(command.latActive), reason == 'long_pause') + if reason in ('denial', 'long_denial'): + self.assertFalse(sd.enabled or sd.active) + self.assertEqual(state.alertText2, 'Controls Mismatch') + self.assertEqual(state.alertSound, log.SelfdriveState.AudibleAlert.warningImmediate) + self.assertTrue(sd.aol_authority_lost) + self._assert_withdrawal_alert(controls, sd.pm.messages['selfdriveState']) + else: + self.assertNotIn(log.OnroadEvent.EventName.controlsMismatch, sd.events.names) + self.assertFalse(sd.aol_authority_lost) + + def test_actual_eps_monitor_fault_reaches_both_axis_callers(self): + from opendbc.can import CANPacker + from opendbc.car import Bus + from opendbc.car.gm.interface import CarInterface + from opendbc.car.gm.tests.test_ascm_intercept import TestAscmIntercept + from opendbc.car.gm.values import DBC + for full_op in (False, True): + with self.subTest(full_op=full_op), OpenpilotPrefix(): + sd, _, step = self._authority_callers(full_op) + ci = CarInterface(sd.CP) + packer = CANPacker(DBC[sd.CP.carFingerprint][Bus.pt]) + producer = TestAscmIntercept() + for tick in range(20): + producer.authority_frame(ci, packer, tick, torque=0.) + healthy = producer.authority_frame(ci, packer, 20, torque=0.) + self.assertTrue(healthy.canValid and not healthy.canTimeout) + self.assertTrue(ci.CS.steering_authority.seen_active) + self.assertFalse(healthy.steerFaultTemporary) + self.assertTrue(step(0, observed_state=healthy)[0].lateralActive) + for tick in range(21, 30): + producer.authority_frame(ci, packer, tick, torque=0.) + loss = producer.authority_frame(ci, packer, 30, status=0, torque=0.) + self.assertTrue(loss.canValid and not loss.canTimeout) + self.assertTrue(loss.steerFaultTemporary) + axis, state, command = step(1, observed_state=loss, authority_unavailable=ci.CS.steering_authority.latched) + self.assertFalse(axis.lateralActive or command.latActive) + self.assertEqual(state.alertText2, 'Controls Mismatch') + self.assertEqual(state.alertSound, log.SelfdriveState.AudibleAlert.warningImmediate) + self.assertFalse(sd.enabled or sd.active) + self.assertIn(log.OnroadEvent.EventName.controlsMismatch, sd.events.names) + for tick in range(31, 40): + producer.authority_frame(ci, packer, tick, status=0, torque=0.) + recovered_status = producer.authority_frame(ci, packer, 40, torque=0.) + self.assertTrue(recovered_status.canValid and not recovered_status.canTimeout) + self.assertTrue(recovered_status.steerFaultTemporary) + self.assertFalse(step(2, observed_state=recovered_status, authority_unavailable=ci.CS.steering_authority.latched)[0].lateralActive) + # A fresh physical status2 is ordinary temporary EPS limitation, not the monitor's latched loss. + sd, _, step = self._authority_callers(full_op) + ci = CarInterface(sd.CP) + for tick in range(20): + producer.authority_frame(ci, packer, tick, torque=0.) + healthy = producer.authority_frame(ci, packer, 20, torque=0.) + self.assertTrue(healthy.canValid and not healthy.canTimeout) + self.assertTrue(step(0, observed_state=healthy)[0].lateralActive) + for tick in range(21, 30): + producer.authority_frame(ci, packer, tick, torque=0.) + temporary = producer.authority_frame(ci, packer, 30, status=2, torque=0.) + self.assertTrue(temporary.canValid and not temporary.canTimeout) + self.assertFalse(ci.CS.steering_authority.latched) + axis, state, command = step(1, observed_state=temporary) + self.assertFalse(axis.lateralActive or command.latActive) + self.assertEqual(state.alertSound, log.SelfdriveState.AudibleAlert.prompt) + self.assertNotIn(log.OnroadEvent.EventName.controlsMismatch, sd.events.names) + def test_car_state_timeout_retains_only_original_fresh_valid_sample(self): sd = SelfdriveD.__new__(SelfdriveD) sd.car_state_sock = object() @@ -727,6 +966,7 @@ class IpcAxisContractTests(unittest.TestCase): sd.initialized = True sd.aol_replay = True sd.aol_car_state_log_ns = 0 + sd.conditional_car_state_valid = True sd.aol_session_id = 'drive-session' sd.aol_axis_decision = AxisDecision() sd.aol_dm_lateral_inhibit = False @@ -799,6 +1039,7 @@ class IpcAxisContractTests(unittest.TestCase): sd.enabled = sd.active = False sd.initialized = sd.aol_replay = True sd.aol_car_state_log_ns = 0 + sd.conditional_car_state_valid = True sd.aol_session_id = 'drive-session' sd.aol_axis_decision = AxisDecision() sd.aol_dm_lateral_inhibit = False @@ -1299,6 +1540,7 @@ class IpcAxisContractTests(unittest.TestCase): sd.initialized = True sd.aol_replay = aol_enabled sd.aol_car_state_log_ns = 0 + sd.conditional_car_state_valid = True sd.aol_session_id = 'drive-session' sd.aol_axis_decision = AxisDecision() sd.aol_dm_lateral_inhibit = False diff --git a/openpilot/starpilot/gps/publisher.py b/openpilot/starpilot/gps/publisher.py index a262345647..dcb14afe74 100644 --- a/openpilot/starpilot/gps/publisher.py +++ b/openpilot/starpilot/gps/publisher.py @@ -15,25 +15,31 @@ class CarGpsPublisher: def __init__(self, *, mono_clock=time.monotonic_ns, boot_clock=boot_time_ns): self.mono_clock, self.boot_clock = mono_clock, boot_clock self.last_source_ns = 0 + self.cached_gps = None - def update(self, car_state, pm): + def update(self, car_state, pm, message=None): getter = getattr(car_state, "get_car_gps", None) fix = getter() if getter is not None else None - if fix is None: - return - source_ns = int(fix["timestamp_nanos"]) - if source_ns <= self.last_source_ns: - return - self.last_source_ns = source_ns - before, boot, after = self.mono_clock(), self.boot_clock(), self.mono_clock() - # pandad timestamps received CAN in BOOTTIME; Python Events use MONOTONIC. - age = boot - source_ns - if not 0 <= after - before <= 1_000_000 or not 0 <= age <= GPS_MAX_AGE_NS: - return - stamp = before - age - if stamp <= 0: - return - message = messaging.new_message("starpilotCarState", valid=bool(fix["hasFix"]), logMonoTime=after) - message.starpilotCarState.gps = {key: fix[key] for key in FIELDS} - message.starpilotCarState.gps.sourceMonoTime = stamp + gps, after = None, 0 + if fix is not None: + source_ns = int(fix["timestamp_nanos"]) + if source_ns > self.last_source_ns: + self.last_source_ns = source_ns + before, boot, after = self.mono_clock(), self.boot_clock(), self.mono_clock() + # pandad timestamps received CAN in BOOTTIME; Python Events use MONOTONIC. + age = boot - source_ns + if 0 <= after - before <= 1_000_000 and 0 <= age <= GPS_MAX_AGE_NS: + stamp = before - age + if stamp > 0: + gps = {key: fix[key] for key in FIELDS} + gps["vNED"] = list(fix["vNED"]) + gps["sourceMonoTime"] = stamp + self.cached_gps = gps if fix["hasFix"] else None + if message is None: + if gps is None: + return + message = messaging.new_message("starpilotCarState", valid=bool(gps["hasFix"]), logMonoTime=after) + gps = gps if gps is not None else self.cached_gps + if gps is not None and 0 < gps["sourceMonoTime"] <= int(message.logMonoTime) <= gps["sourceMonoTime"] + GPS_MAX_AGE_NS: + message.starpilotCarState.gps = gps pm.send("starpilotCarState", message) diff --git a/openpilot/starpilot/gps/tests/test_car_gps.py b/openpilot/starpilot/gps/tests/test_car_gps.py index 04c89e2288..03cb8e3394 100644 --- a/openpilot/starpilot/gps/tests/test_car_gps.py +++ b/openpilot/starpilot/gps/tests/test_car_gps.py @@ -1,4 +1,5 @@ from types import SimpleNamespace +from typing import Any, cast from unittest.mock import Mock, patch import pytest @@ -6,6 +7,7 @@ import pytest from opendbc.can import CANPacker from opendbc.car.gm.interface import CarInterface from opendbc.car.gm.values import CAR +from opendbc.car import structs as car from openpilot.cereal import log, messaging from openpilot.starpilot.gps.publisher import CarGpsPublisher from openpilot.starpilot.gps.source import GPS_SOURCES, observation, select_location @@ -130,3 +132,115 @@ def test_old_car_state_wire_has_no_gps_fix(): sm = Messages() sm.send('starpilotCarState', event) assert select_location(sm, 101_000_000_000) is None + + +def test_fault_companion_without_gps_retains_car_state_stamp_and_validity(): + owner, sm = CarGpsPublisher(), Messages() + for valid in (True, False): + cs_send = messaging.new_message('carState', valid=valid, logMonoTime=101_000_000_000) + companion = messaging.new_message('starpilotCarState', valid=bool(cs_send.valid), logMonoTime=int(cs_send.logMonoTime)) + companion.starpilotCarState.lateralAuthorityUnavailable = True + companion.starpilotCarState.sourceCarStateMonoTime = int(cs_send.logMonoTime) + owner.update(SimpleNamespace(), sm, companion) + assert sm.valid['starpilotCarState'] == cs_send.valid + assert sm.logMonoTime['starpilotCarState'] == cs_send.logMonoTime + assert sm['starpilotCarState'].sourceCarStateMonoTime == cs_send.logMonoTime + assert sm['starpilotCarState'].lateralAuthorityUnavailable + assert not sm['starpilotCarState'].gps.hasFix + assert observation(sm, 'starpilotCarState', cs_send.logMonoTime) is None + + +def test_fault_companion_cached_gps_keeps_original_source_stamp_and_expires(): + state, sm, clock = live_can_fix(), Messages(), [101_000_000_000] + owner = CarGpsPublisher(mono_clock=lambda: clock[0], boot_clock=lambda: clock[0] + 50_000_000_000) + owner.update(state, sm) + source_stamp = sm['starpilotCarState'].gps.sourceMonoTime + for elapsed in (1_000_000_000, 2_500_000_001): + clock[0] = source_stamp + elapsed + companion = messaging.new_message('starpilotCarState', valid=True, logMonoTime=clock[0]) + companion.starpilotCarState.sourceCarStateMonoTime = clock[0] + companion.starpilotCarState.lateralAuthorityUnavailable = True + owner.update(state, sm, companion) + assert sm.logMonoTime['starpilotCarState'] == clock[0] + assert sm['starpilotCarState'].sourceCarStateMonoTime == clock[0] + assert sm['starpilotCarState'].lateralAuthorityUnavailable + if elapsed <= 2_500_000_000: + assert sm['starpilotCarState'].gps.sourceMonoTime == source_stamp + assert location(sm, clock[0]) == (source_stamp, (-110., 40.), 0., None) + else: + assert not sm['starpilotCarState'].gps.hasFix + assert location(sm, clock[0]) is None + + +def test_fault_companion_clock_interruption_cannot_invent_gps_fix(): + clock, sm = iter([101_000_000_000, 101_002_000_000]), Messages() + owner = CarGpsPublisher(mono_clock=lambda: next(clock), boot_clock=lambda: 151_000_000_000) + companion = messaging.new_message('starpilotCarState', valid=True, logMonoTime=101_002_000_000) + companion.starpilotCarState.sourceCarStateMonoTime = companion.logMonoTime + companion.starpilotCarState.lateralAuthorityUnavailable = True + owner.update(live_can_fix(), sm, companion) + assert sm['starpilotCarState'].lateralAuthorityUnavailable + assert not sm['starpilotCarState'].gps.hasFix + assert location(sm, companion.logMonoTime) is None + + +@pytest.mark.parametrize('brand,enabled,valid', [('gm', True, True), ('gm', True, False), ('gm', False, True), ('honda', True, True)]) +def test_actual_card_publishes_fault_companion_before_matching_car_state(brand, enabled, valid): + from openpilot.selfdrive.car.card import Car + + card = cast(Any, Car.__new__(Car)) + card.CP = SimpleNamespace(brand=brand) + card.CI = SimpleNamespace(CS=SimpleNamespace(steering_authority=SimpleNamespace(enabled=enabled, latched=True))) + card.pm = Messages() + card.sm = SimpleNamespace(frame=1, all_checks=lambda _services: True) + card.car_params_published = True + card.last_actuators_output = car.CarControl.Actuators.new_message() + card.slc_replay = card.curve_replay = card.aol_replay = False + card.can_rcv_cum_timeout_counter = 0 + card.rk = SimpleNamespace(remaining=0.) + card.v_cruise_helper = SimpleNamespace(slc_cruise_change=None) + card.car_gps_publisher = CarGpsPublisher() + CS = car.CarState.new_message(canValid=valid) + card.state_publish(CS, None) + assert ('starpilotCarState' in card.pm.data) == (brand == 'gm' and enabled) + if brand == 'gm' and enabled: + assert list(card.pm.data).index('starpilotCarState') < list(card.pm.data).index('carState') + assert card.pm.valid['starpilotCarState'] == card.pm.valid['carState'] == valid + assert card.pm.logMonoTime['starpilotCarState'] == card.pm.logMonoTime['carState'] + assert card.pm['starpilotCarState'].sourceCarStateMonoTime == card.pm.logMonoTime['carState'] + assert card.pm['starpilotCarState'].lateralAuthorityUnavailable + assert not card.pm['starpilotCarState'].gps.hasFix + + +@pytest.mark.parametrize('brand,enabled,keepalive', [('gm', True, False), ('gm', False, False), ('honda', True, False), + ('gm', False, True)]) +def test_actual_card_monitored_apply_and_sendcan_use_same_boottime(brand, enabled, keepalive): + from openpilot.selfdrive.car.card import Car + + card = cast(Any, Car.__new__(Car)) + card.CP = SimpleNamespace(brand=brand, alternativeExperience=0) + card.CI = SimpleNamespace(CS=SimpleNamespace(steering_authority=SimpleNamespace(enabled=enabled)), CC=SimpleNamespace(), + apply=Mock(return_value=(car.CarControl.Actuators.new_message(), []))) + card.pm = Mock() + card.sm = SimpleNamespace(seen={'carControl': True}, alive={'carControl': True}, valid={'carControl': True}, + logMonoTime={'carControl': 101_000_000_000}, recv_time={'carControl': 101.}, + all_alive=lambda _services: True) + card.ci_initialized = True + card.ioniq6_long_prearmed = False + card.vehicle_startup = SimpleNamespace(owner=None, check=Mock()) + card.volt_startup_keepalive = Mock(return_value=keepalive) + CS, CC = car.CarState.new_message(canValid=True), car.CarControl.new_message() + with (patch('openpilot.selfdrive.car.card.REPLAY', False), + patch('openpilot.selfdrive.car.card.time.monotonic', return_value=101.), + patch('openpilot.selfdrive.car.card.time.monotonic_ns', return_value=101_000_000_000), + patch('openpilot.selfdrive.car.card.time.clock_gettime_ns', return_value=151_000_000_000)): + card.controls_update(CS, CC) + expected = 151_000_000_000 if keepalive or brand == 'gm' and enabled else 101_000_000_000 + card.CI.apply.assert_called_once_with(CC, expected) + service, packet = card.pm.send.call_args.args + assert service == 'sendcan' + with log.Event.from_bytes(packet) as event: + assert event.logMonoTime == expected + assert event.valid + assert len(event.sendcan) == 0 + card.vehicle_startup.check.assert_called_once_with()