mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-28 00:53:45 +08:00
Restore Rivian GEN1 always-on lateral
This commit is contained in:
@@ -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):
|
||||
|
||||
@@ -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)
|
||||
@@ -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 = {
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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))))),
|
||||
|
||||
@@ -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<Panda *> &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.
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
@@ -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};
|
||||
@@ -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
|
||||
@@ -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))
|
||||
@@ -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')
|
||||
@@ -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",
|
||||
|
||||
Reference in New Issue
Block a user