diff --git a/opendbc_repo/opendbc/car/rivian/carstate.py b/opendbc_repo/opendbc/car/rivian/carstate.py index cbeaabed54..e19769d8ca 100644 --- a/opendbc_repo/opendbc/car/rivian/carstate.py +++ b/opendbc_repo/opendbc/car/rivian/carstate.py @@ -12,6 +12,7 @@ class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) self.last_speed = V_CRUISE_MAX + self.cancel_request = False self.toi_fault = self.toi_active = self.toi_unavailable = False self.acm_lka_hba_cmd: dict | None = None @@ -92,6 +93,21 @@ class CarState(CarStateBase): # AEB ret.stockAeb = cp_cam.vl["ACM_AebRequest"]["ACM_EnableRequest"] != 0 + # AE32 is finalized only for the exact GEN1 owner. Availability cannot + # arm it; it records the actual ACM standby/ACC states, matching native. + if self.CP.flags == 0 and self.CP.alternativeExperience == 32: + feature = int(cp_cam.vl["ACM_Status"]["ACM_FeatureStatus"]) + ret.cruiseState.available = feature in (0, 1) + ret.accFaulted |= feature >= 4 + ret.steerFaultTemporary |= self.toi_fault or self.toi_unavailable + ret.steerFaultPermanent = cp.vl["EPAS_AdasStatus"]["EPAS_EacStatus"] == 4 + # UP_1 is the same factual cancel request emitted by riviancan. Do not + # invent MAIN/LKAS or SET/RES events from availability/cruise changes. + cancel = cp.vl["VDM_AdasSts"]["VDM_UserAdasRequest"] == 1 + if cancel != self.cancel_request: + ret.buttonEvents = [structs.CarState.ButtonEvent(type=structs.CarState.ButtonEvent.Type.cancel, pressed=cancel)] + self.cancel_request = cancel + # Messages needed by carcontroller self.acm_lka_hba_cmd = copy.copy(cp_cam.vl["ACM_lkaHbaCmd"]) if not (self.CP.flags & RivianFlags.GEN2): diff --git a/opendbc_repo/opendbc/car/rivian/stock_aol.py b/opendbc_repo/opendbc/car/rivian/stock_aol.py new file mode 100644 index 0000000000..943b27497a --- /dev/null +++ b/opendbc_repo/opendbc/car/rivian/stock_aol.py @@ -0,0 +1,41 @@ +"""Exact BASE GEN1 AOL admission; never infer Extreme/bridge capability.""" +from opendbc.car import structs +from opendbc.car.rivian.values import CAR + + +def qualified(cp, *, marked_only=False): + try: + return bool(cp.brand == 'rivian' and cp.carFingerprint == CAR.RIVIAN_R1 and int(cp.flags) == 0 and + not (cp.passive or cp.notCar or cp.dashcamOnly) and cp.radarUnavailable and + cp.fingerprintSource != structs.CarParams.FingerprintSource.fixed and + len(cp.safetyConfigs) == 1 and cp.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.rivian and + cp.safetyConfigs[0].safetyParam == int(cp.openpilotLongitudinalControl) and + cp.alternativeExperience in ((32,) if marked_only else (0, 32))) + except (AttributeError, TypeError, ValueError, IndexError): + return False + + +def temporary_restriction(cp, state): + return bool(qualified(cp, marked_only=True) and state is not None and state.canValid and not state.canTimeout and + not state.steerFaultPermanent and not state.accFaulted and + (state.gearShifter != structs.CarState.GearShifter.drive or state.steerFaultTemporary or state.standstill)) + + +def native_observation(native, *, latched, panda_ready, restricted, now_ns, pending_since_ns): + lost = not panda_ready or native is None + if not latched or lost or restricted: + return 0, lost, False + if native.requestedLateral and native.lateralAllowed: + return 0, False, False + pending_since_ns = pending_since_ns or now_ns + return pending_since_ns, False, now_ns - pending_since_ns >= 200_000_000 + + +def temporary_eps_restriction(cp, state): + """Exact armed LAT-only EPS withdrawal; physical disarms stay authoritative.""" + return bool(qualified(cp, marked_only=True) and state is not None and state.canValid and not state.canTimeout and + state.steerFaultTemporary and not state.steerFaultPermanent and + state.gearShifter == structs.CarState.GearShifter.drive and state.cruiseState.available and + not state.cruiseState.enabled and not state.brakePressed and not state.regenBraking and not state.gasPressed and + not state.doorOpen and not state.seatbeltUnlatched and not state.accFaulted and + not state.vehicleSensorsInvalid and not state.steeringDisengage and not state.buttonEvents) diff --git a/opendbc_repo/opendbc/safety/modes/rivian.h b/opendbc_repo/opendbc/safety/modes/rivian.h index 43c8c3df1d..5ce9cbbdf3 100644 --- a/opendbc_repo/opendbc/safety/modes/rivian.h +++ b/opendbc_repo/opendbc/safety/modes/rivian.h @@ -39,9 +39,18 @@ static uint32_t rivian_compute_checksum(const CANPacket_t *msg) { if (msg->addr == 0x100U) { chksum = _rivian_compute_checksum(msg, 0x1D, 0x5F); } + if (msg->addr == 0x390U) { + chksum = crc8_update(0U, 0x90U, 0x1DU); + chksum = crc8_update(chksum, 0x03U, 0x1DU); + for (int i = 1; i < GET_LEN(msg); i++) { + chksum = crc8_update(chksum, msg->data[i], 0x1DU); + } + } return chksum; } +#include "opendbc/safety/modes/rivian_stock_aol.h" + static bool rivian_get_quality_flag_valid(const CANPacket_t *msg) { bool valid = false; if (msg->addr == 0x208U) { @@ -89,6 +98,7 @@ static void rivian_rx_hook(const CANPacket_t *msg) { const int feature_status = msg->data[2] >> 5U; pcm_cruise_check(feature_status == 1); } + rivian_aol_observe(msg); } static bool rivian_tx_hook(const CANPacket_t *msg) { @@ -159,6 +169,7 @@ static safety_config rivian_init(uint16_t param) { {.msg = {{0x38f, 0, 6, 50U, .max_counter = 14U}, { 0 }, { 0 }}}, // iBESP2 (brakes) {.msg = {{0x380, 0, 5, 100U, .max_counter = 14U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // EPAS_SystemStatus (driver torque) {.msg = {{0x100, 2, 8, 100U, .max_counter = 14U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // ACM_Status (cruise state) + {.msg = {{0x390, 0, 7, 100U, .max_counter = 14U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // EPAS angle fault/status }; bool rivian_longitudinal = false; @@ -169,11 +180,30 @@ static safety_config rivian_init(uint16_t param) { rivian_longitudinal = GET_FLAG(param, FLAG_RIVIAN_LONG_CONTROL); #endif - // FIXME: cppcheck thinks that rivian_longitudinal is always false. This is not true - // if ALLOW_DEBUG is defined but cppcheck is run without ALLOW_DEBUG - // cppcheck-suppress knownConditionTrueFalse - return rivian_longitudinal ? BUILD_SAFETY_CFG(rivian_rx_checks, RIVIAN_LONG_TX_MSGS) : \ - BUILD_SAFETY_CFG(rivian_rx_checks, RIVIAN_TX_MSGS); + rivian_aol_reset(); + rivian_aol_longitudinal = rivian_longitudinal; + rivian_aol_enabled = ((unsigned int)alternative_experience == 32U) && + ((param == 0U) || ((param == 1U) && rivian_longitudinal)); + if (rivian_aol_enabled) { + static const AolSafetyPolicy policy = { + .reset = rivian_aol_reset, + .host_request = rivian_aol_request, + .request_mask = rivian_aol_request_mask, + .permission_mask = rivian_aol_permission, + .rx_invalid = rivian_aol_clear, + }; + aol_policy = &policy; + } + safety_config ret = rivian_longitudinal ? BUILD_SAFETY_CFG(rivian_rx_checks, RIVIAN_LONG_TX_MSGS) : + BUILD_SAFETY_CFG(rivian_rx_checks, RIVIAN_TX_MSGS); + if (!rivian_aol_enabled) { + ret.rx_checks_len -= 1; // Ordinary upstream RX catalog remains exactly five rows. + } + if (((unsigned int)alternative_experience == 32U) && !rivian_aol_enabled) { + ret.tx_msgs = NULL; + ret.tx_msgs_len = 0; + } + return ret; } const safety_hooks rivian_hooks = { diff --git a/opendbc_repo/opendbc/safety/modes/rivian_stock_aol.h b/opendbc_repo/opendbc/safety/modes/rivian_stock_aol.h new file mode 100644 index 0000000000..6b8da55cc1 --- /dev/null +++ b/opendbc_repo/opendbc/safety/modes/rivian_stock_aol.h @@ -0,0 +1,113 @@ +#pragma once + +// A normal stock engagement grants one GEN1 torque session. Standby is +// availability only; host requests cannot manufacture a physical grant. +static bool rivian_aol_enabled = false; +static bool rivian_aol_longitudinal = false; +static bool rivian_aol_token = false; +static bool rivian_aol_session = false; +static bool rivian_aol_requested = false; +static bool rivian_aol_pending = false; +static bool rivian_aol_drive = false; +static bool rivian_aol_eps_ok = false; +static bool rivian_aol_toi_ok = false; +static uint8_t rivian_aol_feature = 0U; +static uint32_t rivian_aol_grant_ts = 0U; + +static void rivian_aol_clear(void) { + rivian_aol_token = false; + rivian_aol_pending = false; + rivian_aol_session = false; +} + +static void rivian_aol_reset(void) { + rivian_aol_clear(); + rivian_aol_enabled = false; + rivian_aol_longitudinal = false; + rivian_aol_requested = false; + rivian_aol_drive = false; + rivian_aol_eps_ok = false; + rivian_aol_toi_ok = false; + rivian_aol_feature = 0U; + rivian_aol_grant_ts = 0U; +} + +static void rivian_aol_request(uint8_t mask) { + rivian_aol_requested |= mask != 0U; +} + +static uint8_t rivian_aol_request_mask(void) { + const uint32_t now = microsecond_timer_get(); + const uint32_t grant_age = safety_get_ts_elapsed(now, rivian_aol_grant_ts); + const bool request_expired = rivian_aol_requested && + (safety_get_ts_elapsed(now, aol_host_request_ts) > AOL_HOST_REQUEST_TIMEOUT_US); + const bool pending_expired = rivian_aol_pending && (grant_age > AOL_HOST_REQUEST_TIMEOUT_US); + uint8_t request = 0U; + if (relay_malfunction || safety_rx_checks_invalid || !aol_rx_healthy() || request_expired || pending_expired || + (rivian_aol_session && !heartbeat_engaged)) { + rivian_aol_clear(); + } else if (heartbeat_engaged) { + // Pending activation needs a strictly later host request, inside the + // original physical grant deadline. Neither observation renews that grant. + const bool joined = !rivian_aol_pending || ((aol_host_axis_mask != 0U) && + (safety_get_ts_elapsed(now, aol_host_request_ts) < grant_age)); + if (joined) { + rivian_aol_pending = false; + rivian_aol_session = rivian_aol_token; + request = aol_host_axis_mask; + } + } else { + } + return request; +} + +static uint8_t rivian_aol_permission(void) { + const uint8_t request = rivian_aol_request_mask(); + uint8_t permission = 0U; + if (acc_main_on && rivian_aol_drive && rivian_aol_eps_ok && rivian_aol_toi_ok) { + if (rivian_aol_token && !rivian_aol_pending) { + permission = request & 1U; + } + if (rivian_aol_longitudinal && controls_allowed && !brake_pressed && !gas_pressed) { + permission |= request & 2U; + } + } + return permission; +} + +static void rivian_aol_observe(const CANPacket_t *msg) { + if (rivian_aol_enabled) { + if (msg_matches(msg, 0x150U, 0U)) { + const uint8_t gear = msg->data[2] & 0xFU; + rivian_aol_drive = gear == 4U; + if ((gear == 0U) || (gear == 1U)) { + rivian_aol_clear(); + } + } + if (msg_matches(msg, 0x380U, 0U)) { + rivian_aol_toi_ok = (msg->data[4] & 0x28U) == 0U; + } + if (msg_matches(msg, 0x390U, 0U)) { + const uint8_t state = (msg->data[4] >> 1U) & 7U; + const uint8_t error = msg->data[4] >> 4U; + rivian_aol_eps_ok = (error == 0U) && (state <= 3U); + if (state == 4U) { + rivian_aol_clear(); + } + } + if (msg_matches(msg, 0x100U, 2U)) { + const uint8_t feature = msg->data[2] >> 5U; + if ((feature == 1U) && (rivian_aol_feature != 1U) && controls_allowed && rivian_aol_drive && + !brake_pressed && !gas_pressed && rivian_aol_eps_ok && rivian_aol_toi_ok) { + rivian_aol_token = true; + rivian_aol_pending = true; + rivian_aol_grant_ts = microsecond_timer_get(); + } + rivian_aol_feature = feature; + acc_main_on = (feature == 0U) || (feature == 1U); + if (!acc_main_on) { + rivian_aol_clear(); + } + } + } +} diff --git a/openpilot/selfdrive/car/card.py b/openpilot/selfdrive/car/card.py index f8361073d6..c168e44a3f 100644 --- a/openpilot/selfdrive/car/card.py +++ b/openpilot/selfdrive/car/card.py @@ -43,6 +43,10 @@ from opendbc.car.honda.stock_aol import ( from opendbc.car.volkswagen.stock_aol import native_observation as volkswagen_native_observation from openpilot.starpilot.car.volkswagen.aol import (qualified as qualified_volkswagen_aol, temporary_restriction as volkswagen_temporary_restriction) +from openpilot.starpilot.car.rivian.aol import retained_brake_lateral_intent as rivian_brake_intent_retained +from opendbc.car.rivian.stock_aol import (qualified as qualified_rivian_aol, + native_observation as rivian_native_observation, + temporary_restriction as rivian_temporary_restriction) from opendbc.car.mazda.stock_aol import (qualified as qualified_mazda_aol, native_observation as mazda_native_observation, temporary_restriction as mazda_temporary_restriction) @@ -588,7 +592,9 @@ class Car: if cancel_source_ns > cancel_feedback.last_cancel_source_ns: cancel_feedback.longitudinal_cancel_ns = cancel_source_ns cancel_feedback.last_cancel_source_ns = cancel_source_ns - transport_reset = not hkg_cancel_retained and self.aol_transport_feedback.observe(self.sm, self.CP, CS, now_ns, + rivian_brake_retained = rivian_brake_intent_retained(self.CP, CS, self.sm, self.aol_transport_feedback, + self.aol_card_intent, now_ns=now_ns, producer=self.slc_producer_session) + transport_reset = not (hkg_cancel_retained or rivian_brake_retained) and self.aol_transport_feedback.observe(self.sm, self.CP, CS, now_ns, latched=self.aol_card_intent.allowed_latch, cancel_source_ns=cancel_source_ns, brake_source_ns=brake_source_ns, gm_withdrawal_source_ns=gm_withdrawal_source_ns, @@ -603,7 +609,7 @@ class Car: self.sm.valid['onroadEvents'] and 0 < event_ns <= now_ns and now_ns - event_ns <= 1_500_000_000): feedback = self.aol_transport_feedback # Reuse only this call's healthy observation; fault/physical paths reobserve. - reuse_transport = bool(not transport_reset and not cancel_source_ns and not brake_source_ns and + reuse_transport = rivian_brake_retained or bool(not transport_reset and not cancel_source_ns and not brake_source_ns and not gm_withdrawal_source_ns and CS.canValid and not CS.canTimeout and not CS.steerFaultTemporary and not CS.steerFaultPermanent and not CS.accFaulted and feedback.signature is not None and feedback.signature[5] == 'none' and @@ -616,7 +622,8 @@ class Car: transport_observed_healthy=reuse_transport) angle_aol, ford_aol, honda_aol, mazda_aol, preap_aol, volkswagen_aol = self.aol_permission_owners() nissan_aol = qualified_nissan_aol(self.CP, marked_only=True) - permission_owner = angle_aol or ford_aol or honda_aol or mazda_aol or preap_aol or nissan_aol or volkswagen_aol + rivian_aol = qualified_rivian_aol(self.CP, marked_only=True) + permission_owner = angle_aol or ford_aol or honda_aol or mazda_aol or preap_aol or nissan_aol or volkswagen_aol or rivian_aol angle_panda_ready = not permission_owner or self.startup_panda_configured() native = ( current_native(self.sm, self.CP, now_ns=now_ns, axis_session_id=self.aol_axis_session() if permission_owner else None) @@ -625,10 +632,12 @@ class Car: ) native_reset = False if permission_owner: - observe = (volkswagen_native_observation if volkswagen_aol else nissan_native_observation if nissan_aol else + observe = (rivian_native_observation if rivian_aol else + volkswagen_native_observation if volkswagen_aol else nissan_native_observation if nissan_aol else preap_native_observation if preap_aol else mazda_native_observation if mazda_aol else self.aol_honda_native_observation if honda_aol else ford_native_observation if ford_aol else native_observation) - restrict = (volkswagen_temporary_restriction if volkswagen_aol else nissan_temporary_restriction if nissan_aol else + restrict = (rivian_temporary_restriction if rivian_aol else + volkswagen_temporary_restriction if volkswagen_aol else nissan_temporary_restriction if nissan_aol else preap_temporary_restriction if preap_aol else mazda_temporary_restriction if mazda_aol else honda_temporary_restriction if honda_aol else ford_temporary_restriction if ford_aol else temporary_restriction) preap_observation = {} @@ -643,7 +652,7 @@ class Car: latched=self.aol_card_intent.allowed_latch, panda_ready=angle_panda_ready, restricted=(restrict(self.CP, CS) or - ((angle_aol or mazda_aol or preap_aol) and self.aol_card_intent.pause_lateral) or + ((angle_aol or mazda_aol or preap_aol or rivian_aol) and self.aol_card_intent.pause_lateral) or (angle_aol and CS.brakePressed and (not CS.standstill) and (CS.vEgo < self.aol_settings.pause_brake_mps)) or (volkswagen_aol and (self.aol_card_intent.pause_lateral or (CS.brakePressed and (not CS.standstill) and (CS.vEgo < self.aol_card_intent.settings.pause_brake_mps))))), diff --git a/openpilot/selfdrive/pandad/pandad.cc b/openpilot/selfdrive/pandad/pandad.cc index 8b03af8e94..3e7d1352a4 100644 --- a/openpilot/selfdrive/pandad/pandad.cc +++ b/openpilot/selfdrive/pandad/pandad.cc @@ -5,6 +5,7 @@ #include "selfdrive/pandad/aol_wire.h" #include "openpilot/starpilot/car/honda/aol_policy.h" #include "openpilot/starpilot/car/mazda/aol_policy.h" +#include "openpilot/starpilot/car/rivian/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" @@ -479,7 +480,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, SUBARU_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, RIVIAN_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/starpilot/aol/transport_pause.py b/openpilot/starpilot/aol/transport_pause.py index 85c322d14a..658ece3892 100644 --- a/openpilot/starpilot/aol/transport_pause.py +++ b/openpilot/starpilot/aol/transport_pause.py @@ -375,6 +375,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.rivian.stock_aol import qualified as rivian_qualified from opendbc.car.mazda.stock_aol import qualified as mazda_qualified from opendbc.car.subaru.stock_aol import qualified as subaru_qualified from opendbc.car.hyundai.canfd_angle_aol import qualified as angle_qualified @@ -391,7 +392,7 @@ class TransportPauseFeedback: 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) - or subaru_qualified(cp, marked_only=True) or mazda_qualified(cp, marked_only=True)) and users and + or subaru_qualified(cp, marked_only=True) or mazda_qualified(cp, marked_only=True) or rivian_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 @@ -645,7 +646,11 @@ class TransportPauseFeedback: not cs.vehicleSensorsInvalid and not cs.steeringDisengage and not cs.buttonEvents and all(not event.userDisable or event.name == log.OnroadEvent.EventName.pcmDisable and not event.softDisable and not event.immediateDisable for event in events)) - restricted_eps = bool((temporary_eps_restriction(cp, cs) or stock_ford_eps or stock_mazda_eps) and self.lateral_only_baseline and + from opendbc.car.rivian.stock_aol import temporary_eps_restriction as rivian_temporary_eps_restriction + stock_rivian_eps = bool(rivian_temporary_eps_restriction(cp, cs) and + all(not event.userDisable or event.name == log.OnroadEvent.EventName.pcmDisable and + not event.softDisable and not event.immediateDisable for event in events)) + restricted_eps = bool((temporary_eps_restriction(cp, cs) or stock_ford_eps or stock_mazda_eps or stock_rivian_eps) and self.lateral_only_baseline and not axis.desiredLongitudinal and not axis.longitudinalActive) native = _configured_transport_native(sm, cp, self.session, now_ns, allow_restricted_lateral=restricted_eps) if (native is None or not native.pandaInventory or @@ -1202,6 +1207,14 @@ class TransportPauseFeedback: qualified_cp=mazda_qualified(cp, marked_only=True), informational_events=(log.OnroadEvent.EventName.userBookmarkNotPaired,), allow_withdrawal_join=True): return False + if cp.brand == 'rivian' and current and continuity: + from opendbc.car.rivian.stock_aol import qualified as rivian_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=rivian_qualified(cp, marked_only=True), + informational_events=(log.OnroadEvent.EventName.userBookmarkNotPaired,), allow_withdrawal_join=True): + return False if cp.brand == 'nissan' and current and continuity: from openpilot.starpilot.car.nissan.aol import qualified as nissan_qualified from openpilot.starpilot.aol.reverse_intent import retain_reverse_intent diff --git a/openpilot/starpilot/aol/vehicle.py b/openpilot/starpilot/aol/vehicle.py index 6d2b0d0da8..4442a64b06 100644 --- a/openpilot/starpilot/aol/vehicle.py +++ b/openpilot/starpilot/aol/vehicle.py @@ -16,11 +16,12 @@ 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.rivian import aol as rivian 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, 'subaru': subaru} + 'tesla': tesla, 'toyota': toyota, 'chrysler': chrysler, 'nissan': nissan, 'volkswagen': volkswagen, 'subaru': subaru, 'rivian': rivian} _POLICY_CACHE_SIZE = 8 _POLICY_CACHE_MAX_BYTES = 64 * 1024 diff --git a/openpilot/starpilot/car/rivian/__init__.py b/openpilot/starpilot/car/rivian/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/starpilot/car/rivian/aol.py b/openpilot/starpilot/car/rivian/aol.py new file mode 100644 index 0000000000..9344d3e1f2 --- /dev/null +++ b/openpilot/starpilot/car/rivian/aol.py @@ -0,0 +1,136 @@ +"""Rivian BASE GEN1: successful ordinary engagement owns lateral intent.""" +from opendbc.car import structs +from opendbc.car.rivian.stock_aol import qualified, temporary_restriction +from openpilot.starpilot.aol.policy import AolVehiclePolicy +from openpilot.starpilot.aol.intent import AolCardIntent, AOL_TOGGLE, ButtonType +from openpilot.selfdrive.car.cruise import CRUISE_LONG_PRESS + + +def policy_for(cp): + admitted = qualified(cp) + return AolVehiclePolicy(intent_supported=admitted, settings_supported=admitted, runtime_supported=admitted, + normal_runtime_supported=admitted, ordinary_axis_ack_required=qualified(cp, marked_only=True), + explicit_latch=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.rivian) and param in (0, 1) + + +def native_accepts_cp(cp, model, param): + return qualified(cp, marked_only=True) and native_profile_supported(model, param) and param == cp.safetyConfigs[0].safetyParam + + +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 + + +class RivianStockCardIntent(AolCardIntent): + observe_stock_engagement = True + + def __init__(self, settings): + super().__init__(settings, explicit_latch=True, lkas_button_supported=False) + self.cancel_held = False + self.cancel_ticks = 0 + + def _cancel_action(self, action): + if action == AOL_TOGGLE and self.settings.enabled: + if self.allowed_latch: + self._record_latch_clear('configured_cancel_toggle') + self.allowed_latch = not self.allowed_latch + self.pause_lateral = not self.allowed_latch + if not self.allowed_latch: + self._withdraw_full_engagement() + else: + self._perform(action) + + def update(self, CS, **kwargs): + # The stalk UP_1 producer owns these actions. No availability/Main edge + # is converted into a button, and no received cancel echo can arm a session. + state = structs.CarState(**CS.to_dict()) + state.buttonEvents = [structs.CarState.ButtonEvent(**event.to_dict()) for event in CS.buttonEvents + if event.type != ButtonType.cancel] + for event in CS.buttonEvents: + if event.type == ButtonType.cancel: + if event.pressed: + self.cancel_held, self.cancel_ticks = True, 0 + if not any(self.settings.cancel_actions): + self._record_latch_clear('physical_cancel') + self.allowed_latch = False + self._withdraw_full_engagement() + elif self.cancel_held: + if 0 < self.cancel_ticks < CRUISE_LONG_PRESS: + self._cancel_action(self.settings.cancel_actions[0]) + self.cancel_held, self.cancel_ticks = False, 0 + if not CS.canValid or CS.canTimeout: + self.cancel_held, self.cancel_ticks = False, 0 + super().update(state.as_reader(), **kwargs) + if self.cancel_held: + self.cancel_ticks += 1 + if self.cancel_ticks == CRUISE_LONG_PRESS: + self._cancel_action(self.settings.cancel_actions[1]) + elif self.cancel_ticks == CRUISE_LONG_PRESS * 5: + self._cancel_action(self.settings.cancel_actions[2]) + + + +def create_intent(cp, settings): + return RivianStockCardIntent(settings) + + +def retained_brake_lateral_intent(cp, cs, sm, feedback, owner, *, now_ns, producer): + """Join one native-first FULL -> LAT brake occurrence; preserve intent only.""" + from openpilot.cereal import log + from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS, SAFETY_MAX_AGE_NS + from openpilot.starpilot.aol.transport_pause import cp_identity, independent_lateral_health + try: + if (not qualified(cp, marked_only=True) or not cp.openpilotLongitudinalControl or + owner is None or not owner.allowed_latch or not owner.settings.enabled or owner.pause_lateral or + feedback.cp != cp_identity(cp) or feedback.failed or feedback.paused or feedback.pending_event or + feedback.retention_started_ns or feedback.source_retention_started_ns or + not feedback.session or not producer or producer != feedback.availability_producer or + not cs.canValid or cs.canTimeout or not cs.brakePressed or cs.gasPressed or cs.regenBraking or + not cs.cruiseState.available or cs.steerFaultTemporary or cs.steerFaultPermanent or + cs.accFaulted or cs.doorOpen or cs.seatbeltUnlatched or cs.steeringDisengage or cs.buttonEvents or + cs.gearShifter != structs.CarState.GearShifter.drive): + return False + axis = sm['aolAxisState'] + stamp = int(sm.logMonoTime['aolAxisState']) + previous = feedback.signature + if (not sm.seen['aolAxisState'] or not sm.valid['aolAxisState'] or not sm.alive['aolAxisState'] or + not axis.qualified or axis.sessionId != feedback.session or axis.faultSessionId != feedback.session or + str(axis.faultReason) != 'none' or str(axis.mode) != 'combined' or not axis.nativeAcknowledged or + not axis.lateralActive or not axis.longitudinalActive or not axis.desiredLateral or not axis.desiredLongitudinal or + axis.intentProducerSessionId != producer or axis.intentIntegrityEpoch != feedback.availability_epoch or + not 0 < axis.sourceCarStateMonoTime <= stamp == axis.observedMonoTime < now_ns <= axis.validUntilMonoTime or + now_ns - axis.sourceCarStateMonoTime > AXIS_MAX_AGE_NS or + axis.validUntilMonoTime - stamp > AXIS_MAX_AGE_NS or + previous is None or previous[0] != feedback.session or previous[5] != 'none' or previous[7] != feedback.session or + not all(previous[index] for index in (9, 10, 11, 12, 13)) or + previous[14] != producer or previous[15] != feedback.availability_epoch or + axis.sequence < feedback.sequence or + not 0 < feedback.healthy_feedback_ns <= now_ns or now_ns - feedback.healthy_feedback_ns > AXIS_MAX_AGE_NS): + return False + native = independent_lateral_health(sm, cp, cs, feedback.session, now_ns) + if (native is None or not native.pandaInventory or not native.requestedLongitudinal or native.longitudinalAllowed or + native.observedMonoTime < stamp or now_ns - native.observedMonoTime > SAFETY_MAX_AGE_NS or + (native.pandaSerial, tuple(p.hardwareSerial for p in native.pandaInventory)) != feedback.healthy_inventory or + not all(not p.controlsAllowed for p in native.pandaInventory)): + return False + event_ns = int(sm.logMonoTime['onroadEvents']) + if (not sm.seen['onroadEvents'] or not sm.valid['onroadEvents'] or not sm.alive['onroadEvents'] or + not 0 < event_ns <= now_ns or now_ns - event_ns > 1_500_000_000): + return False + return all(not e.immediateDisable and not e.softDisable and + (not e.userDisable or e.name in (log.OnroadEvent.EventName.pedalPressed, log.OnroadEvent.EventName.pcmDisable)) + for e in sm['onroadEvents']) + except (AttributeError, KeyError, IndexError, TypeError, ValueError, OverflowError): + return False diff --git a/openpilot/starpilot/car/rivian/aol_policy.h b/openpilot/starpilot/car/rivian/aol_policy.h new file mode 100644 index 0000000000..cd635b528f --- /dev/null +++ b/openpilot/starpilot/car/rivian/aol_policy.h @@ -0,0 +1,10 @@ +#pragma once + +#include "openpilot/selfdrive/pandad/aol_protocol.h" + +inline bool rivian_aol_param(uint16_t param) { + return (param == 0U) || (param == 1U); +} + +// Native capability is additionally required. RELEASE param1 advertises none. +inline constexpr AolSafetyProfile RIVIAN_AOL_PROFILE{33U, rivian_aol_param, true}; diff --git a/openpilot/starpilot/car/rivian/tests/__init__.py b/openpilot/starpilot/car/rivian/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/starpilot/car/rivian/tests/test_aol_lifecycle.py b/openpilot/starpilot/car/rivian/tests/test_aol_lifecycle.py new file mode 100644 index 0000000000..655c276c0d --- /dev/null +++ b/openpilot/starpilot/car/rivian/tests/test_aol_lifecycle.py @@ -0,0 +1,367 @@ +"""Synthetic acquisition only; actual registered factory/Card/SD/Controls/native CAN chain.""" + +import json +import os +import threading +from collections import defaultdict +from contextlib import ExitStack +from pathlib import Path +from unittest.mock import patch + +import pytest +from opendbc.can import CANPacker +from opendbc.car import gen_empty_fingerprint, structs +from opendbc.car.car_helpers import interfaces +from opendbc.car.rivian.values import CAR +from opendbc.safety.tests.libsafety import libsafety_py + + +class Chassis: + """Actual DBC packing. Native checked rows use declared 50/100 Hz cadence. + + Unchecked ancillary snapshots are synthesized at 100 Hz, as in the frozen + parser fixture; this does not claim a recorded physical bus schedule. + """ + + def __init__(self): + self.packer = CANPacker('rivian_primary_actuator') + self.counters = defaultdict(int) + + def frames(self, tick, *, ack=False, fault=False, unavailable=False, gear=4, gas=False, feature=0, cancel=False, speed=18.0): + rows = [ + ('ESP_Status', 0, 2, 'ESP_Status_Counter', {'ESP_Vehicle_Speed': speed * 3.6, 'ESP_Vehicle_Speed_Q': 1}), + ( + 'VDM_PropStatus', + 0, + 2, + 'VDM_PropStatus_Counter', + {'VDM_VehicleSpeed': speed * 3.6, 'VDM_VehicleSpeedQ': 1, 'VDM_Prndl_Status': gear, 'VDM_AcceleratorPedalPosition': 1 if gas else 0}, + ), + ('iBESP2', 0, 2, 'iBESP2_AliveCounter', {'iBESP2_BrakePedalApplied': False, 'iBESP2_BrakePedalApplied_Q': 1}), + ( + 'EPAS_SystemStatus', + 0, + 1, + 'EPAS_SystemStatus_Counter', + {'EPAS_TorsionBarTorque': 0.0, 'H_CAN_EPSS_ToiActive': ack, 'H_CAN_EPSS_ToiFlt': fault, 'H_CAN_EPS_ToiUnavailable': unavailable}, + ), + ('EPAS_AdasStatus', 0, 1, 'EPAS_AdasStatus_Counter', {'EPAS_InternalSas': 1.0, 'EPAS_EacErrorCode': 0, 'EPAS_EacStatus': 1}), + ('VDM_AdasSts', 0, 1, 'VDM_AdasStatus_Counter', {'VDM_UserAdasRequest': int(cancel)}), + ('RCM_Status', 0, 1, None, {'RCM_Status_IND_WARN_BELT_DRIVER': 0}), + ('ACM_Status', 2, 1, 'ACM_Status_Counter', {'ACM_FeatureStatus': feature}), + ('ACM_lkaHbaCmd', 2, 1, 'ACM_lkaHbaCmd_Counter', {}), + ('ACM_AebRequest', 2, 1, None, {}), + ('Cluster', 1, 1, None, {'Cluster_Unit': 0, 'Cluster_VehicleSpeed': speed * 3.6}), + ('IndicatorLights', 1, 1, None, dict.fromkeys(('RearDriverDoor', 'FrontPassengerDoor', 'DriverDoor', 'RearPassengerDoor'), 2)), + ('SCCM_WheelTouch', 0, 1, 'SCCM_WheelTouch_Counter', {}), + ] + result = [] + for name, bus, period, counter, values in rows: + if (tick - 1) % period: + continue + if counter: + values[counter] = self.counters[name] % 15 + self.counters[name] += 1 + result.append(self.packer.make_can_msg(name, bus, values)) + return result + + +@pytest.mark.parametrize('alpha', [False, True], ids=['stock', 'debug-alpha']) +def test_actual_rivian_aol_engage_brake_reverse_fault_recovery(alpha, monkeypatch, tmp_path): + from opendbc.safety.tests.common import make_msg + from openpilot.cereal import messaging + from openpilot.common.params import Params + from openpilot.common.prefix import OpenpilotPrefix + from openpilot.selfdrive.car import card as card_module + from openpilot.selfdrive.car.card import Car + from openpilot.starpilot.aol.tests.test_ioniq_availability import ( + IoniqAvailabilityTests, + ) + from openpilot.starpilot.aol.wire import ( + PandaSlot, + SafetyState, + decode_intent, + encode_safety, + ) + from openpilot.starpilot.lateral.learning_preferences import KEY, replace_preference + + original_environment = {key: os.environ.get(key) for key in ('REPLAY', 'SIMULATION')} + monkeypatch.setenv('REPLAY', '1') + monkeypatch.setenv('SIMULATION', '0') + monkeypatch.setattr(card_module, 'REPLAY', False) + trace, previous_sd, received, queued = [], {}, {}, [] + now, tick = [10_000_000_000], 0 + with OpenpilotPrefix(shared_download_cache=True): + saved = Params() + for key, value in [('OpenpilotEnabledToggle', True), ('AlwaysOnLateral', True), ('AlphaLongitudinalEnabled', alpha), ('IsReleaseBranch', False)]: + saved.put_bool(key, value, block=True) + fp = gen_empty_fingerprint() + fp[0][0x321] = 7 + factory = interfaces[CAR.RIVIAN_R1] + cp = factory.get_params(CAR.RIVIAN_R1, fp, [], alpha, False, False) + saved.put(KEY, replace_preference(None, cp, False), block=True) + card = Car(CI=factory(cp), RI=factory.RadarInterface(cp)) + assert card.CP.flags == 0 and card.CP.alternativeExperience == 32 + assert card.aol_qualified and card.aol_card_intent is not None + assert not card.ci_initialized and not Path(saved.get_param_path('ControlsReady')).exists() + card.CI.update([]) + case = IoniqAvailabilityTests(methodName='runTest') + io_contexts = ExitStack() + safety = libsafety_py.libsafety + try: + sd, controls, drive = case._authority_callers(False, cp=card.CP.as_reader().as_builder(), genuine_events=True) + assert sd.CP.to_dict() == card.CP.to_dict() == controls.CP.to_dict() + # Exercise live Controls decision time; replay source-time selection can + # round Card's float-produced source stamp one ns before exact axis time. + monkeypatch.setenv('REPLAY', '0') + safety.init_tests() + safety.set_alternative_experience(32) + assert safety.set_safety_hooks(structs.CarParams.SafetyModel.rivian, int(alpha)) == 0 + monkeypatch.setattr(card_module.time, 'monotonic_ns', lambda: now[0]) + monkeypatch.setattr(card_module.time, 'monotonic', lambda: now[0] / 1e9) + monkeypatch.setattr(card_module.time, 'clock_gettime_ns', lambda clock: now[0]) + monkeypatch.setattr(card_module.messaging, 'drain_sock_raw', lambda *a, **kw: list(queued)) + io_contexts.enter_context(patch.object(card.sm, 'update', lambda *a: None)) + # The reused SD helper owns a clock mock too. Bind each NEW producer event + # to this acquisition clock, as the qualified Mazda full-caller harness + # does, while held messages keep their original serialized source stamp. + create_event = messaging.new_message + def new_event(service=None, size=None, **kwargs): + kwargs.setdefault('logMonoTime', now[0]) + return create_event(service, size, **kwargs) + monkeypatch.setattr(messaging, 'new_message', new_event) + + send = card.pm.send + + def capture(service, event): + raw = event if isinstance(event, bytes) else event.to_bytes() + received[service] = messaging.log_from_bytes(raw).as_builder() + send(service, raw) + + io_contexts.enter_context(patch.object(card.pm, 'send', capture)) + chassis = Chassis() + ready_observed = False + metadata = {} + + def step(*, feature=0, brake=False, gas=False, gear=4, fault=False, cancel=False, speed=18.0): + nonlocal tick, ready_observed + tick += 1 + now[0] = 10_000_000_000 + tick * 10_000_000 + frames = chassis.frames(tick, feature=feature, ack=True, gas=gas, gear=gear, fault=fault, cancel=cancel, speed=speed) + # Brake is a physical independently checksummed chassis source. + if (tick - 1) % 2 == 0 and brake: + frames = [(a, d, b) for a, d, b in frames if a != 0x38F] + frames.append( + chassis.packer.make_can_msg( + 'iBESP2', 0, {'iBESP2_AliveCounter': (chassis.counters['iBESP2'] - 1) % 15, 'iBESP2_BrakePedalApplied': True, 'iBESP2_BrakePedalApplied_Q': 1} + ) + ) + safety.set_timer(now[0] // 1000) + prior_axis = previous_sd.get('aolAxisState') + prior_state = previous_sd.get('selfdriveState') + lat = bool(prior_axis is not None and prior_axis.aolAxisState.desiredLateral) + long = bool(prior_axis is not None and prior_axis.aolAxisState.desiredLongitudinal) + heartbeat = lat or long or bool(prior_state is not None and prior_state.selfdriveState.enabled) + prior_intent = received.get('aolIntentWire') + armed = decode_intent(prior_intent.aolIntentWire) if prior_intent is not None else None + # Match pandad's retained-arm heartbeat input from the original Card wire. + # This conveys no actuator request and cannot arm a native token. + if prior_axis is not None and armed is not None: + axis = prior_axis.aolAxisState + stamp = int(prior_intent.logMonoTime) + retained = bool(armed.settingsQualified and armed.lateralArmed and + armed.producerSessionId == axis.intentProducerSessionId and + armed.carStateLogMonoTime == stamp == armed.observedMonoTime and + 0 < stamp <= now[0] <= armed.validUntilMonoTime and now[0] - stamp <= 30_000_000 and + abs(stamp - axis.sourceCarStateMonoTime) <= 30_000_000) + heartbeat |= retained + + safety.set_aol_test_heartbeat(heartbeat) + for a, d, b in frames: + assert safety.safety_rx_hook(make_msg(b, a, dat=d)) + safety.safety_tick() + safety.aol_set_host_request(int(lat) | (int(long) << 1)) + request = safety.aol_get_request_mask() + permission = safety.aol_get_permission_mask() + events = [] + if (tick - 1) % 50 == 0: + device = messaging.new_message('deviceState', valid=True, logMonoTime=now[0]) + device.deviceState.started = True + device.deviceState.startedMonoTime = 9_000_000_000 + events.append(device) + if (tick - 1) % 10 == 0: + metadata['panda'] = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=now[0]) + panda = metadata['panda'].pandaStates[0] + panda.safetyModel = card.CP.safetyConfigs[0].safetyModel + panda.safetyParam = int(alpha) + panda.alternativeExperience = 32 + panda.controlsAllowed = safety.get_controls_allowed() + panda.safetyRxChecksInvalid = not safety.safety_config_valid() + events.append(metadata['panda']) + if 'onroadEvents' not in previous_sd: + events.append(messaging.new_message('onroadEvents', 0, valid=True, logMonoTime=now[0])) + for service in ('selfdriveState', 'aolAxisState', 'onroadEvents'): + if service in previous_sd: + events.append(previous_sd[service].as_builder()) + wire = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=now[0]) + wire.aolSafetyWire = encode_safety( + SafetyState( + 1, + True, + now[0], + now[0] + 30_000_000, + int(card.CP.safetyConfigs[0].safetyModel.raw), + int(alpha), + bool(permission & 1), + bool(permission & 2), + bool(request & 1), + bool(request & 2), + 'rivian-native-test', + card.aol_axis_session(), + now[0], + (PandaSlot(0, 'rivian-native-test', 33, int(alpha), 32, safety.get_controls_allowed(), not safety.safety_config_valid(), False, 0),), + ) + ) + events.append(wire) + card.sm.update_msgs(now[0] / 1e9, [e.as_reader() for e in events]) + can_source = messaging.log_from_bytes(card_module.can_list_to_can_capnp(frames)).as_builder() + can_source.logMonoTime = now[0] # synthetic CAN producer's original acquisition time + queued[:] = [can_source.to_bytes()] + state, _ = card.state_update(defer_radar=True) + card.state_publish(state, None) + axis, host, command = drive( + tick, + car_state_message=received['carState'], + car_output_message=received['carOutput'], + authority_message=received.get('starpilotCarState'), + intent_message=received['aolIntentWire'], + allowed=bool(permission & 1), + long_allowed=bool(permission & 2), + native_request_lat=bool(request & 1), + native_request_long=bool(request & 2), + panda_controls_allowed=safety.get_controls_allowed(), + panda_state_message=metadata['panda'], + ) + previous_sd.update(sd.pm.messages) + # Later receiver instant; original Controls source stamp is preserved. + now[0] += 1 + card.sm.update_msgs(now[0] / 1e9, [controls.pm.messages['carControl']]) + received.pop('sendcan', None) + card.controls_update(state, card.sm['carControl']) + if not ready_observed and card.ci_initialized: + done = threading.Event() + values = [] + + def observe(): + values.append(saved.get_bool('ControlsReady', block=True)) + done.set() + + reader = threading.Thread(target=observe, daemon=True) + reader.start() + assert done.wait(2) + reader.join(2) + assert values == [True] and not reader.is_alive() + ready_observed = True + tx = [] + if 'sendcan' in received: + for frame in received['sendcan'].sendcan: + tx.append(bool(safety.safety_tx_hook(make_msg(frame.src, frame.address, dat=frame.dat)))) + assert all(tx) + row = { + 'tick': tick, + 'feature': feature, + 'gear': str(state.gearShifter), + 'brake': state.brakePressed, + 'temporary': state.steerFaultTemporary, + 'native_allowed': safety.get_controls_allowed(), + 'request': request, + 'permission': permission, + 'latch': card.aol_card_intent.allowed_latch, + 'latch_reason': card.aol_card_intent.latch_clear_reason, + 'sd_axis': axis.to_dict(), + 'events': [e.to_dict() for e in card.sm['onroadEvents']], + 'host_enabled': host.enabled, + 'axis_lat': axis.lateralActive, + 'cc_lat': command.latActive, + 'cc_long': command.longActive, + 'torque': float(card.last_actuators_output.torqueOutputCan), + 'tx': tx, + 'cs_ns': int(controls.sm.logMonoTime['carState']), + 'axis_ns': int(controls.sm.logMonoTime['aolAxisState']), + 'decision_ns': now[0] - 1, + 'axis_ack': bool(axis.nativeAcknowledged), + 'native_witness': controls.flm_native_witness is not None, + } + trace.append(row) + return row + + for _ in range(120): + step() + assert not card.aol_card_intent.allowed_latch + for _ in range(35): + row = step(feature=1) + assert row['latch'] and row['host_enabled'] and row['cc_lat'] and abs(row['torque']) > 0 + # Brake is independent 50 Hz CAN. Observe its actual parsed press before + # dropping the 100 Hz ACM feature, avoiding a synthetic cancel-before-brake. + for _ in range(2): + row = step(feature=1, brake=True) + for _ in range(18): + row = step(brake=True) + assert not row['native_allowed'] and row['permission'] == 1 + assert row['latch'] and row['axis_lat'] and row['cc_lat'] and not row['cc_long'] + # Exercise the genuine release/Drive producer interval before a separate + # Reverse maneuver; no stale pedal USER event is reclassified as gear. + for _ in range(110): + row = step() + assert row['latch'] and row['cc_lat'] and not row['cc_long'] + # An accelerator press cannot reactivate the withdrawn longitudinal axis. + for _ in range(10): + row = step(gas=True) + assert row['latch'] and row['permission'] == 1 and row['cc_lat'] and not row['cc_long'] + for _ in range(12): + row = step() + assert row['latch'] and row['cc_lat'] and not row['cc_long'] + for _ in range(25): + row = step(speed=0.0) + assert row['latch'] and row['permission'] == 1 and not row['cc_lat'] and not row['cc_long'] and row['torque'] == 0 + for _ in range(12): + row = step() + assert row['latch'] and row['permission'] == 1 and row['cc_lat'] and not row['cc_long'] + for _ in range(4): + row = step(gear=2) + assert row['latch'] and not row['cc_lat'] and row['permission'] == 0 and row['torque'] == 0 + for _ in range(12): + row = step() + assert row['latch'] and row['permission'] == 1 and row['cc_lat'] and not row['cc_long'] + for _ in range(4): + row = step(fault=True) + assert row['temporary'] and row['latch'] and row['permission'] == 0 and row['torque'] == 0 + for _ in range(15): + row = step() + assert row['latch'] and row['permission'] == 1 and row['cc_lat'] and abs(row['torque']) > 0 + assert not row['cc_long'] + # Default physical UP_1 cancels the retained session. Availability alone + # cannot manufacture a new engagement after that explicit action. + for _ in range(3): + row = step(cancel=True) + assert not row['latch'] and not row['cc_lat'] and not row['cc_long'] and row['torque'] == 0 + for _ in range(8): + row = step() + assert not row['latch'] and not row['cc_lat'] and row['permission'] == 0 + assert ready_observed + finally: + tmp_path.joinpath('aol-' + str(alpha) + '.json').write_text(json.dumps(trace, indent=2) + '\n') + safety.init_tests() + safety.set_alternative_experience(0) + safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0) + io_contexts.close() + monkeypatch.undo() + case.doCleanups() + card.vehicle_startup.close() + # The helper restores its own environment snapshot after clock cleanup. + # Preserve the surrounding suite's original replay/simulation settings. + for key, value in original_environment.items(): + if value is None: + os.environ.pop(key, None) + else: + os.environ[key] = value diff --git a/openpilot/starpilot/car/rivian/tests/test_aol_native.py b/openpilot/starpilot/car/rivian/tests/test_aol_native.py new file mode 100644 index 0000000000..8421e2e5cd --- /dev/null +++ b/openpilot/starpilot/car/rivian/tests/test_aol_native.py @@ -0,0 +1,101 @@ +"""Native permission tests: physical DBC RX grants, never controls_allowed setters.""" + +import pytest +from opendbc.car import structs +from opendbc.car.rivian.tests.test_base_driving import ControllerRig +from opendbc.safety.tests.libsafety import libsafety_py + + +class NativeRig: + def __init__(self, *, alpha=False, alternative=32): + self.car = ControllerRig(alpha=alpha) + self.native = libsafety_py.libsafety + self.native.init_tests() + self.native.set_alternative_experience(alternative) + assert self.native.set_safety_hooks(structs.CarParams.SafetyModel.rivian, int(alpha)) == 0 + self.native.set_aol_test_heartbeat(True) + self.tick = 0 + for _ in range(3): + self.feed(feature=0) + + def feed(self, *, request=1, **kwargs): + self.tick += 1 + self.native.set_timer(1_000_000 + self.tick * 10_000) + self.car.feed(**kwargs) + for address, data, bus in self.car.last_frames: + assert self.native.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data)) + self.native.safety_tick() + self.native.aol_set_host_request(request) + return self.native.aol_get_permission_mask() + + def engage(self): + assert self.feed(feature=1) == 0 # grant and request same instant cannot join + assert self.feed(feature=1) == 1 + assert self.native.get_controls_allowed() + + +@pytest.mark.parametrize('alpha', [False, True]) +def test_native_real_engagement_brake_reverse_and_recovery(alpha): + rig = NativeRig(alpha=alpha) + assert rig.native.safety_config_valid() + assert rig.native.aol_get_permission_mask() == 0 # Standby is not a grant. + rig.engage() + assert rig.feed(feature=1, request=3) == (3 if alpha else 1) + assert rig.feed(brake=True, feature=0) == 1 + assert not rig.native.get_controls_allowed() + assert rig.feed(gear=2, feature=0) == 0 + assert rig.feed(gear=4, feature=0) == 1 + assert rig.feed(fault=True, feature=0) == 0 + assert rig.feed(feature=0) == 1 + assert rig.feed(unavailable=True, feature=0) == 0 + assert rig.feed(feature=0) == 1 + assert rig.feed(feature=0, request=0) == 0 + assert rig.feed(feature=0, request=1) == 1 + rig.native.set_aol_test_heartbeat(False) + assert rig.native.aol_get_permission_mask() == 0 + rig.native.set_aol_test_heartbeat(True) + assert rig.feed(feature=0, request=1) == 0 + rig.engage() + + +@pytest.mark.parametrize('cause', ['request_lease', 'heartbeat', 'corrupt', 'park', 'faulted_acm']) +def test_native_source_or_session_loss_needs_new_engagement(cause): + rig = NativeRig() + rig.engage() + if cause == 'request_lease': + rig.native.set_timer(1_000_000 + rig.tick * 10_000 + 200_001) + assert rig.native.aol_get_permission_mask() == 0 + rig.tick += 21 # next source remains later than the expired observation + elif cause == 'heartbeat': + rig.native.set_aol_test_heartbeat(False) + assert rig.native.aol_get_permission_mask() == 0 + rig.native.set_aol_test_heartbeat(True) + elif cause == 'corrupt': + address, data, bus = next(m for m in rig.car.last_frames if m[0] == 0x380) + data = bytes([data[0] ^ 0x80]) + data[1:] + assert not rig.native.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data)) + assert rig.native.aol_get_permission_mask() == 0 + elif cause == 'park': + assert rig.feed(gear=1, feature=0) == 0 + else: + assert rig.feed(feature=4) == 0 + assert rig.feed(feature=0) == 0 + rig.engage() + + +def test_native_zero_alternative_does_not_retain_lateral_after_brake(): + rig = NativeRig(alternative=0) + rig.feed(feature=1) + assert rig.native.get_controls_allowed() + rig.feed(feature=0, brake=True) + assert not rig.native.get_controls_allowed() + assert not rig.native.lateral_controls_allowed() + + +def test_native_long_axis_not_inherited_from_lateral_token(): + rig = NativeRig(alpha=True) + rig.engage() + rig.feed(feature=0, brake=True) + assert rig.feed(request=3, feature=0) == 1 + address, data, bus = rig.car.packer.make_can_msg('ACM_longitudinalRequest', 0, {'ACM_AccelerationRequest': 0.1}) + assert not rig.native.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)) diff --git a/openpilot/starpilot/car/rivian/tests/test_aol_policy.py b/openpilot/starpilot/car/rivian/tests/test_aol_policy.py new file mode 100644 index 0000000000..47110f0501 --- /dev/null +++ b/openpilot/starpilot/car/rivian/tests/test_aol_policy.py @@ -0,0 +1,74 @@ +import pytest +from opendbc.car import gen_empty_fingerprint, structs +from opendbc.car.car_helpers import interfaces +from opendbc.car.rivian.stock_aol import qualified +from opendbc.car.rivian.values import CAR +from openpilot.starpilot.aol.vehicle import policy_for + + +@pytest.mark.parametrize('alpha', [False, True]) +def test_actual_registered_gen1_owner(alpha): + fp = gen_empty_fingerprint() + fp[0][0x321] = 7 + cp = interfaces[CAR.RIVIAN_R1].get_params(CAR.RIVIAN_R1, fp, [], alpha, False, False) + assert qualified(cp) + assert policy_for(cp).intent_supported + assert cp.openpilotLongitudinalControl == alpha + assert cp.safetyConfigs[0].safetyParam == int(alpha) + + +@pytest.mark.parametrize('mutation', ['gen2', 'fixed', 'passive', 'not_car', 'dashcam', 'multi_panda', 'unknown_param', 'unknown_flag']) +def test_family_owner_rejects_unsupported_configuration(mutation): + fp = gen_empty_fingerprint() + if mutation != 'gen2': + fp[0][0x321] = 7 + cp = interfaces[CAR.RIVIAN_R1].get_params(CAR.RIVIAN_R1, fp, [], False, False, False) + if mutation == 'fixed': + cp.fingerprintSource = structs.CarParams.FingerprintSource.fixed + elif mutation == 'passive': + cp.passive = True + elif mutation == 'not_car': + cp.notCar = True + elif mutation == 'dashcam': + cp.dashcamOnly = True + elif mutation == 'multi_panda': + cp.safetyConfigs = [cp.safetyConfigs[0], cp.safetyConfigs[0]] + elif mutation == 'unknown_param': + cp.safetyConfigs[0].safetyParam = 4 + elif mutation == 'unknown_flag': + cp.flags = 2 + assert not qualified(cp) + assert not policy_for(cp).intent_supported + + +@pytest.mark.parametrize('mutation', [ + 'none', 'gen2', 'permanent', 'brake', 'regen', 'gas', 'door', 'belt', 'acc', 'sensors', + 'steering', 'button', 'enabled', 'unavailable', 'reverse', 'invalid', +]) +def test_temporary_eps_registration_preserves_physical_disarms(mutation): + from opendbc.car.rivian.stock_aol import temporary_eps_restriction + + fp = gen_empty_fingerprint() + fp[0][0x321] = 7 + cp = interfaces[CAR.RIVIAN_R1].get_params(CAR.RIVIAN_R1, fp, [], False, False, False) + cp.alternativeExperience = 32 + cs = structs.CarState(canValid=True, steerFaultTemporary=True, gearShifter=structs.CarState.GearShifter.drive, + cruiseState=structs.CarState.CruiseState(available=True)) + names = {'permanent': 'steerFaultPermanent', 'brake': 'brakePressed', 'regen': 'regenBraking', + 'gas': 'gasPressed', 'door': 'doorOpen', 'belt': 'seatbeltUnlatched', 'acc': 'accFaulted', + 'sensors': 'vehicleSensorsInvalid', 'steering': 'steeringDisengage'} + if mutation in names: + setattr(cs, names[mutation], True) + elif mutation == 'gen2': + cp.flags = 1 + elif mutation == 'button': + cs.buttonEvents = [structs.CarState.ButtonEvent(type=structs.CarState.ButtonEvent.Type.cancel, pressed=True)] + elif mutation == 'enabled': + cs.cruiseState.enabled = True + elif mutation == 'unavailable': + cs.cruiseState.available = False + elif mutation == 'reverse': + cs.gearShifter = structs.CarState.GearShifter.reverse + elif mutation == 'invalid': + cs.canValid = False + assert temporary_eps_restriction(cp, cs) == (mutation == 'none') diff --git a/tools/ci/run_vehicle_tests.py b/tools/ci/run_vehicle_tests.py index 0ebe3a5cbb..4f17f9f7de 100644 --- a/tools/ci/run_vehicle_tests.py +++ b/tools/ci/run_vehicle_tests.py @@ -24,6 +24,9 @@ from tools.ci import vehicle_pytest # Representative production callers plus their source/native refusal cases. # The emitted selected_test_ids record exact parametrized configurations. AOL_LIFECYCLE_TARGETS = ( + "openpilot/starpilot/car/rivian/tests/test_aol_policy.py", + "openpilot/starpilot/car/rivian/tests/test_aol_native.py", + "openpilot/starpilot/car/rivian/tests/test_aol_lifecycle.py", "opendbc_repo/opendbc/safety/tests/test_ev9_combined_callers.py", "openpilot/starpilot/aol/tests/test_ev9_main_withdrawal_guards.py", "openpilot/starpilot/aol/tests/test_native_inventory_control_cadence.py", @@ -69,6 +72,8 @@ AOL_LIFECYCLE_TARGETS = ( ) AOL_LIFECYCLE_SCOPE = { + "rivian": ("GEN1 base stock/DEBUG Alpha: actual factory/Card/SD/Controls/native engagement, brake LAT-only, gas, standstill zero output, " + + "Reverse/Drive, temporary ToI recovery and physical UP_1 cancel; GEN2/unknown profiles refused"), "mazda": ("CX5 2022/CX9 2021 GEN1 stock AE32: engagement/brake/Cancel pause/toggle/Reverse/standstill/EPS recovery; " + "native permanent-fault rearm vetoes"), "subaru": "nine stock global identities; five actual factory-to-native lifecycles, brake, Reverse/Drive, EPS recovery, MAIN-off and refusal guards",