Compare commits

..

2 Commits

Author SHA1 Message Date
whoisdomi 799c5effeb Low Speed Twitches 2026-11-09 11:23:17 -06:00
whoisdomi 18e52b36d1 Force Stop Improvements
1. Rolling stops fixed: a latched sign survives the light clearing, and the stop target can't jump farther away.

2. Sign over a long road: a sign can latch even when the model sees far ahead, once Force Stop is already slowing.

3. Vision distance: sign distance comes from the box size, averaged with the dashboard's 27 m.

4. Sign behind a car: if you're stopped behind a car at a sign, it stops at the line once that car leaves.

5. False early sign: an early dashboard sign is ignored if stopping for it would need hard braking.
2026-11-09 11:01:43 -06:00
487 changed files with 8930 additions and 26711 deletions
+1 -1
View File
@@ -350,7 +350,7 @@ jobs:
- name: Build native host dependencies
run: scons
- name: Gate every assigned lifecycle module serially
timeout-minutes: 15
timeout-minutes: 10
env:
RAYLIB_BACKEND: headless
run: python3 tools/ci/run_vehicle_tests.py --suite aol-lifecycle --shard-index "${{ matrix.shard }}" --shard-count 16 --output "ci-results/aol-lifecycle-${{ matrix.shard }}"
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
+2 -2
View File
@@ -15,7 +15,7 @@ from opendbc.car.ford.discovery import (normal_bus0_selection as ford_normal_bus
normal_bus0_queries as ford_normal_bus0_queries, normal_bus_query_brand)
from opendbc.car.volvo.discovery import normal_bus_topology as volvo_normal_bus_topology
from opendbc.car.hyundai.discovery import normal_bus_topology
from opendbc.car.tesla.discovery import normal_bus0_topology as tesla_normal_bus0_topology
from opendbc.car.tesla.discovery import hw1_normal_bus0_topology
from opendbc.car.values import BRANDS
from opendbc.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN
@@ -171,7 +171,7 @@ def fingerprint(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_mu
can_recv, normal_bus_topology,
gm_probe=gm_normal_bus0_topology if _gm_fw_uses_normal_bus0() else None,
volvo_probe=volvo_normal_bus_topology if not fw_versions.FW_QUERY_CONFIGS['volvo'].requests else None,
tesla_probe=tesla_normal_bus0_topology if _tesla_fw_uses_normal_bus0() else None)) is not None:
tesla_probe=hw1_normal_bus0_topology if _tesla_fw_uses_normal_bus0() else None)) is not None:
set_obd_multiplexing(False)
warmup_remaining = 0.1 - (time.monotonic() - start_time)
+1 -5
View File
@@ -360,7 +360,6 @@ class CarController(CarControllerBase):
self.volt_cc_cancel_pending = False
self.volt_cc_last_cancel_frame = 0
self.volt_cc_metric = False
self.volt_cc_speed_ceiling_mps = None
self.volt_cc_last_button_frame = 0
self.volt_cc_last_direction_frame = None
self.volt_cc_consumed_source_ns = 0
@@ -1397,10 +1396,7 @@ class CarController(CarControllerBase):
self.volt_cc_consumed_source_ns = source_ns
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.POWERTRAIN, (CS.buttons_counter + 1) % 4, button))
elif self.frame % 4 == 0:
cruise_kph = CS.out.vCruise
if self.volt_cc_speed_ceiling_mps is not None:
cruise_kph = min(cruise_kph, self.volt_cc_speed_ceiling_mps * CV.MS_TO_KPH)
button, rate = button_request(CS.out.vEgo, CS.out.cruiseState.speed, actuators.accel, cruise_kph,
button, rate = button_request(CS.out.vEgo, CS.out.cruiseState.speed, actuators.accel, CS.out.vCruise,
bool(CC.hudControl.leadVisible), self.volt_cc_metric)
source_ns = physical.button_credit_ns
if ((self.frame - self.volt_cc_last_button_frame) * DT_CTRL > rate and
@@ -335,8 +335,8 @@ def test_actual_card_active_observation_requires_fresh_selfdrive_state(monkeypat
host.observe_ioniq6_long_authority = lambda *args: None
host.update_vehicle_state_context = lambda *args: None
def update_cruise_speed(_state, _pending, *, wheel_context=None):
assert wheel_context is None
def update_cruise_speed(_state, _pending, *, approved=None):
assert approved is None
host.update_cruise_speed = update_cruise_speed
host.v_cruise_helper = SimpleNamespace(slc_consumed_button=None, toyota=None)
host.startup_panda_configured = lambda: True
@@ -5,9 +5,6 @@ Shared Ioniq and generic Hyundai initialization are intentionally independent.
"""
from dataclasses import dataclass
from enum import Enum
from functools import lru_cache
from capnp.lib.capnp import _DynamicStructBuilder, _DynamicStructReader
from opendbc.car.hyundai.values import CAR, is_blended, HyundaiFlags, HyundaiSafetyFlags
@@ -197,7 +194,7 @@ class BlendedLongitudinalController:
return messages
def _mixed_startup_qualified_uncached(cp, *, is_release=False, allow_marked=False):
def mixed_startup_qualified(cp, *, is_release=False, allow_marked=False):
"""Exact prepared mixed owner; stock topology remains immutable."""
from opendbc.car import structs
from opendbc.car.hyundai.hyundaicanfd import CanBus
@@ -541,23 +538,3 @@ def startup_owner(cp, callbacks, *, requested):
candidate = candidate_from_stock(cp, alpha_requested=requested,
native_qualified=cp.alphaLongitudinalAvailable and mixed_startup_qualified(cp))
return None if candidate is None else BlendedStartup(cp, candidate, callbacks)
@lru_cache(maxsize=32)
def _mixed_startup_for_bytes(key, is_release, allow_marked):
from opendbc.car import structs
with structs.CarParams.from_bytes(key) as cp:
return _mixed_startup_qualified_uncached(cp, is_release=is_release, allow_marked=allow_marked)
def mixed_startup_qualified(cp, *, is_release=False, allow_marked=False):
from opendbc.car import structs
if type(cp) in (_DynamicStructBuilder, _DynamicStructReader) and cp.schema == structs.CarParams.schema:
try:
snapshot = cp.copy() if type(cp) is _DynamicStructBuilder else cp.as_builder()
key = snapshot.to_bytes()
if len(key) <= 65536:
return _mixed_startup_for_bytes(key, is_release, allow_marked)
except Exception:
pass
return _mixed_startup_qualified_uncached(cp, is_release=is_release, allow_marked=allow_marked)
@@ -52,6 +52,7 @@ def _base_word(cp, *, experiences=(0,)):
return None
if (
cp.brand != 'hyundai'
or factory == CAR.HYUNDAI_IONIQ_6
or not factory.config.flags & HyundaiFlags.CANFD
or factory.config.flags & (HyundaiFlags.CANFD_ANGLE_STEERING | HyundaiFlags.NON_SCC)
or cp.passive
@@ -84,8 +85,6 @@ def _base_word(cp, *, experiences=(0,)):
word |= 0x0400
if factory in CARNIVAL_CARS and alt_buttons:
word |= 0x2000
if factory == CAR.HYUNDAI_IONIQ_6 and word != 0x09:
return None
return word if word | STOCK_AOL_MARKER in STOCK_AOL_WORDS else None
@@ -101,7 +100,7 @@ CARNIVAL_LONG_AOL_WORD = 0x28B4
LONG_AOL_WORDS = frozenset((CARNIVAL_LONG_AOL_WORD, 0x080D, 0x0815, 0x0895, 0x2815, 0x2895)) | SPORTAGE_LONG_AOL_WORDS
LONG_EV_CARS = frozenset((CAR.HYUNDAI_IONIQ_5, CAR.HYUNDAI_KONA_EV_2ND_GEN))
INDEPENDENT_LONG_BUTTON_CARS = LONG_EV_CARS | {
CAR.HYUNDAI_IONIQ_6, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN, CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KIA_SPORTAGE_5TH_GEN, CAR.KIA_CARNIVAL_2025,
CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN, CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KIA_SPORTAGE_5TH_GEN, CAR.KIA_CARNIVAL_2025,
}
@@ -116,7 +115,7 @@ def qualified_long(cp, *, marked_only=False):
if not eligible(cp):
return False
word = _base_word(cp, experiences=(0, 32))
if cp.carFingerprint in (CAR.GENESIS_GV60_EV_1ST_GEN, CAR.HYUNDAI_IONIQ_6):
if cp.carFingerprint == CAR.GENESIS_GV60_EV_1ST_GEN:
if word != 0x09:
return False
elif cp.carFingerprint == CAR.KIA_SPORTAGE_5TH_GEN:
@@ -43,7 +43,6 @@ from opendbc.car.interfaces import CarControllerBase
from opendbc.car.vehicle_model import VehicleModel
EV9_OP_LONG_ANGLE_TARGET_MAX = 140.0
PE_OP_LONG_ANGLE_TARGET_MAX = 175.0
VisualAlert = structs.CarControl.HUDControl.VisualAlert
LongCtrlState = structs.CarControl.Actuators.LongControlState
@@ -138,8 +137,7 @@ class CarController(CarControllerBase):
self.last_ccnc_now_ns = 0
self.ccnc_baselined = False
self.ioniq6_longitudinal = (Ioniq6LongitudinalPolicy()
if CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and CP.openpilotLongitudinalControl and
int(CP.safetyConfigs[-1].safetyParam) in (0x8015, 0x8095, 0x8815, 0x8895) else None)
if CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and CP.openpilotLongitudinalControl else None)
self.pe_longitudinal = PELongitudinalPolicy() if pe_long_qualified(CP) else None
self.pe_startup = None
self.ev9_longitudinal = EV9LongitudinalPolicy() if ev9_long_qualified(CP) else None
@@ -450,11 +448,8 @@ class CarController(CarControllerBase):
if self.ev9_angle_filter is not None:
self.ev9_angle_filter.x = self.apply_angle_last
safety_speed = max(CS.out.vEgoRaw - 1.0, 1.0)
target_max = PE_OP_LONG_ANGLE_TARGET_MAX if pe_long else EV9_OP_LONG_ANGLE_TARGET_MAX
bound = min(get_max_angle_vm(safety_speed, self.ev9_legacy_envelope, self.params),
get_max_angle_vm(safety_speed, self.angle_vm, self.params))
if pe_long:
bound = min(bound, target_max)
ev9_lateral_allowed = lateral_request_allowed(CC, CS.out, CS.angle_steering_fault,
independent=(ev9_long_aol_qualified(self.CP, marked_only=True) or
pe_long_aol_qualified(self.CP, marked_only=True)))
@@ -462,8 +457,8 @@ class CarController(CarControllerBase):
abs(measured) <= bound and abs(self.apply_angle_last) <= bound)
self.ev9_angle_filter.update_alpha(float(np.interp(CS.out.vEgo, [5.0, 10.0, 20.0], [0.2, 0.1, 0.0])))
desired = self.ev9_angle_filter.update(float(np.clip(CC.actuators.steeringAngleDeg,
-target_max,
target_max)))
-(self.params.ANGLE_LIMITS.STEER_ANGLE_MAX if pe_long else EV9_OP_LONG_ANGLE_TARGET_MAX),
self.params.ANGLE_LIMITS.STEER_ANGLE_MAX if pe_long else EV9_OP_LONG_ANGLE_TARGET_MAX)))
# Preserve the original two VM passes and its final tolerance-envelope delta.
angle = apply_steer_angle_limits_vm(desired, self.apply_angle_last, CS.out.vEgoRaw, measured,
active, self.params, self.angle_vm)
@@ -600,7 +595,7 @@ class CarController(CarControllerBase):
if self.gv60_longitudinal and self.frame % 4 == 0:
can_sends.append(hyundaicanfd.create_gv60_radar_heartbeat(self.frame // 4, CS.out.brakePressed, CS.out.gasPressed))
if self.ioniq6_longitudinal is not None and self.frame % 4 == 0:
can_sends.append(hyundaicanfd.create_ioniq6_radar_heartbeat(self.frame // 4, CS.out.brakePressed, CS.out.gasPressed, bus=self.CAN.ACAN))
can_sends.append(hyundaicanfd.create_ioniq6_radar_heartbeat(self.frame // 4, CS.out.brakePressed, CS.out.gasPressed))
if self.ioniq6_bsm_enabled and self.frame % 5 == 0:
sources = getattr(CS, "ioniq6_bsm_sources", None)
can_now_ns = getattr(CS, "ioniq6_bsm_now_ns", 0)
@@ -611,7 +606,7 @@ class CarController(CarControllerBase):
if (CS.out.vEgoRaw < 5 * CV.MPH_TO_MS or
status.left == 2 and not left_queued or status.right == 2 and not right_queued):
status = BlindspotStatus(0, 0)
can_sends.extend(hyundaicanfd.create_ioniq6_blindspot_status(self.ioniq6_bsm_counter, status, bus=self.CAN.ECAN))
can_sends.extend(hyundaicanfd.create_ioniq6_blindspot_status(self.ioniq6_bsm_counter, status))
self.ioniq6_bsm_counter = (self.ioniq6_bsm_counter + 1) & 0xff
self.ioniq6_bsm_last_can_ns = can_now_ns
elif not ccnc_non_lka:
+4 -6
View File
@@ -685,19 +685,17 @@ class CarState(CarStateBase):
if CP.flags & HyundaiFlags.CANFD_LKA_STEER_MSG:
cam_msgs.append(("CAM_0x362" if CP.flags & HyundaiFlags.CANFD_LKA_STEER_MSG_ALT else "CAM_0x2a4", 20))
elif CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and CP.openpilotLongitudinalControl:
# Both topologies require genuine chassis inputs; only HDA-II uses
# the camera template after acknowledged ADAS takeover.
# The acknowledged ADAS takeover removes stock SCC from E-CAN. Require
# every independent native input and the matching camera source instead.
msgs.extend((("ACCELERATOR", 100), ("TCS", 50), ("WHEEL_SPEEDS", 100), ("MDPS", 100)))
msgs.extend((("CRUISE_BUTTONS_ALT", math.nan), ("DOORS_SEATBELTS", math.nan),
("STEERING_SENSORS", math.nan), ("BLINKERS", math.nan)))
if CP.flags & HyundaiFlags.CANFD_LKA_STEER_MSG:
cam_msgs.append(("CAM_0x362" if CP.flags & HyundaiFlags.CANFD_LKA_STEER_MSG_ALT else "CAM_0x2a4", 20))
cam_msgs.append(("CAM_0x362" if CP.flags & HyundaiFlags.CANFD_LKA_STEER_MSG_ALT else "CAM_0x2a4", 20))
elif CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and not CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
# Stock standard-button Ioniq 6 reads alternate DISTANCE_UNIT too, but
# that frame may be absent. Subscribe before the lazy lookup makes it
# required; existing native and CarState safety inputs remain required.
if not any(name == "CRUISE_BUTTONS_ALT" for name, _ in msgs):
msgs.append(("CRUISE_BUTTONS_ALT", math.nan))
msgs.append(("CRUISE_BUTTONS_ALT", math.nan))
from opendbc.car.hyundai.ev6_startup import eligible as ev6_eligible
from opendbc.car.hyundai.gv70_startup import eligible as gv70_eligible
if ev6_eligible(CP) or gv70_eligible(CP):
@@ -246,20 +246,18 @@ def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_ov
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
def create_ioniq6_radar_heartbeat(counter: int, brake_pressed: bool, gas_pressed: bool, *, bus: int = 0) -> CanData:
def create_ioniq6_radar_heartbeat(counter: int, brake_pressed: bool, gas_pressed: bool) -> CanData:
"""Recorded Ioniq 6 ADAS replacement heartbeat, 24 bytes on A-CAN.
Preserve the observed non-pedal body; this is not the 32-byte ICE message
sharing address 0x100. Only rolling integrity and actual pedal state vary.
"""
if type(bus) is not int or bus not in (0, 4):
raise ValueError("reviewed Ioniq 6 A-CAN bus required")
data = bytearray.fromhex("000000020000fcff000000000020000055ff000068000000")
data[2] = counter & 0xff
data[4] = int(brake_pressed)
data[22] = int(gas_pressed)
data[:2] = hkg_can_fd_checksum(0x100, None, data).to_bytes(2, 'little')
return CanData(0x100, bytes(data), bus)
return CanData(0x100, bytes(data), 0)
def create_gv60_radar_heartbeat(counter: int, brake_pressed: bool, gas_pressed: bool) -> CanData:
@@ -272,16 +270,14 @@ def create_gv60_radar_heartbeat(counter: int, brake_pressed: bool, gas_pressed:
return CanData(0x100, bytes(data), 0)
def create_ioniq6_blindspot_status(counter: int, status: BlindspotStatus, *, bus: int = 1) -> list[CanData]:
def create_ioniq6_blindspot_status(counter: int, status: BlindspotStatus) -> list[CanData]:
"""Paired HDA-II dashboard status from the frozen Ioniq 6 indicator layout."""
if type(bus) is not int or bus not in (1, 5):
raise ValueError("reviewed Ioniq 6 E-CAN bus required")
frames = []
for address, body in ((0x1BA, rear_body(status)), (0x1E5, FRONT_NEUTRAL_BODY)):
data = bytearray(body)
data[2] = counter & 0xff
data[:2] = hkg_can_fd_checksum(address, None, data).to_bytes(2, 'little')
frames.append(CanData(address, bytes(data), bus))
frames.append(CanData(address, bytes(data), 1))
return frames
@@ -142,9 +142,6 @@ class CarInterface(CarInterfaceBase):
CAN = CanBus(None, fingerprint, lka_steering)
ret.alphaLongitudinalAvailable = not (ret.flags & HyundaiFlags.CANFD_NO_RADAR_DISABLE)
if (candidate == CAR.HYUNDAI_IONIQ_6 and not lka_steering and
fingerprint[CAN.CAM].get(0x1A0) == 32 and fingerprint[CAN.ECAN].get(0x1CF) == 8):
ret.alphaLongitudinalAvailable = True
if ret.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
# These manual angle profiles retain stock SCC in both builds.
ret.alphaLongitudinalAvailable = False
@@ -13,8 +13,7 @@ from enum import Enum, auto
from opendbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable
from opendbc.car import make_tester_present_msg
from opendbc.car.hyundai.hyundaicanfd import CanBus, hkg_can_fd_checksum
from opendbc.car.hyundai.canfd_owner import config_index
from opendbc.car.hyundai.hyundaicanfd import hkg_can_fd_checksum
from opendbc.car.isotp_parallel_query import IsoTpParallelQuery
@@ -28,12 +27,6 @@ IONIQ6_LONG_PREARM_ENABLED = True
IONIQ6_DEBUG_LONG_PARAMS = frozenset((0x8015, 0x8095, 0x8815, 0x8895))
def _ecan_bus(bus: int) -> int:
if type(bus) is not int or bus not in (1, 5):
raise ValueError("reviewed Ioniq 6 E-CAN bus required")
return bus
class HandoffOutcome(Enum):
NOT_READY = auto()
STOCK = auto()
@@ -56,35 +49,29 @@ def build_ioniq6_hda2_long_candidate(stock_cp, fingerprint):
from opendbc.car.hyundai.radar_interface import MRR35_RADAR_START_ADDR
from opendbc.car.hyundai.values import CAR
owner = config_index(stock_cp)
if (stock_cp.carFingerprint != CAR.HYUNDAI_IONIQ_6 or stock_cp.radarUnavailable or owner is None or
stock_cp.passive or stock_cp.dashcamOnly or stock_cp.notCar or
stock_cp.openpilotLongitudinalControl or not stock_cp.pcmCruise):
if stock_cp.carFingerprint != CAR.HYUNDAI_IONIQ_6 or stock_cp.radarUnavailable:
return None
CAN = CanBus(stock_cp)
if CanBus(None, fingerprint, lka_steering=True).offset != CAN.offset:
return None
camera = fingerprint[CAN.CAM]
camera = fingerprint[2]
direct = camera.get(0x50) == 16 and 0x110 not in camera
alternate = camera.get(0x110) == 32 and 0x50 not in camera
if not (direct or alternate):
return None
camera_support = (0x362, 32) if alternate else (0x2A4, 24)
if (camera.get(camera_support[0]) != camera_support[1] or
fingerprint[CAN.ECAN].get(0x1CF) != 8 or
fingerprint[CAN.ECAN].get(0x35) != 32 or fingerprint[CAN.ECAN].get(0x175) != 24 or
fingerprint[CAN.ECAN].get(0xA0) != 24 or fingerprint[CAN.ECAN].get(0xEA) != 24 or
any(fingerprint[CAN.ECAN].get(addr) != length for addr, length in {**IONIQ6_STOCK_BSM, 0x36A: 16}.items()) or
fingerprint[CAN.ACAN].get(MRR35_RADAR_START_ADDR) != 24):
fingerprint[1].get(0x1CF) != 8 or
fingerprint[1].get(0x35) != 32 or fingerprint[1].get(0x175) != 24 or
fingerprint[1].get(0xA0) != 24 or fingerprint[1].get(0xEA) != 24 or
any(fingerprint[1].get(addr) != length for addr, length in {**IONIQ6_STOCK_BSM, 0x36A: 16}.items()) or
fingerprint[0].get(MRR35_RADAR_START_ADDR) != 24):
return None
if int(stock_cp.safetyConfigs[owner].safetyParam) not in (0x11, 0x91):
if int(stock_cp.safetyConfigs[-1].safetyParam) not in (0x11, 0x91):
return None
long_cp = stock_cp.as_reader().as_builder()
long_cp.alphaLongitudinalAvailable = True
long_cp.openpilotLongitudinalControl = True
long_cp.pcmCruise = False
long_cp.safetyConfigs[owner].safetyParam = 0x8095 if alternate else 0x8015
long_cp.safetyConfigs[-1].safetyParam = 0x8095 if alternate else 0x8015
return long_cp
@@ -103,8 +90,7 @@ class TimestampedCanPacket(list[CanData]):
class ObservingCanRecv:
"""Keep SCC source evidence while ISO-TP consumes the shared CAN stream."""
def __init__(self, can_recv: CanRecvCallable, clock: Clock = time.monotonic, *, ecan_bus: int = IONIQ6_ECAN_BUS):
self.ecan_bus = _ecan_bus(ecan_bus)
def __init__(self, can_recv: CanRecvCallable, clock: Clock = time.monotonic):
self.can_recv = can_recv
self.clock = clock
self.source_seen_count = 0
@@ -115,23 +101,22 @@ class ObservingCanRecv:
packets = self.can_recv(wait_for_one=wait_for_one)
for packet in packets:
for msg in packet:
if msg.src == self.ecan_bus and msg.address in (IONIQ6_STOCK_SCC_ADDR, *IONIQ6_STOCK_BSM):
if msg.src == IONIQ6_ECAN_BUS and msg.address in (IONIQ6_STOCK_SCC_ADDR, *IONIQ6_STOCK_BSM):
self.source_seen_count += 1
self.latest_source_time = self.clock()
elif msg.src == self.ecan_bus - 1 and msg.address == IONIQ6_STOCK_HEARTBEAT_ADDR:
elif msg.src == 0 and msg.address == IONIQ6_STOCK_HEARTBEAT_ADDR:
self.heartbeat_seen_count += 1
return packets
def query_ioniq6_adas(can_recv: CanRecvCallable, can_send: CanSendCallable, request: bytes, response: bytes,
*, ecan_bus: int = IONIQ6_ECAN_BUS) -> bool:
def query_ioniq6_adas(can_recv: CanRecvCallable, can_send: CanSendCallable, request: bytes, response: bytes) -> bool:
"""Validate the service-specific positive response from 0x738 on E-CAN.
DiagnosticSessionControl may include four P2/P2* timing bytes after 0x50 xx;
CommunicationControl has no corresponding response payload.
"""
try:
query = IsoTpParallelQuery(can_send, can_recv, _ecan_bus(ecan_bus), [IONIQ6_ADAS_ADDR], [request], [response])
query = IsoTpParallelQuery(can_send, can_recv, IONIQ6_ECAN_BUS, [IONIQ6_ADAS_ADDR], [request], [response])
payload = query.get_data(0.15, total_timeout=0.25).get((IONIQ6_ADAS_ADDR, None))
if request in (b"\x10\x03", b"\x10\x01") and response == bytes((0x50, request[1])):
return payload is not None and len(payload) in (0, 4)
@@ -147,9 +132,8 @@ def _valid_canfd_crc(msg: CanData) -> bool:
def _read_window(can_recv: CanRecvCallable, clock: Clock, duration: float, *, wait_for_one: bool = True,
ready_samples=None, ecan_bus: int = IONIQ6_ECAN_BUS) -> tuple[list[tuple[float, int, float]], set[int], set[int], int, bool, set[int]]:
ready_samples=None) -> tuple[list[tuple[float, int, float]], set[int], set[int], int, bool, set[int]]:
"""Collect both stock ECU TX sources and required E-CAN input health."""
ecan_bus = _ecan_bus(ecan_bus)
source_samples: list[tuple[float, int, float]] = []
healthy: set[int] = set()
source_seen: set[int] = set()
@@ -167,12 +151,12 @@ def _read_window(can_recv: CanRecvCallable, clock: Clock, duration: float, *, wa
producer_ns = getattr(packet, "log_mono_time_ns", 0)
sample_time = producer_ns * 1e-9 if producer_ns > 0 else received_at
for msg in packet:
if msg.src == ecan_bus - 1 and msg.address == IONIQ6_STOCK_HEARTBEAT_ADDR:
if msg.src == 0 and msg.address == IONIQ6_STOCK_HEARTBEAT_ADDR:
heartbeat_seen = True
if len(msg.dat) == 24 and _valid_canfd_crc(msg):
heartbeat_valid += 1
continue
if msg.src != ecan_bus:
if msg.src != IONIQ6_ECAN_BUS:
continue
if msg.address == IONIQ6_STOCK_SCC_ADDR:
# Even a bad CRC proves the stock ECU is still transmitting.
@@ -249,8 +233,7 @@ def _qualified_ioniq6_ready(samples, now):
return samples[-1][3]
def inspect_ioniq6_long_sources(can_recv: CanRecvCallable, clock: Clock = time.monotonic,
*, ecan_bus: int = IONIQ6_ECAN_BUS) -> HandoffResult:
def inspect_ioniq6_long_sources(can_recv: CanRecvCallable, clock: Clock = time.monotonic) -> HandoffResult:
"""Receive-only prerequisite check before selecting a longitudinal profile."""
# Each attempt stands alone: never combine health or cadence across windows.
# Drain fingerprint/retry backlog before measuring current SCC cadence.
@@ -259,7 +242,7 @@ def inspect_ioniq6_long_sources(can_recv: CanRecvCallable, clock: Clock = time.m
can_recv(wait_for_one=False)
ready_samples = []
pre_times, pre_health, pre_sources, pre_heartbeat_valid, _, pre_status_valid = _read_window(
can_recv, clock, 0.25, wait_for_one=False, ready_samples=ready_samples, ecan_bus=ecan_bus)
can_recv, clock, 0.25, wait_for_one=False, ready_samples=ready_samples)
period = _measured_period(pre_times)
missing = []
if period is None:
@@ -278,14 +261,13 @@ def inspect_ioniq6_long_sources(can_recv: CanRecvCallable, clock: Clock = time.m
return HandoffResult(HandoffOutcome.NOT_READY, 'preflight_' + ','.join(missing))
def run_ioniq6_handoff(can_recv: CanRecvCallable, exchange: DiagnosticExchange, clock: Clock = time.monotonic,
*, ecan_bus: int = IONIQ6_ECAN_BUS) -> HandoffResult:
def run_ioniq6_handoff(can_recv: CanRecvCallable, exchange: DiagnosticExchange, clock: Clock = time.monotonic) -> HandoffResult:
"""Resolve stock or long authority before constructing or publishing CarParams.
The 50 Hz SCC source yields a provisional five-period silence/resumption
window; these bounds require route calibration before feature activation.
"""
preflight = inspect_ioniq6_long_sources(can_recv, clock, ecan_bus=ecan_bus)
preflight = inspect_ioniq6_long_sources(can_recv, clock)
period = preflight.source_period
if period is None:
return preflight
@@ -294,7 +276,7 @@ def run_ioniq6_handoff(can_recv: CanRecvCallable, exchange: DiagnosticExchange,
session_ack = exchange(b"\x10\x03", b"\x50\x03")
disable_ack = session_ack and exchange(b"\x28\x01\x01", b"\x68\x01")
if disable_ack:
_, post_health, post_source_seen, _, post_heartbeat_seen, _ = _read_window(can_recv, clock, silence_window, ecan_bus=ecan_bus)
_, post_health, post_source_seen, _, post_heartbeat_seen, _ = _read_window(can_recv, clock, silence_window)
if not post_source_seen and not post_heartbeat_seen and post_health == IONIQ6_REQUIRED_RX.keys():
return HandoffResult(HandoffOutcome.CONFIRMED, "positive UDS response and observed SCC/heartbeat/BSM silence", period, True)
@@ -304,7 +286,7 @@ def run_ioniq6_handoff(can_recv: CanRecvCallable, exchange: DiagnosticExchange,
restore_session_ack = exchange(b"\x10\x01", b"\x50\x01")
restored = restore_tx_ack and restore_session_ack
restored_times, restored_health, restored_sources, restored_heartbeat_valid, _, restored_status_valid = _read_window(
can_recv, clock, max(0.25, silence_window), ecan_bus=ecan_bus)
can_recv, clock, max(0.25, silence_window))
if (restored and _measured_period(restored_times) is not None and restored_heartbeat_valid >= 4 and
restored_health == IONIQ6_REQUIRED_RX.keys() and
restored_sources == {IONIQ6_STOCK_SCC_ADDR, *IONIQ6_STOCK_BSM} and restored_status_valid == IONIQ6_STOCK_BSM.keys()):
@@ -312,13 +294,12 @@ def run_ioniq6_handoff(can_recv: CanRecvCallable, exchange: DiagnosticExchange,
return HandoffResult(HandoffOutcome.UNAVAILABLE, "ECU restore or stock SCC/heartbeat/BSM resumption unverified", period, disable_ack, restored)
def restore_ioniq6_adas(can_recv: CanRecvCallable, can_send: CanSendCallable, clock: Clock = time.monotonic,
*, ecan_bus: int = IONIQ6_ECAN_BUS) -> bool:
def restore_ioniq6_adas(can_recv: CanRecvCallable, can_send: CanSendCallable, clock: Clock = time.monotonic) -> bool:
"""Abort a post-publication startup without ever claiming stock CP ownership."""
observed_recv = ObservingCanRecv(can_recv, clock, ecan_bus=ecan_bus)
restore_tx = query_ioniq6_adas(observed_recv, can_send, b"\x28\x00\x01", b"\x68\x00", ecan_bus=ecan_bus)
restore_session = query_ioniq6_adas(observed_recv, can_send, b"\x10\x01", b"\x50\x01", ecan_bus=ecan_bus)
samples, healthy, sources, heartbeat_valid, _, status_valid = _read_window(observed_recv, clock, 0.25, ecan_bus=ecan_bus)
observed_recv = ObservingCanRecv(can_recv, clock)
restore_tx = query_ioniq6_adas(observed_recv, can_send, b"\x28\x00\x01", b"\x68\x00")
restore_session = query_ioniq6_adas(observed_recv, can_send, b"\x10\x01", b"\x50\x01")
samples, healthy, sources, heartbeat_valid, _, status_valid = _read_window(observed_recv, clock, 0.25)
return (restore_tx and restore_session and _measured_period(samples) is not None and heartbeat_valid >= 4 and
healthy == IONIQ6_REQUIRED_RX.keys() and sources == {IONIQ6_STOCK_SCC_ADDR, *IONIQ6_STOCK_BSM} and
status_valid == IONIQ6_STOCK_BSM.keys())
@@ -334,9 +315,7 @@ class Ioniq6StartupKeepalive:
"""
def __init__(self, can_send: CanSendCallable, restore: Callable[[], bool], on_abort: Callable[[], None],
on_restore: Callable[[bool], None], clock: Clock = time.monotonic, max_startup_sec: float = 60.0,
*, ecan_bus: int = IONIQ6_ECAN_BUS):
self.ecan_bus = _ecan_bus(ecan_bus)
on_restore: Callable[[bool], None], clock: Clock = time.monotonic, max_startup_sec: float = 60.0):
self.can_send = can_send
self.restore = restore
self.on_abort = on_abort
@@ -359,7 +338,7 @@ class Ioniq6StartupKeepalive:
self._abort_locked()
elif now >= self.next_tester_at:
try:
self.can_send([make_tester_present_msg(IONIQ6_ADAS_ADDR, self.ecan_bus, suppress_response=True)])
self.can_send([make_tester_present_msg(IONIQ6_ADAS_ADDR, IONIQ6_ECAN_BUS, suppress_response=True)])
self.next_tester_at = now + 0.5
except Exception:
self._abort_locked()
@@ -402,13 +381,12 @@ def finalize_ioniq6_prepublication(stock_cp, long_cp, can_recv: CanRecvCallable,
if not enabled or is_release or long_cp is None:
return stock_cp, False, None
raw = int(long_cp.safetyConfigs[-1].safetyParam)
if config_index(long_cp) is None or not long_cp.openpilotLongitudinalControl or raw not in IONIQ6_DEBUG_LONG_PARAMS:
if not long_cp.openpilotLongitudinalControl or raw not in IONIQ6_DEBUG_LONG_PARAMS:
raise ValueError("unreviewed Ioniq 6 longitudinal CarParams profile")
ecan_bus = _ecan_bus(CanBus(long_cp, lka_steering=True).ECAN)
observed_recv = ObservingCanRecv(can_recv, clock, ecan_bus=ecan_bus)
observed_recv = ObservingCanRecv(can_recv, clock)
result = run_ioniq6_handoff(
observed_recv, lambda request, response: query_ioniq6_adas(observed_recv, can_send, request, response, ecan_bus=ecan_bus), clock, ecan_bus=ecan_bus)
observed_recv, lambda request, response: query_ioniq6_adas(observed_recv, can_send, request, response), clock)
if result.outcome is HandoffOutcome.CONFIRMED:
return long_cp, True, result
if result.outcome is HandoffOutcome.UNAVAILABLE:
@@ -423,14 +401,14 @@ def prepare_ioniq6_long_candidate(stock_cp, long_cp, *, enabled: bool, is_releas
if not enabled or is_release or long_cp is None:
return stock_cp, False
raw = int(long_cp.safetyConfigs[-1].safetyParam)
if config_index(long_cp) is None or not long_cp.openpilotLongitudinalControl or raw not in IONIQ6_DEBUG_LONG_PARAMS:
if not long_cp.openpilotLongitudinalControl or raw not in IONIQ6_DEBUG_LONG_PARAMS:
raise ValueError("unreviewed Ioniq 6 longitudinal CarParams profile")
return long_cp, True
def confirm_ioniq6_prepared_takeover(can_recv: CanRecvCallable, can_send: CanSendCallable,
clock: Clock = time.monotonic, *, ecan_bus: int = IONIQ6_ECAN_BUS) -> HandoffResult:
clock: Clock = time.monotonic) -> HandoffResult:
"""Run the source/UDS transaction only when replacement TX can start."""
observed_recv = ObservingCanRecv(can_recv, clock, ecan_bus=ecan_bus)
observed_recv = ObservingCanRecv(can_recv, clock)
return run_ioniq6_handoff(
observed_recv, lambda request, response: query_ioniq6_adas(observed_recv, can_send, request, response, ecan_bus=ecan_bus), clock, ecan_bus=ecan_bus)
observed_recv, lambda request, response: query_ioniq6_adas(observed_recv, can_send, request, response), clock)
@@ -60,11 +60,7 @@ def update_calibration(state: Ioniq6LongitudinalState, accel: float, speed: floa
elif starting or state.launch_active or (state.long_control_state_last == LongState.starting and control_state == LongState.pid):
state.launch_active = True
hold_jerk = float(np.interp(speed, (0.0, 0.15, 0.6, 1.2, 2.0, 3.0), (0.35, 0.40, 0.48, 0.65, 0.85, 1.10))) * RESPONSE if state.stopping else 0.0
upper_limit = float(np.interp(speed, (0.0, 5.0, 20.0), (2.0, 3.0, 2.0))) * RESPONSE if control_state == LongState.pid else MIN_JERK
if state.stopping and accel > state.accel_last:
# Follow an already gentler planner request without weakening stronger braking.
upper_limit = hold_jerk
lower_limit = float(np.interp(speed, (0.0, 5.0, 20.0), (5.0, 3.5, 3.0))) * RESPONSE
horizon = float(np.interp(speed, LOOKAHEAD_SPEEDS, LOOKAHEAD_TIMES))
desired_jerk = float(np.clip((accel - state.accel_last) / horizon, -MAX_JERK, MAX_JERK))
@@ -74,10 +70,10 @@ def update_calibration(state: Ioniq6LongitudinalState, accel: float, speed: floa
state.jerk_lower = min(lower_jerk, lower_limit)
if state.stopping:
# A saturated braking request must survive the comfort soft-stop cap.
if speed <= 2.0 and (speed <= .01 or accel > CarControllerParams.ACCEL_MIN):
if speed <= 2.0:
brake_cap = float(np.interp(speed, (0.0, 0.08, 0.25, 0.6, 1.2, 2.0, 3.0), (-0.15, -0.16, -0.22, -0.42, -0.78, -1.15, -1.40)))
state.desired_accel = min(0.0, max(accel, brake_cap))
hold_jerk = float(np.interp(speed, (0.0, 0.15, 0.6, 1.2, 2.0, 3.0), (0.35, 0.40, 0.48, 0.65, 0.85, 1.10))) * RESPONSE
state.jerk_upper = min(state.jerk_upper, hold_jerk)
else:
state.desired_accel = float(np.clip(accel, CarControllerParams.ACCEL_MIN, 0.0))
@@ -37,7 +37,7 @@ class Ioniq6MediaButtons:
"""
def __init__(self, bus: int):
if type(bus) is not int or bus not in (0, 1, 2, 3, 5):
if type(bus) is not int or not 0 <= bus <= 3:
raise ValueError("physical CAN bus required")
self.bus = bus
self.epoch = 0
@@ -5,7 +5,6 @@ from opendbc.can import CANParser
from opendbc.car import Bus, structs
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.interfaces import RadarInterfaceBase
from opendbc.car.hyundai.hyundaicanfd import CanBus
from opendbc.car.hyundai.values import (CAR, DBC, HyundaiFlags, HYUNDAI_MRR35_RADAR_DBC, HYUNDAI_MRR30_RADAR_DBC,
HYUNDAI_GV70_RADAR_DBC, HYUNDAI_G90_RADAR_DBC, is_blended)
@@ -133,12 +132,11 @@ def radar_dbc(CP):
def radar_bus(CP) -> int:
offset = CanBus(CP).offset if CP.flags & HyundaiFlags.CANFD else 0
# Frozen Ioniq 6 camera-SCC wiring exposes the MRR35 object bank on bus 1;
# its LKA/HDA-II wiring and the other MRR35 cars expose it on bus 0.
if CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and CP.flags & HyundaiFlags.CANFD_CAMERA_SCC:
return offset + 1
return offset
return 1
return 0
# POC for parsing corner radars: https://github.com/commaai/openpilot/pull/24221/
@@ -1,189 +0,0 @@
import os
from tempfile import TemporaryDirectory
from types import SimpleNamespace
from unittest.mock import Mock, patch
import pytest
from opendbc.car import Bus, gen_empty_fingerprint, structs
from opendbc.car.hyundai.interface import CarInterface
from opendbc.car.hyundai.carcontroller import CarController
from opendbc.car.hyundai.canfd_stock_aol import qualified, qualified_long
from opendbc.car.hyundai.hyundaicanfd import CanBus
from opendbc.car.hyundai.tests.test_ioniq6_handoff import ioniq6_profiles
from opendbc.car.hyundai.values import CAR, DBC
from opendbc.can import CANPacker
from opendbc.safety.tests.libsafety import libsafety_py
from opendbc.safety.tests import test_hyundai_gv60_hda1_long_aol as native_fixture
from openpilot.common.params import Params
from openpilot.starpilot.car.hyundai.aol import policy_for, native_accepts_cp
with patch.dict(os.environ, {"SCALE": "1"}):
from openpilot.selfdrive.ui.layouts.settings import toggles
def camera_params(offset=0, alpha=False):
fp = gen_empty_fingerprint()
fp[offset].update({0x1CF: 8, 0x35: 32, 0x175: 24, 0xA0: 24, 0xEA: 24, 0x1BA: 24, 0x1E5: 16})
fp[offset + 2].update({0x12A: 16, 0x1A0: 32})
fp[offset + 1].update({0x110: 32, 0x100: 24})
return CarInterface.get_params(CAR.HYUNDAI_IONIQ_6, fp, [], alpha, False, False)
@pytest.mark.parametrize('offset', (0, 4))
def test_camera_scc_factory_offers_long_without_hda2_takeover(offset):
stock = camera_params(offset)
cp = camera_params(offset, True)
assert stock.alphaLongitudinalAvailable, 'HDA1_ALPHA_CAPABILITY_MISSING'
assert not stock.openpilotLongitudinalControl and stock.pcmCruise
assert cp.alphaLongitudinalAvailable and cp.openpilotLongitudinalControl and not cp.pcmCruise
assert cp.steerControlType == structs.CarParams.SteerControlType.torque
assert cp.safetyConfigs[-1].safetyParam == 0x0D
assert CanBus(cp).ECAN == offset
assert CarController(DBC[cp.carFingerprint], cp).ioniq6_longitudinal is None
with patch('opendbc.car.hyundai.interface.disable_ecu') as disable:
CarInterface.init(cp, Mock(), Mock())
disable.assert_not_called()
for value in (stock, cp):
policy = policy_for(value)
assert policy.runtime_supported and policy.explicit_latch
value.safetyConfigs[-1].safetyParam |= policy.safety_param_addition
value.alternativeExperience |= policy.alternative_experience_addition
word = 0x80D if value.openpilotLongitudinalControl else 0x809
assert value.safetyConfigs[-1].safetyParam == word
assert native_accepts_cp(value, int(structs.CarParams.SafetyModel.hyundaiCanfd), word)
assert not native_accepts_cp(value, int(structs.CarParams.SafetyModel.hyundai), word)
assert not native_accepts_cp(value, int(structs.CarParams.SafetyModel.hyundaiCanfd), word ^ 16)
@pytest.mark.parametrize('offset', (0, 4))
def test_external_hda2_settings_explain_opt_in_without_claiming_stock_acc_authority(offset):
_, stock, _ = ioniq6_profiles(bus_offset=offset, alternate=True)
with TemporaryDirectory() as root:
params = Params(root)
params.put_bool('ExperimentalMode', True, block=True)
class Toggle:
def __init__(self):
self.action_item = self
self.description = ''
def set_enabled(self, enabled):
self.enabled = enabled
def set_description(self, description):
self.description = description
def set_state(self, state):
self.state = state
panel = toggles.TogglesLayout.__new__(toggles.TogglesLayout)
panel._params = params
panel._is_release = False
panel._initial_cruise_speed = Mock()
panel._long_personality_setting = SimpleNamespace(action_item=Toggle())
panel._toggles = {'ExperimentalMode': Toggle(), 'IsRhdDetected': Toggle()}
panel._toggle_defs = {}
panel._locked_toggles = set()
panel._update_experimental_mode_icon = Mock()
panel._slc_offsets = SimpleNamespace(_raw=lambda key: None)
panel._localization = SimpleNamespace(snapshot=lambda: SimpleNamespace(rows=(None, SimpleNamespace(source=None, available=True))))
ui = SimpleNamespace(CP=stock, has_longitudinal_control=False, update_params=lambda: None)
with patch.object(toggles, 'ui_state', ui):
panel._update_toggles()
assert 'Enable the openpilot longitudinal control (alpha)' in panel._toggles['ExperimentalMode'].description
assert not panel._toggles['ExperimentalMode'].enabled
assert params.get_bool('ExperimentalMode')
@pytest.mark.parametrize('offset', (0, 4))
def test_camera_long_actual_controller_packets_use_existing_native_contract(offset):
cp = camera_params(offset, True)
assert cp.openpilotLongitudinalControl
cp.safetyConfigs[-1].safetyParam |= 0x800
cp.alternativeExperience = 32
ci = CarInterface(cp)
packer = CANPacker(DBC[cp.carFingerprint][Bus.pt])
bus = CanBus(cp)
frames = [packer.make_can_msg(name, bus.ECAN, values) for name, values in (
('ACCELERATOR', {'GEAR': 5}), ('TCS', {'ACCEnable': 0}), ('WHEEL_SPEEDS', {}), ('MDPS', {}),
('CRUISE_BUTTONS', {'COUNTER': 1}), ('DOORS_SEATBELTS', {'DRIVER_SEATBELT': 1}), ('STEERING_SENSORS', {}))]
ci.update([(1_000_000_000, frames)])
case = native_fixture.TestGv60Hda1LongAol(methodName='runTest')
case.setUp()
try:
case.mode(0x80D)
if case.release:
assert not case.safety.safety_config_valid()
return
case.healthy(0x80D, omit='CRUISE_BUTTONS')
case.safety.set_aol_test_heartbeat(True)
case.safety.aol_set_host_request(3)
for tick, counter, button in ((0, 13, 0), (1, 14, 2), (2, 0, 0)):
case.safety.set_timer(1_000_000 + tick * 20_000)
assert case.safety.safety_rx_hook(case.packet(packer.make_can_msg('CRUISE_BUTTONS', 0, {'COUNTER': counter, 'CRUISE_BUTTONS': button})))
case.safety.aol_set_host_request(3)
assert case.safety.aol_get_permission_mask() == 3
cc = structs.CarControl(enabled=True, latActive=True, longActive=True)
cc.actuators.torque = .01
cc.actuators.accel = .1
cc.actuators.longControlState = structs.CarControl.Actuators.LongControlState.pid
controller = CarController(DBC[cp.carFingerprint], cp)
assert controller.ioniq6_longitudinal is None
_, sent = controller.update(cc.as_reader(), ci.CS, 1_000_000_000)
assert any(frame[0] == 0x1A0 for frame in sent)
assert not any(frame[0] == 0x730 for frame in sent)
for frame in sent:
assert frame[2] in (offset, offset + 1, offset + 2)
packet = libsafety_py.make_CANPacket(frame[0], frame[2] - offset, frame[1])
assert case.safety.safety_tx_hook(packet), (hex(frame[0]), frame[2])
finally:
case.tearDown()
@pytest.mark.parametrize('mutation', ('aux_word', 'wrong_model', 'angle', 'lka', 'passive'))
def test_camera_admission_never_borrows_other_topologies(mutation):
cp = camera_params(4, True)
cp.safetyConfigs[-1].safetyParam |= 0x800
cp.alternativeExperience = 32
if mutation == 'aux_word':
cp.safetyConfigs[0].safetyParam = 1
elif mutation == 'wrong_model':
cp.safetyConfigs[-1].safetyModel = structs.CarParams.SafetyModel.hyundai
elif mutation == 'angle':
cp.steerControlType = structs.CarParams.SteerControlType.angle
elif mutation == 'lka':
cp.flags |= 1
else:
cp.passive = True
assert not qualified(cp) and not qualified_long(cp)
assert not policy_for(cp).runtime_supported
@pytest.mark.parametrize('offset', (0, 4))
def test_galaxy_exposes_external_stock_alpha_request_and_preserves_choice(offset):
from openpilot.starpilot.ui.feature_settings_owner import FeatureSettingsOwner
_, stock, _ = ioniq6_profiles(bus_offset=offset, alternate=True)
with TemporaryDirectory() as root:
params = Params(root)
params.put_bool('IsReleaseBranch', False, block=True)
params.put_bool('AlphaLongitudinalEnabled', True, block=True)
stock.radarUnavailable = True
owner = FeatureSettingsOwner(params, lambda scope: True,
vehicle_params=lambda: stock, vehicle_fingerprint=lambda: str(stock.carFingerprint), galaxy=lambda: True)
assert owner._alpha_long_capability() is not None
rows = owner._alpha_long_rows(True)
assert len(rows) == 1 and rows[0].key == 'AlphaLongitudinalEnabled'
assert params.get_bool('AlphaLongitudinalEnabled')
stock.passive = True
assert owner._alpha_long_capability() is None
assert params.get_bool('AlphaLongitudinalEnabled')
@pytest.mark.parametrize('offset', (0, 4))
def test_cached_radar_discovery_can_save_request_but_cannot_grant_long_or_aol(offset):
from opendbc.car.hyundai.ioniq6_handoff import build_ioniq6_hda2_long_candidate
from openpilot.starpilot.car.hyundai.aol import ioniq6_alpha_long_available, ioniq6_settings_capable
fingerprint, stock, _ = ioniq6_profiles(bus_offset=offset, alternate=True)
stock.radarUnavailable = True
assert ioniq6_alpha_long_available(stock), 'CACHED_EXTERNAL_RADAR_BLOCKS_ALPHA_REQUEST'
assert not ioniq6_settings_capable(stock)
assert not policy_for(stock).runtime_supported
assert build_ioniq6_hda2_long_candidate(stock, fingerprint) is None
assert not stock.openpilotLongitudinalControl and stock.pcmCruise
stock.safetyConfigs[-1].safetyParam ^= 16
assert not ioniq6_alpha_long_available(stock)
@@ -1,4 +1,3 @@
from opendbc.car.hyundai.hyundaicanfd import CanBus
import threading
import tempfile
import time
@@ -20,23 +19,13 @@ from opendbc.car.hyundai.values import CAR
from opendbc.car.hyundai.hyundaicanfd import hkg_can_fd_checksum
def ioniq6_profiles(*, alternate=False, bus_offset=0):
fp = gen_empty_fingerprint()
fp[bus_offset + 2].update({0x110: 32, 0x362: 32} if alternate else {0x50: 16, 0x2A4: 24})
fp[bus_offset].update({0x3A5: 24, 0x100: 24})
fp[bus_offset + 1].update({0x1CF: 8, 0x35: 32, 0x175: 24, 0xA0: 24, 0xEA: 24,
0x1BA: 24, 0x1E5: 16, 0x36A: 16})
stock = CarInterface.get_params(CAR.HYUNDAI_IONIQ_6, fp, [], False, False, False)
return fp, stock, build_ioniq6_hda2_long_candidate(stock, fp)
def refresh_host_parser_receipts(card):
from collections import deque
from opendbc.can.parser import MessageState
stamp = time.clock_gettime_ns(time.CLOCK_BOOTTIME)
parsers = {}
for bus in (card.CI.CC.CAN.ECAN, card.CI.CC.CAN.CAM):
for bus in (1, 2):
source = MessageState(address=bus, name=f"source{bus}", size=8, signals=[],
timeout_threshold=100_000_000, timestamps=deque([stamp]))
parsers[bus] = SimpleNamespace(message_states={bus: source})
@@ -47,8 +36,7 @@ class FakeIoniq6Bus:
def __init__(self, *, source=True, heartbeat_source=True, heartbeat_stays_after_disable=False,
bsm_source=True, bsm_stays_after_disable=False, missing_status=None,
disable_works=True, disable_ack=True, restore_works=True,
restore_ack=True, missing_required=None, post_missing_required=None, bus_offset=0):
self.bus_offset = bus_offset
restore_ack=True, missing_required=None, post_missing_required=None):
self.now = 0.0
self.source = source
self.heartbeat_source = heartbeat_source
@@ -98,7 +86,6 @@ class FakeIoniq6Bus:
data[3] |= 0x40
data[:2] = hkg_can_fd_checksum(0x35, None, data).to_bytes(2, 'little')
messages[index] = CanData(0x35, bytes(data), 1)
messages = [CanData(m.address, m.dat, m.src + self.bus_offset) for m in messages]
return [TimestampedCanPacket(messages, int(self.now * 1e9))]
def exchange(self, request, expected):
@@ -122,7 +109,7 @@ class FakeIoniq6Bus:
raise AssertionError(request)
def run(self):
return run_ioniq6_handoff(self.recv, self.exchange, lambda: self.now, ecan_bus=1 + self.bus_offset)
return run_ioniq6_handoff(self.recv, self.exchange, lambda: self.now)
class TestIoniq6PrepublicationHandoff(unittest.TestCase):
@@ -286,7 +273,7 @@ class TestIoniq6PrepublicationHandoff(unittest.TestCase):
with self.subTest(options=options):
self.assert_card_prepublication_fallback(options)
def assert_card_prepublication_fallback(self, options, *, vehicle_ready=False, alternate=False, unknown=False, bus_offset=0):
def assert_card_prepublication_fallback(self, options, *, vehicle_ready=False, alternate=False, unknown=False):
from openpilot.common.params import Params
from openpilot.selfdrive.car import card as card_module
@@ -298,13 +285,7 @@ class TestIoniq6PrepublicationHandoff(unittest.TestCase):
fp[2].update({0x110: 32, 0x362: 32} if alternate else {0x50: 16, 0x2A4: 24})
fp[0][0x3A5] = 24
fp[1].update({0x1CF: 8, 0x35: 32, 0x175: 24, 0xA0: 24, 0xEA: 24, 0x1BA: 24, 0x1E5: 16, 0x36A: 16})
if bus_offset:
shifted = gen_empty_fingerprint()
for bus_number, frames in fp.items():
if frames:
shifted[bus_number + bus_offset].update(frames)
fp = shifted
bus = FakeIoniq6Bus(**options, bus_offset=bus_offset)
bus = FakeIoniq6Bus(**options)
bus.vehicle_ready = vehicle_ready
expected_stock = bool(options) or vehicle_ready or unknown
stock_word = 0x91 if alternate else 0x11
@@ -371,7 +352,7 @@ class TestIoniq6PrepublicationHandoff(unittest.TestCase):
patch.object(card_module, "independent_axis_requested", return_value=True), \
patch("opendbc.car.hyundai.ioniq6_handoff._ioniq6_boot_ns", side_effect=lambda: 0 if unknown else int(bus.now * 1e9)), \
patch.object(card_module, "inspect_ioniq6_long_sources",
side_effect=lambda recv, **kwargs: inspect_ioniq6_long_sources(bus.recv, lambda: bus.now, **kwargs)), \
side_effect=lambda recv: inspect_ioniq6_long_sources(bus.recv, lambda: bus.now)), \
patch.object(card_module, "prepare_ioniq6_long_candidate", side_effect=capture_preparation), \
patch.object(card_module, "confirm_ioniq6_prepared_takeover") as confirm, \
patch.object(card_module, "get_car", side_effect=capture_get_car):
@@ -403,9 +384,11 @@ class TestIoniq6PrepublicationHandoff(unittest.TestCase):
({"post_missing_required": 0xEA, "restore_works": False}, HandoffOutcome.UNAVAILABLE)):
with self.subTest(bus_options=bus_options):
bus = FakeIoniq6Bus(**bus_options)
_, stock, long = ioniq6_profiles()
stock = SimpleNamespace(dashcamOnly=False)
long = SimpleNamespace(openpilotLongitudinalControl=True,
safetyConfigs=[SimpleNamespace(safetyParam=0x8015)])
with patch("opendbc.car.hyundai.ioniq6_handoff.query_ioniq6_adas",
side_effect=lambda recv, send, request, response, active_bus=bus, **kwargs: active_bus.exchange(request, response)):
side_effect=lambda recv, send, request, response, active_bus=bus: active_bus.exchange(request, response)):
selected, prearmed, result = finalize_ioniq6_prepublication(
stock, long, bus.recv, lambda msgs: None, enabled=True, is_release=False, clock=lambda active_bus=bus: active_bus.now)
self.assertEqual(result.outcome, expected)
@@ -414,8 +397,9 @@ class TestIoniq6PrepublicationHandoff(unittest.TestCase):
self.assertEqual(stock.dashcamOnly, expected is HandoffOutcome.UNAVAILABLE)
def test_preparation_selects_immutable_candidate_without_source_transaction(self):
_, stock, prepared = ioniq6_profiles(alternate=True)
prepared.safetyConfigs[-1].safetyParam |= 0x0800
stock = SimpleNamespace()
prepared = SimpleNamespace(openpilotLongitudinalControl=True,
safetyConfigs=[SimpleNamespace(safetyParam=0x8895)])
selected, pending = prepare_ioniq6_long_candidate(stock, prepared, enabled=True, is_release=False)
self.assertIs(selected, prepared)
self.assertTrue(pending)
@@ -426,7 +410,8 @@ class TestIoniq6PrepublicationHandoff(unittest.TestCase):
prepare_ioniq6_long_candidate(stock, prepared, enabled=True, is_release=False)
def test_release_and_unreviewed_raw_do_not_request_takeover(self):
_, stock, long = ioniq6_profiles()
stock = SimpleNamespace(dashcamOnly=False)
long = SimpleNamespace(openpilotLongitudinalControl=True, safetyConfigs=[SimpleNamespace(safetyParam=0x8015)])
bus = FakeIoniq6Bus()
selected, armed, result = finalize_ioniq6_prepublication(
stock, long, bus.recv, lambda msgs: None, enabled=True, is_release=True, clock=lambda: bus.now)
@@ -591,7 +576,7 @@ class TestIoniq6PrepublicationHandoff(unittest.TestCase):
card.ioniq6_long_lost = False
card.ioniq6_keepalive = None
card.ioniq6_handoff_source_floor_ns = 1
card.CI = SimpleNamespace(CC=SimpleNamespace(CAN=CanBus(ioniq6_profiles()[2])))
card.CI = SimpleNamespace()
refresh_host_parser_receipts(card)
invalid = SimpleNamespace(canValid=False)
card.observe_ioniq6_long_authority([], invalid)
@@ -611,7 +596,7 @@ class TestIoniq6PrepublicationHandoff(unittest.TestCase):
self.assertFalse(valid.canValid)
@staticmethod
def ready_card(*, ioniq_selected: bool, bus_offset=0):
def ready_card(*, ioniq_selected: bool):
from openpilot.selfdrive.car.card import Car
class FakeSM:
@@ -644,15 +629,10 @@ class TestIoniq6PrepublicationHandoff(unittest.TestCase):
card.ioniq6_handoff_result = None
card.can_callbacks = (lambda wait_for_one=False: [], lambda messages: None)
card.params = SimpleNamespace(put_bool=Mock())
card.CP = (ioniq6_profiles(bus_offset=bus_offset)[2] if ioniq_selected else
SimpleNamespace(brand='hyundai', safetyConfigs=[object()]))
from opendbc.car.hyundai.hyundaicanfd import CanBus
card.CI = SimpleNamespace(CC=SimpleNamespace(CAN=CanBus(card.CP, lka_steering=True)), CS=SimpleNamespace(),
init=Mock(), apply=Mock(return_value=(object(), [])))
card.CP = SimpleNamespace(brand='hyundai', safetyConfigs=[object()])
card.CI = SimpleNamespace(CC=SimpleNamespace(), CS=SimpleNamespace(), init=Mock(), apply=Mock(return_value=(object(), [])))
card.vehicle_startup = VehicleStartupOwner()
card.sm = FakeSM()
if bus_offset:
card.sm.panda.append(SimpleNamespace(safetyModel=3, safetyParam=1, controlsAllowed=False))
card.publish_sendcan = Mock()
card.learning_send_committed_mono_ns = 0
card.ioniq6_panda_matches = Mock(return_value=False)
@@ -679,7 +659,7 @@ class TestIoniq6PrepublicationHandoff(unittest.TestCase):
takeover.assert_not_called()
cs.canValid = True
card.controls_update(cs, cc)
takeover.assert_called_once_with(*card.can_callbacks, ecan_bus=card.CI.CC.CAN.ECAN)
takeover.assert_called_once_with(*card.can_callbacks)
self.assertEqual(logged.call_args.args, ('ioniq6_long_handoff',))
self.assertEqual(logged.call_args.kwargs['outcome'], 'CONFIRMED')
card.params.put_bool.assert_called_once_with('ControlsReady', True)
@@ -825,7 +805,6 @@ class TestIoniq6PrepublicationHandoff(unittest.TestCase):
card.ioniq6_host_warmed = True
card.ioniq6_long_lost = False
card.ioniq6_keepalive = None
card.CI = SimpleNamespace(CC=SimpleNamespace(CAN=CanBus(ioniq6_profiles()[2])))
valid = SimpleNamespace(canValid=True)
def observe(frames):
@@ -891,7 +870,7 @@ class TestIoniq6PrepublicationHandoff(unittest.TestCase):
card_owner.is_metric = True
card_owner.experimental_mode = False
card_owner.initial_cruise_speed = 40
card_owner.v_cruise_helper = SimpleNamespace(update_v_cruise=lambda *_, slc_enabled=False, wheel_context=None: None,
card_owner.v_cruise_helper = SimpleNamespace(update_v_cruise=lambda *_, slc_enabled=False, slc_approved=None: None,
initialize_v_cruise=lambda *_, **kwargs: None, slc_consumed_button=None, toyota=None,
slc_cruise_change=None, v_cruise_kph=60.0, v_cruise_cluster_kph=60.0)
card_owner.RI = SimpleNamespace(update=lambda _: None)
@@ -922,8 +901,7 @@ class TestIoniq6PrepublicationHandoff(unittest.TestCase):
card_owner.sm = SM()
sent = []
card_owner.pm = SimpleNamespace(send=lambda service, packet: sent.append((service, packet)))
from opendbc.car.hyundai.hyundaicanfd import CanBus
card_owner.CI = SimpleNamespace(CP=cp, CC=SimpleNamespace(CAN=CanBus(cp, lka_steering=True)), CS=SimpleNamespace(),
card_owner.CI = SimpleNamespace(CP=cp, CC=SimpleNamespace(), CS=SimpleNamespace(),
update=Mock(side_effect=lambda packets: car.CarState(canValid=True, canTimeout=False)),
apply=Mock(return_value=(structs.CarControl.Actuators(), [CanData(0x1A0, b'\0' * 8, 1)])),
init=Mock())
@@ -965,7 +943,7 @@ class TestIoniq6PrepublicationHandoff(unittest.TestCase):
patch.object(card_owner, 'card_thread', return_value=None),
patch.object(card_module, 'restore_ioniq6_adas', return_value=True) as restore):
card_module.main()
restore.assert_called_once_with(*card_owner.can_callbacks, ecan_bus=card_owner.CI.CC.CAN.ECAN)
restore.assert_called_once_with(*card_owner.can_callbacks)
self.assertIs(card_module.Car, original_car)
self.assertIs(original_car.__dict__.get("__new__"), original_new)
self.assertIsInstance(object.__new__(original_car), original_car)
@@ -1000,7 +978,6 @@ class TestIoniq6PrepublicationHandoff(unittest.TestCase):
stopped = []
def fail_after_prearm(card):
card.CI = SimpleNamespace(CC=SimpleNamespace(CAN=CanBus(ioniq6_profiles()[2])))
card.ioniq6_keepalive = SimpleNamespace(stop=lambda: stopped.append(True))
card.ioniq6_long_lost = False
card.ioniq6_long_prearmed = True
@@ -1025,7 +1002,6 @@ class TestIoniq6PrepublicationHandoff(unittest.TestCase):
def prepared(card):
cards.append(card)
card.CI = SimpleNamespace(CC=SimpleNamespace(CAN=CanBus(ioniq6_profiles()[2])))
card.ioniq6_keepalive = SimpleNamespace(stop=lambda: stopped.append(True))
card.ioniq6_long_lost = False
card.ioniq6_long_prearmed = True
@@ -1049,7 +1025,7 @@ class TestIoniq6PrepublicationHandoff(unittest.TestCase):
self.assertTrue(cards[0].ioniq6_long_lost)
self.assertTrue(cards[0].ioniq6_restore_attempted)
self.assertEqual(stopped, [True])
restore.assert_called_once_with(*callbacks, ecan_bus=1)
restore.assert_called_once_with(*callbacks)
if restored:
logged.assert_not_called()
else:
@@ -61,36 +61,6 @@ class TestIoniq6Longitudinal(unittest.TestCase):
for value, previous in zip(actual, expected, strict=True):
self.assertAlmostEqual(value, previous, places=12)
def test_saturated_stopping_brake_survives_low_speed_comfort_cap(self):
for speed in (.1, .5, 1., 2.):
with self.subTest(speed=speed):
policy = Ioniq6LongitudinalPolicy()
last_sent = 0.
for frame in range(600):
result = policy.update(frame, active=True, override=False, accel=-3.5, speed=speed,
measured_accel=last_sent, control_state=LongState.stopping,
last_sent_accel=last_sent)
self.assertTrue(result.stopping)
self.assertGreaterEqual(result.accel, -3.5)
if frame % 2 == 0:
self.assertLessEqual(result.accel, last_sent)
self.assertGreaterEqual(result.accel + 1e-12, last_sent - .36)
last_sent = result.accel
self.assertAlmostEqual(last_sent, -3.5)
refused = policy.update(601, active=True, override=True, accel=-3.5, speed=speed,
measured_accel=last_sent, control_state=LongState.stopping,
last_sent_accel=last_sent)
self.assertEqual(refused.accel, 0.)
self.assertFalse(refused.stopping)
self.assertEqual(policy.state, Ioniq6LongitudinalState())
def test_saturated_request_returns_to_soft_hold_at_standstill(self):
state = Ioniq6LongitudinalState(accel_last=-3.5)
update_calibration(state, -3.5, 0., 0., LongState.stopping)
self.assertAlmostEqual(state.desired_accel, -.15)
self.assertTrue(state.stopping)
self.assertGreater(state.actual_accel, -3.5)
def test_soft_target_revokes_launch_on_offcycle_and_scheduled_ticks(self):
for drop_frame in (31, 35):
with self.subTest(drop_frame=drop_frame):
@@ -353,20 +323,6 @@ class TestIoniq6Longitudinal(unittest.TestCase):
counters.append(frame[1][2])
self.assertEqual(counters, [counter & 0xff for counter in range(260)])
def test_already_gentler_request_uses_bounded_stop_release(self):
state = Ioniq6LongitudinalState(accel_last=-1.5)
update_calibration(state, -.5, 3., -1.5, LongState.stopping)
self.assertGreater(state.actual_accel, -1.47)
self.assertLessEqual(state.actual_accel, -.5)
self.assertLessEqual(state.jerk_upper, 1.32)
self.assertTrue(state.stopping)
def test_stop_release_does_not_weaken_independent_braking_above_soft_cap(self):
state = Ioniq6LongitudinalState(accel_last=-3.2)
update_calibration(state, -3.2, 2.5, -3.2, LongState.stopping)
self.assertEqual(state.desired_accel, -3.2)
self.assertEqual(state.actual_accel, -3.2)
def test_stock_ioniq_and_other_cars_do_not_install_this_policy(self):
for car in (CAR.HYUNDAI_IONIQ_6, CAR.HYUNDAI_IONIQ_5, CAR.KIA_EV6):
cp = CarInterface.get_non_essential_params(car)
@@ -1,75 +0,0 @@
"""Actual controller frames stay on the selected Panda and retain native limits."""
from pathlib import Path
from tempfile import TemporaryDirectory
import shutil
import pytest
from opendbc.can import CANPacker
from opendbc.car import Bus, structs
from opendbc.car.hyundai.interface import CarInterface
from opendbc.car.hyundai.carcontroller import CarController
from opendbc.car.hyundai.hyundaicanfd import CanBus
from opendbc.car.hyundai.tests.test_ioniq6_handoff import ioniq6_profiles
from opendbc.car.hyundai.values import DBC
from opendbc.safety.tests.libsafety import libsafety_py
@pytest.mark.parametrize('offset', (0, 4))
@pytest.mark.parametrize('alternate', (False, True))
@pytest.mark.parametrize('lateral,longitudinal', ((False, False), (True, False), (False, True), (True, True)))
def test_actual_controller_transport_and_native_axis_authority(offset, alternate, lateral, longitudinal):
_, _, cp = ioniq6_profiles(bus_offset=offset, alternate=alternate)
cp.safetyConfigs[-1].safetyParam |= 0x800
cp.alternativeExperience = 32
ci = CarInterface(cp)
packer = CANPacker(DBC[cp.carFingerprint][Bus.pt])
bus = CanBus(cp)
physical = [packer.make_can_msg(name, bus.ECAN, values) for name, values in (
('ACCELERATOR', {'GEAR': 5}), ('TCS', {'ACCEnable': 0, 'ACC_REQ': 1}),
('WHEEL_SPEEDS', {}), ('MDPS', {}), ('CRUISE_BUTTONS', {'COUNTER': 1}),
('DOORS_SEATBELTS', {'DRIVER_SEATBELT': 1}), ('STEERING_SENSORS', {}))]
physical.append(packer.make_can_msg('CAM_0x362' if alternate else 'CAM_0x2a4', bus.CAM, {}))
ci.update([(1_000_000_000, physical)])
safety = libsafety_py.libsafety
def packet(frame):
return libsafety_py.make_CANPacket(frame[0], frame[2] - offset, frame[1])
release = safety.set_safety_hooks(structs.CarParams.SafetyModel.allOutput, 0) != 0
assert safety.set_safety_hooks(structs.CarParams.SafetyModel.hyundaiCanfd, cp.safetyConfigs[-1].safetyParam) == 0
safety.init_tests()
safety.set_timer(1_000_000)
safety.set_aol_test_heartbeat(True)
for frame in physical[:5]:
assert safety.safety_rx_hook(packet(frame))
if lateral:
for counter, pressed in ((2, 1), (3, 0)):
assert safety.safety_rx_hook(packet(packer.make_can_msg('CRUISE_BUTTONS', bus.ECAN, {'COUNTER': counter, 'LDA_BTN': pressed})))
if longitudinal:
for counter, button in ((4, 2), (5, 0)):
assert safety.safety_rx_hook(packet(packer.make_can_msg('CRUISE_BUTTONS', bus.ECAN, {'COUNTER': counter, 'CRUISE_BUTTONS': button})))
mask = int(lateral) | (int(longitudinal) << 1)
safety.aol_set_host_request(mask)
assert safety.aol_get_permission_mask() == (0 if release else mask)
cc = structs.CarControl()
cc.enabled = cc.longActive = longitudinal
cc.latActive = lateral
cc.actuators.torque = 0.01
cc.actuators.accel = 0.1
cc.actuators.longControlState = structs.CarControl.Actuators.LongControlState.starting
_, frames = CarController(DBC[cp.carFingerprint], cp).update(cc.as_reader(), ci.CS, 1_000_000_000)
assert frames and {frame[2] for frame in frames} <= {offset, offset + 1, offset + 2}
assert next(frame for frame in frames if frame[0] == 0x100).src == offset
assert all(frame[2] == offset + 1 for frame in frames if frame[0] in (0x1BA, 0x1E5, 0x1A0))
for frame in frames:
accepted = safety.safety_tx_hook(packet(frame))
assert bool(accepted) == (not release), (hex(frame[0]), frame[2])
if offset:
with TemporaryDirectory() as directory:
# A separate loaded library gives the passive internal Panda independent native state.
library = Path(directory) / 'internal.so'
shutil.copyfile(libsafety_py.libpath, library)
aux = libsafety_py.ffi.dlopen(str(library))
assert aux.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0) == 0
aux.init_tests()
for frame in frames:
assert not aux.safety_tx_hook(packet(frame))
@@ -1,224 +0,0 @@
"""Ioniq 6 offset transport preserves the selected owner and fail-closed startup."""
from dataclasses import replace
from types import SimpleNamespace
from unittest.mock import Mock, patch
import pytest
from opendbc.car import gen_empty_fingerprint, structs
from opendbc.car.can_definitions import CanData
from opendbc.car.hyundai import ioniq6_handoff as handoff
from opendbc.car.hyundai.hyundaicanfd import CanBus, create_ioniq6_radar_heartbeat, create_ioniq6_blindspot_status
from opendbc.car.hyundai.ioniq6_bsm import BlindspotStatus
from opendbc.car.hyundai.ioniq6_media import Ioniq6MediaButtons
from opendbc.car.hyundai.radar_interface import RadarInterface, radar_bus
from opendbc.car.hyundai.tests import test_ioniq6_handoff as fixtures
from opendbc.car.hyundai.tests.test_ioniq6_handoff import FakeIoniq6Bus, ioniq6_profiles
from openpilot.selfdrive.car.card import Car
from openpilot.starpilot.car.hyundai.aol import ioniq6_settings_capable, qualified_ioniq6, native_admission_key
from openpilot.starpilot.conditional_mode.manual import ioniq6_media_eligible
from openpilot.starpilot.longitudinal.ioniq6_start import eligible
from openpilot.starpilot.aol.runtime import native_inventory_matches_cp
from openpilot.starpilot.aol.wire import PandaSlot, SafetyState
@pytest.mark.parametrize('offset', (0, 4))
@pytest.mark.parametrize('alternate', (False, True))
def test_factory_and_preparation_retain_exact_transport_owner(offset, alternate):
fingerprint, stock, cp = ioniq6_profiles(alternate=alternate, bus_offset=offset)
assert not stock.radarUnavailable, 'OFFSET_MRR35_RADAR_NOT_DISCOVERED'
assert not stock.openpilotLongitudinalControl and stock.pcmCruise
assert ioniq6_settings_capable(stock)
assert cp is not None, 'OFFSET_IONIQ6_LONG_CANDIDATE_NOT_PREPARED'
assert cp.openpilotLongitudinalControl and cp.alphaLongitudinalAvailable and not cp.pcmCruise
assert eligible(cp)
assert len(cp.safetyConfigs) == 1 + offset // 4
assert cp.safetyConfigs[-1].safetyParam == (0x8095 if alternate else 0x8015)
if offset:
assert cp.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.noOutput
assert cp.safetyConfigs[0].safetyParam == 0
assert (CanBus(cp).ACAN, CanBus(cp).ECAN, CanBus(cp).CAM) == (offset, offset + 1, offset + 2)
assert radar_bus(cp) == offset
assert RadarInterface(cp).rcp.bus == offset
assert ioniq6_media_eligible(cp)
cp.safetyConfigs[-1].safetyParam |= 0x800
cp.alternativeExperience = 32
assert qualified_ioniq6(cp) and eligible(cp)
assert native_admission_key(cp) is not None
wrong = gen_empty_fingerprint()
for bus, frames in fingerprint.items():
if frames:
wrong[(bus + 4) % 8].update(frames)
assert handoff.build_ioniq6_hda2_long_candidate(stock, wrong) is None
@pytest.mark.parametrize('offset', (0, 4))
@pytest.mark.parametrize('option,expected', (({}, handoff.HandoffOutcome.CONFIRMED),
({'disable_works': False}, handoff.HandoffOutcome.STOCK),
({'disable_ack': False}, handoff.HandoffOutcome.STOCK),
({'heartbeat_stays_after_disable': True}, handoff.HandoffOutcome.STOCK),
({'bsm_stays_after_disable': True}, handoff.HandoffOutcome.STOCK),
({'post_missing_required': 0xEA, 'restore_works': False}, handoff.HandoffOutcome.UNAVAILABLE)))
def test_owner_bus_handoff_and_restore_keep_original_evidence(offset, option, expected):
bus = FakeIoniq6Bus(bus_offset=offset, **option)
result = bus.run()
assert result.outcome is expected
if expected is handoff.HandoffOutcome.CONFIRMED:
assert result.disable_acknowledged and result.source_period == pytest.approx(.02, abs=.001)
else:
assert bus.queries[-2:] == [(b'\x28\x00\x01', b'\x68\x00'), (b'\x10\x01', b'\x50\x01')]
def test_foreign_bus_and_echo_cannot_authorize_external_takeover():
bus = FakeIoniq6Bus()
with patch('opendbc.car.hyundai.ioniq6_handoff._ioniq6_boot_ns', side_effect=lambda: int(bus.now * 1e9)):
result = handoff.inspect_ioniq6_long_sources(bus.recv, lambda: bus.now, ecan_bus=5)
assert result.source_period is None
assert bus.queries == []
observed = handoff.ObservingCanRecv(bus.recv, lambda: bus.now, ecan_bus=5)
observed()
assert observed.source_seen_count == observed.heartbeat_seen_count == 0
@pytest.mark.parametrize('offset', (0, 4))
def test_real_isotp_routes_ack_restore_and_tester_to_the_same_owner(offset):
pending = []
sent = []
def recv(wait_for_one=False):
frames = pending[:]
pending.clear()
return [frames] if frames else []
def send(frames):
sent.extend(frames)
for frame in frames:
assert frame.src == offset + 1
request = frame.dat[1:1 + frame.dat[0]]
response = {b'\x10\x03': b'\x50\x03\x00\x32\x01\xf4', b'\x28\x01\x01': b'\x68\x01',
b'\x28\x00\x01': b'\x68\x00', b'\x10\x01': b'\x50\x01\x00\x32\x01\xf4'}[request]
pending.append(CanData(0x738, bytes([len(response)]) + response + bytes(7 - len(response)), offset + 1))
for request, response in ((b'\x10\x03', b'\x50\x03'), (b'\x28\x01\x01', b'\x68\x01'),
(b'\x28\x00\x01', b'\x68\x00'), (b'\x10\x01', b'\x50\x01')):
assert handoff.query_ioniq6_adas(recv, send, request, response, ecan_bus=offset + 1)
keepalive = handoff.Ioniq6StartupKeepalive(sent.extend, lambda: True, Mock(), Mock(),
clock=lambda: 0., ecan_bus=offset + 1)
keepalive.tick()
keepalive.stop()
assert sent[-1].address == 0x730 and sent[-1].src == offset + 1
assert bytes(sent[-1].dat[:3]) == b'\x02\x3e\x80'
@pytest.mark.parametrize('offset', (0, 4))
def test_media_and_replacement_payloads_keep_bytes_and_only_shift_transport(offset):
source = Ioniq6MediaButtons(offset + 1)
payload = bytes((0, 0, 0x40, 0, 0, 0x10, 0, 0))
stamp = 10_000_000_000
assert not source.update([(stamp, [(0x448, payload, 129 + offset)])]).valid
sample = source.update([(stamp + 1, [(0x448, payload, offset + 1)])])
assert sample.valid and sample.samples[0].mode_held and sample.samples[0].custom_held
original = create_ioniq6_radar_heartbeat(19, True, False)
shifted = create_ioniq6_radar_heartbeat(19, True, False, bus=offset)
assert shifted.src == offset and shifted.dat == original.dat
original = create_ioniq6_blindspot_status(19, BlindspotStatus(1, 2))
shifted = create_ioniq6_blindspot_status(19, BlindspotStatus(1, 2), bus=offset + 1)
assert [(m.address, m.dat) for m in original] == [(m.address, m.dat) for m in shifted]
assert all(m.src == offset + 1 for m in shifted)
@pytest.mark.parametrize('mutation', ('aux_word', 'aux_model', 'swapped', 'third', 'owner_model', 'angle'))
def test_invalid_owner_topologies_never_borrow_long_settings_or_admission(mutation):
_, stock, cp = ioniq6_profiles(bus_offset=4, alternate=True)
cp.safetyConfigs[-1].safetyParam |= 0x800
cp.alternativeExperience = 32
if mutation == 'aux_word':
cp.safetyConfigs[0].safetyParam = 1
elif mutation == 'aux_model':
cp.safetyConfigs[0].safetyModel = structs.CarParams.SafetyModel.elm327
elif mutation == 'owner_model':
cp.safetyConfigs[-1].safetyModel = structs.CarParams.SafetyModel.noOutput
elif mutation == 'swapped':
cp.safetyConfigs = list(reversed(cp.safetyConfigs))
elif mutation == 'third':
cp.safetyConfigs = [*cp.safetyConfigs, cp.safetyConfigs[0]]
else:
cp.steerControlType = structs.CarParams.SteerControlType.angle
assert not eligible(cp) and not qualified_ioniq6(cp)
assert not ioniq6_settings_capable(cp)
if mutation != 'angle':
assert native_admission_key(cp) is None
assert handoff.prepare_ioniq6_long_candidate(stock, cp, enabled=False, is_release=False) == (stock, False)
def test_diagnostic_owner_and_configured_passive_slot_remain_separate():
case = fixtures.TestIoniq6PrepublicationHandoff(methodName='runTest')
card = case.ready_card(ioniq_selected=True, bus_offset=4)
assert card.ioniq6_panda_diagnostic_ready()
card.sm.panda[0].safetyParam = 0
assert card.ioniq6_panda_diagnostic_ready() # internal OBD multiplexing, external remains unchanged
card.sm.panda[1].safetyParam = 0
assert not card.ioniq6_panda_diagnostic_ready()
card.sm.panda[1].safetyParam = 1
card.sm.panda[0].controlsAllowed = True
assert not card.ioniq6_panda_diagnostic_ready()
card.sm.panda[0].controlsAllowed = False
card.sm.panda.append(SimpleNamespace(safetyModel=3, safetyParam=1, controlsAllowed=False))
assert not card.ioniq6_panda_diagnostic_ready()
card.sm.panda.pop()
for state, cfg in zip(card.sm.panda, card.CP.safetyConfigs, strict=True):
state.safetyModel, state.safetyParam = cfg.safetyModel, cfg.safetyParam
state.alternativeExperience = card.CP.alternativeExperience
state.safetyRxChecksInvalid = False
assert not card.ioniq6_panda_diagnostic_ready()
assert Car.ioniq6_panda_matches(card)
card.sm.panda.append(card.sm.panda[0])
assert not Car.ioniq6_panda_matches(card)
@pytest.mark.parametrize('fault', ('none', 'aux_controls', 'aux_fault', 'owner_fault', 'aux_rx', 'owner_heartbeat',
'duplicate', 'missing', 'extra', 'swapped', 'wrong_owner', 'unbound'))
def test_complete_native_inventory_rejects_every_fault_or_identity_mismatch(fault):
_, _, cp = ioniq6_profiles(bus_offset=4, alternate=True)
cp.safetyConfigs[-1].safetyParam |= 0x800
cp.alternativeExperience = 32
aux = PandaSlot(0, 'internal', int(cp.safetyConfigs[0].safetyModel.raw), 0, 32, False, False, False, 0)
owner = PandaSlot(1, 'external', int(cp.safetyConfigs[1].safetyModel.raw), 0x8895, 32, True, False, False, 0)
slots = [aux, owner]
serial = 'external'
source = 10_000_000_000
if fault == 'aux_controls':
slots[0] = replace(aux, controlsAllowed=True)
elif fault == 'aux_fault':
slots[0] = replace(aux, faults=4)
elif fault == 'owner_fault':
slots[1] = replace(owner, faults=4)
elif fault == 'aux_rx':
slots[0] = replace(aux, safetyRxChecksInvalid=True)
elif fault == 'owner_heartbeat':
slots[1] = replace(owner, heartbeatLost=True)
elif fault == 'duplicate':
slots[0] = replace(aux, hardwareSerial='external')
elif fault == 'missing':
slots.pop(0)
elif fault == 'extra':
slots.append(aux)
elif fault == 'swapped':
slots.reverse()
elif fault == 'wrong_owner':
serial = 'internal'
elif fault == 'unbound':
source = 0
native = SafetyState(protocolVersion=1, compatible=True, observedMonoTime=source,
validUntilMonoTime=source + 30_000_000, safetyModel=28, safetyParam=0x8895,
lateralAllowed=True, longitudinalAllowed=True, requestedLateral=True, requestedLongitudinal=True,
pandaSerial=serial, axisSessionId='axis-session', sourcePandaStatesMonoTime=source,
pandaInventory=tuple(slots))
assert native_inventory_matches_cp(native, cp) == (fault == 'none')
@pytest.mark.parametrize('alternate', (False, True))
@pytest.mark.parametrize('ready,unknown', ((False, False), (True, False), (False, True)))
def test_actual_card_cold_ready_unknown_selection_on_external_transport(alternate, ready, unknown):
case = fixtures.TestIoniq6PrepublicationHandoff(methodName='runTest')
try:
case.assert_card_prepublication_fallback({}, alternate=alternate, vehicle_ready=ready, unknown=unknown, bus_offset=4)
finally:
case.doCleanups()
@@ -1,82 +0,0 @@
from unittest.mock import patch
import pytest
from opendbc.car.hyundai import blended_longitudinal as mixed
from opendbc.car.hyundai.tests.test_blended_stock_aol import params
@pytest.fixture(params=(False, True))
def cp(request):
cache = getattr(mixed, '_mixed_startup_for_bytes', None)
if cache is not None:
cache.cache_clear()
return params(request.param)
def test_same_static_configuration_is_qualified_once(cp):
before = cp.to_dict()
with patch.object(mixed, 'alpha_eligible', wraps=mixed.alpha_eligible) as evaluate:
for _ in range(20):
assert mixed.mixed_startup_qualified(cp)
assert mixed.mixed_startup_qualified(cp.as_reader())
assert evaluate.call_count == 1
assert cp.to_dict() == before
@pytest.mark.parametrize('field', ('flags', 'brand', 'carFingerprint', 'passive', 'dashcamOnly', 'notCar',
'steerControlType', 'alternativeExperience', 'safetyConfigs', 'safetyModel',
'safetyParam', 'pcmCruise', 'openpilotLongitudinalControl', 'mass', 'carVin', 'carFw'))
def test_changes_on_same_builder_are_requalified(cp, field):
uncached = getattr(mixed, '_mixed_startup_qualified_uncached', mixed.mixed_startup_qualified)
assert mixed.mixed_startup_qualified(cp)
if field in ('passive', 'dashcamOnly', 'notCar', 'pcmCruise', 'openpilotLongitudinalControl'):
setattr(cp, field, not getattr(cp, field))
elif field in ('flags', 'alternativeExperience'):
setattr(cp, field, int(getattr(cp, field)) ^ 1)
elif field == 'safetyConfigs':
cp.safetyConfigs = []
elif field == 'safetyParam':
cp.safetyConfigs[0].safetyParam ^= 1
elif field == 'safetyModel':
cp.safetyConfigs[0].safetyModel = 'noOutput'
elif field == 'steerControlType':
cp.steerControlType = 'angle'
elif field == 'mass':
cp.mass += 1
elif field == 'carFw':
cp.carFw = [{'ecu': 'eps', 'fwVersion': b'changed'}]
else:
setattr(cp, field, 'changed')
with patch.object(mixed, 'alpha_eligible', wraps=mixed.alpha_eligible) as evaluate:
for release in (False, True):
for marked in (False, True):
assert mixed.mixed_startup_qualified(cp, is_release=release, allow_marked=marked) == uncached(cp, is_release=release, allow_marked=marked)
assert evaluate.call_count > 0
@pytest.mark.parametrize('longitudinal', (False, True))
@pytest.mark.parametrize('marked', (False, True))
def test_stock_long_and_aol_profiles_retain_release_restriction(cp, longitudinal, marked):
cp.openpilotLongitudinalControl = longitudinal
cp.pcmCruise = not longitudinal
if longitudinal:
cp.safetyConfigs[0].safetyParam |= 4
cp.alternativeExperience = 32 if marked else 0
assert mixed.mixed_startup_qualified(cp, allow_marked=marked)
assert not mixed.mixed_startup_qualified(cp, is_release=True, allow_marked=marked)
if marked:
assert not mixed.mixed_startup_qualified(cp)
def test_proxy_and_snapshot_failure_retain_original_validation(cp):
class Proxy:
def __getattr__(self, name):
return getattr(cp, name)
assert mixed.mixed_startup_qualified(Proxy())
cached = getattr(mixed, '_mixed_startup_for_bytes', None)
if cached is not None:
with patch.object(mixed, '_mixed_startup_for_bytes', side_effect=RuntimeError('cache unavailable')):
assert mixed.mixed_startup_qualified(cp)
cp.passive = True
assert not mixed.mixed_startup_qualified(cp)
@@ -1,125 +0,0 @@
"""Direct PE EPS output retains its LFA target envelope and native handoff checks."""
from types import SimpleNamespace
import unittest
import pytest
from opendbc.can import CANPacker
from opendbc.car import structs
from opendbc.car.hyundai.interface import CarInterface
from opendbc.car.hyundai.tests.test_angle_fault_recovery import controller, profile
from opendbc.car.hyundai.tests.test_ev9_long_controller import packets
from opendbc.car.hyundai.values import CAR
def direct(frames):
_, data, bus = next(frame for frame in frames if frame[0] == 0xCB)
raw = (int.from_bytes(data, 'little') >> 32) & 0x3FFF
angle = (raw - 0x4000 if raw & 0x2000 else raw) * .1
return angle, (data[3] >> 4) & 15, data[6] * .004, bus
@pytest.mark.parametrize('sign', (-1, 1))
@pytest.mark.parametrize('aol', (False, True))
def test_pe_direct_target_and_measured_history_stay_in_reference_envelope(sign, aol):
ci, _ = controller(CAR.HYUNDAI_IONIQ_5_PE, True, aol)
cs = ci.CS
cs.out.vEgo = cs.out.vEgoRaw = 2.
cs.out.steeringAngleDeg = cs.angle_steering_angle = 0.
cs.out.steeringPressed = False
cs.out.steeringTorque = 0.
command = structs.CarControl(enabled=not aol, latActive=True)
command.actuators.steeringAngleDeg = sign * 360.
peak = 0.
for tick in range(400):
ci.CC.frame = tick + 21
_, frames = ci.apply(command.as_reader(), (tick + 21) * 10_000_000)
angle, active, gain, bus = direct(frames)
assert bus == 1
assert active == 2 and gain > 0
assert abs(angle) <= 175.0001, 'PE_DIRECT_EPS_TARGET_EXCEEDED_REFERENCE_ENVELOPE'
peak = max(peak, abs(angle))
assert peak == pytest.approx(175., abs=.1)
for measured in (sign * 200., sign * 300., sign * 170., sign * 170.):
cs.out.steeringAngleDeg = cs.angle_steering_angle = measured
previous = ci.CC.apply_angle_last
ci.CC.frame += 1
_, frames = ci.apply(command.as_reader(), (ci.CC.frame + 21) * 10_000_000)
angle, active, gain, _ = direct(frames)
if abs(measured) > 175 or abs(previous) > 175:
assert active == 1 and gain == 0
assert angle == pytest.approx(measured, abs=.1)
else:
assert active == 2 and gain == pytest.approx(.004)
assert abs(angle) <= 175.
assert abs(angle - measured) <= 5.
def test_ev9_and_stock_camera_envelopes_are_preserved():
for identity, long, expected in ((CAR.KIA_EV9, True, 140.), (CAR.HYUNDAI_IONIQ_5_PE, False, 360.)):
ci, _ = controller(identity, long, True)
cs = ci.CS
cs.out.vEgo = cs.out.vEgoRaw = 2.
cs.out.steeringAngleDeg = cs.angle_steering_angle = 0.
cs.out.cruiseState.enabled = True
cs.out.steeringPressed = False
cs.out.steeringTorque = 0.
command = structs.CarControl(enabled=True, latActive=True)
command.actuators.steeringAngleDeg = 360.
for tick in range(400):
ci.CC.frame = tick + 21
ci.apply(command.as_reader(), (tick + 21) * 10_000_000)
assert ci.CC.apply_angle_last == pytest.approx(expected, abs=.1)
assert ci.CC.params.ANGLE_LIMITS.STEER_ANGLE_MAX == 360.
@pytest.mark.parametrize('sign', (-1, 1))
def test_actual_pe_direct_output_and_outside_envelope_reentry_are_native_admitted(sign):
from opendbc.safety.tests.libsafety import libsafety_py
from opendbc.safety.tests.test_ioniq5pe_paired_native import PEFixture
class Native(PEFixture, unittest.TestCase):
pass
case = Native(methodName='runTest')
case.setUp()
ci = CarInterface(profile(CAR.HYUNDAI_IONIQ_5_PE, True, False))
ci.CC.pe_startup = SimpleNamespace(handed_off=True, can_emit=lambda controller: True)
packer = CANPacker('hyundai_canfd_generated')
command = structs.CarControl(enabled=True, latActive=True, longActive=True)
command.actuators.steeringAngleDeg = sign * 360.
ci.update([])
active_count = 0
for tick in range(1, 801):
case.now = tick * 10_000
case.safety.set_timer(case.now)
frames = packets(packer, tick, moving=False, advance_counter=True,
cruise_button=2 if tick == 12 else 0, adaptive_main=tick == 8)
wheel = packer.make_can_msg('WHEEL_SPEEDS', 1, {})[0]
mdps = packer.make_can_msg('MDPS', 1, {})[0]
frames = [f for f in frames if f[0] not in (wheel, mdps)]
frames.append(packer.make_can_msg('WHEEL_SPEEDS', 1, {'COUNTER': tick,
**dict.fromkeys(('WHL_SpdFLVal', 'WHL_SpdFRVal', 'WHL_SpdRLVal', 'WHL_SpdRRVal'), 7.2)}))
measured = sign * 300. if 550 <= tick < 570 else 0.
frames.append(packer.make_can_msg('MDPS', 1, {'COUNTER': tick,
'MDPS_EstStrAnglVal': measured, 'MDPS_StrTqSnsrVal': 0.}))
for address, data, bus in frames:
assert case.safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data)), (tick, hex(address))
ci.update([(tick * 10_000_000, frames)])
command.enabled = command.latActive = tick >= 14
_, sends = ci.apply(command.as_reader(), tick * 10_000_000)
if tick < 14:
continue
angle, active, gain, _ = direct(sends)
for address, data, bus in sends:
assert case.safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)), (tick, hex(address), bus)
if active == 2:
active_count += 1
assert abs(angle) <= 175.0001, 'PE_DIRECT_EPS_TARGET_EXCEEDED_REFERENCE_ENVELOPE'
if 550 <= tick <= 570:
assert active == 1 and gain == 0
assert angle == pytest.approx(measured, abs=.1)
if tick == 800:
assert active == 2 and abs(angle) == pytest.approx(175., abs=.1)
assert active_count > 600
+1 -18
View File
@@ -1,15 +1,8 @@
"""Passive Tesla query-wiring selection; live firmware still owns vehicle identity."""
"""Passive AP1 query-wiring selection; live firmware still owns vehicle identity."""
HW1_NORMAL_BUS0_LAYOUT = {(0, 0x108): 8, (0, 0x155): 8, (0, 0x20A): 8,
(0, 0x368): 8, (0, 0x370): 8, (2, 0x488): 4, (2, 0x2B9): 8}
# Modern party-bus driver/chassis state and stock Autopilot messages are already
# visible without switching CAN2 to OBD wiring. This only scopes live queries;
# it does not distinguish Model 3/Y, establish firmware support or grant control.
MODERN_NORMAL_BUS0_LAYOUT = {(0, 0x118): 8, (0, 0x145): 8, (0, 0x155): 8,
(0, 0x257): 8, (0, 0x311): 7, (0, 0x370): 8,
(2, 0x488): 4, (2, 0x2B9): 8}
def hw1_normal_bus0_topology(fingerprint):
try:
@@ -17,13 +10,3 @@ def hw1_normal_bus0_topology(fingerprint):
for (bus, address), length in HW1_NORMAL_BUS0_LAYOUT.items())
except (AttributeError, TypeError, ValueError):
return False
def normal_bus0_topology(fingerprint):
if hw1_normal_bus0_topology(fingerprint):
return True
try:
return all(fingerprint.get(bus, {}).get(address) == length
for (bus, address), length in MODERN_NORMAL_BUS0_LAYOUT.items())
except (AttributeError, TypeError, ValueError):
return False
@@ -1,143 +0,0 @@
"""Modern Tesla discovery must keep normal wiring without inferring authority."""
from dataclasses import replace
from unittest import mock
import pytest
from opendbc.car import car_helpers, fw_versions, gen_empty_fingerprint
from opendbc.car.can_definitions import CanData
from opendbc.car.ford.discovery import normal_bus_query_brand
from opendbc.car.tesla.discovery import HW1_NORMAL_BUS0_LAYOUT, MODERN_NORMAL_BUS0_LAYOUT, normal_bus0_topology
from opendbc.car.tesla.interface import CarInterface
from opendbc.car.tesla.tests.test_automatic_hw1_discovery import AP1Ecu, automatic
from opendbc.car.tesla.tests.test_selected_discovery import VIN
from opendbc.car.tesla.values import CAR
from opendbc.car.vin import VIN_UNKNOWN
FIRMWARE = {
CAR.TESLA_MODEL_3: b'TeMYG4_Main_0.0.0 (92),E4L015.10.2',
CAR.TESLA_MODEL_Y: b'TeMYG4_Main_0.0.0 (92),Y4003.10.2',
}
class ModernEcu(AP1Ecu):
def __init__(self, platform=CAR.TESLA_MODEL_3, *, marker=True, vehicle_bus=False, firmware=None):
super().__init__(FIRMWARE[platform] if firmware is None else firmware)
self.layout = dict(MODERN_NORMAL_BUS0_LAYOUT)
self.layout[2, 0x293] = 8
if marker:
self.layout[0, 0x054] = 8
self.layout[2, 0x489] = 8
if vehicle_bus:
self.layout[1, 0x3DF] = 8
def recv(self, wait_for_one=False):
self.clock += .001
values, self.frames = self.frames, []
return [values + [CanData(address, bytes(length), bus)
for (bus, address), length in self.layout.items()]]
@pytest.mark.parametrize('platform', list(FIRMWARE))
@pytest.mark.parametrize('alpha,release', [(False, False), (True, False), (True, True)])
@pytest.mark.parametrize('vehicle_bus', [False, True])
def test_automatic_modern_tesla_keeps_live_vin_fw_can_and_factory(platform, alpha, release, vehicle_bus):
peer = ModernEcu(platform, vehicle_bus=vehicle_bus)
ci, callbacks = automatic(peer, alpha=alpha, release=release)
assert callbacks and set(callbacks) == {False}
assert peer.sent and all(frame.src == 0 for frame in peer.sent)
assert ci.CP.carFingerprint == platform and ci.CP.carVin == VIN
assert ci.CP.fingerprintSource == car_helpers.CarParams.FingerprintSource.fw
assert ci.CP.carFw and all(fw.brand == 'tesla' and fw.bus == 0 for fw in ci.CP.carFw)
assert any(fw.fwVersion == FIRMWARE[platform] for fw in ci.CP.carFw)
fp = gen_empty_fingerprint()
for (bus, address), length in peer.layout.items():
fp[bus][address] = length
direct = CarInterface.get_params(platform, fp, list(ci.CP.carFw), alpha, release, False)
for field in ('flags', 'alternativeExperience', 'pcmCruise', 'openpilotLongitudinalControl',
'alphaLongitudinalAvailable', 'radarUnavailable', 'dashcamOnly'):
assert getattr(ci.CP, field) == getattr(direct, field)
assert [cfg.to_dict() for cfg in ci.CP.safetyConfigs] == [cfg.to_dict() for cfg in direct.safetyConfigs]
@pytest.mark.parametrize('source', list(MODERN_NORMAL_BUS0_LAYOUT))
@pytest.mark.parametrize('defect', ['missing', 'length', 'bus', 'echo'])
def test_modern_query_scope_requires_every_source(source, defect):
fp = {}
for (bus, address), length in MODERN_NORMAL_BUS0_LAYOUT.items():
if (bus, address) == source:
if defect == 'missing':
continue
if defect == 'length':
length -= 1
else:
bus += 128 if defect == 'echo' else 1
fp.setdefault(bus, {})[address] = length
assert not normal_bus0_topology(fp)
@pytest.mark.parametrize('layout', [HW1_NORMAL_BUS0_LAYOUT, MODERN_NORMAL_BUS0_LAYOUT])
def test_conflicting_lengths_reject_both_tesla_query_layouts(layout):
(bus, address), length = next(iter(layout.items()))
rows = [CanData(address, bytes(length - 1), bus),
*(CanData(address, bytes(length), bus) for (bus, address), length in layout.items())]
assert normal_bus_query_brand(lambda **kwargs: [rows], lambda recv: bool(recv() and False),
tesla_probe=normal_bus0_topology, clock=lambda: 0.) is None
@pytest.mark.parametrize('firmware', [b'', b'unknown-modern-tesla-eps', b'TeMYG4_Main'])
def test_modern_layout_cannot_replace_live_identity(firmware):
ci, callbacks = automatic(ModernEcu(firmware=firmware))
assert set(callbacks) == {False}
assert ci.CP.carFingerprint not in FIRMWARE
def test_modern_layout_cannot_enable_old_steering_encoding():
ci, callbacks = automatic(ModernEcu(marker=False))
assert set(callbacks) == {False}
assert ci.CP.carFingerprint == CAR.TESLA_MODEL_3 and ci.CP.dashcamOnly
def test_missing_vin_keeps_live_firmware_without_switching_wiring():
peer = ModernEcu()
reply = peer.reply
with mock.patch.object(peer, 'reply', side_effect=lambda tx, rx, value: reply(tx, rx, value) if rx != 0x7E8 else None):
ci, callbacks = automatic(peer)
assert set(callbacks) == {False} and all(frame.src == 0 for frame in peer.sent)
assert ci.CP.carVin == VIN_UNKNOWN and ci.CP.carFingerprint == CAR.TESLA_MODEL_3
assert any(fw.fwVersion == FIRMWARE[CAR.TESLA_MODEL_3] for fw in ci.CP.carFw)
def test_modern_probe_keeps_shared_read_budget():
calls = []
assert normal_bus_query_brand(lambda **kwargs: calls.append(kwargs) or [], lambda recv: bool(recv() and False),
tesla_probe=normal_bus0_topology, clock=lambda: 0.) is None
assert len(calls) == 24
@pytest.mark.parametrize('registry', ['configured', 'effective'])
@pytest.mark.parametrize('change', ['bus1', 'empty'])
def test_automatic_scope_requires_unchanged_query_topology(registry, change):
if registry == 'configured':
target, name = fw_versions.FW_QUERY_CONFIGS['tesla'], 'requests'
altered = [replace(request, bus=1) for request in target.requests] if change == 'bus1' else []
else:
target, name = fw_versions, 'REQUESTS'
altered = [(brand, config, replace(request, bus=1) if brand == 'tesla' else request)
for brand, config, request in fw_versions.REQUESTS] if change == 'bus1' else [
item for item in fw_versions.REQUESTS if item[0] != 'tesla']
with mock.patch.object(target, name, altered), \
mock.patch.object(car_helpers, 'normal_bus_query_brand', return_value=None) as probe, \
mock.patch.object(car_helpers, 'get_present_ecus', return_value=set()), \
mock.patch.object(car_helpers, 'get_fw_versions_ordered', return_value=[]):
automatic(ModernEcu())
assert probe.call_args.kwargs['tesla_probe'] is None
@pytest.mark.parametrize('platform', list(FIRMWARE))
def test_discovery_keeps_native_can_fault_blocking(platform):
from openpilot.cereal import log
from openpilot.starpilot.aol.panda_faults import blocking_fault_list
ci, _ = automatic(ModernEcu(platform))
assert blocking_fault_list([log.PandaState.FaultType.interruptRateCan2], ci.CP)
assert blocking_fault_list([log.PandaState.FaultType.relayMalfunction], ci.CP)
+1 -10
View File
@@ -277,8 +277,6 @@ struct StarPilotLongitudinalPlan @0xda96579883444c35 {
modelMonoTime @2 :UInt64;
driveStartMonoTime @3 :UInt64;
forceStopHolding @4 :Bool;
forceStopBraking @5 :Bool;
forceStopEpisodeMonoTime @6 :UInt64;
}
struct SlcState @0xa1680744031fdb2d {
@@ -341,7 +339,7 @@ struct SlcState @0xa1680744031fdb2d {
hasPendingAdjustedLimit @45 :Bool;
presentationAdjustedLimit @46 :Float32;
hasPresentationAdjustedLimit @47 :Bool;
overrideBasis @48 :Text; # none/pedal/persistent/wheel; qualified contribution only
overrideBasis @48 :Text; # none/pedal/persistent; qualified contribution only
retainedOverride @49 :Bool;
commandActionSequenceId @50 :UInt64; # UI sequence that issued this command
automaticAdoptionPending @51 :Bool;
@@ -354,10 +352,6 @@ struct SlcState @0xa1680744031fdb2d {
# Qualified adoption proposal; the current displayed observation stays above.
upcomingSpeedLimit @57 :Float32;
upcomingBoundaryId @58 :Text;
acceptedIsPreview @59 :Bool; # a temporary ceiling, ineligible for initializing SET
wheelOverrideSince @60 :UInt64; # nonzero while a wheel tap may cross the current adjusted limit
wheelAdoptMonoTime @61 :UInt64; # Card's original wheel event timestamp, only on the approval frame
wheelAdoptTargetMps @62 :Float32; # exact adjusted SET approved for that event
struct SourceReading {
source @0 :Text;
enabled @1 :Bool;
@@ -459,7 +453,6 @@ struct SlcAction @0x9ccdc8676701b412 {
conditionalManual @5 :ConditionalManualAction;
controllerCruise @6 :ControllerCruiseAction;
controllerMode @7 :ControllerCruiseAction;
wheelAdoptMonoTime @8 :UInt64; # original timestamp of a qualified Adopt Speed Limit wheel event
struct ControllerCruiseAction {
version @0 :UInt16;
@@ -527,7 +520,6 @@ struct SlcCruiseEvent @0xcd96dafb67a082d0 {
trafficMode @13 :TrafficModeGesture;
wheelAction @14 :SlcAction.ControllerCruiseAction;
teslaStockCruise @15 :TeslaStockCruise;
wheelOverrideSince @16 :UInt64; # SLC eligibility captured by a physical + tap; zero for ordinary SET changes
struct TeslaStockCruise {
version @0 :UInt16;
@@ -1579,4 +1571,3 @@ struct LiveTrackingResponseWitness @0xdaa1a9f0d8968e31 {
windowStartMonoTime @4 :UInt64; independentPoses @5 :UInt16;
residualRmse @6 :Float64; validUntilMonoTime @7 :UInt64;
}
File diff suppressed because it is too large Load Diff
+4 -156
View File
@@ -729,7 +729,7 @@ struct StarPilotLongitudinalPlan {
class Pipeline;
struct _capnpPrivate {
CAPNP_DECLARE_STRUCT_HEADER(da96579883444c35, 5, 0)
CAPNP_DECLARE_STRUCT_HEADER(da96579883444c35, 4, 0)
#if !CAPNP_LITE
static constexpr ::capnp::_::RawBrandedSchema const* brand() { return &schema->defaultBrand; }
#endif // !CAPNP_LITE
@@ -749,7 +749,7 @@ struct SlcState {
struct ConditionalModeState;
struct _capnpPrivate {
CAPNP_DECLARE_STRUCT_HEADER(a1680744031fdb2d, 21, 22)
CAPNP_DECLARE_STRUCT_HEADER(a1680744031fdb2d, 18, 22)
#if !CAPNP_LITE
static constexpr ::capnp::_::RawBrandedSchema const* brand() { return &schema->defaultBrand; }
#endif // !CAPNP_LITE
@@ -845,7 +845,7 @@ struct SlcAction {
struct _capnpPrivate {
CAPNP_DECLARE_STRUCT_HEADER(9ccdc8676701b412, 5, 4)
CAPNP_DECLARE_STRUCT_HEADER(9ccdc8676701b412, 4, 4)
#if !CAPNP_LITE
static constexpr ::capnp::_::RawBrandedSchema const* brand() { return &schema->defaultBrand; }
#endif // !CAPNP_LITE
@@ -897,7 +897,7 @@ struct SlcCruiseEvent {
struct ManualModeGesture;
struct _capnpPrivate {
CAPNP_DECLARE_STRUCT_HEADER(cd96dafb67a082d0, 8, 6)
CAPNP_DECLARE_STRUCT_HEADER(cd96dafb67a082d0, 7, 6)
#if !CAPNP_LITE
static constexpr ::capnp::_::RawBrandedSchema const* brand() { return &schema->defaultBrand; }
#endif // !CAPNP_LITE
@@ -4298,10 +4298,6 @@ public:
inline bool getForceStopHolding() const;
inline bool getForceStopBraking() const;
inline ::uint64_t getForceStopEpisodeMonoTime() const;
private:
::capnp::_::StructReader _reader;
template <typename, ::capnp::Kind>
@@ -4345,12 +4341,6 @@ public:
inline bool getForceStopHolding();
inline void setForceStopHolding(bool value);
inline bool getForceStopBraking();
inline void setForceStopBraking(bool value);
inline ::uint64_t getForceStopEpisodeMonoTime();
inline void setForceStopEpisodeMonoTime( ::uint64_t value);
private:
::capnp::_::StructBuilder _builder;
template <typename, ::capnp::Kind>
@@ -4534,14 +4524,6 @@ public:
inline bool hasUpcomingBoundaryId() const;
inline ::capnp::Text::Reader getUpcomingBoundaryId() const;
inline bool getAcceptedIsPreview() const;
inline ::uint64_t getWheelOverrideSince() const;
inline ::uint64_t getWheelAdoptMonoTime() const;
inline float getWheelAdoptTargetMps() const;
private:
::capnp::_::StructReader _reader;
template <typename, ::capnp::Kind>
@@ -4835,18 +4817,6 @@ public:
inline void adoptUpcomingBoundaryId(::capnp::Orphan< ::capnp::Text>&& value);
inline ::capnp::Orphan< ::capnp::Text> disownUpcomingBoundaryId();
inline bool getAcceptedIsPreview();
inline void setAcceptedIsPreview(bool value);
inline ::uint64_t getWheelOverrideSince();
inline void setWheelOverrideSince( ::uint64_t value);
inline ::uint64_t getWheelAdoptMonoTime();
inline void setWheelAdoptMonoTime( ::uint64_t value);
inline float getWheelAdoptTargetMps();
inline void setWheelAdoptTargetMps(float value);
private:
::capnp::_::StructBuilder _builder;
template <typename, ::capnp::Kind>
@@ -5699,8 +5669,6 @@ public:
inline bool hasControllerMode() const;
inline ::cereal::SlcAction::ControllerCruiseAction::Reader getControllerMode() const;
inline ::uint64_t getWheelAdoptMonoTime() const;
private:
::capnp::_::StructReader _reader;
template <typename, ::capnp::Kind>
@@ -5769,9 +5737,6 @@ public:
inline void adoptControllerMode(::capnp::Orphan< ::cereal::SlcAction::ControllerCruiseAction>&& value);
inline ::capnp::Orphan< ::cereal::SlcAction::ControllerCruiseAction> disownControllerMode();
inline ::uint64_t getWheelAdoptMonoTime();
inline void setWheelAdoptMonoTime( ::uint64_t value);
private:
::capnp::_::StructBuilder _builder;
template <typename, ::capnp::Kind>
@@ -6163,8 +6128,6 @@ public:
inline bool hasTeslaStockCruise() const;
inline ::cereal::SlcCruiseEvent::TeslaStockCruise::Reader getTeslaStockCruise() const;
inline ::uint64_t getWheelOverrideSince() const;
private:
::capnp::_::StructReader _reader;
template <typename, ::capnp::Kind>
@@ -6265,9 +6228,6 @@ public:
inline void adoptTeslaStockCruise(::capnp::Orphan< ::cereal::SlcCruiseEvent::TeslaStockCruise>&& value);
inline ::capnp::Orphan< ::cereal::SlcCruiseEvent::TeslaStockCruise> disownTeslaStockCruise();
inline ::uint64_t getWheelOverrideSince();
inline void setWheelOverrideSince( ::uint64_t value);
private:
::capnp::_::StructBuilder _builder;
template <typename, ::capnp::Kind>
@@ -18968,34 +18928,6 @@ inline void StarPilotLongitudinalPlan::Builder::setForceStopHolding(bool value)
::capnp::bounded<16>() * ::capnp::ELEMENTS, value);
}
inline bool StarPilotLongitudinalPlan::Reader::getForceStopBraking() const {
return _reader.getDataField<bool>(
::capnp::bounded<17>() * ::capnp::ELEMENTS);
}
inline bool StarPilotLongitudinalPlan::Builder::getForceStopBraking() {
return _builder.getDataField<bool>(
::capnp::bounded<17>() * ::capnp::ELEMENTS);
}
inline void StarPilotLongitudinalPlan::Builder::setForceStopBraking(bool value) {
_builder.setDataField<bool>(
::capnp::bounded<17>() * ::capnp::ELEMENTS, value);
}
inline ::uint64_t StarPilotLongitudinalPlan::Reader::getForceStopEpisodeMonoTime() const {
return _reader.getDataField< ::uint64_t>(
::capnp::bounded<4>() * ::capnp::ELEMENTS);
}
inline ::uint64_t StarPilotLongitudinalPlan::Builder::getForceStopEpisodeMonoTime() {
return _builder.getDataField< ::uint64_t>(
::capnp::bounded<4>() * ::capnp::ELEMENTS);
}
inline void StarPilotLongitudinalPlan::Builder::setForceStopEpisodeMonoTime( ::uint64_t value) {
_builder.setDataField< ::uint64_t>(
::capnp::bounded<4>() * ::capnp::ELEMENTS, value);
}
inline bool SlcState::Reader::hasSlotId() const {
return !_reader.getPointerField(
::capnp::bounded<0>() * ::capnp::POINTERS).isNull();
@@ -20282,62 +20214,6 @@ inline ::capnp::Orphan< ::capnp::Text> SlcState::Builder::disownUpcomingBoundary
::capnp::bounded<21>() * ::capnp::POINTERS));
}
inline bool SlcState::Reader::getAcceptedIsPreview() const {
return _reader.getDataField<bool>(
::capnp::bounded<141>() * ::capnp::ELEMENTS);
}
inline bool SlcState::Builder::getAcceptedIsPreview() {
return _builder.getDataField<bool>(
::capnp::bounded<141>() * ::capnp::ELEMENTS);
}
inline void SlcState::Builder::setAcceptedIsPreview(bool value) {
_builder.setDataField<bool>(
::capnp::bounded<141>() * ::capnp::ELEMENTS, value);
}
inline ::uint64_t SlcState::Reader::getWheelOverrideSince() const {
return _reader.getDataField< ::uint64_t>(
::capnp::bounded<18>() * ::capnp::ELEMENTS);
}
inline ::uint64_t SlcState::Builder::getWheelOverrideSince() {
return _builder.getDataField< ::uint64_t>(
::capnp::bounded<18>() * ::capnp::ELEMENTS);
}
inline void SlcState::Builder::setWheelOverrideSince( ::uint64_t value) {
_builder.setDataField< ::uint64_t>(
::capnp::bounded<18>() * ::capnp::ELEMENTS, value);
}
inline ::uint64_t SlcState::Reader::getWheelAdoptMonoTime() const {
return _reader.getDataField< ::uint64_t>(
::capnp::bounded<19>() * ::capnp::ELEMENTS);
}
inline ::uint64_t SlcState::Builder::getWheelAdoptMonoTime() {
return _builder.getDataField< ::uint64_t>(
::capnp::bounded<19>() * ::capnp::ELEMENTS);
}
inline void SlcState::Builder::setWheelAdoptMonoTime( ::uint64_t value) {
_builder.setDataField< ::uint64_t>(
::capnp::bounded<19>() * ::capnp::ELEMENTS, value);
}
inline float SlcState::Reader::getWheelAdoptTargetMps() const {
return _reader.getDataField<float>(
::capnp::bounded<40>() * ::capnp::ELEMENTS);
}
inline float SlcState::Builder::getWheelAdoptTargetMps() {
return _builder.getDataField<float>(
::capnp::bounded<40>() * ::capnp::ELEMENTS);
}
inline void SlcState::Builder::setWheelAdoptTargetMps(float value) {
_builder.setDataField<float>(
::capnp::bounded<40>() * ::capnp::ELEMENTS, value);
}
inline bool SlcState::SourceReading::Reader::hasSource() const {
return !_reader.getPointerField(
::capnp::bounded<0>() * ::capnp::POINTERS).isNull();
@@ -21845,20 +21721,6 @@ inline ::capnp::Orphan< ::cereal::SlcAction::ControllerCruiseAction> SlcAction::
::capnp::bounded<3>() * ::capnp::POINTERS));
}
inline ::uint64_t SlcAction::Reader::getWheelAdoptMonoTime() const {
return _reader.getDataField< ::uint64_t>(
::capnp::bounded<4>() * ::capnp::ELEMENTS);
}
inline ::uint64_t SlcAction::Builder::getWheelAdoptMonoTime() {
return _builder.getDataField< ::uint64_t>(
::capnp::bounded<4>() * ::capnp::ELEMENTS);
}
inline void SlcAction::Builder::setWheelAdoptMonoTime( ::uint64_t value) {
_builder.setDataField< ::uint64_t>(
::capnp::bounded<4>() * ::capnp::ELEMENTS, value);
}
inline ::uint16_t SlcAction::ControllerCruiseAction::Reader::getVersion() const {
return _reader.getDataField< ::uint16_t>(
::capnp::bounded<0>() * ::capnp::ELEMENTS);
@@ -22727,20 +22589,6 @@ inline ::capnp::Orphan< ::cereal::SlcCruiseEvent::TeslaStockCruise> SlcCruiseEve
::capnp::bounded<5>() * ::capnp::POINTERS));
}
inline ::uint64_t SlcCruiseEvent::Reader::getWheelOverrideSince() const {
return _reader.getDataField< ::uint64_t>(
::capnp::bounded<7>() * ::capnp::ELEMENTS);
}
inline ::uint64_t SlcCruiseEvent::Builder::getWheelOverrideSince() {
return _builder.getDataField< ::uint64_t>(
::capnp::bounded<7>() * ::capnp::ELEMENTS);
}
inline void SlcCruiseEvent::Builder::setWheelOverrideSince( ::uint64_t value) {
_builder.setDataField< ::uint64_t>(
::capnp::bounded<7>() * ::capnp::ELEMENTS, value);
}
inline ::uint16_t SlcCruiseEvent::TeslaStockCruise::Reader::getVersion() const {
return _reader.getDataField< ::uint16_t>(
::capnp::bounded<0>() * ::capnp::ELEMENTS);
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
+7 -9
View File
@@ -159,7 +159,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"PIPPreviewShowOnBlinker", {PERSISTENT, BOOL, "0"}},
{"PIPPreviewShowOnBSM", {PERSISTENT, BOOL, "0"}},
{"PIPPreviewInvert", {PERSISTENT, BOOL, "0"}},
{"CabinWhiteBalance", {PERSISTENT, BOOL, "1"}},
{"StarPilotDisplayPreferencesEnabled", {PERSISTENT, BOOL}},
{"ScreenBrightness", {PERSISTENT, INT}},
{"ScreenBrightnessOnroad", {PERSISTENT, INT}},
@@ -328,12 +327,12 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"HideDMIcon", {PERSISTENT, BOOL, "0"}},
{"HidePersonalityAlerts", {PERSISTENT, BOOL, "0"}},
{"ShowSpeedLimits", {PERSISTENT, BOOL, "1"}},
{"SLCOnEngage", {PERSISTENT, BOOL, "0"}},
{"SLCConfirmation", {PERSISTENT, INT, "0"}},
{"SLCAdvanceHigherSeconds", {PERSISTENT, INT, "0"}},
{"SLCAdvanceLowerSeconds", {PERSISTENT, INT, "0"}},
{"SLCOverrideMode", {PERSISTENT, INT, "1"}},
{"SLCWheelOverride", {PERSISTENT, BOOL, "0"}},
{"SLCConfirmation", {PERSISTENT, BOOL, "0"}},
{"SLCAdvanceSeconds", {PERSISTENT, INT, "0"}},
{"SLCConfirmationHigher", {PERSISTENT, BOOL, "0"}},
{"SLCConfirmationLower", {PERSISTENT, BOOL, "0"}},
{"SLCFallback", {PERSISTENT, INT}}, // Retired; preserve saved bytes for older versions.
{"SLCOverride", {PERSISTENT, INT, "1"}},
{"SLCQualifiedHistory", {PERSISTENT, JSON}},
{"ConditionalModeConfig", {PERSISTENT | DONT_LOG, JSON}},
{"ConditionalManualState", {PERSISTENT | DONT_LOG, JSON}},
@@ -362,8 +361,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"VASMPreferences", {PERSISTENT | DONT_LOG, JSON}},
{"LaneChangePreferences", {PERSISTENT | DONT_LOG, JSON}},
{"SLCPriority1", {PERSISTENT, STRING, "Vision"}},
{"SLCPriority2", {PERSISTENT, STRING, "Map Data"}},
{"SLCPriority3", {PERSISTENT, STRING, "Off"}},
{"SLCPriority2", {PERSISTENT, STRING, "Dashboard"}},
{"SpeedLimitController", {PERSISTENT, BOOL, "0"}},
{"RouteCount", {PERSISTENT, INT, "0"}},
{"SnoozeUpdate", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
+2 -4
View File
@@ -7,7 +7,6 @@ from opendbc.car.toyota.values import ToyotaFlags
from openpilot.selfdrive.selfdrived.events import Events
from openpilot.starpilot.car.events import event_rules, unsupported_gear
from openpilot.starpilot.car.ford.aol import stock_conventional_cruise_supported
ButtonType = structs.CarState.ButtonEvent.Type
GearShifter = structs.CarState.GearShifter
@@ -86,8 +85,7 @@ class CarEvents:
events.add(EventName.buttonEnable)
# Enabling at a standstill with brake is allowed
# TODO: verify 17 Volt can enable for the first time at a stop and allow for all GMs
# Zero permits standstill, including a filtered-speed undershoot below zero.
if self.CP.minEnableSpeed > 0. and CS.vEgo < self.CP.minEnableSpeed and not (CS.standstill and CS.brakePressed and
if CS.vEgo < self.CP.minEnableSpeed and not (CS.standstill and CS.brakePressed and
self.CP.networkLocation == NetworkLocation.fwdCamera):
events.add(EventName.belowEngageSpeed)
if CS.cruiseState.standstill:
@@ -139,7 +137,7 @@ class CarEvents:
events.add(EventName.stockLkas)
if CS.vEgo > MAX_CTRL_SPEED:
events.add(EventName.speedTooHigh)
if CS.cruiseState.nonAdaptive and not stock_conventional_cruise_supported(self.CP):
if CS.cruiseState.nonAdaptive:
events.add(EventName.wrongCruiseMode)
if CS.brakeHoldActive and self.CP.openpilotLongitudinalControl:
events.add(EventName.brakeHold)
+102 -106
View File
@@ -4,6 +4,7 @@ import os
import time
import threading
import uuid
from collections import deque
from typing import cast
import openpilot.cereal.messaging as messaging
@@ -24,15 +25,14 @@ from opendbc.car.gm.distance_button import GMDistanceButtons
from opendbc.car.gm.interface import CarInterface as GMInterface
from opendbc.car.gm.profiles import profiles_supported as gm_profiles_supported
from opendbc.car.hyundai.ioniq6_handoff import (IONIQ6_LONG_PREARM_ENABLED, HandoffOutcome,
TimestampedCanPacket, build_ioniq6_hda2_long_candidate,
IONIQ6_ECAN_BUS, TimestampedCanPacket, build_ioniq6_hda2_long_candidate,
prepare_ioniq6_long_candidate, confirm_ioniq6_prepared_takeover, restore_ioniq6_adas,
inspect_ioniq6_long_sources)
from opendbc.car.hyundai.hyundaicanfd import CanBus as HyundaiCanBus
from opendbc.car.hyundai.ioniq6_media import Ioniq6MediaButtons
from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR
from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase
from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
from openpilot.selfdrive.car.cruise import SlcWheelContext, VCruiseHelper
from openpilot.selfdrive.car.cruise import VCruiseHelper, SlcPendingConfirmation
from openpilot.starpilot.speed_limits import physical_actions as slc_physical
from openpilot.starpilot.aol.intent import AolProcessFaultContext, independent_axis_requested, read_settings
from openpilot.starpilot.aol.transport_pause import TransportPauseFeedback, cp_identity
@@ -95,7 +95,7 @@ from openpilot.starpilot.vehicle_selection import startup_candidate
from openpilot.starpilot.vehicle_preferences import VehicleStartupPreferences
from openpilot.starpilot.controller_extensions import configure_controller, observe_controller_can, order_controller_can
from openpilot.starpilot.vehicle_startup import VehicleStartupOwner
from openpilot.starpilot.controllers.wheel_actions import LIFETIME_NS as WHEEL_ACTION_MAX_AGE_NS, WheelPublisher, key_for
from openpilot.starpilot.controllers.wheel_actions import WheelPublisher, key_for
from openpilot.starpilot.controllers.cruise_action import CruiseActionConsumer, eligible as cruise_action_eligible
from openpilot.starpilot.schema_cache import get_cache, prewarm_cache_contracts, put_cache
@@ -141,15 +141,15 @@ def can_comm_callbacks(logcan: messaging.SubSocket, sendcan: messaging.PubSocket
def curve_press_receipt(CS: car.CarState, helper: VCruiseHelper, sm, *, curve_replay: bool,
slc_replay: bool, host_long_active: bool, observed_ns: int):
"""A confirmation or native standstill RES must not also override Curve control."""
"""Reserve SLC presentation presses even when their command is stale or rejected."""
accelerate = frozenset((car.CarState.ButtonEvent.Type.accelCruise,
car.CarState.ButtonEvent.Type.resumeCruise))
pressed = {b.type.raw for b in CS.buttonEvents if b.pressed and b.type.raw in accelerate}
if (not curve_replay or not host_long_active or not CS.canValid or CS.canTimeout or not CS.cruiseState.available or
CS.standstill or CS.cruiseState.standstill or not pressed):
not pressed):
return None
if (helper.slc_consumed_button is not None or bool(pressed & helper.slc_suppressed_buttons) or
slc_replay and bool(sm['slcState'].hasPending)):
if slc_replay and (helper.slc_consumed_button is not None or bool(pressed & helper.slc_suppressed_buttons) or
bool(sm['slcState'].hasPending)):
return None
return ('curveAccelPress', None, float(helper.v_cruise_kph_last / 3.6),
float(helper.v_cruise_kph / 3.6), observed_ns)
@@ -189,6 +189,10 @@ class Car:
ignore_valid=['autoSteeringLearning'], ignore_avg_freq=['autoSteeringLearning'])
self.slc_cruise_event_id = 0
self.slc_producer_session = uuid.uuid4().hex
self.slc_command_sock = messaging.sub_sock('slcCruiseCommand', conflate=False) if self.slc_replay else None
self.slc_commands = deque(maxlen=8)
self.slc_last_command_id = 0
self.slc_command_session = ''
self.slc_receipts: list[tuple[str, object, float, float, int]] = []
self.manual_receipt = None
self.manual_event_sequence = 0
@@ -279,7 +283,7 @@ class Car:
any(ps.ignitionLine or ps.ignitionCan for ps in self.sm['pandaStates']))
long_cp = build_ioniq6_hda2_long_candidate(cp, fingerprints) if eligible else None
if long_cp is not None:
preflight = inspect_ioniq6_long_sources(self.can_callbacks[0], ecan_bus=HyundaiCanBus(cp).ECAN)
preflight = inspect_ioniq6_long_sources(self.can_callbacks[0])
cloudlog.event('ioniq6_long_preflight', reason=preflight.reason)
if preflight.source_period is None or preflight.vehicle_ready is not False:
# Reboot in READY retains original stock CP before CI/parsers and
@@ -325,7 +329,7 @@ class Car:
self.CP.alternativeExperience = 0
self.switchback_capable = ioniq6_media_eligible(self.CP)
self.ioniq6_media = Ioniq6MediaButtons(HyundaiCanBus(self.CP).ECAN) if self.switchback_capable else None
self.ioniq6_media = Ioniq6MediaButtons(IONIQ6_ECAN_BUS) if self.switchback_capable else None
self.switchback_button_tracker = ButtonTracker() if self.switchback_capable else None
self.switchback_settings_owner = ConditionalSettingsOwner(self.params) if self.switchback_capable else None
self.gm_distance = GMDistanceButtons() if self.conditional_replay and gm_profiles_supported(self.CP) else None
@@ -425,13 +429,14 @@ class Car:
for ignored in (self.sm.ignore_alive, self.sm.ignore_valid, self.sm.ignore_average_freq):
if 'slcState' not in ignored:
ignored.append('slcState')
if self.slc_command_sock is None:
self.slc_command_sock = messaging.sub_sock('slcCruiseCommand', conflate=False)
if 'slcDashboardObservation' not in self.pm.sock:
self.pm.sock['slcDashboardObservation'] = messaging.pub_sock('slcDashboardObservation')
self.slc_requested_configuration = self.read_slc_configuration()
self.slc_configuration = self.slc_requested_configuration
self.slc_replay = self.slc_configuration[0]
self.slc_control_enabled = self.slc_replay and self.slc_configuration[1].enabled
self.slc_match_on_engage = self.slc_transport_available and self.read_slc_match_on_engage()
self.slc_source_floor_ns = 0
# Write previous route's CarParams
@@ -472,9 +477,6 @@ class Car:
feature_enabled(self.params, self.CP, 'slc', os.environ))
return bool(admitted), settings
def read_slc_match_on_engage(self) -> bool:
return read_saved(self.params, 'SLCOnEngage', 1) == (b'1', True)
def refresh_slc_configuration(self, now_ns: int) -> None:
configuration = getattr(self, 'slc_requested_configuration', None)
if configuration is None or configuration == getattr(self, 'slc_configuration', None):
@@ -482,8 +484,12 @@ class Car:
self.slc_configuration = configuration
self.slc_replay = configuration[0]
self.slc_control_enabled = self.slc_replay and configuration[1].enabled
# Invalidate old SLC choices while keeping consumed buttons latched until release.
self.slc_source_floor_ns = max(getattr(self, 'slc_source_floor_ns', 0), now_ns)
self.slc_commands.clear()
self.slc_command_session = ''
self.slc_last_command_id = 0
# Keep the shared event producer and held suppression until physical release.
# Only SLC's sourced/issued work predating this admission is retired.
def observe_distance_personality(self, CS: car.CarState, now_ns: int) -> None:
tracker = getattr(self, 'distance_personality_tracker', None)
@@ -576,22 +582,18 @@ class Car:
0 < control_log_ns <= now_ns and now_ns - control_log_ns <= slc_physical.STATE_MAX_AGE_NS and
self.sm['carControl'].enabled)
host_control_enabled = bool(self.CP.openpilotLongitudinalControl and not self.CP.pcmCruise and host_enabled)
host_long_active = bool(self.CP.openpilotLongitudinalControl and host_enabled and self.sm['carControl'].longActive)
host_long_active = host_control_enabled and self.sm['carControl'].longActive
slc_long_active = bool(getattr(self, 'slc_control_enabled', self.slc_replay) and self.CP.openpilotLongitudinalControl and
host_enabled and (self.sm['carControl'].longActive or CS.gasPressed))
pending = None
if self.slc_replay:
pending = slc_physical.pending_confirmation(
self.sm, now_ns, long_active=slc_long_active, after_ns=floor_ns)
wheel_context = None
slc_settings = getattr(self, 'slc_configuration', (False, None))[1]
cruise_helper = self.v_cruise_helper
if (self.slc_replay and getattr(slc_settings, 'wheel_override', False) and host_long_active and can_rcv_valid and
not self.CP.pcmCruise and pending is None and (cruise_helper.slc_wheel_press is not None or len(CS.buttonEvents) > 0) and
not (CS.brakePressed or CS.gasPressed or CS.standstill or CS.cruiseState.standstill)):
wheel_context = slc_physical.wheel_override_context(self.sm, now_ns, after_ns=floor_ns,
selected_kph=cruise_helper.v_cruise_kph, is_metric=self.is_metric)
self.update_cruise_speed(CS, pending, wheel_context=wheel_context)
approved = (slc_physical.state_current(self.sm, observation, self.slc_producer_session, now_ns)
if self.v_cruise_helper.toyota is not None and self.slc_control_enabled else None)
if approved is not None and int(approved.frameMonoTime) <= floor_ns:
approved = None
self.update_cruise_speed(CS, pending, approved=approved)
if self.controller_cruise_sock is not None:
for _ in range(8):
request = messaging.recv_one_or_none(self.controller_cruise_sock)
@@ -600,8 +602,6 @@ class Car:
self.controller_cruise_consumer.apply(request, self.CP, CS, self.sm, self.v_cruise_helper,
now_ns=now_ns, is_metric=self.is_metric,
enabled=bool(host_control_enabled and can_rcv_valid and not self.params.get_bool("SafeMode")))
if self.slc_replay:
self.apply_slc_adopt(now_ns, host_control_enabled=host_control_enabled, after_ns=floor_ns)
consumed = self.v_cruise_helper.slc_consumed_button
if self.aol_card_intent is not None:
if self.aol_process_fault_context is None:
@@ -641,8 +641,7 @@ class Car:
brake_source_ns=brake_source_ns,
gm_withdrawal_source_ns=gm_withdrawal_source_ns,
intent_producer_session=getattr(self, 'slc_producer_session', ''),
main_arm_ns=(getattr(self.aol_card_intent, 'main_arm_ns', 0) or
getattr(self.aol_card_intent, 'physical_arm_ns', 0)),
main_arm_ns=getattr(self.aol_card_intent, 'main_arm_ns', 0),
startup_calibration_pending=getattr(self.aol_card_intent, 'startup_calibration_pending', False),
intent_paused=(getattr(self.aol_card_intent, 'pause_lateral', False) or
ford_brake_pause_active(self.CP, CS, self.aol_settings)))
@@ -801,6 +800,52 @@ class Car:
slc_replay=self.slc_replay, host_long_active=host_long_active, observed_ns=now_ns)
if curve_receipt is not None:
self.slc_receipts.append(curve_receipt)
if self.slc_command_sock is not None:
for _ in range(8):
command_message = messaging.recv_one_or_none(self.slc_command_sock)
if command_message is None:
break
if self.slc_replay and command_message.valid:
self.slc_commands.append(command_message)
for _ in range(len(self.slc_commands)):
command_message = self.slc_commands.popleft()
command = command_message.slcCruiseCommand
if int(command.issuedMonoTime) <= floor_ns:
continue
if now_ns < int(command.issuedMonoTime):
if now_ns <= int(command.expiresMonoTime):
self.slc_commands.append(command_message)
continue
state = slc_physical.state_current(self.sm, observation, self.slc_producer_session, now_ns)
if (state is None or int(self.sm.logMonoTime['slcState']) < int(command.issuedMonoTime)) and now_ns < int(command.expiresMonoTime):
self.slc_commands.append(command_message)
continue
if state is not None and str(state.sessionId) != self.slc_command_session:
self.slc_command_session = str(state.sessionId)
self.slc_last_command_id = 0
if not self.slc_command_session:
self.slc_command_session = str(command.sessionId)
if str(command.sessionId) != self.slc_command_session or int(command.commandId) <= self.slc_last_command_id:
continue
self.slc_last_command_id = int(command.commandId)
previous_mps = float(self.v_cruise_helper.v_cruise_kph / 3.6)
origin = SlcPendingConfirmation(str(command.sessionId), int(command.decisionId), int(command.presentationId))
owners = {**self.v_cruise_helper.slc_suppression_owner, **self.v_cruise_helper.slc_released_owner}
allowed_buttons = {button for button, owner in owners.items() if owner == origin} if int(command.decisionId) > 0 else set()
intervening_button = (bool(self.v_cruise_helper.slc_cruise_change) or
any(b.type.raw not in allowed_buttons for b in CS.buttonEvents) or
any(timer > 0 and button not in allowed_buttons
for button, timer in self.v_cruise_helper.button_timers.items()))
applicable = slc_physical.command_applicable(
command, state, observation, self.slc_producer_session, now_ns, previous_mps,
long_active=slc_long_active,
button_event=intervening_button)
if self.v_cruise_helper.toyota is not None:
applicable = applicable and self.v_cruise_helper.toyota.can_apply(float(command.targetMps))
if applicable:
self.v_cruise_helper.apply_slc_target(float(command.targetMps), self.is_metric)
self.slc_receipts.append(('commandApplied' if applicable else 'commandRejected', command,
previous_mps, float(self.v_cruise_helper.v_cruise_kph / 3.6), now_ns))
blocked_keys = set()
if self.manual_receipt is not None:
blocked_keys.add(key_for(self.manual_receipt[0]))
@@ -816,49 +861,28 @@ class Car:
return CS, RD
def apply_slc_adopt(self, now_ns: int, *, host_control_enabled: bool, after_ns: int = 0) -> None:
pending = getattr(self, 'slc_wheel_adopt', None)
if pending is None:
return
wheel_ns, selected_kph = pending
helper = self.v_cruise_helper
if (not after_ns < wheel_ns <= now_ns <= wheel_ns + WHEEL_ACTION_MAX_AGE_NS or
not host_control_enabled or not self.slc_control_enabled or
not helper.v_cruise_initialized or helper.v_cruise_kph != selected_kph or any(helper.button_timers.values())):
self.slc_wheel_adopt = None
return
target = slc_physical.adopted_target(self.sm, now_ns, wheel_ns=wheel_ns)
if target is not None:
self.slc_wheel_adopt = None
helper.apply_slc_target(target, self.is_metric)
def update_cruise_speed(self, CS, pending=None, *, wheel_context: SlcWheelContext | None = None):
def update_cruise_speed(self, CS, pending=None, *, approved=None):
self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric, pending,
slc_enabled=getattr(self, 'slc_control_enabled', getattr(self, 'slc_replay', False)),
wheel_context=wheel_context)
slc_approved=approved)
if self.sm['carControl'].enabled and not self.CC_prev.enabled:
target, initial = None, None
initial = None
now_ns = int(self.can_log_mono_time) if REPLAY and hasattr(self, 'can_log_mono_time') else time.monotonic_ns()
if (getattr(self, 'slc_match_on_engage', False) and getattr(self, 'slc_control_enabled', False) and
not self.CP.pcmCruise and self.CP.openpilotLongitudinalControl and
if (getattr(self, 'slc_control_enabled', False) and self.CP.openpilotLongitudinalControl and
CS.canValid and not CS.canTimeout and CS.cruiseState.available and
self.sm.valid['carControl'] and self.sm.alive['carControl'] and
0 < int(self.sm.logMonoTime['carControl']) <= now_ns and
now_ns - int(self.sm.logMonoTime['carControl']) <= slc_physical.STATE_MAX_AGE_NS):
long_requested = self.sm['carControl'].longActive or CS.gasPressed
if not long_requested and 'aolAxisState' in self.sm.logMonoTime:
# AOL can acknowledge the request before longitudinal actuators become active.
axis = current_axis(self.sm, now_ns=now_ns)
long_requested = axis is not None and axis.nativeAcknowledged and axis.desiredLongitudinal
if long_requested:
evidence = getattr(self.CI.CS, 'dashboard_limit', None)
target, initial = slc_physical.engagement_target(
self.sm, evidence.observation if evidence is not None else None, self.slc_producer_session,
now_ns, after_ns=getattr(self, 'slc_source_floor_ns', 0))
now_ns - int(self.sm.logMonoTime['carControl']) <= slc_physical.STATE_MAX_AGE_NS and
(self.sm['carControl'].longActive or CS.gasPressed)):
evidence = getattr(self.CI.CS, 'dashboard_limit', None)
initial = slc_physical.engagement_state(
self.sm, evidence.observation if evidence is not None else None, self.slc_producer_session,
now_ns, after_ns=getattr(self, 'slc_source_floor_ns', 0))
# Initialization owns this edge; evaluate commands against its final speed.
adopted = self.v_cruise_helper.initialize_v_cruise(
self.CS_prev, self.experimental_mode, resume=self.vehicle_startup.consume_cruise_resume(), initial_speed=self.initial_cruise_speed,
slc_target_mps=target, is_metric=self.is_metric)
if adopted and initial is not None:
slc_target_mps=float(initial.presentationAdjustedLimit) if initial is not None else None, is_metric=self.is_metric)
if adopted:
self.slc_receipts.append(('engagementAdopt', initial, 0.0, float(self.v_cruise_helper.v_cruise_kph / 3.6), now_ns))
def aol_cancel_source(self, CS, now_ns: int) -> int:
@@ -927,8 +951,7 @@ class Car:
cancel_source_ns=self.aol_cancel_source(CS, now_ns), brake_source_ns=self.aol_honda_brake_source(CS, now_ns),
gm_withdrawal_source_ns=self.aol_gm_withdrawal_source(CS, now_ns),
intent_producer_session=getattr(self, 'slc_producer_session', ''),
main_arm_ns=(getattr(self.aol_card_intent, 'main_arm_ns', 0) or
getattr(self.aol_card_intent, 'physical_arm_ns', 0)),
main_arm_ns=getattr(self.aol_card_intent, 'main_arm_ns', 0),
startup_calibration_pending=getattr(self.aol_card_intent, 'startup_calibration_pending', False),
intent_paused=(getattr(self.aol_card_intent, 'pause_lateral', False) or
ford_brake_pause_active(self.CP, CS, self.aol_settings))):
@@ -969,8 +992,6 @@ class Car:
try:
axis = current_axis(self.sm, now_ns=now_ns)
feedback = self.aol_transport_feedback
# A pre-publication OFF cannot revoke this newer host request.
# Unmatched denial still uses the existing bounded pending timeout.
host_requested = bool(axis is not None and axis.sessionId == native.axisSessionId and
axis.observedMonoTime == self.sm.logMonoTime['aolAxisState'] and
axis.validUntilMonoTime == axis.observedMonoTime + AXIS_MAX_AGE_NS and
@@ -981,7 +1002,7 @@ class Car:
not any(event.immediateDisable or event.softDisable for event in self.sm['onroadEvents']) and
axis.sequence > 0 and axis.nativeAcknowledged and axis.desiredLateral and axis.lateralActive and
axis.intentProducerSessionId == self.slc_producer_session and
0 < axis.sourceCarStateMonoTime <= axis.observedMonoTime <= native.observedMonoTime <= now_ns and
0 < axis.sourceCarStateMonoTime <= native.observedMonoTime <= now_ns and
now_ns - axis.sourceCarStateMonoTime <= AXIS_MAX_AGE_NS and
not feedback.qualified and not feedback.retention_qualified and not feedback.paused and
not feedback.awaiting and not feedback.retention_pending)
@@ -1025,35 +1046,21 @@ class Car:
self.pm.send('carParams', cp_send)
self.car_params_published = True
timing = getattr(self, '_cycle_timing', None)
if timing is not None:
timing.publication_mark('begin')
# publish new carOutput
co_send = messaging.new_message('carOutput')
co_send.valid = self.sm.all_checks(['carControl'])
co_send.carOutput.actuatorsOutput = self.last_actuators_output
self.learning_feedback_publication = None
if timing is not None:
timing.publication_mark('car_output_create')
capture = self._feedback_capture(int(co_send.logMonoTime))
if timing is not None:
timing.publication_mark('feedback_capture')
if capture is not None:
try:
self.learning_feedback_publication = capture.publication(co_send)
except Exception:
capture.invalidate() # Optional observation cannot affect the original CO.
if timing is not None:
timing.publication_mark('feedback_publication')
self.pm.send('carOutput', co_send)
if timing is not None:
timing.publication_mark('car_output_send')
# kick off controlsd step while we actuate the latest carControl packet
cs_send = messaging.new_message('carState')
if timing is not None:
timing.publication_mark('car_state_create')
if (self.slc_replay or self.curve_replay or getattr(self, 'conditional_replay', False)) and REPLAY and hasattr(self, 'can_log_mono_time'):
cs_send.logMonoTime = int(self.can_log_mono_time)
cs_send.valid = CS.canValid
@@ -1144,6 +1151,11 @@ class Car:
elif self.CP.brand == 'hyundai':
from openpilot.starpilot.car.hyundai.dashboard import compose as hyundai_compose
companion = hyundai_compose(self.CP, self.CI.CS, CS, companion, car_state_stamp=int(cs_send.logMonoTime))
if self.v_cruise_helper.toyota is not None:
if companion is None:
companion = messaging.new_message('starpilotCarState', valid=bool(cs_send.valid), logMonoTime=int(cs_send.logMonoTime))
companion.starpilotCarState.sourceCarStateMonoTime = int(cs_send.logMonoTime)
self.v_cruise_helper.toyota.publish(companion.starpilotCarState.toyotaCruise)
self.car_gps_publisher.update(self.CI.CS, self.pm, companion)
self.pm.send('carState', cs_send)
if getattr(self, 'learning_stamp_observer', None) is not None:
@@ -1151,9 +1163,6 @@ class Car:
wheel = getattr(self, 'wheel_publisher', None)
if wheel is not None:
if (getattr(self, 'slc_control_enabled', False) and self.CC_prev.enabled and self.sm['carControl'].enabled and
any(action == 10 for _, action in self.wheel_commands)):
self.slc_wheel_adopt = (int(cs_send.logMonoTime), self.v_cruise_helper.v_cruise_kph)
wheel.publish(self.wheel_commands, self.CP, self.pm, now_ns=int(cs_send.logMonoTime),
drive_id=int(self.sm['deviceState'].startedMonoTime), source_car_ns=int(cs_send.logMonoTime),
source_control_ns=int(self.sm.logMonoTime['carControl']))
@@ -1193,8 +1202,6 @@ class Car:
if kind == 'driverChange':
assert isinstance(detail, tuple) and len(detail) == 4
record.button, record.longPress = detail[2], detail[3]
if self.v_cruise_helper.slc_wheel_tap is not None:
record.sessionId, record.wheelOverrideSince = self.v_cruise_helper.slc_wheel_tap
elif kind in ('confirmationAccept', 'confirmationReject'):
record.button = detail.button
record.sessionId = detail.pending.session_id
@@ -1202,11 +1209,13 @@ class Car:
record.presentationId = detail.pending.presentation_id
elif kind == 'curveAccelPress':
record.button = 'accel'
elif kind == 'engagementAdopt':
else:
record.sessionId = str(detail.sessionId)
record.commandId = 0 if kind == 'engagementAdopt' else int(detail.commandId)
record.decisionId = int(detail.decisionId)
record.presentationId = int(detail.presentationId)
record.button = 'decel'
if kind == 'engagementAdopt':
record.button = 'decel'
self.pm.send('slcCruiseEvent', event)
if getattr(self, 'conditional_replay', False) and self.manual_receipt is not None:
@@ -1318,8 +1327,7 @@ class Car:
return
if not self.ioniq6_long_prearmed:
return
CAN = self.CI.CC.CAN
if any((src == CAN.ECAN and address in (0x1A0, 0x1BA, 0x1E5)) or (src == CAN.ACAN and address == 0x100)
if any((src == 1 and address in (0x1A0, 0x1BA, 0x1E5)) or (src == 0 and address == 0x100)
for _, packet in can_list for address, _, src in packet):
self.ioniq6_long_lost = True
if self.ioniq6_host_warmed:
@@ -1336,24 +1344,18 @@ class Car:
panda_states = self.sm['pandaStates']
return bool(
self.sm.valid['pandaStates'] and self.sm.alive['pandaStates'] and
len(panda_states) == len(self.CP.safetyConfigs) and
len(panda_states) >= len(self.CP.safetyConfigs) and
all(ps.safetyModel == config.safetyModel and ps.safetyParam == config.safetyParam and
ps.alternativeExperience == self.CP.alternativeExperience and not ps.safetyRxChecksInvalid
for ps, config in zip(panda_states, self.CP.safetyConfigs, strict=False))
)
def ioniq6_panda_diagnostic_ready(self):
if not ioniq6_long_eligible(self.CP):
return False
owner = hyundai_canfd_config_index(self.CP)
if owner is None:
return False
if not self.sm.valid['pandaStates'] or not self.sm.alive['pandaStates']:
return False
panda_states = self.sm['pandaStates']
return bool(
len(panda_states) == len(self.CP.safetyConfigs) and
(owner == 0 or panda_states[owner].safetyParam == 1) and
all(ps.safetyModel == structs.CarParams.SafetyModel.elm327 and
ps.safetyParam in (0, 1) and not ps.controlsAllowed
for ps in panda_states)
@@ -1635,10 +1637,6 @@ class Car:
if physical.button_credit_ns - self.volt_cc_boot_offset_ns <= max(self.volt_cc_drive_id, self.volt_cc_source_floor_ns):
self.CI.CC.volt_cc_consumed_source_ns = physical.button_credit_ns
self.CI.CC.volt_cc_metric = self.is_metric
self.CI.CC.volt_cc_speed_ceiling_mps = (slc_physical.active_ceiling(
self.sm, self.volt_cc_now_mono_ns,
after_ns=max(self.volt_cc_drive_id, self.volt_cc_source_floor_ns, getattr(self, 'slc_source_floor_ns', 0)))
if getattr(self, 'slc_control_enabled', False) else None)
if getattr(self.CI.CC, "bolt_cc_profile", False):
self.CI.CC.bolt_cc_metric = self.is_metric
@@ -1700,7 +1698,7 @@ class Car:
return
self.ioniq6_last_preflight_at = handoff_started
try:
self.ioniq6_handoff_result = confirm_ioniq6_prepared_takeover(*self.can_callbacks, ecan_bus=self.CI.CC.CAN.ECAN)
self.ioniq6_handoff_result = confirm_ioniq6_prepared_takeover(*self.can_callbacks)
except Exception:
cloudlog.exception("Ioniq 6 takeover transaction failed")
self.ioniq6_handoff_result = None
@@ -1900,8 +1898,6 @@ class Car:
now = time.monotonic()
if now >= next_cruise_read:
self.slc_requested_configuration = self.read_slc_configuration()
if getattr(self, 'slc_transport_available', False):
self.slc_match_on_engage = self.read_slc_match_on_engage()
self.v_cruise_helper.intervals = read_cruise_intervals(self.params, self.CP)
pitch_input = getattr(self.CI.CC, 'long_pitch_input', None)
if pitch_input is not None:
@@ -1965,7 +1961,7 @@ def main():
if callbacks is not None:
car.ioniq6_restore_attempted = True
try:
restored = restore_ioniq6_adas(*callbacks, ecan_bus=car.CI.CC.CAN.ECAN)
restored = restore_ioniq6_adas(*callbacks)
except Exception:
restored = False
if not restored:
+59 -40
View File
@@ -43,9 +43,6 @@ class SlcConsumedButton:
button: str
SlcWheelContext = tuple[str, int]
def validated_initial_cruise_speed(value: object) -> int:
if type(value) is int and V_CRUISE_INITIAL <= value <= V_CRUISE_MAX:
return value
@@ -69,27 +66,35 @@ class VCruiseHelper:
self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers}
self.pcm_slc_sample: tuple[float, int] | None = None
self.pcm_slc_button = ButtonType.unknown
# Set by software cruise steps or a checked Toyota PCM physical +/- change.
self.slc_cruise_change: tuple[float, float, str, bool] | None = None
self.slc_consumed_button: SlcConsumedButton | None = None
self.slc_suppressed_buttons: set[ButtonType] = set()
self.slc_suppression_owner: dict[ButtonType, SlcPendingConfirmation] = {}
self.slc_released_owner: dict[ButtonType, SlcPendingConfirmation] = {}
self.slc_released_suppressed = False
self.slc_wheel_press: SlcWheelContext | None = None
self.slc_wheel_tap: SlcWheelContext | None = None
self.slc_pcm = False
from openpilot.starpilot.speed_limits.toyota import eligible, ToyotaCruise
self.toyota = ToyotaCruise() if eligible(CP) else None
@property
def v_cruise_initialized(self):
return self.v_cruise_kph != V_CRUISE_UNSET
def update_v_cruise(self, CS, enabled, is_metric, slc_pending: SlcPendingConfirmation | None = None, *,
slc_enabled: bool = False, wheel_context: SlcWheelContext | None = None):
def update_v_cruise(self, CS, enabled, is_metric, slc_pending: SlcPendingConfirmation | None = None, *, slc_enabled: bool = False,
slc_approved=None):
self.slc_pcm = bool(slc_enabled and self.CP.pcmCruise and self.CP.openpilotLongitudinalControl)
from opendbc.car.subaru.software_cruise import selected as subaru_software_selected
subaru_software = subaru_software_selected(self.CP)
software_cruise = not self.CP.pcmCruise or subaru_software
software_cruise = not self.CP.pcmCruise or self.slc_pcm or subaru_software
self.slc_cruise_change = None
self.slc_consumed_button = None
self.slc_released_suppressed = False
self.slc_wheel_tap = None
self.slc_released_owner = {}
self.v_cruise_kph_last = self.v_cruise_kph
if self.toyota is not None:
self.slc_pcm = self.toyota.update(self, CS, enabled, slc_enabled, slc_approved, is_metric)
software_cruise = self.slc_pcm
now_ns = monotonic_ns()
if not CS.cruiseState.available or now_ns - self.pending_resume_ns > 500_000_000:
self.pending_resume_ns = 0
@@ -101,26 +106,21 @@ class VCruiseHelper:
button.type in (ButtonType.accelCruise, ButtonType.resumeCruise) and self.v_cruise_initialized):
self.pending_resume_ns = now_ns
if software_cruise or slc_enabled and self.CP.openpilotLongitudinalControl or self.slc_suppressed_buttons:
if software_cruise or self.slc_suppressed_buttons:
self._update_slc_button_latches(CS, enabled, slc_pending)
if (not slc_enabled or not enabled or self.CP.pcmCruise or slc_pending is not None or
not CS.cruiseState.available or ButtonType.accelCruise in self.slc_suppressed_buttons):
wheel_context = None
if wheel_context != self.slc_wheel_press:
self.slc_wheel_press = None
if wheel_context is not None and self.button_timers[ButtonType.accelCruise] == 0:
pressed = {b.type.raw for b in CS.buttonEvents if b.pressed}
if ButtonType.accelCruise in pressed and ButtonType.resumeCruise not in pressed:
self.slc_wheel_press = wheel_context
if CS.cruiseState.available:
if software_cruise:
if subaru_software and (not enabled or not self.v_cruise_initialized):
if self.toyota is None and (self.slc_pcm or subaru_software) and (not enabled or not self.v_cruise_initialized):
# PCM still owns engagement; SLC owns the software set speed while system long is selected.
stock_kph = CS.cruiseState.speed * CV.MS_TO_KPH
self.v_cruise_kph = stock_kph if 0 < stock_kph < V_CRUISE_UNSET else V_CRUISE_UNSET
# if stock cruise is completely disabled, then we can use our own set speed logic
self._update_v_cruise_non_pcm(CS, enabled, is_metric)
self.v_cruise_cluster_kph = self.v_cruise_kph
if self.toyota is None:
self.v_cruise_cluster_kph = self.v_cruise_kph
if self.toyota is not None:
self.toyota.constrain(self)
self.update_button_timers(CS, enabled)
else:
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
@@ -139,8 +139,6 @@ class VCruiseHelper:
self.pcm_slc_button = ButtonType.unknown
if software_cruise:
self.update_button_timers(CS, enabled)
if self.button_timers[ButtonType.accelCruise] == 0:
self.slc_wheel_press = None
def _update_toyota_pcm_slc_change(self, CS, enabled: bool, now_ns: int) -> None:
"""Observe a physical PCM speed change without writing stock selected speed."""
@@ -167,9 +165,8 @@ class VCruiseHelper:
held = ButtonType.unknown
if continuous:
delta = self.v_cruise_kph - previous[0]
if (not self.slc_suppressed_buttons and not self.slc_released_suppressed and
((button == ButtonType.accelCruise and delta > 0.) or
(button == ButtonType.decelCruise and delta < 0.))):
if ((button == ButtonType.accelCruise and delta > 0.) or
(button == ButtonType.decelCruise and delta < 0.)):
self.slc_cruise_change = (previous[0] / 3.6, self.v_cruise_kph / 3.6,
'accel' if delta > 0. else 'decel', False)
self.pcm_slc_button = held
@@ -182,13 +179,17 @@ class VCruiseHelper:
continue
if not b.pressed and button in self.slc_suppressed_buttons:
self.slc_suppressed_buttons.remove(button)
prior_owner = self.slc_suppression_owner.pop(button, None)
if prior_owner is not None:
self.slc_released_owner[button] = prior_owner
self.slc_released_suppressed = True
self.button_timers[button] = 0
elif (b.pressed and enabled and CS.cruiseState.available and slc_pending is not None and
not (self.CP.pcmCruise and button == ButtonType.accelCruise and
not (self.toyota is not None and button == ButtonType.accelCruise and
(CS.standstill or CS.cruiseState.standstill or not CS.cruiseState.enabled)) and
button not in self.slc_suppressed_buttons):
self.slc_suppressed_buttons.add(button)
self.slc_suppression_owner[button] = slc_pending
self.slc_consumed_button = SlcConsumedButton(slc_pending, 'accel' if button == ButtonType.accelCruise else 'decel')
def _update_v_cruise_non_pcm(self, CS, enabled, is_metric):
@@ -230,20 +231,26 @@ class VCruiseHelper:
if not self.button_change_states[button_type]["enabled"]:
return
changed = self.adjust_v_cruise(button_type, CS, is_metric, long_press=long_press)
if changed and button_type == ButtonType.accelCruise and not long_press and self.button_timers[button_type] > 0:
self.slc_wheel_tap = self.slc_wheel_press
self.adjust_v_cruise(button_type, CS, is_metric, long_press=long_press)
def adjust_v_cruise(self, button_type, CS, is_metric: bool, *, long_press: bool = False) -> bool:
"""Apply the normal software cruise step without synthesizing button events."""
from opendbc.car.subaru.software_cruise import selected as subaru_software_selected
subaru_software = subaru_software_selected(self.CP)
if ((self.CP.pcmCruise and not subaru_software) or button_type not in (ButtonType.accelCruise, ButtonType.decelCruise) or
if ((self.CP.pcmCruise and not self.slc_pcm and not subaru_software) or button_type not in (ButtonType.accelCruise, ButtonType.decelCruise) or
not 0 < self.v_cruise_kph < V_CRUISE_UNSET or
(button_type == ButtonType.accelCruise and CS.cruiseState.standstill)):
return False
v_cruise_delta = 1. if is_metric else IMPERIAL_INCREMENT
previous_kph = self.v_cruise_kph
toyota = self.toyota if self.slc_pcm else None
previous_cluster_kph = self.v_cruise_cluster_kph
if toyota is not None:
if toyota.requested is None:
return False
if button_type == ButtonType.decelCruise:
previous_kph = toyota.requested[0]
self.v_cruise_kph = toyota.requested[1]
interval = self.intervals.held if long_press else self.intervals.short
v_cruise_delta = v_cruise_delta * interval
display_speed = round(self.v_cruise_kph if is_metric else self.v_cruise_kph * CV.KPH_TO_MPH)
@@ -259,6 +266,11 @@ class VCruiseHelper:
self.v_cruise_kph = np.clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX)
self.v_cruise_cluster_kph = self.v_cruise_kph
if toyota is not None:
toyota.requested = (float(self.v_cruise_kph), float(self.v_cruise_cluster_kph))
toyota.manual_step = (previous_kph, previous_cluster_kph, 'accel' if button_type == ButtonType.accelCruise else 'decel', long_press)
toyota.constrain(self)
return self.v_cruise_kph != previous_kph
if 0 < previous_kph < V_CRUISE_UNSET and self.v_cruise_kph != previous_kph:
button = "accel" if button_type == ButtonType.accelCruise else "decel"
self.slc_cruise_change = (float(previous_kph / 3.6), float(self.v_cruise_kph / 3.6), button, long_press)
@@ -279,19 +291,23 @@ class VCruiseHelper:
self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled}
def apply_slc_target(self, target_mps: float, is_metric: bool) -> None:
"""Set the software cruise ceiling for engagement or a qualified driver request."""
"""Apply a qualified planner command to the card-owned software set speed."""
# Match ordinary button precision so stepping away and back restores this target.
self.v_cruise_kph = slc_target_kph(target_mps, is_metric)
self.v_cruise_cluster_kph = self.v_cruise_kph
if self.toyota is not None:
self.toyota.manual_step = None
self.toyota.constrain(self)
self.toyota.requested = (self.v_cruise_kph, self.v_cruise_cluster_kph)
def initialize_v_cruise(self, CS, experimental_mode: bool, *, resume: bool | None = None,
initial_speed: object = V_CRUISE_INITIAL, slc_target_mps: float | None = None, is_metric: bool = False) -> bool:
"""Return whether engagement used the supplied SLC target."""
"""Return whether engagement adopts the qualified current SLC limit."""
if (not self.CP.openpilotLongitudinalControl or slc_target_mps is None or type(slc_target_mps) not in (int, float) or
not math.isfinite(slc_target_mps) or not V_CRUISE_MIN / 3.6 - 1e-5 <= slc_target_mps <= V_CRUISE_MAX / 3.6 + 1e-5):
slc_target_mps = None
# Native cruise owns PCM SET, including when SLC supplies a planner cap.
if self.CP.pcmCruise:
# PCM initializes when SLC does not own SET.
if self.CP.pcmCruise and (not self.slc_pcm or slc_target_mps is None):
return False
# A car-owned acknowledgment can outlive the physical button event.
if resume is None:
@@ -299,12 +315,15 @@ class VCruiseHelper:
resume = (0 < self.pending_resume_ns and 0 <= age <= 500_000_000) or any(
b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents)
self.pending_resume_ns = 0
if resume and self.v_cruise_initialized:
self.v_cruise_kph = self.v_cruise_kph_last
self.v_cruise_cluster_kph = self.v_cruise_kph
if slc_target_mps is None and resume and self.v_cruise_initialized:
if not self.CP.pcmCruise:
self.v_cruise_kph = self.v_cruise_kph_last
self.v_cruise_cluster_kph = self.v_cruise_kph
return False
elif slc_target_mps is not None:
self.apply_slc_target(slc_target_mps, is_metric)
# Toyota still needs its native capacity/release approval before application.
if self.toyota is None:
self.apply_slc_target(slc_target_mps, is_metric)
return True
else:
self.v_cruise_kph = int(round(np.clip(max(CS.vEgo * CV.MS_TO_KPH, validated_initial_cruise_speed(initial_speed)), V_CRUISE_INITIAL, V_CRUISE_MAX)))
@@ -84,7 +84,6 @@ class CardManualEventTests(unittest.TestCase):
def test_traffic_media_uses_only_new_packet_durations_across_empty_card_polls(self):
self.cp.carFingerprint = HYUNDAI_CAR.HYUNDAI_IONIQ_6
self.cp.safetyConfigs = [{'safetyModel': 'hyundaiCanfd'}]
self.cp.flags = int(HyundaiFlags.CANFD_LKA_STEER_MSG)
self.params.put('ModeButtonControl', 6, block=True)
self.params.put('LongModeButtonControl', 6, block=True)
@@ -125,7 +124,6 @@ class CardManualEventTests(unittest.TestCase):
def test_exact_ioniq_media_assignment_requires_source_and_host_authority(self):
self.params.put('ModeButtonControl', 5, block=True)
self.cp.carFingerprint = HYUNDAI_CAR.HYUNDAI_IONIQ_6
self.cp.safetyConfigs = [{'safetyModel': 'hyundaiCanfd'}]
self.assertIsNone(self.candidate(media=self.media((False, False, 700_000_000),
(True, False, 900_000_000),
(False, False, 1_000_000_000)))) # ECAN is 0 in this CP.
@@ -159,7 +157,6 @@ class CardManualEventTests(unittest.TestCase):
def test_ordinary_buttons_do_not_depend_on_optional_media_cache(self):
self.cp.carFingerprint = HYUNDAI_CAR.HYUNDAI_IONIQ_6
self.cp.safetyConfigs = [{'safetyModel': 'hyundaiCanfd'}]
self.cp.flags = int(HyundaiFlags.CANFD_LKA_STEER_MSG)
self.params.put('LKASButtonControl', 5, block=True)
self.params.put('DistanceButtonControl', 5, block=True)
@@ -180,7 +177,6 @@ class CardManualEventTests(unittest.TestCase):
def test_ioniq_media_hold_cannot_cross_map_or_drive_change(self):
self.cp.carFingerprint = HYUNDAI_CAR.HYUNDAI_IONIQ_6
self.cp.safetyConfigs = [{'safetyModel': 'hyundaiCanfd'}]
self.cp.flags = int(HyundaiFlags.CANFD_LKA_STEER_MSG)
self.params.put('ModeButtonControl', 5, block=True)
self.candidate(media=self.media((False, False, 1_000_000_000)))
@@ -52,7 +52,7 @@ class TestCruiseIntervals(unittest.TestCase):
Path(self.params.get_param_path("CustomCruiseLong")).write_bytes(b"200")
self.assertEqual(read_cruise_intervals(self.params, self.cp), CruiseIntervals())
def test_slc_preserves_software_intervals_and_native_pcm_set(self):
def test_slc_uses_saved_intervals_for_every_other_make_and_pcm_mode(self):
brands = [brand.__module__.split('.')[-2] for brand in BRANDS if brand.__module__.split('.')[-2] != 'toyota'] + ['future']
self.params.put("CustomCruise", 3.0, block=True)
self.params.put("CustomCruiseLong", 7.0, block=True)
@@ -67,11 +67,6 @@ class TestCruiseIntervals(unittest.TestCase):
helper.v_cruise_kph = helper.v_cruise_cluster_kph = initial
helper.intervals = read_cruise_intervals(self.params, cp)
helper.update_v_cruise(state(button, True, native=140.), True, metric, slc_enabled=True)
if pcm:
helper.update_v_cruise(state(button, False, native=140.), True, metric, slc_enabled=True)
self.assertAlmostEqual(helper.v_cruise_kph, 140., places=4)
self.assertIsNone(helper.slc_cruise_change)
continue
if held:
for _ in range(CRUISE_LONG_PRESS):
helper.update_v_cruise(state(native=140.), True, metric, slc_enabled=True)
@@ -1,5 +1,4 @@
import numpy as np
import math
from opendbc.car.structs import car
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
@@ -97,9 +96,6 @@ class LongControl:
output_accel -= self.stopping_decel_rate * DT_CTRL # m/s^2/s while trying to stop
if self.extension is not None:
output_accel = self.extension.stopping_output(output_accel, a_target, should_stop, CS)
if (context.force_stop_braking and self.extension is not None and self.extension.ioniq6_start is not None and
should_stop and not CS.standstill and CS.vEgo > .3 and math.isfinite(a_target)):
output_accel = min(a_target, 0.0)
self.reset(reset_start=False)
elif self.long_control_state == LongCtrlState.starting and self.extension is not None and self.extension.starting:
@@ -123,4 +119,3 @@ class LongControl:
output_accel = min(output_accel, planner_target)
self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
return self.last_output_accel
@@ -32,7 +32,6 @@ from openpilot.starpilot.longitudinal.ioniq6_start import eligible as ioniq6_lon
from openpilot.starpilot.longitudinal.lane_change_gap import LaneChangeGap, project as project_lane_gap
from openpilot.starpilot.longitudinal.experimental_release import ConditionalHandoff, ExperimentalRelease, project as project_release
from openpilot.starpilot.longitudinal.force_stop import StopPlan
from openpilot.starpilot.longitudinal.force_stop_braking import distance_target
from openpilot.starpilot.longitudinal.follow_jerk import FollowJerk
from openpilot.starpilot.longitudinal.lead_takeoff import LeadTakeoff, project as project_takeoff
from openpilot.starpilot.longitudinal.lead_behavior import adjust as adjust_lead_behavior
@@ -375,11 +374,12 @@ class LongitudinalPlanner:
cruise_profile = replace(cruise_profile, cruise_brake_magnitude=global_magnitude)
if sm['controlsState'].forceDecel:
cruise_profile = None
elif curve.effective_mps is not None and cruise_profile is not None:
# Curve approach retains native braking when the saved cruise response is softer.
elif (ceiling.effective_mps is not None or curve.effective_mps is not None) and cruise_profile is not None:
# An eligible lower ceiling must retain at least native approach
# braking even when the saved profile requests a softer cruise response.
cruise_profile = replace(cruise_profile, cruise_brake_magnitude=max(cruise_profile.cruise_brake_magnitude,
abs(A_CRUISE_MIN)))
if curve.effective_mps is not None:
if ceiling.effective_mps is not None or curve.effective_mps is not None:
global_floor = min(global_floor, A_CRUISE_MIN)
if selected_acceleration is not None and cruise_profile is not None:
# The selected curve owns acceleration; following/jerks retain their owner.
@@ -410,9 +410,6 @@ class LongitudinalPlanner:
candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
(self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop)]
if self.ioniq6_throttle_gate_enabled and self.force_stop_plan.forcing and self.force_stop_plan.obstacle_m is not None:
stop_target = distance_target(v_ego, sm['carState'].aEgo, self.force_stop_plan.obstacle_m, action_t)
candidates.append((stop_target, LongitudinalPlanSource.cruise, False))
if sm['selfdriveState'].experimentalMode:
candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e))
@@ -514,9 +511,5 @@ class LongitudinalPlanner:
hold.modelMonoTime = longitudinalPlan.modelMonoTime
hold.driveStartMonoTime = self.plan_drive_id
hold.forceStopHolding = bool(self.force_stop_plan.manual_hold)
hold.forceStopBraking = bool(self.ioniq6_throttle_gate_enabled and self.force_stop_plan.forcing and
self.force_stop_plan.should_stop)
hold.forceStopEpisodeMonoTime = self.force_stop_plan.episode_ns
pm.send('longitudinalPlan', plan_send)
pm.send('starpilotLongitudinalPlan', companion)
+116 -37
View File
@@ -1,6 +1,8 @@
#!/usr/bin/env python3
import os
import json
from collections import deque
from dataclasses import replace
from opendbc.car.structs import car
from openpilot.common.params import Params
@@ -10,9 +12,8 @@ from openpilot.selfdrive.controls.lib.ldw import LaneDepartureWarning
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, NATIVE_PLAN_INPUTS
import openpilot.cereal.messaging as messaging
from openpilot.cereal.services import SERVICE_LIST
from openpilot.starpilot.speed_limits.planner_host import (
SLC_ACTIONS, SavedSlcLifecycle, action_from_message, cruise_event_from_message, current_cruise_event, receive_slc_events,
)
from openpilot.starpilot.speed_limits import history as slc_history
from openpilot.starpilot.speed_limits.runtime import Action as SlcAction, CruiseEvent as SlcCruiseEvent, Runtime as SlcRuntime
from openpilot.starpilot.longitudinal.profile_runtime import ProfileHost
from openpilot.starpilot.longitudinal.lane_change_gap import LaneGapPreferences
from openpilot.starpilot.longitudinal.experimental_release import ConditionalHandoff
@@ -39,8 +40,62 @@ from openpilot.starpilot.feature_capabilities import longitudinal_supported
from openpilot.starpilot.conditional_mode.projection import paired_clocks_ns
from openpilot.starpilot.conditional_mode.ui_action import observation as ui_manual_observation
from openpilot.starpilot.longitudinal.cruise_ceiling import CurveCeiling
from openpilot.starpilot.feature_runtime import enabled as feature_enabled, requested as feature_requested, slc_transport_capable
from openpilot.starpilot.feature_runtime import (enabled as feature_enabled, requested as feature_requested,
slc_runtime_settings, slc_transport_capable, vision_control_enabled)
import time
from openpilot.starpilot.speed_limits.runtime_settings import read_params as read_slc_settings
class SavedSlcLifecycle:
"""Refresh saved admission; offsets and display units preserve driver choices."""
def __init__(self, params, cp, environment):
self.params, self.cp, self.environment = params, cp, dict(environment)
self.runtime = None
self.configuration = None
self.last_refresh_ns = None
def refresh(self, now_ns: int, *, pending_actions=None, pending_cruise=None):
if self.last_refresh_ns is not None and 0 <= now_ns - self.last_refresh_ns < 1_000_000_000:
return self.runtime, False
self.last_refresh_ns = now_ns
# One immutable parsed snapshot owns this refresh. Repeated qualification
# used to reopen the same saved keys five times in a due model iteration.
saved_slc = read_slc_settings(self.params)
admitted = feature_enabled(self.params, self.cp, 'slc', self.environment, saved_slc=saved_slc)
settings = slc_runtime_settings(self.params, self.cp, self.environment, saved_slc=saved_slc)
vision = admitted and feature_enabled(self.params, self.cp, 'vision', self.environment, saved_slc=saved_slc)
configuration = (admitted and not settings.errors, settings, vision,
vision_control_enabled(self.params, self.cp, saved_slc=saved_slc),
feature_enabled(self.params, self.cp, 'vision', {}, saved_slc=saved_slc),
self.params.get_bool('IsMetric'))
if configuration == self.configuration:
return self.runtime, False
previous_runtime = self.runtime
if (self.runtime is not None and self.configuration is not None and
configuration[:-1] == (self.configuration[0], replace(self.runtime.settings, offsets=settings.offsets,
advance_seconds=settings.advance_seconds),
*self.configuration[2:-1])):
if settings.offsets != self.runtime.settings.offsets or configuration[5] != self.configuration[5]:
self.runtime.update_offsets(settings.offsets, is_metric=configuration[5])
if settings.advance_seconds != self.runtime.settings.advance_seconds:
self.runtime.update_advance(settings.advance_seconds)
else:
set_speed_after_ns = self.runtime.set_speed_after_ns if self.runtime is not None else 0
if self.runtime is not None:
self.runtime.reset()
self.runtime = (SlcRuntime(settings, vision_enabled=vision,
vision_control_qualified=configuration[3],
vision_display_qualified=configuration[4], is_metric=configuration[5])
if configuration[0] else None)
if self.runtime is not None:
self.runtime.set_speed_after_ns = set_speed_after_ns
self.configuration = configuration
if pending_actions is not None:
pending_actions.clear()
if pending_cruise is not None and (self.runtime is None or self.runtime is not previous_runtime):
pending_cruise.clear()
return self.runtime, True
def profile_for_frame(host: ProfileHost | None, sm, CP, now_ns: int, *, traffic_mode: bool | None = False):
@@ -104,13 +159,11 @@ def traffic_profile_status(mode: bool | None, target, applied, settings) -> tupl
def curve_for_frame(host: CurveHost | None, sm, CP, now_ns: int, *, drive_id: int = 0,
event: CurveDriverEvent | None = None, follow_time_s: float | None = None,
cruise_ceiling_mps: float | None = None):
event: CurveDriverEvent | None = None, follow_time_s: float | None = None):
"""A diagnostic runtime result is not planner authority by itself."""
if host is None:
return None, None
result = host.sample(sm, CP, now_ns, drive_id=drive_id, event=event, follow_time_s=follow_time_s,
cruise_ceiling_mps=cruise_ceiling_mps)
result = host.sample(sm, CP, now_ns, drive_id=drive_id, event=event, follow_time_s=follow_time_s)
if result.ceiling_mps is None:
return None, result
model_ns = sm.logMonoTime.get('modelV2', 0)
@@ -124,12 +177,6 @@ def confirm_curve_frame(host: CurveHost | None, planner: LongitudinalPlanner, no
host.runtime.confirm_applied(now_ns, applied=bool(planner.last_curve_ceiling_applied))
def wheel_override_blocked(force_stop: ForceStopRuntime | None, curve_host: CurveHost | None, *, curve_backlog: bool = False) -> bool:
# Force Stop can consume RES on the tick it commits, before a forcing plan is published.
return ((force_stop is not None and (force_stop.policy.can_commit_next_frame or force_stop.plan.speed_ceiling_mps is not None)) or
(curve_host is not None and (curve_backlog or curve_host.runtime.was_controlling)))
def conditional_handoff_for_frame(host: ConditionalPlannerHost | None, sm, CP, now_ns: int,
drive_id: int) -> ConditionalHandoff | None:
if host is None or not CP.openpilotLongitudinalControl or CP.passive or CP.dashcamOnly:
@@ -243,8 +290,7 @@ def update_curve_frame(planner: LongitudinalPlanner, sm, CP, now_ns: int, *, hos
results = []
def provider(follow_time_s: float | None):
ceiling, result = curve_for_frame(host, sm, CP, now_ns, drive_id=drive_id,
event=event, follow_time_s=follow_time_s,
cruise_ceiling_mps=cruise_ceiling.speed_mps if cruise_ceiling is not None else None)
event=event, follow_time_s=follow_time_s)
results.append(result)
return ceiling
planner.update(sm, cruise_ceiling=cruise_ceiling, profile_tuning=profile_tuning,
@@ -274,7 +320,18 @@ def queue_cruise_event(event, slc_events, curve_events, conditional_events=None,
curve_events.append(CurveDriverEvent(str(cruise.producerSessionId), int(cruise.eventId),
int(cruise.observedMonoTime), kind, str(cruise.button), int(event.logMonoTime)))
else:
slc_events.append(cruise_event_from_message(event))
slc_events.append(SlcCruiseEvent(int(cruise.eventId), int(cruise.observedMonoTime),
float(cruise.previousMps), float(cruise.selectedMps), str(cruise.button),
str(cruise.producerSessionId), int(event.logMonoTime), kind,
str(cruise.sessionId), int(cruise.decisionId),
int(cruise.presentationId), int(cruise.commandId)))
def current_cruise_event(events, now_ns: int):
max_event_age_ns = int(2e9 / SERVICE_LIST['modelV2'].frequency)
while events and now_ns - events[0].observed_ns > max_event_age_ns:
events.popleft()
return events.popleft() if events and events[0].observed_ns <= now_ns else None
def queue_ui_action(event, slc_actions, conditional_actions, mode_actions=None) -> None:
@@ -286,8 +343,9 @@ def queue_ui_action(event, slc_actions, conditional_actions, mode_actions=None)
mode_actions.append(event)
elif kind == 'conditionalModeCycle':
conditional_actions.append(event)
elif kind in SLC_ACTIONS:
slc_actions.append(action_from_message(event))
elif kind in ('accept', 'reject', 'adopt'):
slc_actions.append((int(event.logMonoTime), SlcAction(str(request.sessionId), int(request.sequenceId),
int(request.decisionId), int(request.presentationId), kind)))
def current_ui_manual_event(events, now_ns: int):
@@ -359,7 +417,7 @@ def starpilot_main():
pending_switchback = deque(maxlen=8)
mode_settings = ConditionalSettingsOwner(params) if mode_owner is not None else None
slc_lifecycle = SavedSlcLifecycle(params, CP, os.environ)
slc_lifecycle.refresh(0)
slc_runtime, _ = slc_lifecycle.refresh(0)
slc_action_sock = (messaging.sub_sock("slcAction", conflate=False)
if slc_available or conditional_host is not None or mode_owner is not None else None)
slc_cruise_sock = messaging.sub_sock("slcCruiseEvent", conflate=False)
@@ -373,14 +431,18 @@ def starpilot_main():
publish_services = ['longitudinalPlan', 'starpilotLongitudinalPlan', 'driverAssistance']
if slc_available or curve_host is not None or conditional_host is not None or traffic_owner is not None or mode_owner is not None:
publish_services.append('slcState')
if slc_available:
publish_services.append('slcCruiseCommand')
pm = messaging.PubMaster(publish_services)
native_plan_checks = list(NATIVE_PLAN_INPUTS)
subscribed_services = list(NATIVE_PLAN_INPUTS)
subscribed_services.append('deviceState')
if conditional_host is not None:
subscribed_services.append('starpilotRadarState')
optional_slc = slc_lifecycle.source_services() if slc_available else []
if force_stop_owner is not None:
optional_slc = ['slcDashboardObservation', 'slcVisionObservation', 'mapdOut', 'starpilotCarState'] if slc_available else []
if slc_available and feature_enabled(params, CP, 'aol', os.environ):
optional_slc.append('aolAxisState')
if force_stop_owner is not None and 'starpilotCarState' not in optional_slc:
optional_slc.append('starpilotCarState')
if force_stop_owner is not None and 'slcVisionObservation' not in optional_slc:
optional_slc.append('slcVisionObservation')
@@ -399,14 +461,16 @@ def starpilot_main():
# Due saved-setting verification can block on Params. Complete it
# before acquiring the model and fast-source batch, not after that
# batch has already begun aging toward the CEM freshness deadline.
slc_lifecycle.refresh(time.monotonic_ns(), pending_actions=pending_slc_actions, pending_cruise=pending_slc_cruise)
slc_runtime, _ = slc_lifecycle.refresh(time.monotonic_ns(), pending_actions=pending_slc_actions,
pending_cruise=pending_slc_cruise)
sm.update()
if sm.updated['modelV2']:
# Only explicit replay uses the recorded model timeline. In a live host,
# modelV2 may lag newer inputs in the same SubMaster update.
now_ns = int(sm.logMonoTime['modelV2']) if 'REPLAY' in os.environ else time.monotonic_ns()
if 'REPLAY' in os.environ:
slc_lifecycle.refresh(now_ns, pending_actions=pending_slc_actions, pending_cruise=pending_slc_cruise)
slc_runtime, _ = slc_lifecycle.refresh(now_ns, pending_actions=pending_slc_actions,
pending_cruise=pending_slc_cruise)
current_start_ns = (int(sm['deviceState'].startedMonoTime)
if 'deviceState' in subscribed_services and sm.valid['deviceState'] and
sm.alive['deviceState'] and 0 < sm.logMonoTime['deviceState'] <= now_ns else 0)
@@ -417,7 +481,10 @@ def starpilot_main():
planner_status = None
drive_changed = current_start_ns > 0 and drive_start_ns > 0 and current_start_ns != drive_start_ns
if slc_cruise_sock is not None:
for event in receive_slc_events(slc_cruise_sock):
for _ in range(8):
event = messaging.recv_one_or_none(slc_cruise_sock)
if event is None:
break
if str(event.slcCruiseEvent.kind) == 'wheelAction' and event.valid:
pending_wheel.append(event)
continue
@@ -440,7 +507,10 @@ def starpilot_main():
if traffic_owner is not None:
traffic_owner.reset()
if slc_action_sock is not None:
for event in receive_slc_events(slc_action_sock):
for _ in range(8):
event = messaging.recv_one_or_none(slc_action_sock)
if event is None:
break
queue_ui_action(event, pending_slc_actions, pending_conditional_ui, pending_modes)
waiting_wheel = deque(maxlen=32)
while pending_wheel:
@@ -452,13 +522,25 @@ def starpilot_main():
else:
wheel_coast.receive(event, params, CP, sm, now_ns=now_ns)
pending_wheel = waiting_wheel
# Curve observes the press before SLC sees the release. A queued Curve press can still own the tap.
curve_event = current_cruise_event(pending_curve_events, now_ns)
slc_output = slc_lifecycle.step(sm, now_ns, pending_slc_actions, pending_slc_cruise, drive_changed=drive_changed,
wheel_override_blocked=wheel_override_blocked(force_stop_owner, curve_host, curve_backlog=bool(pending_curve_events)))
cruise_ceiling = slc_output.result.ceiling if slc_output is not None else None
if slc_output is not None:
planner_status = slc_output.message
if slc_runtime is not None:
if drive_changed or slc_runtime.state.reset_required or slc_runtime.ledger.reset_required:
slc_runtime.reset()
pending_slc_actions.clear()
pending_slc_cruise.clear()
max_event_age_ns = int(2e9 / SERVICE_LIST['modelV2'].frequency)
while pending_slc_actions and now_ns - pending_slc_actions[0][0] > max_event_age_ns:
pending_slc_actions.popleft()
cruise_event = current_cruise_event(pending_slc_cruise, now_ns)
request = (pending_slc_actions.popleft()[1] if cruise_event is None and pending_slc_actions and
pending_slc_actions[0][0] <= now_ns else None)
output = slc_runtime.step(sm, CP, now_ns=now_ns, request=request, cruise_event=cruise_event)
planner_status = output.message
if output.command is not None:
pm.send('slcCruiseCommand', output.command)
history_write = output.result.acceptance.history_write if output.result.acceptance is not None else None
# A camera episode has no road-location identity for a later drive.
if history_write is not None and history_write.candidate.source != 'vision':
params.put("SLCQualifiedHistory", json.loads(slc_history.encode(history_write, time.time_ns())))
needs_clock = conditional_host is not None or traffic_owner is not None or force_stop_owner is not None or mode_owner is not None
clock_pair = paired_clocks_ns() if needs_clock and 'REPLAY' not in os.environ else None
controller_traffic_toggle = bool(wheel_coast.traffic_toggles % 2)
@@ -518,6 +600,7 @@ def starpilot_main():
lead_approach_key = lead_approach_for_frame(approach_preferences, sm, CP, now_ns)
if curve_preferences is not None and curve_host is not None:
curve_preferences.refresh(curve_host, now_ns)
curve_event = current_cruise_event(pending_curve_events, now_ns)
conditional_handoff = (conditional_handoff_for_frame(conditional_host, sm, CP, clock_pair[0], current_start_ns)
if clock_pair is not None else None)
wheel_coast.conditional_stop = bool(conditional_handoff is not None and conditional_host is not None and
@@ -534,7 +617,6 @@ def starpilot_main():
drive_id=current_start_ns, follow_seconds=follow_seconds,
traffic_mode=traffic_mode, takeoff_enabled=faster_lead_takeoff)
curve_result = update_curve_frame(longitudinal_planner, sm, CP, now_ns, host=curve_host,
cruise_ceiling=cruise_ceiling,
profile_tuning=profile_tuning,
traffic_mode=traffic_mode, lane_change_policy=lane_change_policy,
global_braking_response=global_braking_response, selected_profiles=selected_profiles,
@@ -561,7 +643,6 @@ def starpilot_main():
proposal, snapshot = conditional_host.sample(
sm, CP, longitudinal_planner, now_mono_ns=conditional_now_ns, now_boot_ns=conditional_boot_ns,
sample_skew_ns=conditional_skew_ns, drive_id=current_start_ns,
cruise_ceiling_mps=cruise_ceiling.speed_mps if cruise_ceiling is not None else None,
owner_context=ConditionalOwnerContext(
traffic_mode=traffic_observation(traffic_verdict, model_ns=int(sm.logMonoTime['modelV2']),
owner_present=traffic_owner is not None),
@@ -596,8 +677,6 @@ def starpilot_main():
mode_owner.sample('switchback', sm, CP, now_ns=now_ns), session=mode_session,
sequence=mode_sequence, now_ns=now_ns, source_car_control_ns=int(sm.logMonoTime['carControl']))
if planner_status is not None:
if planner_status.slcState.wheelOverrideSince and wheel_override_blocked(force_stop_owner, curve_host):
planner_status.slcState.wheelOverrideSince = 0
pm.send('slcState', planner_status)
if current_start_ns > 0:
drive_start_ns = current_start_ns
@@ -9,7 +9,6 @@ import unittest
from openpilot.cereal import messaging
from openpilot.common.params import Params
from openpilot.starpilot import feature_runtime
from openpilot.starpilot.speed_limits import planner_host as slc_host
from openpilot.starpilot.speed_limits.runtime_settings import read_params as read_slc_settings
from openpilot.selfdrive.controls import plannerd
from openpilot.starpilot.conditional_mode.tests import test_status_consumer as transport
@@ -99,7 +98,7 @@ class TestCemInputAcquisitionOrder(unittest.TestCase):
def read(params):
reads.append(True)
return read_slc_settings(params)
with patch.object(slc_host, 'read_slc_settings', side_effect=read), \
with patch.object(plannerd, 'read_slc_settings', side_effect=read, create=True), \
patch.object(feature_runtime, 'read_slc_settings', side_effect=read):
runtime, changed = owner.refresh(MONO)
self.assertTrue(changed)
@@ -23,21 +23,21 @@ from openpilot.starpilot.conditional_mode.projection import ConditionalOwnerCont
from openpilot.starpilot.conditional_mode.runtime_settings import DOCUMENT_KEY
from openpilot.starpilot.conditional_mode.status import settings_fingerprint
from openpilot.starpilot.conditional_mode.tests.test_projection import BOOT, MONO, FakeSubMaster, serialized_scene
from openpilot.starpilot.speed_limits.runtime import Action as SlcAction, CruiseEvent as SlcCruiseEvent
class TestConditionalRuntimeLoop(unittest.TestCase):
def test_slc_unit_change_preserves_cap_and_queued_driver_event(self):
def test_slc_unit_change_keeps_receipts_for_the_current_command(self):
from openpilot.selfdrive.car.cruise import slc_target_kph
from openpilot.starpilot.speed_limits import offset_document as od
from openpilot.starpilot.speed_limits.runtime import Action, CruiseEvent
from openpilot.starpilot.speed_limits.tests import test_runtime_replay as replay
fixture = replay.RuntimeReplayTests()
from openpilot.starpilot.speed_limits.tests.test_runtime_replay import RuntimeReplayTests
fixture = RuntimeReplayTests()
fixture.setUp()
sm, cp = fixture.sm, fixture.cp
with tempfile.TemporaryDirectory() as directory:
params = Params(directory)
params.put_bool('SpeedLimitController', True, block=True)
params.put('SLCConfirmation', 1, block=True)
params.put_bool('SLCConfirmation', True, block=True)
params.put('SLCPriority1', 'Dashboard', block=True)
params.put('SLCOffsetSchedule', od.to_value(od.default_document(False)), block=True)
owner = plannerd.SavedSlcLifecycle(params, cp, {})
@@ -46,19 +46,20 @@ class TestConditionalRuntimeLoop(unittest.TestCase):
sm.advance(2_900_000_000)
pending = runtime.step(sm, cp, now_ns=2_900_000_000).message.slcState
sm.advance(2_950_000_000)
accepted = runtime.step(sm, cp, now_ns=2_950_000_000,
issued = runtime.step(sm, cp, now_ns=2_950_000_000,
request=Action(runtime.session_id, 1, pending.decisionId, pending.presentationId, 'accept'))
self.assertEqual(accepted.result.ceiling.speed_mps, 25.)
events = deque([CruiseEvent(1, 2_990_000_000, 60/3.6, 65/3.6, 'accel', 'card-producer', 2_990_000_000)])
command = issued.command.slcCruiseCommand
selected = slc_target_kph(command.targetMps, False)
receipts = deque([CruiseEvent(1, 2_990_000_000, 60 / 3.6, selected / 3.6, '', 'card-producer',
2_990_000_000, 'commandApplied', runtime.session_id, command.decisionId, command.presentationId, command.commandId)])
params.put_bool('IsMetric', True, block=True)
metric, _ = owner.refresh(3_000_000_000, pending_cruise=events)
self.assertIs(metric, runtime)
self.assertEqual(len(events), 1)
sm['carState'].vCruise = sm['carState'].vCruiseCluster = 65.
metric, _ = owner.refresh(3_000_000_000, pending_cruise=receipts)
self.assertEqual(len(receipts), 1)
sm['carState'].vCruise = sm['carState'].vCruiseCluster = selected
sm.advance(3_000_000_000)
output = metric.step(sm, cp, now_ns=3_000_000_000, cruise_event=events.popleft())
self.assertEqual(output.result.ceiling.speed_mps, 25.)
self.assertEqual(sm['carState'].vCruise, 65.)
applied = metric.step(sm, cp, now_ns=3_000_000_000, cruise_event=receipts.popleft())
self.assertEqual(applied.message.slcState.commandStatus, 'applied')
self.assertIsNone(applied.command)
def test_slc_unit_changes_preserve_set_speed_and_driver_choices(self):
from openpilot.common.constants import CV
@@ -67,14 +68,14 @@ class TestConditionalRuntimeLoop(unittest.TestCase):
from openpilot.starpilot.speed_limits.runtime import Action, CruiseEvent
from openpilot.starpilot.speed_limits.tests.test_runtime_replay import RuntimeReplayTests
now = 2_000_000_000
for confirmation, choice in ((False, 'applied'), (True, 'applied'), (False, 'ceiling'), (True, 'decline')):
for confirmation, choice in ((False, 'applied'), (True, 'applied'), (False, 'override'), (True, 'decline')):
with self.subTest(confirmation=confirmation, choice=choice), tempfile.TemporaryDirectory() as directory:
fixture = RuntimeReplayTests()
fixture.setUp()
sm, cp = fixture.sm, fixture.cp
params = Params(directory)
params.put_bool('SpeedLimitController', True, block=True)
params.put('SLCConfirmation', int(confirmation), block=True)
params.put_bool('SLCConfirmation', confirmation, block=True)
params.put('SLCPriority1', 'Dashboard', block=True)
params.put('SLCOffsetSchedule', od.to_value(od.default_document(False)), block=True)
owner = plannerd.SavedSlcLifecycle(params, cp, {})
@@ -83,14 +84,14 @@ class TestConditionalRuntimeLoop(unittest.TestCase):
baseline = slc_target_kph(65 * CV.MPH_TO_MS, False)
sm['carState'].vCruise = sm['carState'].vCruiseCluster = baseline
aligned = original.step(sm, cp, now_ns=now)
self.assertTrue(aligned.message.slcState.hasAccepted)
if choice == 'ceiling':
self.assertIsNone(aligned.command)
if choice == 'override':
selected = slc_target_kph(70 * CV.MPH_TO_MS, False)
sm['carState'].vCruise = sm['carState'].vCruiseCluster = selected
sm.advance(now + 50_000_000)
chosen = original.step(sm, cp, now_ns=now + 50_000_000, cruise_event=CruiseEvent(
1, now + 50_000_000, baseline / 3.6, selected / 3.6, 'accel', 'card-producer', now + 50_000_000))
self.assertFalse(chosen.message.slcState.driverOverrideActive)
self.assertTrue(chosen.message.slcState.driverOverrideActive)
elif choice == 'decline':
sm['slcDashboardObservation'].speedMps = 55 * CV.MPH_TO_MS
sm.advance(now + 50_000_000)
@@ -104,10 +105,11 @@ class TestConditionalRuntimeLoop(unittest.TestCase):
self.assertTrue(changed)
sm.advance(now + 1_000_000_000)
output = metric.step(sm, cp, now_ns=now + 1_000_000_000)
self.assertIsNone(output.command)
self.assertFalse(output.message.slcState.hasPending)
self.assertTrue(output.message.slcState.hasAccepted)
self.assertEqual(sm['carState'].vCruise, selected)
self.assertFalse(output.message.slcState.driverOverrideActive)
self.assertEqual(output.message.slcState.driverOverrideActive, choice == 'override')
if choice == 'decline':
self.assertEqual(output.result.acceptance.basis, 'rejected')
@@ -120,11 +122,13 @@ class TestConditionalRuntimeLoop(unittest.TestCase):
owner = plannerd.SavedSlcLifecycle(params, cp, {})
imperial, _ = owner.refresh(MONO)
self.assertFalse(imperial.is_metric)
imperial.set_speed_after_ns = MONO + 1_050_000_000
actions, cruise = deque([object()]), deque([object()])
params.put_bool('IsMetric', True, block=True)
metric, changed = owner.refresh(MONO + 1_000_000_000, pending_actions=actions, pending_cruise=cruise)
self.assertTrue(changed)
self.assertTrue(metric.is_metric)
self.assertEqual(metric.set_speed_after_ns, MONO + 1_050_000_000)
self.assertIs(metric, imperial)
self.assertFalse(actions)
self.assertEqual(len(cruise), 1)
@@ -164,7 +168,7 @@ class TestConditionalRuntimeLoop(unittest.TestCase):
# Status and transport are reserved independently of saved SLC/CEM
# admission, so a later saved change can attach without restarting.
self.assertEqual(publisher.call_args.args[0], ['longitudinalPlan', 'starpilotLongitudinalPlan',
'driverAssistance', 'slcState'])
'driverAssistance', 'slcState', 'slcCruiseCommand'])
self.assertEqual(conditional_ctor.call_count, int(enabled))
if enabled:
self.assertIs(conditional_ctor.call_args.args[0], params)
@@ -196,8 +200,8 @@ class TestConditionalRuntimeLoop(unittest.TestCase):
self.assertFalse(vision.vision_control_qualified)
self.assertEqual(vision.settings.selection.slots[0], Source.VISION)
session = vision.session_id
pending_actions = deque([(3_000_000_000, SlcAction(session, 1, 1, 1, 'accept'))])
pending_cruise = deque([SlcCruiseEvent(1, 3_000_000_000, 20., 21., 'set', session_id=session)])
pending_actions = deque([(3_000_000_000, plannerd.SlcAction(session, 1, 1, 1, 'accept'))])
pending_cruise = deque([plannerd.SlcCruiseEvent(1, 3_000_000_000, 20., 21., 'set', session_id=session)])
self.assertEqual(owner.refresh(3_050_000_000, pending_actions=pending_actions,
pending_cruise=pending_cruise), (vision, False))
self.assertEqual(len(pending_actions), 1)
@@ -213,8 +217,8 @@ class TestConditionalRuntimeLoop(unittest.TestCase):
self.assertNotEqual(vision.session_id, session) # Retired session was reset.
params.put_bool('ShowSpeedLimits', False, block=True)
params.put_bool('SpeedLimitController', False, block=True)
pending_actions.append((4_050_000_000, SlcAction(control.session_id, 1, 1, 1, 'accept')))
pending_cruise.append(SlcCruiseEvent(2, 4_050_000_000, 20., 21., 'set', session_id=control.session_id))
pending_actions.append((4_050_000_000, plannerd.SlcAction(control.session_id, 1, 1, 1, 'accept')))
pending_cruise.append(plannerd.SlcCruiseEvent(2, 4_050_000_000, 20., 21., 'set', session_id=control.session_id))
self.assertEqual(owner.refresh(5_000_000_000, pending_actions=pending_actions,
pending_cruise=pending_cruise), (None, True))
self.assertFalse(pending_actions)
@@ -247,7 +251,7 @@ class TestConditionalRuntimeLoop(unittest.TestCase):
patch.object(messaging, 'SubMaster', return_value=sm) as subscriber, patch.object(messaging, 'sub_sock'):
with self.assertRaises(EndLoop):
plannerd.starpilot_main()
self.assertNotIn('slcCruiseCommand', publisher.call_args.args[0])
self.assertIn('slcCruiseCommand', publisher.call_args.args[0])
for service in ('slcDashboardObservation', 'slcVisionObservation'):
self.assertIn(service, subscriber.call_args.args[0])
for check in ('ignore_alive', 'ignore_valid', 'ignore_avg_freq'):
@@ -156,7 +156,7 @@ class CurveLoopTests(unittest.TestCase):
stamp = NOW + n * 50_000_000
sm = bus(stamp)
result = update_curve_frame(planner, sm, cp, stamp, host=host, cruise_ceiling=slc)
self.assertIsNone(result.ceiling_mps)
self.assertIsNotNone(result.ceiling_mps)
self.assertFalse(planner.last_curve_ceiling_applied)
self.assertFalse(host.runtime.was_controlling)
self.assertEqual(host.runtime.curve.revision, 0)
@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
* *
* This file is part of 'ekf' *
******************************************************************************/
void err_fun(double *nom_x, double *delta_x, double *out_3392229238668395910) {
out_3392229238668395910[0] = delta_x[0] + nom_x[0];
out_3392229238668395910[1] = delta_x[1] + nom_x[1];
out_3392229238668395910[2] = delta_x[2] + nom_x[2];
out_3392229238668395910[3] = delta_x[3] + nom_x[3];
out_3392229238668395910[4] = delta_x[4] + nom_x[4];
out_3392229238668395910[5] = delta_x[5] + nom_x[5];
out_3392229238668395910[6] = delta_x[6] + nom_x[6];
out_3392229238668395910[7] = delta_x[7] + nom_x[7];
out_3392229238668395910[8] = delta_x[8] + nom_x[8];
void err_fun(double *nom_x, double *delta_x, double *out_999653426340036453) {
out_999653426340036453[0] = delta_x[0] + nom_x[0];
out_999653426340036453[1] = delta_x[1] + nom_x[1];
out_999653426340036453[2] = delta_x[2] + nom_x[2];
out_999653426340036453[3] = delta_x[3] + nom_x[3];
out_999653426340036453[4] = delta_x[4] + nom_x[4];
out_999653426340036453[5] = delta_x[5] + nom_x[5];
out_999653426340036453[6] = delta_x[6] + nom_x[6];
out_999653426340036453[7] = delta_x[7] + nom_x[7];
out_999653426340036453[8] = delta_x[8] + nom_x[8];
}
void inv_err_fun(double *nom_x, double *true_x, double *out_8775703397352493346) {
out_8775703397352493346[0] = -nom_x[0] + true_x[0];
out_8775703397352493346[1] = -nom_x[1] + true_x[1];
out_8775703397352493346[2] = -nom_x[2] + true_x[2];
out_8775703397352493346[3] = -nom_x[3] + true_x[3];
out_8775703397352493346[4] = -nom_x[4] + true_x[4];
out_8775703397352493346[5] = -nom_x[5] + true_x[5];
out_8775703397352493346[6] = -nom_x[6] + true_x[6];
out_8775703397352493346[7] = -nom_x[7] + true_x[7];
out_8775703397352493346[8] = -nom_x[8] + true_x[8];
void inv_err_fun(double *nom_x, double *true_x, double *out_4610969769563887824) {
out_4610969769563887824[0] = -nom_x[0] + true_x[0];
out_4610969769563887824[1] = -nom_x[1] + true_x[1];
out_4610969769563887824[2] = -nom_x[2] + true_x[2];
out_4610969769563887824[3] = -nom_x[3] + true_x[3];
out_4610969769563887824[4] = -nom_x[4] + true_x[4];
out_4610969769563887824[5] = -nom_x[5] + true_x[5];
out_4610969769563887824[6] = -nom_x[6] + true_x[6];
out_4610969769563887824[7] = -nom_x[7] + true_x[7];
out_4610969769563887824[8] = -nom_x[8] + true_x[8];
}
void H_mod_fun(double *state, double *out_4824001013826507625) {
out_4824001013826507625[0] = 1.0;
out_4824001013826507625[1] = 0.0;
out_4824001013826507625[2] = 0.0;
out_4824001013826507625[3] = 0.0;
out_4824001013826507625[4] = 0.0;
out_4824001013826507625[5] = 0.0;
out_4824001013826507625[6] = 0.0;
out_4824001013826507625[7] = 0.0;
out_4824001013826507625[8] = 0.0;
out_4824001013826507625[9] = 0.0;
out_4824001013826507625[10] = 1.0;
out_4824001013826507625[11] = 0.0;
out_4824001013826507625[12] = 0.0;
out_4824001013826507625[13] = 0.0;
out_4824001013826507625[14] = 0.0;
out_4824001013826507625[15] = 0.0;
out_4824001013826507625[16] = 0.0;
out_4824001013826507625[17] = 0.0;
out_4824001013826507625[18] = 0.0;
out_4824001013826507625[19] = 0.0;
out_4824001013826507625[20] = 1.0;
out_4824001013826507625[21] = 0.0;
out_4824001013826507625[22] = 0.0;
out_4824001013826507625[23] = 0.0;
out_4824001013826507625[24] = 0.0;
out_4824001013826507625[25] = 0.0;
out_4824001013826507625[26] = 0.0;
out_4824001013826507625[27] = 0.0;
out_4824001013826507625[28] = 0.0;
out_4824001013826507625[29] = 0.0;
out_4824001013826507625[30] = 1.0;
out_4824001013826507625[31] = 0.0;
out_4824001013826507625[32] = 0.0;
out_4824001013826507625[33] = 0.0;
out_4824001013826507625[34] = 0.0;
out_4824001013826507625[35] = 0.0;
out_4824001013826507625[36] = 0.0;
out_4824001013826507625[37] = 0.0;
out_4824001013826507625[38] = 0.0;
out_4824001013826507625[39] = 0.0;
out_4824001013826507625[40] = 1.0;
out_4824001013826507625[41] = 0.0;
out_4824001013826507625[42] = 0.0;
out_4824001013826507625[43] = 0.0;
out_4824001013826507625[44] = 0.0;
out_4824001013826507625[45] = 0.0;
out_4824001013826507625[46] = 0.0;
out_4824001013826507625[47] = 0.0;
out_4824001013826507625[48] = 0.0;
out_4824001013826507625[49] = 0.0;
out_4824001013826507625[50] = 1.0;
out_4824001013826507625[51] = 0.0;
out_4824001013826507625[52] = 0.0;
out_4824001013826507625[53] = 0.0;
out_4824001013826507625[54] = 0.0;
out_4824001013826507625[55] = 0.0;
out_4824001013826507625[56] = 0.0;
out_4824001013826507625[57] = 0.0;
out_4824001013826507625[58] = 0.0;
out_4824001013826507625[59] = 0.0;
out_4824001013826507625[60] = 1.0;
out_4824001013826507625[61] = 0.0;
out_4824001013826507625[62] = 0.0;
out_4824001013826507625[63] = 0.0;
out_4824001013826507625[64] = 0.0;
out_4824001013826507625[65] = 0.0;
out_4824001013826507625[66] = 0.0;
out_4824001013826507625[67] = 0.0;
out_4824001013826507625[68] = 0.0;
out_4824001013826507625[69] = 0.0;
out_4824001013826507625[70] = 1.0;
out_4824001013826507625[71] = 0.0;
out_4824001013826507625[72] = 0.0;
out_4824001013826507625[73] = 0.0;
out_4824001013826507625[74] = 0.0;
out_4824001013826507625[75] = 0.0;
out_4824001013826507625[76] = 0.0;
out_4824001013826507625[77] = 0.0;
out_4824001013826507625[78] = 0.0;
out_4824001013826507625[79] = 0.0;
out_4824001013826507625[80] = 1.0;
void H_mod_fun(double *state, double *out_1018390387391438144) {
out_1018390387391438144[0] = 1.0;
out_1018390387391438144[1] = 0.0;
out_1018390387391438144[2] = 0.0;
out_1018390387391438144[3] = 0.0;
out_1018390387391438144[4] = 0.0;
out_1018390387391438144[5] = 0.0;
out_1018390387391438144[6] = 0.0;
out_1018390387391438144[7] = 0.0;
out_1018390387391438144[8] = 0.0;
out_1018390387391438144[9] = 0.0;
out_1018390387391438144[10] = 1.0;
out_1018390387391438144[11] = 0.0;
out_1018390387391438144[12] = 0.0;
out_1018390387391438144[13] = 0.0;
out_1018390387391438144[14] = 0.0;
out_1018390387391438144[15] = 0.0;
out_1018390387391438144[16] = 0.0;
out_1018390387391438144[17] = 0.0;
out_1018390387391438144[18] = 0.0;
out_1018390387391438144[19] = 0.0;
out_1018390387391438144[20] = 1.0;
out_1018390387391438144[21] = 0.0;
out_1018390387391438144[22] = 0.0;
out_1018390387391438144[23] = 0.0;
out_1018390387391438144[24] = 0.0;
out_1018390387391438144[25] = 0.0;
out_1018390387391438144[26] = 0.0;
out_1018390387391438144[27] = 0.0;
out_1018390387391438144[28] = 0.0;
out_1018390387391438144[29] = 0.0;
out_1018390387391438144[30] = 1.0;
out_1018390387391438144[31] = 0.0;
out_1018390387391438144[32] = 0.0;
out_1018390387391438144[33] = 0.0;
out_1018390387391438144[34] = 0.0;
out_1018390387391438144[35] = 0.0;
out_1018390387391438144[36] = 0.0;
out_1018390387391438144[37] = 0.0;
out_1018390387391438144[38] = 0.0;
out_1018390387391438144[39] = 0.0;
out_1018390387391438144[40] = 1.0;
out_1018390387391438144[41] = 0.0;
out_1018390387391438144[42] = 0.0;
out_1018390387391438144[43] = 0.0;
out_1018390387391438144[44] = 0.0;
out_1018390387391438144[45] = 0.0;
out_1018390387391438144[46] = 0.0;
out_1018390387391438144[47] = 0.0;
out_1018390387391438144[48] = 0.0;
out_1018390387391438144[49] = 0.0;
out_1018390387391438144[50] = 1.0;
out_1018390387391438144[51] = 0.0;
out_1018390387391438144[52] = 0.0;
out_1018390387391438144[53] = 0.0;
out_1018390387391438144[54] = 0.0;
out_1018390387391438144[55] = 0.0;
out_1018390387391438144[56] = 0.0;
out_1018390387391438144[57] = 0.0;
out_1018390387391438144[58] = 0.0;
out_1018390387391438144[59] = 0.0;
out_1018390387391438144[60] = 1.0;
out_1018390387391438144[61] = 0.0;
out_1018390387391438144[62] = 0.0;
out_1018390387391438144[63] = 0.0;
out_1018390387391438144[64] = 0.0;
out_1018390387391438144[65] = 0.0;
out_1018390387391438144[66] = 0.0;
out_1018390387391438144[67] = 0.0;
out_1018390387391438144[68] = 0.0;
out_1018390387391438144[69] = 0.0;
out_1018390387391438144[70] = 1.0;
out_1018390387391438144[71] = 0.0;
out_1018390387391438144[72] = 0.0;
out_1018390387391438144[73] = 0.0;
out_1018390387391438144[74] = 0.0;
out_1018390387391438144[75] = 0.0;
out_1018390387391438144[76] = 0.0;
out_1018390387391438144[77] = 0.0;
out_1018390387391438144[78] = 0.0;
out_1018390387391438144[79] = 0.0;
out_1018390387391438144[80] = 1.0;
}
void f_fun(double *state, double dt, double *out_6057528982108647722) {
out_6057528982108647722[0] = state[0];
out_6057528982108647722[1] = state[1];
out_6057528982108647722[2] = state[2];
out_6057528982108647722[3] = state[3];
out_6057528982108647722[4] = state[4];
out_6057528982108647722[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
out_6057528982108647722[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
out_6057528982108647722[7] = state[7];
out_6057528982108647722[8] = state[8];
void f_fun(double *state, double dt, double *out_8437871470129484601) {
out_8437871470129484601[0] = state[0];
out_8437871470129484601[1] = state[1];
out_8437871470129484601[2] = state[2];
out_8437871470129484601[3] = state[3];
out_8437871470129484601[4] = state[4];
out_8437871470129484601[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
out_8437871470129484601[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
out_8437871470129484601[7] = state[7];
out_8437871470129484601[8] = state[8];
}
void F_fun(double *state, double dt, double *out_178208695147004115) {
out_178208695147004115[0] = 1;
out_178208695147004115[1] = 0;
out_178208695147004115[2] = 0;
out_178208695147004115[3] = 0;
out_178208695147004115[4] = 0;
out_178208695147004115[5] = 0;
out_178208695147004115[6] = 0;
out_178208695147004115[7] = 0;
out_178208695147004115[8] = 0;
out_178208695147004115[9] = 0;
out_178208695147004115[10] = 1;
out_178208695147004115[11] = 0;
out_178208695147004115[12] = 0;
out_178208695147004115[13] = 0;
out_178208695147004115[14] = 0;
out_178208695147004115[15] = 0;
out_178208695147004115[16] = 0;
out_178208695147004115[17] = 0;
out_178208695147004115[18] = 0;
out_178208695147004115[19] = 0;
out_178208695147004115[20] = 1;
out_178208695147004115[21] = 0;
out_178208695147004115[22] = 0;
out_178208695147004115[23] = 0;
out_178208695147004115[24] = 0;
out_178208695147004115[25] = 0;
out_178208695147004115[26] = 0;
out_178208695147004115[27] = 0;
out_178208695147004115[28] = 0;
out_178208695147004115[29] = 0;
out_178208695147004115[30] = 1;
out_178208695147004115[31] = 0;
out_178208695147004115[32] = 0;
out_178208695147004115[33] = 0;
out_178208695147004115[34] = 0;
out_178208695147004115[35] = 0;
out_178208695147004115[36] = 0;
out_178208695147004115[37] = 0;
out_178208695147004115[38] = 0;
out_178208695147004115[39] = 0;
out_178208695147004115[40] = 1;
out_178208695147004115[41] = 0;
out_178208695147004115[42] = 0;
out_178208695147004115[43] = 0;
out_178208695147004115[44] = 0;
out_178208695147004115[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
out_178208695147004115[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
out_178208695147004115[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_178208695147004115[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_178208695147004115[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
out_178208695147004115[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
out_178208695147004115[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
out_178208695147004115[52] = dt*stiffness_front*state[0]/(mass*state[1]);
out_178208695147004115[53] = -9.8100000000000005*dt;
out_178208695147004115[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
out_178208695147004115[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
out_178208695147004115[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_178208695147004115[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_178208695147004115[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
out_178208695147004115[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
out_178208695147004115[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
out_178208695147004115[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_178208695147004115[62] = 0;
out_178208695147004115[63] = 0;
out_178208695147004115[64] = 0;
out_178208695147004115[65] = 0;
out_178208695147004115[66] = 0;
out_178208695147004115[67] = 0;
out_178208695147004115[68] = 0;
out_178208695147004115[69] = 0;
out_178208695147004115[70] = 1;
out_178208695147004115[71] = 0;
out_178208695147004115[72] = 0;
out_178208695147004115[73] = 0;
out_178208695147004115[74] = 0;
out_178208695147004115[75] = 0;
out_178208695147004115[76] = 0;
out_178208695147004115[77] = 0;
out_178208695147004115[78] = 0;
out_178208695147004115[79] = 0;
out_178208695147004115[80] = 1;
void F_fun(double *state, double dt, double *out_7222922179546997313) {
out_7222922179546997313[0] = 1;
out_7222922179546997313[1] = 0;
out_7222922179546997313[2] = 0;
out_7222922179546997313[3] = 0;
out_7222922179546997313[4] = 0;
out_7222922179546997313[5] = 0;
out_7222922179546997313[6] = 0;
out_7222922179546997313[7] = 0;
out_7222922179546997313[8] = 0;
out_7222922179546997313[9] = 0;
out_7222922179546997313[10] = 1;
out_7222922179546997313[11] = 0;
out_7222922179546997313[12] = 0;
out_7222922179546997313[13] = 0;
out_7222922179546997313[14] = 0;
out_7222922179546997313[15] = 0;
out_7222922179546997313[16] = 0;
out_7222922179546997313[17] = 0;
out_7222922179546997313[18] = 0;
out_7222922179546997313[19] = 0;
out_7222922179546997313[20] = 1;
out_7222922179546997313[21] = 0;
out_7222922179546997313[22] = 0;
out_7222922179546997313[23] = 0;
out_7222922179546997313[24] = 0;
out_7222922179546997313[25] = 0;
out_7222922179546997313[26] = 0;
out_7222922179546997313[27] = 0;
out_7222922179546997313[28] = 0;
out_7222922179546997313[29] = 0;
out_7222922179546997313[30] = 1;
out_7222922179546997313[31] = 0;
out_7222922179546997313[32] = 0;
out_7222922179546997313[33] = 0;
out_7222922179546997313[34] = 0;
out_7222922179546997313[35] = 0;
out_7222922179546997313[36] = 0;
out_7222922179546997313[37] = 0;
out_7222922179546997313[38] = 0;
out_7222922179546997313[39] = 0;
out_7222922179546997313[40] = 1;
out_7222922179546997313[41] = 0;
out_7222922179546997313[42] = 0;
out_7222922179546997313[43] = 0;
out_7222922179546997313[44] = 0;
out_7222922179546997313[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
out_7222922179546997313[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
out_7222922179546997313[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_7222922179546997313[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_7222922179546997313[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
out_7222922179546997313[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
out_7222922179546997313[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
out_7222922179546997313[52] = dt*stiffness_front*state[0]/(mass*state[1]);
out_7222922179546997313[53] = -9.8100000000000005*dt;
out_7222922179546997313[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
out_7222922179546997313[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
out_7222922179546997313[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_7222922179546997313[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_7222922179546997313[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
out_7222922179546997313[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
out_7222922179546997313[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
out_7222922179546997313[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_7222922179546997313[62] = 0;
out_7222922179546997313[63] = 0;
out_7222922179546997313[64] = 0;
out_7222922179546997313[65] = 0;
out_7222922179546997313[66] = 0;
out_7222922179546997313[67] = 0;
out_7222922179546997313[68] = 0;
out_7222922179546997313[69] = 0;
out_7222922179546997313[70] = 1;
out_7222922179546997313[71] = 0;
out_7222922179546997313[72] = 0;
out_7222922179546997313[73] = 0;
out_7222922179546997313[74] = 0;
out_7222922179546997313[75] = 0;
out_7222922179546997313[76] = 0;
out_7222922179546997313[77] = 0;
out_7222922179546997313[78] = 0;
out_7222922179546997313[79] = 0;
out_7222922179546997313[80] = 1;
}
void h_25(double *state, double *unused, double *out_8550702395314279714) {
out_8550702395314279714[0] = state[6];
void h_25(double *state, double *unused, double *out_5845994727189855200) {
out_5845994727189855200[0] = state[6];
}
void H_25(double *state, double *unused, double *out_2187444088811081431) {
out_2187444088811081431[0] = 0;
out_2187444088811081431[1] = 0;
out_2187444088811081431[2] = 0;
out_2187444088811081431[3] = 0;
out_2187444088811081431[4] = 0;
out_2187444088811081431[5] = 0;
out_2187444088811081431[6] = 1;
out_2187444088811081431[7] = 0;
out_2187444088811081431[8] = 0;
void H_25(double *state, double *unused, double *out_1618166537623988050) {
out_1618166537623988050[0] = 0;
out_1618166537623988050[1] = 0;
out_1618166537623988050[2] = 0;
out_1618166537623988050[3] = 0;
out_1618166537623988050[4] = 0;
out_1618166537623988050[5] = 0;
out_1618166537623988050[6] = 1;
out_1618166537623988050[7] = 0;
out_1618166537623988050[8] = 0;
}
void h_24(double *state, double *unused, double *out_3063831735768408815) {
out_3063831735768408815[0] = state[4];
out_3063831735768408815[1] = state[5];
void h_24(double *state, double *unused, double *out_2733857150290724482) {
out_2733857150290724482[0] = state[4];
out_2733857150290724482[1] = state[5];
}
void H_24(double *state, double *unused, double *out_5745369687613404567) {
out_5745369687613404567[0] = 0;
out_5745369687613404567[1] = 0;
out_5745369687613404567[2] = 0;
out_5745369687613404567[3] = 0;
out_5745369687613404567[4] = 1;
out_5745369687613404567[5] = 0;
out_5745369687613404567[6] = 0;
out_5745369687613404567[7] = 0;
out_5745369687613404567[8] = 0;
out_5745369687613404567[9] = 0;
out_5745369687613404567[10] = 0;
out_5745369687613404567[11] = 0;
out_5745369687613404567[12] = 0;
out_5745369687613404567[13] = 0;
out_5745369687613404567[14] = 1;
out_5745369687613404567[15] = 0;
out_5745369687613404567[16] = 0;
out_5745369687613404567[17] = 0;
void H_24(double *state, double *unused, double *out_3790816136629487616) {
out_3790816136629487616[0] = 0;
out_3790816136629487616[1] = 0;
out_3790816136629487616[2] = 0;
out_3790816136629487616[3] = 0;
out_3790816136629487616[4] = 1;
out_3790816136629487616[5] = 0;
out_3790816136629487616[6] = 0;
out_3790816136629487616[7] = 0;
out_3790816136629487616[8] = 0;
out_3790816136629487616[9] = 0;
out_3790816136629487616[10] = 0;
out_3790816136629487616[11] = 0;
out_3790816136629487616[12] = 0;
out_3790816136629487616[13] = 0;
out_3790816136629487616[14] = 1;
out_3790816136629487616[15] = 0;
out_3790816136629487616[16] = 0;
out_3790816136629487616[17] = 0;
}
void h_30(double *state, double *unused, double *out_8471988271360455383) {
out_8471988271360455383[0] = state[4];
void h_30(double *state, double *unused, double *out_2208545198935832850) {
out_2208545198935832850[0] = state[4];
}
void H_30(double *state, double *unused, double *out_2340252241316526767) {
out_2340252241316526767[0] = 0;
out_2340252241316526767[1] = 0;
out_2340252241316526767[2] = 0;
out_2340252241316526767[3] = 0;
out_2340252241316526767[4] = 1;
out_2340252241316526767[5] = 0;
out_2340252241316526767[6] = 0;
out_2340252241316526767[7] = 0;
out_2340252241316526767[8] = 0;
void H_30(double *state, double *unused, double *out_6145862867751596248) {
out_6145862867751596248[0] = 0;
out_6145862867751596248[1] = 0;
out_6145862867751596248[2] = 0;
out_6145862867751596248[3] = 0;
out_6145862867751596248[4] = 1;
out_6145862867751596248[5] = 0;
out_6145862867751596248[6] = 0;
out_6145862867751596248[7] = 0;
out_6145862867751596248[8] = 0;
}
void h_26(double *state, double *unused, double *out_3470315003699609738) {
out_3470315003699609738[0] = state[7];
void h_26(double *state, double *unused, double *out_604006904334020533) {
out_604006904334020533[0] = state[7];
}
void H_26(double *state, double *unused, double *out_1554059230062974793) {
out_1554059230062974793[0] = 0;
out_1554059230062974793[1] = 0;
out_1554059230062974793[2] = 0;
out_1554059230062974793[3] = 0;
out_1554059230062974793[4] = 0;
out_1554059230062974793[5] = 0;
out_1554059230062974793[6] = 0;
out_1554059230062974793[7] = 1;
out_1554059230062974793[8] = 0;
void H_26(double *state, double *unused, double *out_5359669856498044274) {
out_5359669856498044274[0] = 0;
out_5359669856498044274[1] = 0;
out_5359669856498044274[2] = 0;
out_5359669856498044274[3] = 0;
out_5359669856498044274[4] = 0;
out_5359669856498044274[5] = 0;
out_5359669856498044274[6] = 0;
out_5359669856498044274[7] = 1;
out_5359669856498044274[8] = 0;
}
void h_27(double *state, double *unused, double *out_4147430428900407732) {
out_4147430428900407732[0] = state[3];
void h_27(double *state, double *unused, double *out_844321459529009555) {
out_844321459529009555[0] = state[3];
}
void H_27(double *state, double *unused, double *out_4515015553116951678) {
out_4515015553116951678[0] = 0;
out_4515015553116951678[1] = 0;
out_4515015553116951678[2] = 0;
out_4515015553116951678[3] = 1;
out_4515015553116951678[4] = 0;
out_4515015553116951678[5] = 0;
out_4515015553116951678[6] = 0;
out_4515015553116951678[7] = 0;
out_4515015553116951678[8] = 0;
void H_27(double *state, double *unused, double *out_8320626179552021159) {
out_8320626179552021159[0] = 0;
out_8320626179552021159[1] = 0;
out_8320626179552021159[2] = 0;
out_8320626179552021159[3] = 1;
out_8320626179552021159[4] = 0;
out_8320626179552021159[5] = 0;
out_8320626179552021159[6] = 0;
out_8320626179552021159[7] = 0;
out_8320626179552021159[8] = 0;
}
void h_29(double *state, double *unused, double *out_6344999803363309424) {
out_6344999803363309424[0] = state[1];
void h_29(double *state, double *unused, double *out_2793128068725012795) {
out_2793128068725012795[0] = state[1];
}
void H_29(double *state, double *unused, double *out_1830020897002134583) {
out_1830020897002134583[0] = 0;
out_1830020897002134583[1] = 1;
out_1830020897002134583[2] = 0;
out_1830020897002134583[3] = 0;
out_1830020897002134583[4] = 0;
out_1830020897002134583[5] = 0;
out_1830020897002134583[6] = 0;
out_1830020897002134583[7] = 0;
out_1830020897002134583[8] = 0;
void H_29(double *state, double *unused, double *out_5635631523437204064) {
out_5635631523437204064[0] = 0;
out_5635631523437204064[1] = 1;
out_5635631523437204064[2] = 0;
out_5635631523437204064[3] = 0;
out_5635631523437204064[4] = 0;
out_5635631523437204064[5] = 0;
out_5635631523437204064[6] = 0;
out_5635631523437204064[7] = 0;
out_5635631523437204064[8] = 0;
}
void h_28(double *state, double *unused, double *out_7296550941016078105) {
out_7296550941016078105[0] = state[0];
void h_28(double *state, double *unused, double *out_4484828697352558873) {
out_4484828697352558873[0] = state[0];
}
void H_28(double *state, double *unused, double *out_133609374563191668) {
out_133609374563191668[0] = 1;
out_133609374563191668[1] = 0;
out_133609374563191668[2] = 0;
out_133609374563191668[3] = 0;
out_133609374563191668[4] = 0;
out_133609374563191668[5] = 0;
out_133609374563191668[6] = 0;
out_133609374563191668[7] = 0;
out_133609374563191668[8] = 0;
void H_28(double *state, double *unused, double *out_3672001251871877813) {
out_3672001251871877813[0] = 1;
out_3672001251871877813[1] = 0;
out_3672001251871877813[2] = 0;
out_3672001251871877813[3] = 0;
out_3672001251871877813[4] = 0;
out_3672001251871877813[5] = 0;
out_3672001251871877813[6] = 0;
out_3672001251871877813[7] = 0;
out_3672001251871877813[8] = 0;
}
void h_31(double *state, double *unused, double *out_2132776421574599461) {
out_2132776421574599461[0] = state[8];
void h_31(double *state, double *unused, double *out_2046944767694668159) {
out_2046944767694668159[0] = state[8];
}
void H_31(double *state, double *unused, double *out_2180267332296326269) {
out_2180267332296326269[0] = 0;
out_2180267332296326269[1] = 0;
out_2180267332296326269[2] = 0;
out_2180267332296326269[3] = 0;
out_2180267332296326269[4] = 0;
out_2180267332296326269[5] = 0;
out_2180267332296326269[6] = 0;
out_2180267332296326269[7] = 0;
out_2180267332296326269[8] = 1;
void H_31(double *state, double *unused, double *out_5985877958731395750) {
out_5985877958731395750[0] = 0;
out_5985877958731395750[1] = 0;
out_5985877958731395750[2] = 0;
out_5985877958731395750[3] = 0;
out_5985877958731395750[4] = 0;
out_5985877958731395750[5] = 0;
out_5985877958731395750[6] = 0;
out_5985877958731395750[7] = 0;
out_5985877958731395750[8] = 1;
}
#include <eigen3/Eigen/Dense>
#include <iostream>
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
}
void car_err_fun(double *nom_x, double *delta_x, double *out_3392229238668395910) {
err_fun(nom_x, delta_x, out_3392229238668395910);
void car_err_fun(double *nom_x, double *delta_x, double *out_999653426340036453) {
err_fun(nom_x, delta_x, out_999653426340036453);
}
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8775703397352493346) {
inv_err_fun(nom_x, true_x, out_8775703397352493346);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_4610969769563887824) {
inv_err_fun(nom_x, true_x, out_4610969769563887824);
}
void car_H_mod_fun(double *state, double *out_4824001013826507625) {
H_mod_fun(state, out_4824001013826507625);
void car_H_mod_fun(double *state, double *out_1018390387391438144) {
H_mod_fun(state, out_1018390387391438144);
}
void car_f_fun(double *state, double dt, double *out_6057528982108647722) {
f_fun(state, dt, out_6057528982108647722);
void car_f_fun(double *state, double dt, double *out_8437871470129484601) {
f_fun(state, dt, out_8437871470129484601);
}
void car_F_fun(double *state, double dt, double *out_178208695147004115) {
F_fun(state, dt, out_178208695147004115);
void car_F_fun(double *state, double dt, double *out_7222922179546997313) {
F_fun(state, dt, out_7222922179546997313);
}
void car_h_25(double *state, double *unused, double *out_8550702395314279714) {
h_25(state, unused, out_8550702395314279714);
void car_h_25(double *state, double *unused, double *out_5845994727189855200) {
h_25(state, unused, out_5845994727189855200);
}
void car_H_25(double *state, double *unused, double *out_2187444088811081431) {
H_25(state, unused, out_2187444088811081431);
void car_H_25(double *state, double *unused, double *out_1618166537623988050) {
H_25(state, unused, out_1618166537623988050);
}
void car_h_24(double *state, double *unused, double *out_3063831735768408815) {
h_24(state, unused, out_3063831735768408815);
void car_h_24(double *state, double *unused, double *out_2733857150290724482) {
h_24(state, unused, out_2733857150290724482);
}
void car_H_24(double *state, double *unused, double *out_5745369687613404567) {
H_24(state, unused, out_5745369687613404567);
void car_H_24(double *state, double *unused, double *out_3790816136629487616) {
H_24(state, unused, out_3790816136629487616);
}
void car_h_30(double *state, double *unused, double *out_8471988271360455383) {
h_30(state, unused, out_8471988271360455383);
void car_h_30(double *state, double *unused, double *out_2208545198935832850) {
h_30(state, unused, out_2208545198935832850);
}
void car_H_30(double *state, double *unused, double *out_2340252241316526767) {
H_30(state, unused, out_2340252241316526767);
void car_H_30(double *state, double *unused, double *out_6145862867751596248) {
H_30(state, unused, out_6145862867751596248);
}
void car_h_26(double *state, double *unused, double *out_3470315003699609738) {
h_26(state, unused, out_3470315003699609738);
void car_h_26(double *state, double *unused, double *out_604006904334020533) {
h_26(state, unused, out_604006904334020533);
}
void car_H_26(double *state, double *unused, double *out_1554059230062974793) {
H_26(state, unused, out_1554059230062974793);
void car_H_26(double *state, double *unused, double *out_5359669856498044274) {
H_26(state, unused, out_5359669856498044274);
}
void car_h_27(double *state, double *unused, double *out_4147430428900407732) {
h_27(state, unused, out_4147430428900407732);
void car_h_27(double *state, double *unused, double *out_844321459529009555) {
h_27(state, unused, out_844321459529009555);
}
void car_H_27(double *state, double *unused, double *out_4515015553116951678) {
H_27(state, unused, out_4515015553116951678);
void car_H_27(double *state, double *unused, double *out_8320626179552021159) {
H_27(state, unused, out_8320626179552021159);
}
void car_h_29(double *state, double *unused, double *out_6344999803363309424) {
h_29(state, unused, out_6344999803363309424);
void car_h_29(double *state, double *unused, double *out_2793128068725012795) {
h_29(state, unused, out_2793128068725012795);
}
void car_H_29(double *state, double *unused, double *out_1830020897002134583) {
H_29(state, unused, out_1830020897002134583);
void car_H_29(double *state, double *unused, double *out_5635631523437204064) {
H_29(state, unused, out_5635631523437204064);
}
void car_h_28(double *state, double *unused, double *out_7296550941016078105) {
h_28(state, unused, out_7296550941016078105);
void car_h_28(double *state, double *unused, double *out_4484828697352558873) {
h_28(state, unused, out_4484828697352558873);
}
void car_H_28(double *state, double *unused, double *out_133609374563191668) {
H_28(state, unused, out_133609374563191668);
void car_H_28(double *state, double *unused, double *out_3672001251871877813) {
H_28(state, unused, out_3672001251871877813);
}
void car_h_31(double *state, double *unused, double *out_2132776421574599461) {
h_31(state, unused, out_2132776421574599461);
void car_h_31(double *state, double *unused, double *out_2046944767694668159) {
h_31(state, unused, out_2046944767694668159);
}
void car_H_31(double *state, double *unused, double *out_2180267332296326269) {
H_31(state, unused, out_2180267332296326269);
void car_H_31(double *state, double *unused, double *out_5985877958731395750) {
H_31(state, unused, out_5985877958731395750);
}
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
predict(in_x, in_P, in_Q, dt);
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_err_fun(double *nom_x, double *delta_x, double *out_3392229238668395910);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8775703397352493346);
void car_H_mod_fun(double *state, double *out_4824001013826507625);
void car_f_fun(double *state, double dt, double *out_6057528982108647722);
void car_F_fun(double *state, double dt, double *out_178208695147004115);
void car_h_25(double *state, double *unused, double *out_8550702395314279714);
void car_H_25(double *state, double *unused, double *out_2187444088811081431);
void car_h_24(double *state, double *unused, double *out_3063831735768408815);
void car_H_24(double *state, double *unused, double *out_5745369687613404567);
void car_h_30(double *state, double *unused, double *out_8471988271360455383);
void car_H_30(double *state, double *unused, double *out_2340252241316526767);
void car_h_26(double *state, double *unused, double *out_3470315003699609738);
void car_H_26(double *state, double *unused, double *out_1554059230062974793);
void car_h_27(double *state, double *unused, double *out_4147430428900407732);
void car_H_27(double *state, double *unused, double *out_4515015553116951678);
void car_h_29(double *state, double *unused, double *out_6344999803363309424);
void car_H_29(double *state, double *unused, double *out_1830020897002134583);
void car_h_28(double *state, double *unused, double *out_7296550941016078105);
void car_H_28(double *state, double *unused, double *out_133609374563191668);
void car_h_31(double *state, double *unused, double *out_2132776421574599461);
void car_H_31(double *state, double *unused, double *out_2180267332296326269);
void car_err_fun(double *nom_x, double *delta_x, double *out_999653426340036453);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_4610969769563887824);
void car_H_mod_fun(double *state, double *out_1018390387391438144);
void car_f_fun(double *state, double dt, double *out_8437871470129484601);
void car_F_fun(double *state, double dt, double *out_7222922179546997313);
void car_h_25(double *state, double *unused, double *out_5845994727189855200);
void car_H_25(double *state, double *unused, double *out_1618166537623988050);
void car_h_24(double *state, double *unused, double *out_2733857150290724482);
void car_H_24(double *state, double *unused, double *out_3790816136629487616);
void car_h_30(double *state, double *unused, double *out_2208545198935832850);
void car_H_30(double *state, double *unused, double *out_6145862867751596248);
void car_h_26(double *state, double *unused, double *out_604006904334020533);
void car_H_26(double *state, double *unused, double *out_5359669856498044274);
void car_h_27(double *state, double *unused, double *out_844321459529009555);
void car_H_27(double *state, double *unused, double *out_8320626179552021159);
void car_h_29(double *state, double *unused, double *out_2793128068725012795);
void car_H_29(double *state, double *unused, double *out_5635631523437204064);
void car_h_28(double *state, double *unused, double *out_4484828697352558873);
void car_H_28(double *state, double *unused, double *out_3672001251871877813);
void car_h_31(double *state, double *unused, double *out_2046944767694668159);
void car_H_31(double *state, double *unused, double *out_5985877958731395750);
void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
void car_set_mass(double x);
void car_set_rotational_inertia(double x);
File diff suppressed because it is too large Load Diff
@@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void pose_err_fun(double *nom_x, double *delta_x, double *out_944171533359057280);
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5987343215061063396);
void pose_H_mod_fun(double *state, double *out_922696574028504956);
void pose_f_fun(double *state, double dt, double *out_7271275588779717019);
void pose_F_fun(double *state, double dt, double *out_1488608338000593967);
void pose_h_4(double *state, double *unused, double *out_7790743839692054955);
void pose_H_4(double *state, double *unused, double *out_1533656544988368493);
void pose_h_10(double *state, double *unused, double *out_9133294609098034128);
void pose_H_10(double *state, double *unused, double *out_1083055239647106891);
void pose_h_13(double *state, double *unused, double *out_7261076778239694426);
void pose_H_13(double *state, double *unused, double *out_4745930370320701294);
void pose_h_14(double *state, double *unused, double *out_3309539515902900024);
void pose_H_14(double *state, double *unused, double *out_5496897401327853022);
void pose_err_fun(double *nom_x, double *delta_x, double *out_8942860553158829980);
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2254641408306223903);
void pose_H_mod_fun(double *state, double *out_76292315910108330);
void pose_f_fun(double *state, double dt, double *out_1412378285720339068);
void pose_F_fun(double *state, double dt, double *out_5059879879978958987);
void pose_h_4(double *state, double *unused, double *out_8306941651867152905);
void pose_H_4(double *state, double *unused, double *out_41214293034027689);
void pose_h_10(double *state, double *unused, double *out_4127406154043783729);
void pose_H_10(double *state, double *unused, double *out_7440694459650338948);
void pose_h_13(double *state, double *unused, double *out_2904306600296367164);
void pose_H_13(double *state, double *unused, double *out_3171059532298305112);
void pose_h_14(double *state, double *unused, double *out_6520989930447577741);
void pose_H_14(double *state, double *unused, double *out_3922026563305456840);
void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
+12 -40
View File
@@ -316,19 +316,7 @@ def main(demo=False):
recovery_small_only = os.getenv("STARPILOT_MODELD_RECOVERY_SMALL_ONLY") == "1"
selection = resolve_runtime(chestnut_available=chestnut_available and not recovery_small_only, randomize=not recovery_small_only)
requested_model_id = requested_runtime_id(chestnut_available)
from openpilot.starpilot.models.laboratory import freeze_request
from openpilot.starpilot.models.laboratory_runner import LaboratoryPair, load_laboratory_pair
from openpilot.starpilot.models.runtime_bridge.runtime import bind_laboratory_request
from openpilot.starpilot.models.manager import ROOT as MODEL_ROOT
laboratory_request = None
if chestnut_available and not recovery_small_only:
try:
laboratory_request = freeze_request(MODEL_ROOT)
if laboratory_request is not None:
bind_laboratory_request(laboratory_request)
except (OSError, ValueError):
cloudlog.exception("Model Laboratory request unavailable; using the normal model")
CHESTNUT = laboratory_request is not None or selection.allow_big and selection.big_path is not None
CHESTNUT = selection.allow_big and selection.big_path is not None
if CHESTNUT:
from tinygrad.runtime.support.am import startup_trace
startup_trace.ENABLED = True
@@ -404,11 +392,11 @@ def main(demo=False):
# A timed-out GPU worker must never hold up access to the small runner.
small_model = small_prepared = None
selected_small_failed = False
if laboratory_request is not None or selection.big_path is not None and selection.small_path is not None:
if selection.big_path is not None and selection.small_path is not None:
small_model, small_prepared, selected_small_failed = load_small()
if CHESTNUT:
big_path = selection.big_path or modeld_pkl_path(True)
big_artifact_identity = receipt_owner.capture(big_path) if laboratory_request is None else None
big_artifact_identity = receipt_owner.capture(big_path)
big_model = None
def load_big():
nonlocal big_model, big_prepared
@@ -417,11 +405,7 @@ def main(demo=False):
if not demo:
wait_for_chestnut_power(CP, timeout=min(30., BIG_MODEL_TIMEOUT / 2))
set_core_affinity([7])
if laboratory_request is not None:
config = {key: laboratory_request[key] for key in ('enabled', 'lateralModel', 'longitudinalModel')}
m = load_laboratory_pair(cam_w, cam_h, config, root=MODEL_ROOT)
prepared = None
elif selection.big_path is not None:
if selection.big_path is not None:
m, prepared = load_verified_model(cam_w, cam_h,
big_path, selection.big_version, True, selection.big_id, selection.big_sha256)
else:
@@ -451,18 +435,12 @@ def main(demo=False):
initial_chestnut_fallback = CHESTNUT and model is None
if model is None:
model = small_model
if isinstance(model, LaboratoryPair):
import hashlib
import json
request_sha256 = hashlib.sha256(json.dumps(laboratory_request, sort_keys=True, separators=(',', ':')).encode()).hexdigest()
receipt_owner.loaded_pair(model, request_sha256)
else:
receipt_owner.loaded(big_prepared if model.chestnut else small_prepared,
ModelVariant.CHESTNUT if model.chestnut else ModelVariant.SMALL,
"chestnut-run-stalled" if recovery_small_only else
"chestnut-load-failed" if initial_chestnut_fallback else
"selected-load-failed" if (selected_small_failed and not model.chestnut) or requested_model_id != model.model_id else None,
model_id=model.model_id)
receipt_owner.loaded(big_prepared if model.chestnut else small_prepared,
ModelVariant.CHESTNUT if model.chestnut else ModelVariant.SMALL,
"chestnut-run-stalled" if recovery_small_only else
"chestnut-load-failed" if initial_chestnut_fallback else
"selected-load-failed" if (selected_small_failed and not model.chestnut) or requested_model_id != model.model_id else None,
model_id=model.model_id)
HOOKS = jetlink_adapter.hooks()
jetlink_model = None
# before going realtime: the GPU thread it starts would inherit SCHED_FIFO 54 on core 7
@@ -604,7 +582,7 @@ def main(demo=False):
if not sm['carControl'].latActive:
prev_action.desiredCurvature = sm['controlsState'].curvature
if isinstance(model, (CatalogModelState, LaboratoryPair)) or model is jetlink_model:
if isinstance(model, CatalogModelState) or model is jetlink_model:
inputs['prev_action'] = np.array([prev_action.desiredCurvature * max(1.0, v_ego) ** 2,
prev_action.desiredAcceleration], dtype=np.float32)
inputs['lateral_control_params'] = np.array([v_ego, lat_delay], dtype=np.float32)
@@ -616,8 +594,6 @@ def main(demo=False):
send_chestnut = (chestnut_state is not None and
run_count % round(ModelConstants.MODEL_RUN_FREQ / SERVICE_LIST['chestnutGpuState'].frequency) == 0)
model_output = model.run(bufs, transforms, inputs, chestnut_state.send if send_chestnut else None)
laboratory_action = (model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
if isinstance(model, LaboratoryPair) else None)
except Exception:
if not model.chestnut:
raise
@@ -652,11 +628,7 @@ def main(demo=False):
drivingdata_send = messaging.new_message('drivingModelData')
posenet_send = messaging.new_message('cameraOdometry')
if isinstance(model, LaboratoryPair):
assert laboratory_action is not None
action = laboratory_action
else:
action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
filter_action(action, prev_action, v_ego, subaru_tau)
prev_action = action
fill_model_msg(modelv2_send, model_output, action,
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
+3 -4
View File
@@ -1792,7 +1792,6 @@ class SelfdriveD:
from openpilot.starpilot.car.toyota.legacy_lateral import below_engage_lateral_allowed as toyota_below_engage_lateral_allowed
retained_dm_lateral, lateral_events = lateral_event_context(self, CS, intent, native, now_ns, ordinary_lag)
reverse_pause = known_reverse_pause(CS, getattr(self, 'car_state_samples', ()))
pause_brake_mps = self.aol_settings.pause_brake_mps if self.aol_settings is not None else 0.0
if reverse_pause:
lateral_events = [event for event in lateral_events if event not in (EventName.wrongGear, EventName.reverseGear)]
return decide_axes(
@@ -1834,12 +1833,12 @@ class SelfdriveD:
not retained_dm_lateral and (self.sm['driverMonitoringState'].lockout or
self.sm['driverMonitoringState'].alwaysOnLockout or
self.sm['driverMonitoringState'].alertLevel == AlertLevel.three)),
pause_brake_mps=pause_brake_mps,
pause_brake_mps=self.aol_settings.pause_brake_mps if self.aol_settings is not None else 0.0,
gear_pause=reverse_pause,
lateral_inhibit=self.aol_dm_lateral_inhibit or sng_standstill_lateral_inhibit(self.CP, CS) or
volkswagen_lateral_output_inhibited(self.CP, CS) or any(
sample.steerFaultTemporary or pause_brake_mps > 0 and sample.brakePressed and not sample.standstill and
sample.vEgo < pause_brake_mps
sample.steerFaultTemporary or sample.brakePressed and not sample.standstill and
sample.vEgo < (self.aol_settings.pause_brake_mps if self.aol_settings is not None else 0.0)
for sample, _ in getattr(self, 'car_state_samples', ())))
def cold_gm_park_readiness(self, CS, intent, now_ns):
@@ -87,23 +87,6 @@
"segment":"regenAFB9780D823|2025-04-08--23-00-34--0",
"process":"card",
"file":"regenAFB9780D823_2025-04-08--23-00-34--0_card.zst",
"bytes":479871,
"sha256":"5d4a787c2233d913f9c320dbb0aebe277b14959306f3b4bfd2167350ab1e8386",
"input_sha256":"4cc51a77bf8d128d5720fd692e5a0994c887ee35fdd949cc0ce54d4215e2b7a2",
"input_bytes":12527194,
"official_baseline_commit":"ec95db3f1fa19f76940497fedfdb62e09ea19912",
"official_artifact_commit":"9ae451c262c7402c9623f3baa1580c6242d6777b",
"official_baseline_url":"https://raw.githubusercontent.com/commaai/ci-artifacts/9ae451c262c7402c9623f3baa1580c6242d6777b/regenAFB9780D823_2025-04-08--23-00-34--0_card_ec95db3f1fa19f76940497fedfdb62e09ea19912.zst",
"official_baseline_bytes":282094,
"official_baseline_sha256":"e3ac4fc1ce239d1bb864e32d1acf6b33ad134a1bf898ef739a0cc3faf3b18d58",
"config_sha256":"560c1060f4d2e43810e7bbd55d5f361a828bdb027d60f308b2e1d614222a2078",
"car_fingerprint":"KIA_EV6",
"contract":"CAN-FD torque cap 409 with speed-dependent slew limits; torque, normalized torque output and dependent LKAS checksum reflect that contract. Counters, enables and remaining compared fields are unchanged. Initial CarState consumes the first recorded CAN batch; its speed-filter seed and subsequent acceleration transient follow that batch. Camera LKAS damping is 0 as in Dom; all 5902 changed payloads match the hosted HYUNDAI2 replay byte-for-byte. Only damping byte 8 and its CRC differ; all other compared fields and messages are retained. External-Panda radar discovery and parsing use the selected CAN-FD bus offset. This EV6 has a quiet first Panda and selected owner on index 1: radarUnavailable changes from true to false and all 1200 existing radarTracks messages contain received MRR35 points. Message counts and all carOutput/sendcan payloads are identical to the previous reviewed expectation; no additional ignored fields or messages.",
"producing_source_snapshot":"c8e629571b0305eb998bbf9f4a8a7e24c6c7485e",
"previous_reviewed_expectation":{
"segment":"regenAFB9780D823|2025-04-08--23-00-34--0",
"process":"card",
"file":"regenAFB9780D823_2025-04-08--23-00-34--0_card.zst",
"bytes":301448,
"sha256":"8a4ad9101c3a26776b1fe8339e8575b50442c4264f102aa0027d441d176091ca",
"input_sha256":"4cc51a77bf8d128d5720fd692e5a0994c887ee35fdd949cc0ce54d4215e2b7a2",
@@ -118,14 +101,6 @@
"contract":"CAN-FD torque cap 409 with speed-dependent slew limits; torque, normalized torque output and dependent LKAS checksum reflect that contract. Counters, enables and remaining compared fields are unchanged. Initial CarState consumes the first recorded CAN batch; its speed-filter seed and subsequent acceleration transient follow that batch. Camera LKAS damping is 0 as in Dom; all 5902 changed payloads match the hosted HYUNDAI2 replay byte-for-byte. Only damping byte 8 and its CRC differ; all other compared fields and messages are retained.",
"producing_source_snapshot":"d41159e65b399ff3b5ef5fd339822b6c628c7096"
},
"intentional_change_commit":"3edb88f68cae7d160dacf75cd1edcacb8c9654d2",
"hosted_evidence_run":42882033585,
"review_evidence":{
"ci_run":42882033585,
"ci_artifact":13797188172,
"review_status":"Compared every decoded output payload against the previous reviewed reference. Only radar points, two radarUnavailable values, and existing ignored cumLagMs differ. Causal source change is the selected CAN-FD radar bus offset; native safety and command output are unchanged."
}
},
{
"segment":"regen1455E3B4BDF|2025-04-09--03-26-06--0",
"process":"controlsd",
@@ -1,7 +1,7 @@
from collections.abc import Callable
from openpilot.common.params import Params
from openpilot.starpilot.car.hyundai.aol import ioniq6_alpha_long_available
from openpilot.starpilot.car.hyundai.aol import ioniq6_settings_capable
from openpilot.starpilot.car.ford.longitudinal_preferences import release_long_available as ford_release_long_available
from opendbc.car.volkswagen.meb_modes import MEB_CARS, settings_capable as meb_settings_capable
from openpilot.starpilot.car.volkswagen.preferences import stock_long_requested
@@ -144,7 +144,7 @@ class DeveloperLayout(Widget):
# CP gating
if ui_state.CP is not None:
alpha_avail = (meb_settings_capable(ui_state.CP) if ui_state.CP.brand == 'volkswagen' and ui_state.CP.carFingerprint in MEB_CARS
else ui_state.CP.alphaLongitudinalAvailable or ioniq6_alpha_long_available(ui_state.CP))
else ui_state.CP.alphaLongitudinalAvailable or ioniq6_settings_capable(ui_state.CP))
if not alpha_avail or (self._is_release and not (meb_settings_capable(ui_state.CP) or ford_release_long_available(ui_state.CP))):
self._alpha_long_toggle.set_visible(False)
else:
@@ -216,7 +216,7 @@ class DeveloperLayout(Widget):
if result == DialogResult.CONFIRM:
if (not ui_state.is_offroad() or ui_state.CP is None or
not (meb_settings_capable(ui_state.CP) if ui_state.CP.brand == 'volkswagen' and ui_state.CP.carFingerprint in MEB_CARS
else ui_state.CP.alphaLongitudinalAvailable or ioniq6_alpha_long_available(ui_state.CP)) or
else ui_state.CP.alphaLongitudinalAvailable or ioniq6_settings_capable(ui_state.CP)) or
self._is_release and not (meb_settings_capable(ui_state.CP) or ford_release_long_available(ui_state.CP))):
self._alpha_long_toggle.action_item.set_state(False)
return
@@ -1,4 +1,3 @@
from openpilot.starpilot.car.hyundai.aol import ioniq6_alpha_long_available
from openpilot.starpilot.car.ford.longitudinal_preferences import release_long_available as ford_release_long_available
from openpilot.cereal import log
from openpilot.common.params import Params, UnknownKeyName
@@ -197,7 +196,7 @@ class TogglesLayout(Widget):
unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control.")
long_desc = unavailable + " " + tr("openpilot longitudinal control may come in a future update.")
if ui_state.CP.alphaLongitudinalAvailable or ioniq6_alpha_long_available(ui_state.CP):
if ui_state.CP.alphaLongitudinalAvailable:
if self._is_release and not ford_release_long_available(ui_state.CP):
long_desc = unavailable + " " + tr("An alpha version of openpilot longitudinal control can be tested, along with " +
"Experimental mode, on non-release branches.")
@@ -7,7 +7,7 @@ from openpilot.selfdrive.ui.mici.widgets.dialog import BigDialog, BigInputDialog
from openpilot.system.ui.lib.application import gui_app
from openpilot.selfdrive.ui.layouts.settings.common import restart_needed_callback
from openpilot.selfdrive.ui.ui_state import ui_state
from openpilot.starpilot.car.hyundai.aol import ioniq6_alpha_long_available
from openpilot.starpilot.car.hyundai.aol import ioniq6_settings_capable
from openpilot.starpilot.car.ford.longitudinal_preferences import release_long_available as ford_release_long_available
from opendbc.car.volkswagen.meb_modes import MEB_CARS, settings_capable as meb_settings_capable
from openpilot.starpilot.car.volkswagen.preferences import stock_long_requested
@@ -177,7 +177,7 @@ class DeveloperLayoutMici(NavScroller):
# CP gating
if ui_state.CP is not None:
alpha_avail = (meb_settings_capable(ui_state.CP) if ui_state.CP.brand == 'volkswagen' and ui_state.CP.carFingerprint in MEB_CARS
else ui_state.CP.alphaLongitudinalAvailable or ioniq6_alpha_long_available(ui_state.CP))
else ui_state.CP.alphaLongitudinalAvailable or ioniq6_settings_capable(ui_state.CP))
if not alpha_avail or (ui_state.is_release and not (meb_settings_capable(ui_state.CP) or ford_release_long_available(ui_state.CP))):
self._alpha_long_toggle.set_visible(False)
else:
@@ -201,7 +201,7 @@ class DeveloperLayoutMici(NavScroller):
def do_toggle(_state: bool):
if (not ui_state.is_offroad() or (_state and (ui_state.CP is None or
not (meb_settings_capable(ui_state.CP) if ui_state.CP.brand == 'volkswagen' and ui_state.CP.carFingerprint in MEB_CARS
else ui_state.CP.alphaLongitudinalAvailable or ioniq6_alpha_long_available(ui_state.CP)) or
else ui_state.CP.alphaLongitudinalAvailable or ioniq6_settings_capable(ui_state.CP)) or
ui_state.is_release and not (meb_settings_capable(ui_state.CP) or ford_release_long_available(ui_state.CP))))):
self._update_toggles()
return
+1 -3
View File
@@ -9,10 +9,9 @@ from openpilot.common.realtime import Priority, config_realtime_process, drop_re
from openpilot.system.ui.lib.application import gui_app
from openpilot.selfdrive.ui.layouts.main import MainLayout
from openpilot.selfdrive.ui.mici.layouts.main import MiciMainLayout
from openpilot.selfdrive.ui.ui_state import device, ui_state
from openpilot.selfdrive.ui.ui_state import ui_state
from openpilot.starpilot.ui.presentation import Profile
from openpilot.starpilot.ui.startup import select_ui
from openpilot.starpilot.ui.wheel_input import poll_screen_off_wheel
BIG_UI = gui_app.big_ui()
@@ -26,7 +25,6 @@ def update_frame(preview=None, controllers=None, star=None, game_mode=None, arab
# or another widget covers the StarPilot page.
star._poll_projection_bookmark(time.monotonic_ns())
star._poll_projection_slc(time.monotonic_ns())
poll_screen_off_wheel(star, device.awake, time.monotonic_ns())
if game_mode is not None:
game_mode()
if controllers is not None:
+1 -6
View File
@@ -16,8 +16,6 @@ GearShifter = car.CarState.GearShifter
AOL_TOGGLE = 9
PAUSE_LATERAL = 3
PAUSE_LONGITUDINAL = 4
# The only wheel action WheelPublisher delivers from cruise MAIN.
ADOPT_SPEED_LIMIT = 10
@dataclass(frozen=True)
class AolSettings:
@@ -30,7 +28,6 @@ class AolSettings:
mode_actions: tuple[int, int, int] = (0, 0, 0)
custom_actions: tuple[int, int, int] = (0, 0, 0)
default_physical_buttons: bool = True
main_wheel_action: bool = False
def _button_value(params, key: str) -> int | None:
@@ -67,7 +64,6 @@ def read_settings(params) -> AolSettings:
enabled=params.get_bool('AlwaysOnLateral') and valid_pause, pause_brake_mps=pause_mps if valid_pause else 0.0,
lkas_action=_axis_action(lkas), main_action=_axis_action(main),
default_physical_buttons=lkas == 0 and main == 0,
main_wheel_action=main == ADOPT_SPEED_LIMIT,
distance_actions=(_button_action(params, 'DistanceButtonControl'),
_button_action(params, 'LongDistanceButtonControl'),
_button_action(params, 'VeryLongDistanceButtonControl')),
@@ -324,8 +320,7 @@ class AolCardIntent:
independent_buttons = self.independent_cruise_buttons and (main_managed or lkas_managed)
# The opt-in is bound to existing HKG torque owners. Other physical
# owners keep their current event and native contracts.
main_toggle = (not independent_buttons or main_managed) and not (
self.independent_cruise_buttons and self.settings.main_wheel_action)
main_toggle = not independent_buttons or main_managed
lkas_toggle = not independent_buttons or lkas_managed
previously_held = (self._main_held and main_toggle) or (self._lkas_held and lkas_toggle)
cancel_pressed = False
+4 -20
View File
@@ -49,11 +49,8 @@ def temporary_steering_intent_current(self, sm, cp, cs, axis, stamp, now_ns, *,
now_ns - previous[8] <= AXIS_MAX_AGE_NS and
0 < self.lateral_source_ns <= now_ns and
now_ns - self.lateral_source_ns <= AXIS_MAX_AGE_NS)
repeated_native_first = bool(signature == previous and not axis.lateralActive and
0 < now_ns <= self.hkg_eps_join_until_ns)
held_original = bool(axis.desiredLateral and (axis.lateralActive or
cp.brand == 'hyundai' and str(cp.steerControlType) == 'angle' and
(fresh_active_successor or repeated_native_first)) and axis.nativeAcknowledged and
cp.brand == 'hyundai' and str(cp.steerControlType) == 'angle' and fresh_active_successor) and axis.nativeAcknowledged and
(signature == previous or fresh_active_successor))
if not withdrawn and not held_original:
return False
@@ -64,27 +61,15 @@ def temporary_steering_intent_current(self, sm, cp, cs, axis, stamp, now_ns, *,
any(event.name == stock_user and event.userDisable for event in sm['onroadEvents']))
from openpilot.starpilot.car.subaru.aol import temporary_eps_brake_recovery
subaru_brake = bool(stock_eps_user and temporary_eps_brake_recovery(cp, cs))
from opendbc.car import structs
from opendbc.car.honda.stock_aol import qualified as honda_qualified
# Existing LAT intent only: brake still removes LONG, and EPS still blocks output.
honda_eps = bool(honda_qualified(cp, marked_only=True) and cs.steerFaultTemporary and
self.lateral_only_baseline and not axis.desiredLongitudinal and not axis.longitudinalActive and
cs.gearShifter in (structs.CarState.GearShifter.drive, structs.CarState.GearShifter.sport,
structs.CarState.GearShifter.low, structs.CarState.GearShifter.brake) and
cs.cruiseState.available and not cs.accFaulted and not cs.doorOpen and
not cs.vehicleSensorsInvalid and not cs.steeringDisengage and not cs.buttonEvents)
# Seatbelt blocks normal LONG, not admitted LAT intent. Keep warnings/native/output checks.
events = tuple(event for event in sm['onroadEvents'] if not (
stock_eps_user and event.name == stock_user and event.userDisable and
not event.softDisable and not event.immediateDisable) and not (
(subaru_brake or honda_eps and cs.brakePressed) and
event.name == log.OnroadEvent.EventName.pedalPressed and event.userDisable and
subaru_brake and event.name == log.OnroadEvent.EventName.pedalPressed and event.userDisable and
not event.softDisable and not event.immediateDisable) and not (
self.lateral_only_baseline and cs.seatbeltUnlatched and not axis.desiredLongitudinal and
not axis.longitudinalActive and event.name == log.OnroadEvent.EventName.seatbeltNotLatched and
event.softDisable and event.noEntry and not event.immediateDisable and not event.userDisable))
if honda_eps and any(event.userDisable for event in events):
return False
temporary_names = (log.OnroadEvent.EventName.steerTempUnavailable,
log.OnroadEvent.EventName.steerTempUnavailableSilent)
if (not sm.seen['onroadEvents'] or not sm.valid['onroadEvents'] or not sm.alive['onroadEvents'] or
@@ -127,6 +112,7 @@ def temporary_steering_intent_current(self, sm, cp, cs, axis, stamp, now_ns, *,
stock_ford_eps = bool(ford_temporary_eps_restriction(cp, cs) and
all(not event.userDisable or event.name in (log.OnroadEvent.EventName.pcmDisable,
log.OnroadEvent.EventName.pedalPressed) and not event.softDisable and not event.immediateDisable for event in events))
from opendbc.car import structs
from opendbc.car.mazda.stock_aol import qualified as mazda_qualified
stock_mazda_eps = bool(mazda_qualified(cp, marked_only=True) and
cs.cruiseState.available and not cs.cruiseState.enabled and
@@ -163,7 +149,7 @@ def temporary_steering_intent_current(self, sm, cp, cs, axis, stamp, now_ns, *,
ford_alpha_eps = bool(stock_ford_eps and cp.openpilotLongitudinalControl)
restricted_eps = bool((temporary_eps_restriction(cp, cs) or stock_ford_eps or stock_mazda_eps or stock_rivian_eps or
stock_volkswagen_eps or stock_subaru_eps) and self.lateral_only_baseline and
not axis.desiredLongitudinal and not axis.longitudinalActive) or ford_alpha_eps or hkg_angle_eps or honda_eps
not axis.desiredLongitudinal and not axis.longitudinalActive) or ford_alpha_eps or hkg_angle_eps
native = _configured_transport_native(sm, cp, self.session, now_ns,
require_longitudinal=not ford_alpha_eps or axis.longitudinalActive, allow_restricted_lateral=restricted_eps)
if (native is None or not native.pandaInventory or
@@ -174,8 +160,6 @@ def temporary_steering_intent_current(self, sm, cp, cs, axis, stamp, now_ns, *,
native.observedMonoTime < self.lateral_native_ns or
(native.pandaSerial, tuple(p.hardwareSerial for p in native.pandaInventory)) != self.lateral_inventory):
return False
if hkg_angle_eps and fresh_active_successor and axis.desiredLateral and not axis.lateralActive:
self.hkg_eps_join_until_ns = min(previous[4], self.lateral_source_ns + AXIS_MAX_AGE_NS)
if stock_volkswagen_eps and matched_withdrawal:
self.volkswagen_eps_source_ns = int(axis.sourceCarStateMonoTime)
if cs.steerFaultTemporary and matched_withdrawal:
+2 -3
View File
@@ -40,8 +40,7 @@ class HondaCan:
assert 'BRAKE_PRESSED' in brake_msg.sigs
assert brake_msg.address in pt.addresses
def frames(self, *, cruise_button=0, lkas=0, cruise=0, brake=False, main=True, eps=0, gear=None, gas=0,
omit=(), speed_kph=45., steering_torque=0):
def frames(self, *, cruise_button=0, lkas=0, cruise=0, brake=False, main=True, eps=0, gear=None, gas=0, omit=(), speed_kph=45.):
intended: dict[str, dict[str, float]] = {
'SCM_BUTTONS': {'CRUISE_BUTTONS': cruise_button, 'CRUISE_SETTING': lkas},
'ENGINE_DATA': {'XMISSION_SPEED': speed_kph},
@@ -52,7 +51,7 @@ class HondaCan:
'BRAKE_SWITCH': 0 if self.alt_brake else int(brake),
'PEDAL_GAS': gas,
},
'STEER_STATUS': {'STEER_STATUS': eps, 'STEER_TORQUE_SENSOR': steering_torque},
'STEER_STATUS': {'STEER_STATUS': eps, 'STEER_TORQUE_SENSOR': 0},
self.gear_msg: {'GEAR_SHIFTER': self.drive if gear is None else gear},
'SEATBELT_STATUS': {'SEATBELT_DRIVER_LATCHED': 1, 'SEATBELT_DRIVER_LAMP': 0},
'BRAKE_MODULE': {'BRAKE_PRESSED': int(brake)},
@@ -1,79 +0,0 @@
from dataclasses import replace
import pytest
from opendbc.car.hyundai.classic_long_aol import aol_word
from opendbc.car.hyundai.values import CAR
from openpilot.cereal import messaging
from openpilot.starpilot.aol.tests.test_classic_long_profiles import params
from openpilot.starpilot.aol.tests.test_hkg_shared_cancel_guard import captured_context
from openpilot.starpilot.aol.transport_pause import cp_identity
from openpilot.starpilot.aol.wire import encode_safety
@pytest.mark.parametrize('identity', (CAR.HYUNDAI_ELANTRA_2021, CAR.HYUNDAI_SONATA, CAR.HYUNDAI_IONIQ))
@pytest.mark.parametrize('failure', ('retain', 'rx_fault', 'hardware_fault', 'lat_denied', 'expired', 'can_timeout', 'producer'))
def test_classic_cancel_holds_original_intent_across_newer_health_and_old_long_echo(identity, failure):
_, cs, sm, feedback, _, native, now = captured_context()
cp = params(identity, lda=True)
cp.safetyConfigs[0].safetyParam = aol_word(cp)
cp.alternativeExperience = 32
config = cp.safetyConfigs[0]
feedback.cp = cp_identity(cp)
slot = replace(native.pandaInventory[0], safetyModel=int(config.safetyModel.raw),
safetyParam=config.safetyParam, controlsAllowed=True)
native = replace(native, safetyModel=int(config.safetyModel.raw), safetyParam=config.safetyParam,
longitudinalAllowed=True, validUntilMonoTime=now + 200_000_000, pandaInventory=(slot,))
def publish(tick, *, withdrawn=False, echo=False):
stamp = now + tick * 10_000_000
axis = sm['aolAxisState'].as_builder()
axis.sequence += 1
axis.sourceCarStateMonoTime = axis.observedMonoTime = stamp
axis.validUntilMonoTime = stamp + 30_000_000
if withdrawn:
axis.mode = 'lateralOnly'
axis.desiredLongitudinal = axis.longitudinalActive = False
if failure == 'producer' and tick:
axis.intentProducerSessionId = 'foreign'
sm['aolAxisState'] = axis.as_reader()
sm.logMonoTime['aolAxisState'] = stamp
pandas = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=stamp)
panda = pandas.pandaStates[0]
panda.safetyModel, panda.safetyParam, panda.alternativeExperience = config.safetyModel, config.safetyParam, 32
panda.controlsAllowed = not withdrawn
if failure == 'rx_fault' and tick:
panda.safetyRxChecksInvalid = True
if failure == 'hardware_fault' and tick:
panda.faults = ['interruptRateCan1']
sm['pandaStates'] = pandas.as_reader().pandaStates
sm.logMonoTime['pandaStates'] = stamp
wire = native
if failure == 'lat_denied' and tick:
wire = replace(wire, lateralAllowed=False)
if failure == 'expired' and tick:
wire = replace(wire, validUntilMonoTime=stamp - 1)
if echo:
wire = replace(native, observedMonoTime=stamp, validUntilMonoTime=stamp + 200_000_000,
sourcePandaStatesMonoTime=stamp, requestedLongitudinal=False, longitudinalAllowed=False,
pandaInventory=(replace(slot, controlsAllowed=False),))
sm['aolSafetyWire'] = encode_safety(wire)
sm.logMonoTime['aolSafetyWire'] = wire.observedMonoTime
return stamp
publish(0)
assert not feedback.observe(sm, cp, cs, now, latched=True, cancel_source_ns=now,
intent_producer_session='authority-card')
assert feedback.longitudinal_cancel_ns == now
cs.buttonEvents = []
if failure == 'can_timeout':
cs.canTimeout = True
for tick in range(1, 8):
stamp = publish(tick, withdrawn=True)
rejected = feedback.observe(sm, cp, cs, stamp, latched=True, intent_producer_session='authority-card')
if failure != 'retain':
assert rejected
return
assert not rejected
assert feedback.longitudinal_cancel_ns == now
stamp = publish(8, withdrawn=True, echo=True)
assert not feedback.observe(sm, cp, cs, stamp, latched=True, intent_producer_session='authority-card')
assert feedback.longitudinal_cancel_ns == 0
@@ -1,81 +0,0 @@
"""A zero cruise engagement floor must retain stop-and-go authority."""
from contextlib import ExitStack
import pytest
from opendbc.car import structs
from opendbc.car.gm.tests.test_ordinary_camera import params
from opendbc.car.gm.values import CAR, GMFlags
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.selfdrive.car.car_events import CarEvents, EventName
from openpilot.starpilot.aol.intent import read_settings
from openpilot.starpilot.aol.tests import test_ioniq_availability as fixture
def truck_params():
cp = params(CAR.CHEVROLET_SILVERADO, alpha=True)
cp.flags |= int(GMFlags.HAS_BSM)
assert cp.minEnableSpeed == 0 and cp.openpilotLongitudinalControl and not cp.pcmCruise
return cp
def stopped(speed):
cs = fixture.car_state()
cs.vEgo, cs.vEgoRaw, cs.standstill = speed, 0., True
cs.cruiseState.available = cs.cruiseState.enabled = cs.cruiseState.standstill = True
return cs
@pytest.mark.parametrize('speed', [-.04, -.00876117, -.0000004, 0., .02])
def test_zero_minimum_accepts_filtered_standstill_undershoot(speed):
cp = truck_params()
cs = stopped(speed)
events = CarEvents(cp).update(cs, cs, structs.CarControl(enabled=True))
assert EventName.belowEngageSpeed not in events.names
assert EventName.resumeRequired in events.names
@pytest.mark.parametrize('minimum', [1.3888889, 10.72896])
@pytest.mark.parametrize('speed', [-.04, 0., .5, 1.])
def test_positive_cruise_minimum_remains_enforced(minimum, speed):
cp = truck_params()
cp.minEnableSpeed = minimum
cs = stopped(speed)
cs.standstill = speed <= 0.
events = CarEvents(cp).update(cs, cs, structs.CarControl(enabled=True))
assert EventName.belowEngageSpeed in events.names
@pytest.mark.parametrize('guard,expected', [('reverse', EventName.reverseGear), ('park', EventName.wrongGear),
('acc', EventName.accFaulted), ('brake', EventName.preEnableStandstill)])
def test_zero_minimum_preserves_physical_guards(guard, expected):
cp = truck_params()
cs, previous = stopped(-.008), stopped(0.)
if guard in ('reverse', 'park'):
cs.gearShifter = guard
elif guard == 'acc':
cs.accFaulted = True
else:
cs.brakePressed = True
events = CarEvents(cp).update(cs, previous, structs.CarControl(enabled=True))
assert expected in events.names
def test_combined_authority_survives_repeated_standstill_undershoot():
with OpenpilotPrefix(), ExitStack() as cleanup:
case = fixture.IoniqAvailabilityTests(methodName='runTest')
cleanup.callback(case.doCleanups)
sd, _, step = case._authority_callers(True, cp=truck_params(), genuine_events=True)
sd.aol_settings = read_settings(sd.params)
for tick, speed in enumerate((.02, 0., -.00876117, -.0001, 0., .002) * 4):
axis, host, control = step(tick, observed_state=stopped(speed))
assert host.enabled and host.active
assert EventName.belowEngageSpeed not in sd.events.names
assert axis.desiredLongitudinal and axis.longitudinalActive and axis.nativeAcknowledged
assert control.longActive
# The eligibility correction cannot bypass a native denial or driver brake.
_, _, control = step(24, observed_state=stopped(0.), long_allowed=False)
assert not control.longActive
cs = stopped(0.)
cs.brakePressed = True
_, _, control = step(25, observed_state=cs)
assert not control.longActive
@@ -52,12 +52,6 @@ class TestHondaCurrentCaller(unittest.TestCase):
def test_nidec_civic_temporary_eps_reverse_return(self):
self.run_current_caller(candidate=CAR.HONDA_CIVIC, word=0)
def test_radarless_alpha_civic_eps_2_with_brake_override(self):
self.run_current_caller(candidate=CAR.HONDA_CIVIC_2022, word=10, alpha=True, hybrid=True, eps_pedal='brake')
def test_radarless_alpha_civic_eps_2_with_gas_override(self):
self.run_current_caller(candidate=CAR.HONDA_CIVIC_2022, word=10, alpha=True, hybrid=True, eps_pedal='gas')
def test_diagnostic_during_recovered_reverse(self):
self.run_current_caller(candidate=CAR.HONDA_CIVIC_BOSCH, word=0, fault='diagnostic')
@@ -65,7 +59,7 @@ class TestHondaCurrentCaller(unittest.TestCase):
self.run_current_caller(candidate=CAR.HONDA_CIVIC_BOSCH, word=0, fault='relay')
def run_current_caller(self, *, candidate, word, fault=None, alternate_brake=False, alpha=False, speed_mps=12.5,
pedal=False, host_scenario=None, serialized=False, eps_pedal=None, hybrid=False):
pedal=False, host_scenario=None, serialized=False):
safety = libsafety_py.libsafety
with (
OpenpilotPrefix(),
@@ -90,8 +84,6 @@ class TestHondaCurrentCaller(unittest.TestCase):
pt_bus = 0 if nidec or candidate.config.flags & (HondaFlags.BOSCH_RADARLESS | HondaFlags.BOSCH_CANFD) else 1
gear_msg = CANDBC(DBC[candidate][Bus.pt]).name_to_msg['GEARBOX_AUTO']
fp[pt_bus][gear_msg.address] = gear_msg.size
if hybrid:
fp[pt_bus][0x184] = 8
if pedal:
fp[pt_bus][0x201] = 6
if alternate_brake:
@@ -121,8 +113,6 @@ class TestHondaCurrentCaller(unittest.TestCase):
self.assertEqual(cp.carVin, vin)
self.assertEqual([record.to_dict() for record in cp.carFw], [record.to_dict() for record in fw])
self.assertEqual(cp.carFingerprint, candidate)
if hybrid:
self.assertTrue(cp.flags & HondaFlags.HYBRID)
self.assertEqual((cp.safetyConfigs[0].safetyParam, cp.alternativeExperience), (word, 32))
self.assertEqual(cp.pcmCruise, not (alpha or pedal))
self.assertEqual(cp.openpilotLongitudinalControl, nidec or alpha)
@@ -169,10 +159,8 @@ class TestHondaCurrentCaller(unittest.TestCase):
incoming = stream.frames(
cruise_button=3 if tick == 20 or host_scenario == 'long_reengage' and tick == 130 else 0,
cruise=int(21 <= tick < 50 or reengage),
brake=50 <= tick < 55 or eps_pedal == 'brake' and 80 <= tick < 95,
eps=(2 if eps_pedal else 6) if 80 <= tick < 95 else 0,
gas=20 if eps_pedal == 'gas' and 80 <= tick < 95 else 0,
steering_torque=1301 if eps_pedal and 80 <= tick < 95 else 0,
brake=50 <= tick < 55,
eps=6 if 80 <= tick < 95 else 0,
gear=2 if 110 <= tick < 125 else None,
lkas=0, speed_kph=speed_mps * 3.6,
)
@@ -267,7 +255,7 @@ class TestHondaCurrentCaller(unittest.TestCase):
self.assertFalse(cs.steerFaultPermanent)
self.assertEqual(cs.steerFaultTemporary, 80 <= tick < 95)
self.assertEqual(cs.gearShifter, structs.CarState.GearShifter.reverse if 110 <= tick < 125 else structs.CarState.GearShifter.drive)
self.assertEqual(cs.brakePressed, 50 <= tick < 55 or eps_pedal == 'brake' and 80 <= tick < 95)
self.assertEqual(cs.brakePressed, 50 <= tick < 55)
self.assertTrue(cs.cruiseState.available)
self.assertEqual(cs.cruiseState.enabled, 21 <= tick < 50 or reengage)
intent = decode_intent(published['aolIntentWire'].aolIntentWire)
@@ -373,9 +361,9 @@ class TestHondaCurrentCaller(unittest.TestCase):
if not previous.latActive:
self.assertEqual((tx['torque'], tx['request']), (0, False), row)
else:
if (alpha and address in (0x1DF, 0x1C8) or pedal and address == 0x200) and tick == 50:
if (alpha and address == 0x1DF or pedal and address == 0x200) and tick == 50:
# Card applies the prior Controls envelope. Physical brake has
# already revoked LONG; the native hook must reject this
# already revoked LONG; the native hook must reject this one
# in-flight positive command before the new envelope arrives.
self.assertFalse(accepted, row)
self.assertTrue(previous.longActive, row)
@@ -387,8 +375,6 @@ class TestHondaCurrentCaller(unittest.TestCase):
accel = (data[3] << 3) | (data[4] >> 5)
accel = accel - 0x800 if accel & 0x400 else accel
self.assertEqual((gas, accel), (-30000, 0), row)
if alpha and address == 0x1C8 and tick >= 52 and not reengage:
self.assertEqual(int.from_bytes(data[:2], 'big') >> 4, 0, row)
row['TX'].append(tx)
row['events'] = [str(e.name) for e in driven['onroadEvents'].onroadEvents]
row['fault_reason'] = str(driven['aolAxisState'].aolAxisState.faultReason)
@@ -396,11 +382,6 @@ class TestHondaCurrentCaller(unittest.TestCase):
self.assertTrue(command.latActive, row)
self.assertTrue(any(t.get('accepted') and t.get('request') and t.get('torque') for t in row['TX']), row)
self.assertTrue(intent.allowedLatch, row)
if eps_pedal and 82 <= tick < 95:
self.assertTrue(cs.steerFaultTemporary and cs.steeringPressed, row)
self.assertTrue(cs.brakePressed if eps_pedal == 'brake' else cs.gasPressed, row)
self.assertTrue(card.aol_card_intent.allowed_latch, row)
self.assertFalse(command.latActive or command.longActive or row['permission'] & 1, row)
if tick == 90 or tick == 120 and fault != "relay":
self.assertTrue(card.aol_card_intent.allowed_latch, row)
self.assertTrue(intent.lateralArmed, row)
@@ -1,209 +0,0 @@
"""Civic route regressions: zero brake pause, EPS intent and native receipt order."""
from dataclasses import replace
from types import SimpleNamespace as NS
from typing import cast
import pytest
from opendbc.car import structs
from opendbc.car.honda.tests.test_stock_aol import final_cp
from opendbc.car.honda.values import CAR
from openpilot.cereal import log, messaging
from openpilot.selfdrive.selfdrived.events import Events
from openpilot.selfdrive.selfdrived.selfdrived import SelfdriveD
from openpilot.selfdrive.car.card import Car
from openpilot.starpilot.aol.transport_pause import TransportPauseFeedback
from openpilot.starpilot.aol.intent import AolSettings
from openpilot.starpilot.tests.test_ford_reverse_intent import NOW, PRODUCER, change, native_change, sample
def civic():
cp = final_cp(CAR.HONDA_CIVIC_2022, alpha=True, hybrid=True)
cp.alternativeExperience = 32
return cp
class Peers(dict):
def all_checks(self, _services):
return True
@pytest.mark.parametrize('speed', [-0.0142488591, -0.0008664753, 0.0, 0.002777777845])
@pytest.mark.parametrize('threshold', [0.0, 1.0])
@pytest.mark.parametrize('grant', [False, True])
@pytest.mark.parametrize('history_only', [False, True])
def test_actual_caller_zero_brake_pause_matches_current_and_queued_samples(speed, threshold, grant, history_only):
sd = SelfdriveD.__new__(SelfdriveD)
sd.CP = civic()
sd.sm = cast(messaging.SubMaster, Peers(driverMonitoringState=log.DriverMonitoringState.new_message(),
extrinsicsCalibration=log.ExtrinsicsCalibration.new_message(calStatus='calibrated')))
sd.enabled = sd.active = sd.aol_dm_lateral_inhibit = sd.nostalgia_paddle_cancel = False
sd.initialized = True
sd.events = Events()
sd.aol_settings = AolSettings(True, threshold, 9, 0, (0, 0, 0), (0, 0, 0))
state = structs.CarState(canValid=True, brakePressed=True, vEgo=speed, standstill=False, gearShifter='drive')
current = structs.CarState(**state.to_dict())
if history_only:
current.standstill = True # The latest sample must not erase an earlier valid veto.
sd.car_state_samples = ((state, True), (current, True))
intent = NS(allowedLatch=True, pauseLateral=False, pauseLongitudinal=True, optionalSetRelease=False)
native = NS(requestedLateral=True, lateralAllowed=grant, requestedLongitudinal=False, longitudinalAllowed=False)
result = sd._decide_aol_axes(current, intent, native, now_ns=NOW)
assert result.desired_lateral == (threshold == 0.0)
assert result.lateral_active == (threshold == 0.0 and grant)
assert not result.desired_longitudinal and not result.longitudinal_active
state.steerFaultTemporary = True
result = sd._decide_aol_axes(current, intent, native, now_ns=NOW)
assert not result.desired_lateral and not result.lateral_active
def baseline():
cp = civic()
state = structs.CarState(canValid=True, gearShifter='drive', vEgo=15.)
state.cruiseState.available = True
feedback = TransportPauseFeedback(cp)
sm, native = sample(cp, NOW, NOW, 1)
change(sm, 'onroadEvents', lambda events: setattr(events[0], 'userDisable', False))
assert not feedback.observe(sm, cp, state, NOW, latched=True, intent_producer_session=PRODUCER)
assert feedback.lateral_only_baseline
return cp, state, feedback, sm, native
def eps_sample(cp, stamp, *, withdrawn, brake=False, gas=False):
sm, native = sample(cp, stamp, stamp, 2, active=not withdrawn, allowed=False,
event_stamp=stamp if withdrawn else NOW)
event = sm.messages['onroadEvents'].as_builder()
event.onroadEvents = [{'name': 'steerOverride', 'overrideLateral': True}]
if brake:
event.onroadEvents = [*(e.to_dict() for e in event.onroadEvents), {'name': 'pedalPressed', 'userDisable': True}]
if gas:
event.onroadEvents = [*(e.to_dict() for e in event.onroadEvents), {'name': 'gasPressedOverride', 'overrideLongitudinal': True}]
sm.messages['onroadEvents'] = event.as_reader()
return sm, native
@pytest.mark.parametrize('pedal', ['none', 'brake', 'gas'])
@pytest.mark.parametrize('withdrawn', [False, True])
def test_honda_eps_native_denial_and_pedal_override_keep_only_existing_intent(pedal, withdrawn):
cp, state, feedback, _, _ = baseline()
lease = feedback.lateral_baseline_ns, feedback.lateral_source_ns, feedback.lateral_native_ns
state.steerFaultTemporary = state.steeringPressed = True
state.brakePressed, state.gasPressed = pedal == 'brake', pedal == 'gas'
now = NOW + 10_000_000
sm, native = eps_sample(cp, now, withdrawn=withdrawn, brake=state.brakePressed, gas=state.gasPressed)
assert not feedback.observe(sm, cp, state, now, latched=True, intent_producer_session=PRODUCER)
assert not feedback.failed
assert lease == (feedback.lateral_baseline_ns, feedback.lateral_source_ns, feedback.lateral_native_ns)
assert native.requestedLateral and not native.lateralAllowed and not native.longitudinalAllowed
state.steerFaultTemporary = state.steeringPressed = state.brakePressed = state.gasPressed = False
recovered, native = sample(cp, now + 10_000_000, now + 10_000_000, 3, event_stamp=now + 10_000_000)
change(recovered, 'onroadEvents', lambda events: setattr(events[0], 'userDisable', False))
assert not feedback.observe(recovered, cp, state, now + 10_000_000, latched=True, intent_producer_session=PRODUCER)
assert not feedback.failed and native.lateralAllowed
@pytest.mark.parametrize('fault', ['unarmed', 'no_baseline', 'no_eps', 'permanent_eps', 'can_invalid', 'timeout',
'door', 'sensors', 'disengage', 'main_off', 'acc_fault', 'cancel', 'reverse', 'wrong_profile', 'wrong_session',
'wrong_producer', 'wrong_epoch', 'expired_axis', 'native_missing', 'native_invalid', 'native_stale',
'native_fault', 'native_heartbeat', 'long_request', 'long_desired', 'long_active', 'hard_event', 'other_user'])
def test_honda_eps_intent_retention_preserves_independent_vetoes(fault):
cp, state, feedback, _, _ = baseline()
state.steerFaultTemporary = state.steeringPressed = state.brakePressed = True
now = NOW + 10_000_000
sm, native = eps_sample(cp, now, withdrawn=True, brake=True)
fields = {'no_eps': ('steerFaultTemporary', False), 'permanent_eps': ('steerFaultPermanent', True),
'can_invalid': ('canValid', False), 'timeout': ('canTimeout', True), 'door': ('doorOpen', True),
'sensors': ('vehicleSensorsInvalid', True), 'disengage': ('steeringDisengage', True), 'acc_fault': ('accFaulted', True)}
if fault in fields:
setattr(state, *fields[fault])
elif fault == 'no_baseline':
feedback.lateral_baseline_ns = 0
elif fault == 'main_off':
state.cruiseState.available = False
elif fault == 'cancel':
state.buttonEvents = [structs.CarState.ButtonEvent(type='cancel', pressed=True)]
elif fault == 'reverse':
state.gearShifter = 'reverse'
elif fault == 'wrong_profile':
cp.alternativeExperience = 0
elif fault == 'wrong_session':
native_change(sm, native, axisSessionId='different')
elif fault == 'wrong_epoch':
change(sm, 'aolAxisState', lambda axis: setattr(axis, 'intentIntegrityEpoch', 2))
elif fault == 'expired_axis':
change(sm, 'aolAxisState', lambda axis: setattr(axis, 'validUntilMonoTime', now - 1))
elif fault in ('long_desired', 'long_active'):
change(sm, 'aolAxisState', lambda axis: setattr(axis, 'desiredLongitudinal' if fault == 'long_desired' else 'longitudinalActive', True))
elif fault == 'long_request':
native_change(sm, native, requestedLongitudinal=True)
elif fault == 'native_missing':
sm.seen['aolSafetyWire'] = False
elif fault == 'native_invalid':
sm.valid['aolSafetyWire'] = False
elif fault == 'native_stale':
native_change(sm, native, validUntilMonoTime=now - 1)
elif fault in ('native_fault', 'native_heartbeat'):
native_change(sm, native, pandaInventory=(replace(native.pandaInventory[0],
**({'faults': 1} if fault == 'native_fault' else {'heartbeatLost': True})),))
elif fault == 'hard_event':
change(sm, 'onroadEvents', lambda events: setattr(events[0], 'immediateDisable', True))
elif fault == 'other_user':
change(sm, 'onroadEvents', lambda events: setattr(events[1], 'name', 'buttonCancel'))
rejected = feedback.observe(sm, cp, state, now, latched=fault != 'unarmed',
intent_producer_session='different' if fault == 'wrong_producer' else PRODUCER)
if fault != 'unarmed':
assert rejected
assert not feedback.temporary_host_qualified
def test_pre_request_native_off_does_not_revoke_new_host_request():
cp, _, feedback, sm, native = baseline()
host = Car.__new__(Car)
host.CP, host.sm, host.slc_producer_session = cp, sm, PRODUCER
host.aol_transport_feedback = feedback
kwargs = {'latched': True, 'panda_ready': True, 'restricted': False, 'now_ns': NOW, 'pending_since_ns': 0}
assert host.aol_honda_native_observation(native, **kwargs) == (0, False, False)
# The recorded receipt is newer than carState but older than the new ACTIVE axis.
renewed = NOW + 10_000_000
host.sm, _ = sample(cp, renewed, renewed, 2, event_stamp=renewed)
change(host.sm, 'aolAxisState', lambda axis: setattr(axis, 'sourceCarStateMonoTime', renewed - 5_000_000))
off = replace(native, requestedLateral=False, lateralAllowed=False, observedMonoTime=renewed - 336_000)
kwargs['now_ns'] = renewed + 1_000_000
pending, lost, reset = host.aol_honda_native_observation(off, **kwargs)
assert pending == kwargs['now_ns'] and not lost and not reset
# A fresh OFF after the request still revokes the established grant immediately.
off = replace(off, observedMonoTime=renewed + 1)
assert host.aol_honda_native_observation(off, **kwargs) == (0, False, True)
def test_interleaved_old_off_receipts_cannot_extend_native_denial_deadline():
cp, _, feedback, sm, native = baseline()
host = Car.__new__(Car)
host.CP, host.sm, host.slc_producer_session = cp, sm, PRODUCER
host.aol_transport_feedback = feedback
assert host.aol_honda_native_observation(native, latched=True, panda_ready=True, restricted=False,
now_ns=NOW, pending_since_ns=0) == (0, False, False)
pending = 0
for tick in range(22):
now = NOW + (tick + 1) * 10_000_000
host.sm, _ = sample(cp, now, now, tick + 2, event_stamp=now)
change(host.sm, 'aolAxisState', lambda axis, now=now: setattr(axis, 'sourceCarStateMonoTime', now - 5_000_000))
off = replace(native, requestedLateral=False, lateralAllowed=False, observedMonoTime=now - 1,
validUntilMonoTime=now + 200_000_000)
pending, lost, reset = host.aol_honda_native_observation(off, latched=True, panda_ready=True,
restricted=False, now_ns=now, pending_since_ns=pending)
assert not lost and reset == (tick >= 20)
@pytest.mark.parametrize('warning', ['steerTempUnavailable', 'steerTempUnavailableSilent'])
@pytest.mark.parametrize('allowed', [False, True])
def test_honda_eps_warning_cannot_mask_unrelated_user_disable(warning, allowed):
cp, state, feedback, _, _ = baseline()
state.steerFaultTemporary = state.brakePressed = True
now = NOW + 10_000_000
sm, _ = sample(cp, now, now, 2, active=False, allowed=allowed, event_stamp=now)
event = sm.messages['onroadEvents'].as_builder()
event.onroadEvents = [{'name': warning, 'softDisable': warning == 'steerTempUnavailable'},
{'name': 'pedalPressed', 'userDisable': True}, {'name': 'buttonCancel', 'userDisable': True}]
sm.messages['onroadEvents'] = event.as_reader()
assert feedback.observe(sm, cp, state, now, latched=True, intent_producer_session=PRODUCER)
@@ -1,73 +0,0 @@
"""Cruise MAIN assigned to Adopt speed limit is not also the Hyundai physical lateral toggle."""
import pytest
from opendbc.car import structs
from openpilot.starpilot.aol.intent import AolCardIntent, AolSettings, read_settings
from openpilot.starpilot.aol.tests.test_ioniq6_stock_repair import stock
from openpilot.starpilot.car.hyundai.aol import create_intent, qualified_ioniq6
def long_cp():
cp = stock(False)
cp.pcmCruise = False
cp.openpilotLongitudinalControl = True
cp.safetyConfigs[0].safetyParam = 0x8815
assert qualified_ioniq6(cp)
return cp
def press(owner, cs, button, now_ns, *, engaged=False):
cs.buttonEvents = [structs.CarState.ButtonEvent(type=button, pressed=True)]
owner.update(cs, now_ns=now_ns, fault_active=False, standard_enabled=engaged)
cs.buttonEvents = [structs.CarState.ButtonEvent(type=button, pressed=False)]
owner.update(cs, now_ns=now_ns + 1, fault_active=False, standard_enabled=engaged)
def lateral(owner):
return owner.allowed_latch and not owner.pause_lateral
def armed(owner):
cs = structs.CarState(canValid=True, gearShifter='drive')
cs.cruiseState.available = True
owner.update(cs, now_ns=1, fault_active=False)
press(owner, cs, 'lkas', 2)
assert lateral(owner)
return cs
@pytest.mark.parametrize('main_wheel_action', [False, True])
def test_ioniq6_main_assigned_to_adopt_keeps_lkas_armed_lateral(main_wheel_action):
owner = create_intent(long_cp(), AolSettings(True, 0., 0, 0, (0, 0, 0), (0, 0, 0), main_wheel_action=main_wheel_action))
assert owner.independent_cruise_buttons
cs = armed(owner)
press(owner, cs, 'mainCruise', 10, engaged=True)
assert lateral(owner) == main_wheel_action
press(owner, cs, 'lkas', 20, engaged=True)
assert lateral(owner) != main_wheel_action
def test_main_toggle_is_unchanged_without_independent_cruise_buttons():
owner = AolCardIntent(AolSettings(True, 0., 0, 0, (0, 0, 0), (0, 0, 0), main_wheel_action=True), explicit_latch=True)
cs = armed(owner)
press(owner, cs, 'mainCruise', 10, engaged=True)
assert not lateral(owner)
class SavedParams:
def __init__(self, values):
self.values = values
def get(self, key, return_default=False):
return self.values.get(key)
def get_bool(self, key):
return self.values.get(key) == b'1'
def get_param_path(self, key):
return f'/nonexistent/{key}'
@pytest.mark.parametrize('lkas,main,main_wheel', [(b'0', b'10', True), (b'0', b'9', False), (b'10', b'0', False), (b'0', b'8', False)])
def test_only_main_adopt_is_marked_as_a_main_wheel_action(lkas, main, main_wheel):
settings = read_settings(SavedParams({'AlwaysOnLateral': b'1', 'LKASButtonControl': lkas, 'MainCruiseButtonControl': main}))
assert settings.main_wheel_action == main_wheel
@@ -120,14 +120,14 @@ def test_legacy_and_strict_version_rollout():
encode_safety(replace(state, sourcePandaStatesMonoTime=0))
def test_ioniq6_two_slot_identity_keeps_exact_owner_and_live_inventory_checks():
def test_two_slot_identity_does_not_bypass_existing_port_admission():
cp = configured_cp()
original = cp.safetyConfigs[0].to_dict()
cp.safetyConfigs = [{'safetyModel': 'noOutput', 'safetyParam': 0}, original]
state = bound_state(cp)
assert native_inventory_matches_cp(state, cp)
assert native_matches_cp(cp, state.safetyModel, state.safetyParam)
assert current_native(sm_for(cp, state), cp, now_ns=NOW) is not None
assert not native_matches_cp(cp, state.safetyModel, state.safetyParam)
assert current_native(sm_for(cp, state), cp, now_ns=NOW) is None
for slots in ((state.pandaInventory[1], state.pandaInventory[0]),
(replace(state.pandaInventory[0], hardwareSerial=state.pandaSerial), state.pandaInventory[1]),
(replace(state.pandaInventory[0], controlsAllowed=True), state.pandaInventory[1]),
@@ -1,170 +0,0 @@
"""PE independent LAT intent survives only qualified native withdrawals."""
from contextlib import ExitStack
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.starpilot.aol.tests import test_ioniq_availability as fixture
def state(*, gas=False, eps=False):
cs = fixture.car_state()
cs.vEgoRaw = cs.vEgo
cs.gasPressed = gas
cs.steerFaultTemporary = eps
cs.cruiseState.available = True
return cs
def setup_case(cleanup):
case = fixture.IoniqAvailabilityTests(methodName='runTest')
cleanup.callback(case.doCleanups)
cleanup.enter_context(OpenpilotPrefix())
from opendbc.car.hyundai.tests.test_ioniq5pe_pending_startup import TestPEPendingStartup
startup = TestPEPendingStartup(methodName='runTest')
cleanup.callback(startup.doCleanups)
startup.setUp()
owner, ci, _ = startup.instance(seal=False)
cleanup.callback(owner.close)
sd, controls, drive, card, receive = case._ioniq_availability_callers(full_op=False, cp=ci.CP, startup_owner=owner)
assert sd.CP.safetyConfigs[0].safetyParam == 0x5495
for tick in range(2, 152):
axis, _, command = drive(tick, native_request_long=False, gas=tick >= 150, observed_state=state(gas=tick >= 150))
assert axis.lateralActive and command.latActive and not command.longActive
assert receive(10_000_000_000 + tick * 10_000_000, axis=sd.pm.messages['aolAxisState'],
event=sd.pm.messages['onroadEvents'], state=state(gas=tick >= 150))
return sd, controls, drive, card, receive
def test_pe_expired_output_then_native_zero_then_pedal_denial_keeps_only_intent():
with ExitStack() as cleanup:
sd, _, drive, card, receive = setup_case(cleanup)
old = sd.pm.messages['aolAxisState']
before = card.aol_transport_feedback.lateral_source_ns
axis, _, command = drive(159, gas=True, observed_state=state(gas=True), allowed=False,
native_request_lat=False, native_request_long=False)
assert not axis.lateralActive and not command.latActive and not command.longActive
assert receive(11_590_000_000, axis=old, event=sd.pm.messages['onroadEvents'], state=state(gas=True))
assert receive(11_591_000_000, axis=sd.pm.messages['aolAxisState'], state=state(gas=True))
for tick in (160, 161):
axis, _, command = drive(tick, gas=True, observed_state=state(gas=True), allowed=False,
native_request_lat=True, native_request_long=False)
assert axis.desiredLateral and not axis.lateralActive
assert not command.latActive and not command.longActive
assert receive(10_000_000_000 + tick * 10_000_000, axis=sd.pm.messages['aolAxisState'],
event=sd.pm.messages['onroadEvents'], state=state(gas=True))
assert card.aol_transport_feedback.lateral_source_ns == before
axis, _, command = drive(162, observed_state=state(), native_request_lat=True, native_request_long=False)
assert axis.lateralActive and command.latActive and not command.longActive
assert receive(11_620_000_000, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'], state=state())
def test_pe_native_denial_precedes_first_card_eps_sample_and_host_event():
with ExitStack() as cleanup:
sd, _, drive, card, receive = setup_case(cleanup)
axis, _, command = drive(152, gas=True, observed_state=state(gas=True), allowed=False,
native_request_lat=True, native_request_long=False)
assert axis.desiredLateral and not axis.lateralActive and not command.latActive
eps = state(gas=True, eps=True)
eps.steeringPressed = True
eps.steeringTorque = 549.
eps.steeringAngleDeg = 166.2
assert receive(11_520_000_000, axis=sd.pm.messages['aolAxisState'],
event=sd.pm.messages['onroadEvents'], state=eps)
for tick in (153, 154):
axis, _, command = drive(tick, gas=True, observed_state=eps, allowed=False,
native_request_lat=True, native_request_long=False)
assert not axis.lateralActive and not command.latActive and not command.longActive
assert receive(10_000_000_000 + tick * 10_000_000, axis=sd.pm.messages['aolAxisState'],
event=sd.pm.messages['onroadEvents'], state=eps)
axis, _, command = drive(155, observed_state=state(), native_request_lat=True, native_request_long=False)
assert axis.lateralActive and command.latActive and not command.longActive
assert receive(11_550_000_000, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'], state=state())
def test_pe_duplicate_native_first_eps_cannot_extend_original_output_deadline():
with ExitStack() as cleanup:
sd, _, drive, card, receive = setup_case(cleanup)
drive(152, gas=True, observed_state=state(gas=True), allowed=False,
native_request_lat=True, native_request_long=False)
eps = state(gas=True, eps=True)
proof, event = sd.pm.messages['aolAxisState'], sd.pm.messages['onroadEvents']
assert receive(11_520_000_000, axis=proof, event=event, state=eps)
deadline = card.aol_transport_feedback.hkg_eps_join_until_ns
assert deadline == 11_540_000_000
assert receive(deadline, axis=proof, event=event, state=eps)
assert card.aol_transport_feedback.hkg_eps_join_until_ns == deadline
assert not receive(deadline + 1, axis=proof, event=event, state=eps)
def test_pe_experimental_preference_survives_exact_long_stock_long_restart():
import json
from pathlib import Path
from unittest.mock import patch
from openpilot.common.params import Params
from openpilot.selfdrive.selfdrived import selfdrived
from openpilot.starpilot.conditional_mode.actions import stock_document
from opendbc.car.hyundai.ioniq5pe_long_profile import candidate
from opendbc.car.hyundai.tests.test_ioniq5pe_stock import params
with OpenpilotPrefix():
saved = Params()
saved.put('ConditionalModeConfig', json.loads(stock_document()), block=True)
saved.put_bool('ExperimentalMode', True, block=True)
saved.put_bool('AlphaLongitudinalEnabled', True, block=True)
stock = params()
long = candidate(stock, enabled=True, is_release=False)
for cp, expected in ((long, True), (stock, False), (long, True)):
before = cp.to_dict()
with patch.object(selfdrived, 'Params', return_value=saved), \
patch.object(selfdrived.messaging, 'PubMaster'), \
patch.object(selfdrived.messaging, 'SubMaster'), \
patch.object(selfdrived.messaging, 'sub_sock'), \
patch.object(selfdrived, 'feature_enabled', return_value=False):
drive = selfdrived.SelfdriveD(CP=cp.as_reader())
drive.refresh_saved_driving_mode(1_000_000_000)
drive.update_conditional_mode(None)
assert drive.experimental_mode == expected
assert Path(saved.get_param_path('ExperimentalMode')).read_bytes() == b'1'
assert saved.get_bool('AlphaLongitudinalEnabled')
assert cp.to_dict() == before
def test_pe_native_first_eps_preserves_fault_identity_and_source_vetoes():
from dataclasses import replace
from openpilot.cereal import log
for fault in ('native_invalid', 'rx_invalid', 'native_fault', 'permanent_eps', 'can_invalid', 'can_timeout',
'producer', 'epoch', 'session', 'fault_session', 'source_reordered', 'hard_event', 'settings_disabled'):
with ExitStack() as cleanup:
sd, _, drive, card, receive = setup_case(cleanup)
from openpilot.cereal import messaging
pandas = None
if fault == 'native_fault':
pandas = messaging.new_message('pandaStates', len(sd.sm['pandaStates']), valid=True, logMonoTime=11_520_000_000)
pandas.pandaStates = sd.sm['pandaStates']
pandas.pandaStates[0].faults = ['interruptRateCan1']
drive(152, gas=True, observed_state=state(gas=True), allowed=False, native_request_lat=True,
panda_state_message=pandas,
native_request_long=False, native_valid=fault != 'native_invalid',
panda_fault=fault == 'rx_invalid',
critical_event=log.OnroadEvent.EventName.canError if fault == 'hard_event' else None)
proof = sd.pm.messages['aolAxisState'].as_builder()
eps = state(gas=True, eps=True)
if fault == 'permanent_eps':
eps.steerFaultPermanent = True
elif fault == 'can_invalid':
eps.canValid = False
elif fault == 'can_timeout':
eps.canTimeout = True
elif fault == 'producer':
proof.aolAxisState.intentProducerSessionId = 'other-card'
elif fault == 'epoch':
proof.aolAxisState.intentIntegrityEpoch += 1
elif fault == 'session':
proof.aolAxisState.sessionId = 'other-session'
elif fault == 'fault_session':
proof.aolAxisState.faultSessionId = 'other-session'
elif fault == 'source_reordered':
proof.aolAxisState.sourceCarStateMonoTime = card.aol_transport_feedback.lateral_source_ns - 1
elif fault == 'settings_disabled':
card.aol_card_intent.settings = replace(card.aol_card_intent.settings, enabled=False)
assert not receive(11_520_000_000, axis=proof.as_reader(),
event=sd.pm.messages['onroadEvents'], state=eps), fault
assert not card.aol_card_intent.allowed_latch, fault
@@ -1,103 +0,0 @@
from contextlib import ExitStack
import pytest
from opendbc.car import structs
from opendbc.car.hyundai.values import CAR
from openpilot.common.params import Params
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.starpilot.aol.tests import test_ioniq_availability as fixture
from openpilot.starpilot.aol.tests.test_classic_scc_profiles import params
@pytest.mark.parametrize('identity', (CAR.GENESIS_G70, CAR.HYUNDAI_SONATA, CAR.HYUNDAI_SONATA_HYBRID))
@pytest.mark.parametrize('button', (structs.CarState.ButtonEvent.Type.mainCruise, structs.CarState.ButtonEvent.Type.lkas))
def test_fresh_stopped_eps_press_retains_intent_until_real_permission(identity, button):
case = fixture.IoniqAvailabilityTests(methodName='runTest')
with ExitStack() as cleanup:
cleanup.callback(case.doCleanups)
cleanup.enter_context(OpenpilotPrefix())
Params().put('MainCruiseButtonControl', 9, block=True)
Params().put('LKASButtonControl', 9, block=True)
sd, _, drive, card, receive = case._ioniq_availability_callers(
full_op=False, initially_armed=False, cp=params(identity, 0x391), cold_start=True)
def cycle(tick, *, fault=True, event=None, requested=False, allowed=False, moving=False, cruise=False):
state = fixture.car_state(brake=fault)
state.vEgo = state.vEgoRaw = 8. if moving else 0.
state.standstill = not moving
state.steerFaultTemporary = fault
state.cruiseState.enabled = cruise
if event is not None:
state.buttonEvents = [structs.CarState.ButtonEvent(type=button, pressed=event)]
axis, _, command = drive(tick, observed_state=state, native_request_lat=requested,
native_request_long=False, panda_controls_allowed=cruise, allowed=allowed)
receive(10_000_000_000 + tick * 10_000_000,
axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'], state=state)
return axis, command
cycle(0, fault=False)
assert not card.aol_card_intent.allowed_latch
cycle(1, event=True)
assert card.aol_card_intent.allowed_latch
for tick in range(2, 205):
axis, command = cycle(tick, event=False if tick == 2 else None)
assert card.aol_card_intent.allowed_latch, 'fresh physical EPS-wait intent was erased'
assert not axis.lateralActive and not command.latActive and not command.longActive
for tick in (205, 206):
axis, command = cycle(tick, fault=False)
assert card.aol_card_intent.allowed_latch
assert not axis.lateralActive and not command.latActive
axis, command = cycle(207, fault=False, requested=True, allowed=True, moving=True)
assert axis.lateralActive and command.latActive
assert card.aol_card_intent.allowed_latch
axis, command = cycle(208, fault=False, requested=True, allowed=True, moving=True, cruise=True)
assert sd.enabled and axis.lateralActive and command.latActive
assert card.aol_card_intent.allowed_latch
cycle(209, fault=False, event=True, requested=True, allowed=True, moving=True)
assert not card.aol_card_intent.allowed_latch
@pytest.mark.parametrize('failure', ('can_invalid', 'can_timeout', 'permanent_eps', 'acc_fault',
'rx_checks', 'relay_fault', 'native_invalid', 'critical_event'))
def test_pending_stopped_eps_arm_still_clears_for_real_faults(failure):
from openpilot.cereal import log
case = fixture.IoniqAvailabilityTests(methodName='runTest')
with ExitStack() as cleanup:
cleanup.callback(case.doCleanups)
cleanup.enter_context(OpenpilotPrefix())
Params().put('MainCruiseButtonControl', 9, block=True)
sd, _, drive, card, receive = case._ioniq_availability_callers(
full_op=False, initially_armed=False, cp=params(CAR.GENESIS_G70, 0x391), cold_start=True)
for tick in range(4):
state = fixture.car_state(brake=tick > 0)
state.vEgo = state.vEgoRaw = 0.
state.standstill = True
state.steerFaultTemporary = tick > 0
if tick in (1, 2):
state.buttonEvents = [structs.CarState.ButtonEvent(
type=structs.CarState.ButtonEvent.Type.mainCruise, pressed=tick == 1)]
options = {}
if tick == 3:
if failure == 'can_invalid':
state.canValid = False
elif failure == 'can_timeout':
state.canTimeout = True
elif failure == 'permanent_eps':
state.steerFaultPermanent = True
elif failure == 'acc_fault':
state.accFaulted = True
elif failure == 'rx_checks':
options['panda_fault'] = True
elif failure == 'relay_fault':
options['relay_fault'] = True
elif failure == 'native_invalid':
options['native_valid'] = False
else:
options['critical_event'] = log.OnroadEvent.EventName.controlsMismatch
axis, _, command = drive(tick, observed_state=state, native_request_lat=False,
native_request_long=False, allowed=False, **options)
receive(10_000_000_000 + tick * 10_000_000,
axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'], state=state)
assert not axis.lateralActive and not command.latActive and not command.longActive
if tick in (1, 2):
assert card.aol_card_intent.allowed_latch
assert not card.aol_card_intent.allowed_latch
+9 -28
View File
@@ -289,7 +289,6 @@ class TransportPauseFeedback:
self.cold_park_waiting = False
self.lateral_only_baseline = False
self.temporary_eps_gas_override = False
self.hkg_eps_join_until_ns = 0
self.lateral_source_ns = 0
self.lateral_inventory = None
self.lateral_native_ns = 0
@@ -815,9 +814,16 @@ class TransportPauseFeedback:
from opendbc.car.hyundai.canfd_stock_aol import qualified_long as qualified
from openpilot.starpilot.car.hyundai.aol import qualified_ccnc_long_aol as ev9_qualified
from opendbc.car.hyundai.blended_stock_aol import qualified_alpha as mixed_qualified
from opendbc.car.hyundai.classic_long_aol import independent_buttons_qualified as classic_qualified
from opendbc.car.structs import car
if (not latched or self.failed or self.paused or self.pending_event or
not self.session or str(axis.sessionId) != self.session or
not (qualified(cp, marked_only=True) or ev9_qualified(cp, marked_only=True) or mixed_qualified(cp, marked_only=True)) or
not self.lateral_baseline_ns or not 0 <= now_ns - self.lateral_baseline_ns <= AXIS_MAX_AGE_NS or
str(axis.faultReason) != 'none' or not axis.nativeAcknowledged or
not axis.desiredLateral or not axis.lateralActive or cs.accFaulted or
not cs.canValid or cs.canTimeout or cs.steerFaultTemporary or cs.steerFaultPermanent):
return None
cancel = any(e.type == car.CarState.ButtonEvent.Type.cancel and e.pressed for e in cs.buttonEvents)
fresh_cancel = bool(cancel and self.last_cancel_source_ns < cancel_source_ns <= now_ns and
now_ns - cancel_source_ns <= AXIS_MAX_AGE_NS)
@@ -827,18 +833,6 @@ class TransportPauseFeedback:
return None
if self.longitudinal_cancel_ns and not 0 <= now_ns - self.longitudinal_cancel_ns <= SAFETY_MAX_AGE_NS:
return None
classic = classic_qualified(cp, marked_only=True)
if (not latched or self.failed or self.paused or self.pending_event or
not self.session or str(axis.sessionId) != self.session or
classic and (not self.availability_producer or axis.intentProducerSessionId != self.availability_producer or
axis.intentIntegrityEpoch != self.availability_epoch) or
not (qualified(cp, marked_only=True) or ev9_qualified(cp, marked_only=True) or mixed_qualified(cp, marked_only=True) or classic) or
not self.lateral_baseline_ns or not 0 <= now_ns - self.lateral_baseline_ns <= AXIS_MAX_AGE_NS or
str(axis.faultReason) != 'none' or not axis.nativeAcknowledged or
not axis.desiredLateral or not axis.lateralActive or cs.accFaulted or
not cs.canValid or cs.canTimeout or cs.steerFaultTemporary or cs.steerFaultPermanent):
return None
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 or
@@ -1224,12 +1218,7 @@ class TransportPauseFeedback:
all(slot.controlsAllowed for slot, config in zip(newer.pandaInventory, cp.safetyConfigs, strict=True)
if str(config.safetyModel) not in ('silent', 'noOutput'))):
native = newer
expired_request_off = bool(qualified(cp, marked_only=True) and self.source_retention_started_ns and not axis.lateralActive and
not axis.desiredLongitudinal and not axis.longitudinalActive and
0 < self.lateral_source_ns <= now_ns <= self.lateral_source_ns + SAFETY_MAX_AGE_NS and
0 < self.source_retention_started_ns <= now_ns <= self.source_retention_started_ns + SAFETY_MAX_AGE_NS)
return bool(native is not None and native.pandaInventory and
(native.requestedLateral or expired_request_off and not native.longitudinalAllowed) and
return bool(native is not None and native.pandaInventory and native.requestedLateral and
(not native.lateralAllowed or subaru_stop and not axis.desiredLateral and not axis.lateralActive) and
(pending_stop or not native.requestedLongitudinal and not combined_stop or
combined_stop and native.requestedLongitudinal and native.longitudinalAllowed) and
@@ -1333,7 +1322,6 @@ class TransportPauseFeedback:
self.ordinary_warning_retained = False
self.ordinary_warning_pending_baseline = None
self.temporary_eps_event_ns = 0
self.hkg_eps_join_until_ns = 0
self.ccnc_main_off_source_ns = 0
self.cold_park_waiting = False
self.cold_main_sparse_waiting = False
@@ -1400,13 +1388,6 @@ class TransportPauseFeedback:
self.session = str(axis.sessionId)
self.sequence, self.signature = int(axis.sequence), signature
return False
if cp.brand == 'hyundai' and current and continuity:
from openpilot.starpilot.car.hyundai.aol import retain_stopped_eps_arm
if retain_stopped_eps_arm(self, sm, cp, cs, axis, now_ns, latched=latched,
arm_ns=main_arm_ns, producer=intent_producer_session):
self.session = str(axis.sessionId)
self.sequence, self.signature = int(axis.sequence), signature
return False
if current and continuity and 0 < main_arm_ns <= now_ns <= main_arm_ns + AXIS_MAX_AGE_NS:
from openpilot.starpilot.car.gm.aol import prior_main_off_intent_current
if prior_main_off_intent_current(self, sm, cp, cs, axis, now_ns,
+6 -68
View File
@@ -1,5 +1,5 @@
from openpilot.cereal import log
from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS, current_axis, current_intent
from openpilot.starpilot.aol.runtime import current_axis, current_intent
from openpilot.starpilot.aol.wire import INTENT_SERVICE
AudibleAlert = log.SelfdriveState.AudibleAlert
@@ -25,70 +25,9 @@ class _Transition:
return changed
class _LateralTransition:
"""Announce a requested episode only after its first effective activation."""
def __init__(self):
self.session = None
self.sequence = 0
self.observed_at = 0
self.requested = False
self.active = False
self.request_ns = 0
self.announce = False
self.axis_session = None
self.axis_sequence = 0
self.axis_source_ns = 0
def update(self, intent, requested: bool, axis, now_ns: int) -> int:
if intent is None:
self.session = None
return 0
baseline = intent.producerSessionId != self.session or now_ns - self.observed_at > COALESCE_NS
if not baseline and intent.sequence <= self.sequence:
return 0
if baseline:
self.session = intent.producerSessionId
self.requested, self.active = requested, False
self.request_ns, self.announce = 0, False
elif requested and not self.requested:
self.request_ns, self.announce = intent.carStateLogMonoTime, True
activated = False
if axis is not None:
restarted = self.axis_session is not None and axis.sessionId != self.axis_session
if axis.sessionId != self.axis_session:
self.axis_session, self.axis_sequence, self.axis_source_ns = axis.sessionId, 0, 0
if restarted:
# A restarted producer supplies a baseline, not a new driver gesture.
self.announce = False
if axis.sequence > self.axis_sequence:
source_ns = int(axis.sourceCarStateMonoTime)
source_current = (axis.intentProducerSessionId == intent.producerSessionId and
0 < source_ns <= axis.observedMonoTime <= now_ns and
now_ns - source_ns <= AXIS_MAX_AGE_NS)
activated = bool(source_current and
source_ns >= max(self.request_ns, self.axis_source_ns) and
axis.nativeAcknowledged and axis.desiredLateral and axis.lateralActive)
self.axis_sequence = axis.sequence
if source_current:
self.axis_source_ns = max(self.axis_source_ns, source_ns)
changed = 0
if not requested:
changed = -int(self.active)
self.active, self.announce = False, False
elif activated and not self.active:
changed = int(self.announce)
self.active, self.announce = True, False
# Temporary suppression does not end the accepted driver-intent episode.
# Safety/refusal sounds remain owned by selfdriveState, not this chime latch.
self.requested, self.sequence, self.observed_at = requested, intent.sequence, now_ns
return changed
class AxisAlerts:
def __init__(self):
self.lateral = _LateralTransition()
self.lateral = _Transition()
self.longitudinal = _Transition()
self.last_sound = AudibleAlert.none
self.last_sound_at = 0
@@ -125,16 +64,15 @@ class AxisAlerts:
def update(self, sm, native_sound: int, now_ns: int) -> int:
intent = (current_intent(sm, car_state_ns=int(sm.logMonoTime[INTENT_SERVICE]), now_ns=now_ns)
if INTENT_SERVICE in sm.logMonoTime else None)
# AOL arming is not activation. Confirm its first active axis, then retain
# that episode through temporary suppression without replaying engagement.
# Engagement is persistent driver intent. Temporary native permission loss,
# pedal override and output suppression must not replay engagement sounds.
state_ns = int(sm.logMonoTime.get('selfdriveState', 0))
state_fresh = (state_ns > 0 and 0 <= now_ns - state_ns <= 100_000_000 and
sm.valid['selfdriveState'] and sm.alive['selfdriveState'])
lateral_known = intent is not None and (intent.pauseLateral or intent.lateralArmed or state_fresh)
axis = current_axis(sm, now_ns=now_ns) if 'aolAxisState' in sm.logMonoTime else None
lateral = self.lateral.update(intent if lateral_known else None,
lateral = self.lateral.update(intent.producerSessionId if lateral_known else None, intent.sequence if intent else 0,
bool(intent and not intent.pauseLateral and
(intent.lateralArmed or (state_fresh and sm['selfdriveState'].active))), axis, now_ns)
(intent.lateralArmed or (state_fresh and sm['selfdriveState'].active))), now_ns)
longitudinal = self.longitudinal.update(
intent.producerSessionId if intent is not None and state_fresh else None,
intent.sequence if intent is not None else 0,
Binary file not shown.
@@ -27,10 +27,8 @@ class AudioFixture:
def publish(self, armed=False, lateral=False, longitudinal=False, native=AudibleAlert.none,
*, age_ns=0, valid=True, session='card', sequence=None, acknowledged=True, payload=None,
intent_message=None, enabled=None, pause_longitudinal=False, pause_lateral=False,
active=False, state_age_ns=0, state_valid=True, alert_type='', axis_age_ns=0,
axis_valid=True, axis_qualified=True, desired_lateral=None, axis_sequence=None,
axis_session='selfdrived', axis_source_ns=None, axis_producer=None, step_ns=50_000_000):
self.now += step_ns
active=False, state_age_ns=0, state_valid=True, alert_type=''):
self.now += 50_000_000
self.sequence += 1
stamp = self.now - age_ns
if intent_message is None:
@@ -42,20 +40,17 @@ class AudioFixture:
else:
self.now = intent_message.logMonoTime + 1_000_000
stamp = self.now
message = messaging.new_message('aolAxisState', valid=axis_valid)
message.logMonoTime = stamp - axis_age_ns
message = messaging.new_message('aolAxisState', valid=True)
message.logMonoTime = stamp
axis = message.aolAxisState
axis.sessionId = axis_session
axis.sequence = self.sequence if axis_sequence is None else axis_sequence
axis.sourceCarStateMonoTime = stamp if axis_source_ns is None else axis_source_ns
axis.intentProducerSessionId = session if axis_producer is None else axis_producer
axis.qualified = axis_qualified
axis.sessionId = 'selfdrived'
axis.sequence = self.sequence
axis.qualified = True
axis.nativeAcknowledged = acknowledged
axis.lateralActive, axis.longitudinalActive = lateral, longitudinal
axis.desiredLateral = lateral if desired_lateral is None else desired_lateral
axis.desiredLongitudinal = longitudinal
axis.observedMonoTime = message.logMonoTime
axis.validUntilMonoTime = message.logMonoTime + 30_000_000
axis.desiredLateral, axis.desiredLongitudinal = lateral, longitudinal
axis.observedMonoTime = stamp
axis.validUntilMonoTime = stamp + 30_000_000
state = messaging.new_message('selfdriveState', valid=state_valid)
state.logMonoTime = self.now - state_age_ns
state.selfdriveState.alertSound = native
@@ -88,9 +83,9 @@ def test_park_lkas_producer_wire_and_sound(audio):
card.__dict__.update(vars(fields))
initialize_publication_sources(card)
cs = car.CarState.new_message(canValid=True, gearShifter=car.CarState.GearShifter.park, standstill=True)
for pressed, gear, expected in ((None, 'park', AudibleAlert.none), (True, 'park', AudibleAlert.none),
for pressed, gear, expected in ((None, 'park', AudibleAlert.none), (True, 'park', AudibleAlert.engage),
(True, 'park', AudibleAlert.none), (False, 'park', AudibleAlert.none),
(None, 'drive', AudibleAlert.engage), (None, 'park', AudibleAlert.none),
(None, 'drive', AudibleAlert.none), (None, 'park', AudibleAlert.none),
(True, 'park', AudibleAlert.disengage)):
cs.gearShifter = gear
cs.buttonEvents = [] if pressed is None else [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.lkas, pressed=pressed)]
@@ -100,115 +95,17 @@ def test_park_lkas_producer_wire_and_sound(audio):
assert decoded is not None
assert decoded.lateralArmed == owner.allowed_latch
assert decoded.allowedLatch == (owner.allowed_latch and gear == 'drive')
assert audio.publish(intent_message=published[INTENT_SERVICE], lateral=owner.allowed_latch and gear == 'drive') == expected
assert audio.publish(intent_message=published[INTENT_SERVICE]) == expected
def test_delayed_activation_chimes_once_and_temporary_suppression_is_silent(audio):
def test_lateral_activation_without_latch_change_is_silent(audio):
assert audio.publish() == AudibleAlert.none
assert audio.publish(armed=True) == AudibleAlert.none
assert audio.publish(armed=True, lateral=True) == AudibleAlert.engage
assert audio.publish(armed=True) == AudibleAlert.engage
for lateral in (True, False, True, False):
assert audio.publish(armed=True, lateral=lateral) == AudibleAlert.none
assert audio.publish() == AudibleAlert.disengage
def test_disabled_aol_main_button_is_silent(audio):
owner = AolCardIntent(AolSettings(False, 0, 0, AOL_TOGGLE, (0, 0, 0), (0, 0, 0)), explicit_latch=True)
state = car.CarState.new_message(canValid=True, gearShifter=car.CarState.GearShifter.drive)
audio.publish()
for pressed in (True, False) * 9:
state.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.mainCruise, pressed=pressed)]
owner.update(state)
assert not owner.allowed_latch
assert audio.publish(armed=owner.allowed_latch) == AudibleAlert.none
@pytest.mark.parametrize('blocked', [
{}, {'lateral': True, 'acknowledged': False}, {'lateral': True, 'desired_lateral': False},
{'lateral': True, 'axis_valid': False}, {'lateral': True, 'axis_qualified': False},
{'lateral': True, 'axis_age_ns': 31_000_000}, {'lateral': True, 'axis_age_ns': -1},
{'lateral': True, 'axis_producer': 'old-card'}, {'lateral': True, 'axis_producer': ''},
{'lateral': True, 'axis_source_ns': 0}, {'lateral': True, 'axis_source_ns': 99_000_000_000},
])
def test_never_activated_request_and_withdrawal_are_silent(audio, blocked):
audio.publish()
for _ in range(4):
assert audio.publish(armed=True, **blocked) == AudibleAlert.none
assert audio.publish() == AudibleAlert.none
def test_old_axis_cannot_activate_a_new_intent_episode(audio):
audio.publish()
old_source = audio.now
assert audio.publish(armed=True, lateral=True, axis_source_ns=old_source, step_ns=5_000_000) == AudibleAlert.none
assert audio.publish(armed=True, lateral=True, step_ns=5_000_000) == AudibleAlert.engage
@pytest.mark.parametrize('sequence', [1, 2])
def test_duplicate_or_reordered_axis_cannot_confirm_activation(audio, sequence):
audio.publish()
audio.publish(armed=True)
assert audio.publish(armed=True, lateral=True, axis_sequence=sequence) == AudibleAlert.none
assert audio.publish(armed=True, lateral=True) == AudibleAlert.engage
def test_axis_source_regression_cannot_confirm_activation(audio):
audio.publish()
audio.publish(armed=True, step_ns=5_000_000)
first_source = audio.now
audio.publish(armed=True, step_ns=5_000_000)
assert audio.publish(armed=True, lateral=True, axis_source_ns=first_source, step_ns=5_000_000) == AudibleAlert.none
assert audio.publish(armed=True, lateral=True, step_ns=5_000_000) == AudibleAlert.engage
@pytest.mark.parametrize('invalid', [{'axis_source_ns': 99_000_000_000}, {'axis_producer': 'old-card'}])
def test_rejected_axis_source_does_not_poison_valid_activation(audio, invalid):
audio.publish()
assert audio.publish(armed=True, lateral=True, **invalid) == AudibleAlert.none
assert audio.publish(armed=True, lateral=True) == AudibleAlert.engage
def test_fresh_publication_with_expired_car_source_cannot_activate(audio):
audio.publish()
audio.publish(armed=True)
assert audio.publish(armed=True, lateral=True, axis_source_ns=audio.now) == AudibleAlert.none
assert audio.publish(armed=True, lateral=True) == AudibleAlert.engage
def test_new_axis_waits_for_next_advancing_intent(audio):
audio.publish()
audio.publish(armed=True)
assert audio.publish(armed=True, lateral=True, sequence=2) == AudibleAlert.none
assert audio.publish(armed=True, lateral=True) == AudibleAlert.engage
def test_active_episode_survives_stale_axis_without_replaying(audio):
audio.publish()
assert audio.publish(armed=True, lateral=True) == AudibleAlert.engage
assert audio.publish(armed=True, lateral=True, axis_age_ns=31_000_000) == AudibleAlert.none
assert audio.publish(armed=True, lateral=True) == AudibleAlert.none
assert audio.publish() == AudibleAlert.disengage
def test_axis_restart_seeds_active_episode_without_replaying_chime(audio):
audio.publish()
audio.publish(armed=True)
assert audio.publish(armed=True, lateral=True, axis_session='restarted') == AudibleAlert.none
assert audio.publish(armed=True, lateral=True, axis_session='restarted') == AudibleAlert.none
assert audio.publish(axis_session='restarted') == AudibleAlert.disengage
for _ in range(5):
audio.publish(axis_session='restarted')
assert audio.publish(armed=True, lateral=True, axis_session='restarted') == AudibleAlert.engage
def test_explicit_pause_and_resume_require_new_effective_activation(audio):
audio.publish()
assert audio.publish(armed=True, lateral=True) == AudibleAlert.engage
assert audio.publish(armed=True, pause_lateral=True) == AudibleAlert.disengage
assert audio.publish(armed=True) == AudibleAlert.none
assert audio.publish(armed=True, lateral=True) == AudibleAlert.engage
def test_native_latch_revocation_and_one_lkas_rearm_sound(audio):
owner = AolCardIntent(AolSettings(True, 0, 0, 0, (0, 0, 0), (0, 0, 0)), explicit_latch=True)
state = car.CarState.new_message(canValid=True, gearShifter=car.CarState.GearShifter.drive)
@@ -216,7 +113,7 @@ def test_native_latch_revocation_and_one_lkas_rearm_sound(audio):
assert audio.publish(armed=owner.allowed_latch) == AudibleAlert.none
state.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.lkas, pressed=True)]
owner.update(state, now_ns=200)
assert audio.publish(armed=owner.allowed_latch, lateral=True) == AudibleAlert.engage
assert audio.publish(armed=owner.allowed_latch) == AudibleAlert.engage
state.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.lkas, pressed=False)]
owner.update(state, now_ns=300)
audio.publish(armed=owner.allowed_latch)
@@ -226,7 +123,7 @@ def test_native_latch_revocation_and_one_lkas_rearm_sound(audio):
state.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.lkas, pressed=True)]
owner.update(state, now_ns=500, native_rejection_ns=350)
assert owner.allowed_latch
assert audio.publish(armed=owner.allowed_latch, lateral=True) == AudibleAlert.engage
assert audio.publish(armed=owner.allowed_latch) == AudibleAlert.engage
def test_longitudinal_transitions_without_aol(audio):
@@ -240,7 +137,7 @@ def test_longitudinal_transitions_without_aol(audio):
AudibleAlert.prompt, AudibleAlert.engage, AudibleAlert.disengage])
def test_native_alert_wins_without_queued_chime(audio, native):
audio.publish()
assert audio.publish(armed=True, lateral=True, native=native) == native
assert audio.publish(armed=True, native=native) == native
assert audio.publish(armed=True) == AudibleAlert.none
@@ -248,14 +145,14 @@ def test_native_then_axis_ack_is_one_engagement(audio):
audio.publish()
assert audio.publish(native=AudibleAlert.engage) == AudibleAlert.engage
assert audio.publish(longitudinal=True) == AudibleAlert.none
assert audio.publish(armed=True, lateral=True, longitudinal=True) == AudibleAlert.none
assert audio.publish(armed=True, longitudinal=True) == AudibleAlert.none
@pytest.mark.parametrize('invalid', [{'age_ns': 31_000_000}, {'age_ns': -1}, {'valid': False}, {'payload': b'invalid'}])
def test_invalid_latch_is_silent_and_recovery_does_not_replay(audio, invalid):
audio.publish()
assert audio.publish(armed=True, lateral=True, **invalid) == AudibleAlert.none
assert audio.publish(armed=True, lateral=True) == AudibleAlert.none
assert audio.publish(armed=True, **invalid) == AudibleAlert.none
assert audio.publish(armed=True) == AudibleAlert.none
assert audio.publish() == AudibleAlert.disengage
@@ -263,7 +160,7 @@ def test_startup_new_session_and_old_sequence_do_not_chime(audio):
assert audio.publish(armed=True) == AudibleAlert.none
assert audio.publish(armed=False, session='new-card') == AudibleAlert.none
assert audio.publish(armed=True, session='new-card', sequence=1) == AudibleAlert.none
assert audio.publish(armed=True, lateral=True, session='new-card') == AudibleAlert.engage
assert audio.publish(armed=True, session='new-card') == AudibleAlert.engage
def test_runtime_native_pending_permission_and_loss(audio):
@@ -320,14 +217,14 @@ def test_explicit_longitudinal_pause_and_resume_sound_once(audio):
@pytest.mark.parametrize('invalid', [{'state_age_ns': 101_000_000}, {'state_age_ns': -1}, {'state_valid': False}])
def test_standard_lateral_stale_state_and_recovery_do_not_replay(audio, invalid):
audio.publish(active=True, lateral=True, enabled=True)
audio.publish(active=True, enabled=True)
assert audio.publish(active=True, enabled=True, **invalid) == AudibleAlert.none
assert audio.publish(active=True, lateral=True, enabled=True) == AudibleAlert.none
assert audio.publish(active=True, enabled=True) == AudibleAlert.none
assert audio.publish(active=True, enabled=True, pause_lateral=True) == AudibleAlert.disengage
def test_explicit_lateral_pause_remains_known_without_standard_state(audio):
audio.publish(active=True, lateral=True, enabled=True)
audio.publish(active=True, enabled=True)
assert audio.publish(active=True, enabled=True, pause_lateral=True, state_valid=False) == AudibleAlert.disengage
assert audio.publish(active=True, enabled=True, pause_lateral=True) == AudibleAlert.none
-6
View File
@@ -15,12 +15,6 @@ def policy_for(cp):
return AolVehiclePolicy()
def stock_conventional_cruise_supported(cp):
"""The marked stock-cruise AOL profile does not require adaptive speed control."""
return not cp.openpilotLongitudinalControl and qualified(cp, marked_only=True)
def native_profile_supported(model, param):
return model == int(structs.CarParams.SafetyModel.ford) and profile_word(param) in AOL_WORDS
-15
View File
@@ -18,7 +18,6 @@ from opendbc.bluepilot_lateral.core.lateral_angle_ext import LateralAngleExt
from opendbc.bluepilot_lateral.core.lateral_inputs import lateral_inputs_complete
from opendbc.bluepilot_lateral.can.fordcan_ext import lat_ctl2_checksum, create_lat_ctl2_msg
from opendbc.car.ford.values import CarControllerParams, FordFlags
from opendbc.car.ford.aol import qualified as qualified_aol
from opendbc.car import structs
from opendbc.car.vehicle_model import calc_slip_factor
from opendbc.bluepilot_lateral.core.carcontroller_ext import pinion_vehicle_model
@@ -44,17 +43,6 @@ HOST_SETTINGS = {
SETTINGS_REFRESH_FRAMES = 300 # 3 s of card's 100 Hz loop
def quiet_cluster_display(address, data):
if len(data) != 8:
return False
if address == 0x18A:
return ((data[5] >> 2) & 7) <= 1 and not (
data[0] & 0xE0 or data[1] & 1 or data[3] & 0xF0 or data[4] & 0xC0 or data[5] & 0x80 or data[6] & 0x3F)
if address == 0x3D8:
return data[6] >> 3 in (0, 30) and not (data[4] & 0x0D or data[5] & 0x18 or data[6] & 6)
return False
def startup_params(params, *, enabled: bool) -> dict[str, str]:
"""The startup settings init_ford_safety_param_starpilot reads. BluePilot lateral stays off without the openpilot
toggle, in Safe Mode, or when a startup setting can't be read. Legacy stock selection is retired."""
@@ -246,9 +234,6 @@ class FordBluePilotController(BluePilotLateral):
return self._constrain_curvature(CC, CS, actuators, apply_curvature_last, packets, applied)
def order_can_sends(self, frames):
if not self.CP.flags & (FordFlags.CANFD | FordFlags.NEW_PORT) and qualified_aol(self.CP, marked_only=True) and not self.lateral_active:
# Inactive classic steering returns display ownership to the camera.
frames = [(a, d, b) for a, d, b in frames if b != self.CAN.main or not quiet_cluster_display(a, d)]
if (not self.starpilot_selected or self.curv.primary_lateral_control != PrimaryLateralControl.angle or
self.frame % CarControllerParams.STEER_STEP or self.frame % CarControllerParams.LKA_STEP):
return frames
-73
View File
@@ -1,73 +0,0 @@
"""Receive-only Ford rear-fog telltale, independent of control authority."""
import time
MAX_AGE_NS = 1_500_000_000
def boot_time_ns():
return time.clock_gettime_ns(getattr(time, "CLOCK_BOOTTIME", time.CLOCK_MONOTONIC))
def main_bus(cp):
try:
if cp.brand != "ford" or not cp.carFingerprint:
return None
configs = tuple(cp.safetyConfigs)
if len(configs) == 1 and str(configs[0].safetyModel) == "ford":
return 0
if (len(configs) == 2 and str(configs[0].safetyModel) == "noOutput" and
int(configs[0].safetyParam) == 0 and str(configs[1].safetyModel) == "ford"):
return 4
except (AttributeError, TypeError, ValueError):
pass
return None
def fresh(source_ns, now_ns):
return source_ns > 0 and 0 <= now_ns - source_ns <= MAX_AGE_NS
class RearFogReceiver:
def __init__(self):
self.socket = None
self.identity = None
self.on = False
self.source_ns = 0
def close(self):
self.socket = None
self.identity = None
self.on = False
self.source_ns = 0
def observe(self, event, bus, now_ns):
stamp = int(event.logMonoTime)
if not event.valid or not fresh(stamp, now_ns) or stamp < self.source_ns:
return
for frame in event.can:
if frame.address == 0x3B3 and frame.src == bus and len(frame.dat) == 8:
self.on = bool(frame.dat[0] & 0x02)
self.source_ns = stamp
def sample(self, cp, *, active):
from openpilot.cereal import messaging
bus = main_bus(cp)
if not active or bus is None:
self.close()
return False, 0
identity = (str(cp.carFingerprint), bus)
if identity != self.identity:
self.close()
self.identity = identity
now = boot_time_ns()
try:
if self.socket is None:
self.socket = messaging.sub_sock("can", conflate=False)
for _ in range(32):
event = messaging.recv_one_or_none(self.socket)
if event is None:
break
self.observe(event, bus, now)
except (OSError, RuntimeError, ValueError):
self.close()
return self.on and fresh(self.source_ns, now), self.source_ns
+13 -73
View File
@@ -45,27 +45,27 @@ _REQUIRED_FLAGS = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_
_FORBIDDEN_FLAGS = int(HyundaiFlags.CANFD_ALT_BUTTONS | HyundaiFlags.CANFD_ANGLE_STEERING | HyundaiFlags.CANFD_CAMERA_SCC)
def _ioniq6_hda2_base(CP, *, require_radar=True) -> bool:
def _ioniq6_hda2_base(CP) -> bool:
if (
str(CP.carFingerprint) != str(HyundaiCAR.HYUNDAI_IONIQ_6)
or CP.brand != 'hyundai'
or (require_radar and CP.radarUnavailable)
or CP.radarUnavailable
or CP.notCar
or CP.passive
or CP.dashcamOnly
or CP.steerControlType != car.CarParams.SteerControlType.torque
or config_index(CP) is None
or len(CP.safetyConfigs) != 1
or CP.safetyConfigs[0].safetyModel != car.CarParams.SafetyModel.hyundaiCanfd
):
return False
flags = int(CP.flags)
return flags & _REQUIRED_FLAGS == _REQUIRED_FLAGS and not flags & _FORBIDDEN_FLAGS
def _stock_ioniq6(CP, *, marked_only=False, require_radar=True) -> bool:
if not _ioniq6_hda2_base(CP, require_radar=require_radar) or CP.openpilotLongitudinalControl or not CP.pcmCruise:
def _stock_ioniq6(CP, *, marked_only=False) -> bool:
if not _ioniq6_hda2_base(CP) or CP.openpilotLongitudinalControl or not CP.pcmCruise:
return False
stock = 0x91 if int(CP.flags) & int(HyundaiFlags.CANFD_LKA_STEER_MSG_ALT) else 0x11
raw = int(CP.safetyConfigs[config_index(CP)].safetyParam)
raw = int(CP.safetyConfigs[0].safetyParam)
return raw == (stock | 0x0800) or (not marked_only and raw == stock)
@@ -75,21 +75,21 @@ def qualified_ioniq6(CP) -> bool:
if not _ioniq6_hda2_base(CP) or not CP.openpilotLongitudinalControl or CP.pcmCruise:
return False
flags = int(CP.flags)
raw = int(CP.safetyConfigs[config_index(CP)].safetyParam)
raw = int(CP.safetyConfigs[0].safetyParam)
return raw == (0x8895 if flags & int(HyundaiFlags.CANFD_LKA_STEER_MSG_ALT) else 0x8815)
def native_admission_key(CP):
"""Complete mutable CP inputs to exact selected-owner Ioniq native admission.
"""Complete mutable CP inputs to exact one-slot Ioniq native admission.
Keep this in sync with qualified_ioniq6 and native inventory qualification.
This key is not an admission decision and never contains live authority.
"""
if CP.brand != 'hyundai' or CP.carFingerprint != HyundaiCAR.HYUNDAI_IONIQ_6 or config_index(CP) is None:
if CP.brand != 'hyundai' or CP.carFingerprint != HyundaiCAR.HYUNDAI_IONIQ_6 or len(CP.safetyConfigs) != 1:
return None
return (str(CP.brand), str(CP.carFingerprint), int(CP.flags), bool(CP.radarUnavailable),
bool(CP.notCar), bool(CP.passive), bool(CP.dashcamOnly),
bool(CP.openpilotLongitudinalControl), bool(CP.pcmCruise), int(CP.alternativeExperience), str(CP.steerControlType),
bool(CP.openpilotLongitudinalControl), bool(CP.pcmCruise), int(CP.alternativeExperience),
tuple((int(c.safetyModel.raw), int(c.safetyParam)) for c in CP.safetyConfigs))
@@ -98,12 +98,6 @@ def ioniq6_settings_capable(CP) -> bool:
return _stock_ioniq6(CP) or (IONIQ6_LONG_PREARM_ENABLED and ioniq6_long_eligible(CP))
def ioniq6_alpha_long_available(CP) -> bool:
# Cached radar discovery may predate the external-bus repair. This saves only
# an opt-in; the takeover still requires fresh physical radar and ECU evidence.
return _stock_ioniq6(CP, require_radar=False) or (IONIQ6_LONG_PREARM_ENABLED and ioniq6_long_eligible(CP))
def qualified_ccnc_long_aol(CP, *, marked_only=False):
return qualified_ev9_long_aol(CP, marked_only=marked_only) or qualified_pe_long_aol(CP, marked_only=marked_only)
@@ -238,9 +232,9 @@ def native_profile_supported(model: int, param: int) -> bool:
def native_accepts_cp(CP, model: int, param: int) -> bool:
# HDA-II retains its dedicated namespace; camera SCC uses the shared exact word.
# Ioniq 6 has its own exact contract; no generic Hyundai owner accepts it.
# Revalidate the complete contract on every call, without caching live authority.
if CP.carFingerprint == HyundaiCAR.HYUNDAI_IONIQ_6 and not CP.flags & HyundaiFlags.CANFD_CAMERA_SCC:
if CP.carFingerprint == HyundaiCAR.HYUNDAI_IONIQ_6:
return qualified_ioniq6(CP)
if qualified_ccnc_long_aol(CP, marked_only=True):
return model == int(car.CarParams.SafetyModel.hyundaiCanfd) and param == CP.safetyConfigs[0].safetyParam
@@ -554,57 +548,3 @@ def pending_stock_angle_arm(CP, CS, sm, owner, *, now_ns, producer, acknowledged
not native.requestedLongitudinal and not native.longitudinalAllowed)
except (AttributeError, KeyError, IndexError, TypeError, ValueError, OverflowError):
return False
def retain_stopped_eps_arm(feedback, sm, cp, cs, axis, now_ns, *, latched, arm_ns, producer):
from openpilot.cereal import log
from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS
from openpilot.starpilot.aol.transport_pause import _configured_transport_native
pending = getattr(feedback, '_stopped_eps_arm', None)
feedback._stopped_eps_arm = None
if not cs.steerFaultTemporary and pending is None:
return False
if (not latched or feedback.failed or feedback.paused or feedback.pending_event or
feedback.retention_started_ns or cs.steerFaultPermanent or not cs.canValid or cs.canTimeout or
cs.accFaulted or cs.doorOpen or cs.vehicleSensorsInvalid or cs.steeringDisengage or
cs.gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.sport,
car.CarState.GearShifter.low, car.CarState.GearShifter.brake) or
str(axis.faultReason) != 'none' or axis.longitudinalActive or axis.desiredLongitudinal or
not producer or axis.intentProducerSessionId != producer or
not 0 < axis.sourceCarStateMonoTime <= axis.observedMonoTime <= now_ns or
now_ns - axis.sourceCarStateMonoTime > AXIS_MAX_AGE_NS):
return False
binding = (int(arm_ns), str(axis.sessionId), producer, int(axis.intentIntegrityEpoch))
if pending is None:
if (not cs.steerFaultTemporary or not cs.standstill or not cs.brakePressed or
not 0 < arm_ns <= now_ns <= arm_ns + AXIS_MAX_AGE_NS or
not qualified_classic_scc(cp, marked_only=True) or
axis.lateralActive or axis.desiredLateral):
return False
elif pending[:4] != binding or not 0 <= now_ns - pending[5] <= AXIS_MAX_AGE_NS:
return False
event_ns = int(sm.logMonoTime['onroadEvents'])
temporary = (log.OnroadEvent.EventName.steerTempUnavailable,
log.OnroadEvent.EventName.steerTempUnavailableSilent)
allowed = (*temporary, log.OnroadEvent.EventName.wrongCarMode,
log.OnroadEvent.EventName.preEnableStandstill, log.OnroadEvent.EventName.pcmDisable,
log.OnroadEvent.EventName.pedalPressed, log.OnroadEvent.EventName.gasPressedOverride,
log.OnroadEvent.EventName.steerOverride)
events = sm['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 or
any(e.name not in allowed or e.immediateDisable or e.softDisable and e.name not in temporary for e in events) or
cs.steerFaultTemporary and not any(e.name in temporary for e in events)):
return False
native = _configured_transport_native(sm, cp, str(axis.sessionId), now_ns,
require_longitudinal=False, allow_restricted_lateral=True)
if native is None or not native.pandaInventory or native.requestedLongitudinal or native.longitudinalAllowed:
return False
inventory = (native.pandaSerial, tuple(slot.hardwareSerial for slot in native.pandaInventory))
if pending is not None and inventory != pending[4]:
return False
if axis.lateralActive:
return False
feedback._stopped_eps_arm = (*binding, inventory, now_ns)
return True
@@ -9,7 +9,6 @@ from openpilot.cereal import log
from openpilot.selfdrive.modeld import modeld
from openpilot.starpilot.car.subaru.smoothing import filter_action
from openpilot.starpilot.models.runner import CatalogModelState
from openpilot.starpilot.models.laboratory_runner import LaboratoryPair
def common_join():
@@ -20,13 +19,10 @@ def common_join():
if not isinstance(body, list):
continue
for index, statement in enumerate(body):
if (isinstance(statement, ast.Expr) and isinstance(statement.value, ast.Call) and
ast.unparse(statement.value.func) == 'filter_action'):
assert index > 0
statements = body[index - 1:index + 2]
if (isinstance(statement, ast.Assign) and ast.unparse(statement.targets[0]) == 'action' and
isinstance(statement.value, ast.Call) and ast.unparse(statement.value.func) == 'model.get_action_from_model'):
statements = body[index:index + 3]
assert len(statements) == 3
assert sum(isinstance(call, ast.Call) and ast.unparse(call.func) == 'model.get_action_from_model'
for call in ast.walk(statements[0])) == 1
assert ast.unparse(statements[1]) == 'filter_action(action, prev_action, v_ego, subaru_tau)'
assert ast.unparse(statements[2]) == 'prev_action = action'
matches.append(ast.Module(body=statements, type_ignores=[]))
@@ -34,10 +30,10 @@ def common_join():
return compile(matches[0], modeld.__file__, 'exec')
@pytest.mark.parametrize('owner', ['standard', 'catalog', 'jetlink', 'laboratory'])
@pytest.mark.parametrize('owner', ['standard', 'catalog', 'jetlink'])
@pytest.mark.parametrize('tau', [0., .4])
def test_actual_common_join_once_and_acceleration_unchanged(owner, tau, monkeypatch):
if owner in ('standard', 'laboratory'):
if owner == 'standard':
function = modeld.ModelState.get_action_from_model
elif owner == 'catalog':
catalog = CatalogModelState.__new__(CatalogModelState)
@@ -50,10 +46,7 @@ def test_actual_common_join_once_and_acceleration_unchanged(owner, tau, monkeypa
previous = log.ModelDataV2.Action(desiredCurvature=.01, desiredAcceleration=-.2)
outputs = {'action': np.array([[.08, .5]])}
baseline = function(outputs, previous, .2, .2, 2.)
model = (LaboratoryPair.__new__(LaboratoryPair) if owner == 'laboratory' else
SimpleNamespace(get_action_from_model=function))
paired_action = function(outputs, previous, .2, .2, 2.) if owner == 'laboratory' else None
scope = {'model': model, 'LaboratoryPair': LaboratoryPair, 'laboratory_action': paired_action, 'model_output': outputs, 'prev_action': previous,
scope = {'model': SimpleNamespace(get_action_from_model=function), 'model_output': outputs, 'prev_action': previous,
'lat_action_t': .2, 'long_action_t': .2, 'v_ego': 2., 'subaru_tau': tau, 'filter_action': filter_action}
exec(common_join(), scope)
action = scope['action']
@@ -101,7 +101,7 @@
"discriminantOffset": 0
},
"new_storage": {
"dataWordCount": 21,
"dataWordCount": 18,
"pointerCount": 22,
"preferredListEncoding": "inlineComposite",
"isGroup": false,
@@ -252,78 +252,6 @@
"ordinal": {
"explicit": 58
}
},
{
"slot": {
"offset": 141,
"type": {
"bool": null
},
"defaultValue": {
"bool": false
},
"hadExplicitDefault": false
},
"name": "acceptedIsPreview",
"codeOrder": 59,
"discriminantValue": 65535,
"ordinal": {
"explicit": 59
}
},
{
"slot": {
"offset": 18,
"type": {
"uint64": null
},
"defaultValue": {
"uint64": 0
},
"hadExplicitDefault": false
},
"name": "wheelOverrideSince",
"codeOrder": 60,
"discriminantValue": 65535,
"ordinal": {
"explicit": 60
}
},
{
"slot": {
"offset": 19,
"type": {
"uint64": null
},
"defaultValue": {
"uint64": 0
},
"hadExplicitDefault": false
},
"name": "wheelAdoptMonoTime",
"codeOrder": 61,
"discriminantValue": 65535,
"ordinal": {
"explicit": 61
}
},
{
"slot": {
"offset": 40,
"type": {
"float32": null
},
"defaultValue": {
"float32": 0.0
},
"hadExplicitDefault": false
},
"name": "wheelAdoptTargetMps",
"codeOrder": 62,
"discriminantValue": 65535,
"ordinal": {
"explicit": 62
}
}
]
},
@@ -836,90 +764,6 @@
}
}
]
},
"0x9ccdc8676701b412": {
"kind": "struct",
"before_name": "SlcAction",
"after_name": "SlcAction",
"old_field_count": 8,
"old_storage": {
"dataWordCount": 4,
"pointerCount": 4,
"preferredListEncoding": "inlineComposite",
"isGroup": false,
"discriminantCount": 0,
"discriminantOffset": 0
},
"new_storage": {
"dataWordCount": 5,
"pointerCount": 4,
"preferredListEncoding": "inlineComposite",
"isGroup": false,
"discriminantCount": 0,
"discriminantOffset": 0
},
"added_fields": [
{
"slot": {
"offset": 4,
"type": {
"uint64": null
},
"defaultValue": {
"uint64": 0
},
"hadExplicitDefault": false
},
"name": "wheelAdoptMonoTime",
"codeOrder": 8,
"discriminantValue": 65535,
"ordinal": {
"explicit": 8
}
}
]
},
"0xcd96dafb67a082d0": {
"kind": "struct",
"before_name": "SlcCruiseEvent",
"after_name": "SlcCruiseEvent",
"old_field_count": 16,
"old_storage": {
"dataWordCount": 7,
"pointerCount": 6,
"preferredListEncoding": "inlineComposite",
"isGroup": false,
"discriminantCount": 0,
"discriminantOffset": 0
},
"new_storage": {
"dataWordCount": 8,
"pointerCount": 6,
"preferredListEncoding": "inlineComposite",
"isGroup": false,
"discriminantCount": 0,
"discriminantOffset": 0
},
"added_fields": [
{
"slot": {
"offset": 7,
"type": {
"uint64": null
},
"defaultValue": {
"uint64": 0
},
"hadExplicitDefault": false
},
"name": "wheelOverrideSince",
"codeOrder": 16,
"discriminantValue": 65535,
"ordinal": {
"explicit": 16
}
}
]
}
},
"published_addition_commits": {
@@ -6,8 +6,9 @@ CEM selects Experimental mode for configured speed, road, signal, curve, lead, s
`ConditionalPlannerHost` joins the current native MPC result, current model/car/radar observations, saved preferences, and qualified wheel-button events. It produces a short-lived proposal on `slcState.conditionalMode`; it does not own the effective driving mode. `selfdrived` independently checks the current vehicle capability, longitudinal axis, drive, saved configuration, and input freshness before accepting that proposal. Its ordinary `ExperimentalMode` request remains the fallback. The UI displays an active conditional reason only from a fresh selfdrived acknowledgment matching the exact current `selfdriveState` frame and active longitudinal axis; the saved family is displayed separately.
Speed-limit availability is independent of mode selection. A missing or stale Maps or Vision limit does
not request Experimental or veto Chill. Other scene triggers, manual mode choices and committed
Speed-limit availability is independent of mode selection. The retired `SLCFallback=1`
preference cannot request Experimental or veto Chill, including when Maps or Vision
becomes unknown or stale. Other scene triggers, manual mode choices and committed
stop handling retain their existing behavior. Historical speed-limit reason labels
remain readable in recorded routes.
@@ -48,6 +49,6 @@ Traffic assignment does not suppress ordinary profile tuning.
Scene inputs distinguish false from unknown. Model filters advance once per original model tick; the oldest required source bounds freshness separately, so a slower fresh radar cannot stall a model filter or renew stale data. Startup, clock discontinuities, and drive changes require fresh observations. Conditional Chill requires every applicable veto to be known false. Disabled build features can be known absent; an enabled producer without a qualified observation remains unknown.
Current producers cover native MPC follow time and stop/throttle intent, model lane geometry, qualified adjacent radar, SLC attribution, pedal override, and the effective mode reported by selfdrived. The exact Ioniq media path supplies source-stamped Traffic Mode to CEM and a bounded Traffic follow/jerk, acceleration and cruise-braking profile. The shared committed-turn evaluator feeds both the stop detector and slower-lead retuning from original car/model stamps. Traffic retains the existing 0.75-second effective headway floor; saved values below it remain unsupported and intact. The native planner retains the selected cruise-braking response for an SLC cap. Conditional speed comparisons and curve control receive the same-cycle SLC-capped cruise target while the driver's SET remains unchanged. A saved Traffic braking category reverts to its 0.42 m/s² cruise baseline for a present radar lead or standstill, then recovers through the existing profile smoother; MPC and model braking retain priority. The saved lane-change close-gap preference and current lateral minimum-speed policy are separate from conditional scene selection. Frozen weather and other hazard-context braking inputs remain absent. Stop-sign, forced-stop, and dashboard stop-sign producers remain absent. The historical red-light name is already represented by the current stop-light observation, so it does not need a duplicate owner.
Current producers cover native MPC follow time and stop/throttle intent, model lane geometry, qualified adjacent radar, SLC attribution, pedal override, and the effective mode reported by selfdrived. The exact Ioniq media path supplies source-stamped Traffic Mode to CEM and a bounded Traffic follow/jerk, acceleration and cruise-braking profile. The shared committed-turn evaluator feeds both the stop detector and slower-lead retuning from original car/model stamps. Traffic retains the existing 0.75-second effective headway floor; saved values below it remain unsupported and intact. The native planner shapes the cruise-only floor for a qualified SLC cap, a validated named Eco/Sport braking personality, or the explicit global Braking response. A saved Traffic braking category reverts to its 0.42 m/s² cruise baseline for a present radar lead or standstill, then recovers through the existing profile smoother; MPC and model braking retain priority. The saved lane-change close-gap preference and current lateral minimum-speed policy are separate from conditional scene selection. Frozen weather and other hazard-context braking inputs remain absent. Stop-sign, forced-stop, and dashboard stop-sign producers remain absent. The historical red-light name is already represented by the current stop-light observation, so it does not need a duplicate owner.
The source contract includes frozen `starpilot/controls/lib/conditional_experimental_mode.py`, `conditional_chill_mode.py`, `starpilot/common/experimental_state.py`, and the planner's mode selection. Tests compare executable frozen policy/detector traces and exercise current serialized messages, native MPC/radar joins, lifecycle boundaries, manual controls, and settings concurrency. These are integration evidence, not vehicle qualification. Normal availability is limited to the exact tagged Ioniq 6 LONG profile; the separate ECU handoff and native safety acknowledgment must succeed first. Missing scene producers and road behavior still require evaluation.
@@ -59,7 +59,6 @@ class ConditionalModeHost:
selected_t_follow_s: float | None = None,
selected_t_follow_observed_mono_ns: int | None = None,
settings_owner: ConditionalSettingsOwner | None = None,
cruise_ceiling_mps: float | None = None,
) -> HostProposal:
signal_options = None
settings_revision = None
@@ -121,7 +120,6 @@ class ConditionalModeHost:
selected_t_follow_observed_mono_ns=selected_t_follow_observed_mono_ns,
owner_context=owner_context,
signal_options=signal_options,
cruise_ceiling_mps=cruise_ceiling_mps,
)
authority = projected.authority
if authority is not None and authority.fresh and (not authority.system_long_capable or authority.safe_mode or
@@ -13,7 +13,7 @@ from opendbc.car.structs import car
from opendbc.car.gm.distance_button import DistanceObservation, DistanceSample
from opendbc.car.hyundai.ioniq6_media import MediaObservation
from opendbc.car.hyundai.hyundaicanfd import CanBus
from opendbc.car.hyundai.canfd_owner import config_index
from opendbc.car.hyundai.ioniq6_handoff import IONIQ6_ECAN_BUS
from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR
from openpilot.selfdrive.car.cruise import CRUISE_LONG_PRESS
from openpilot.starpilot.conditional_mode.policy import ModeChoice, next_manual_status, restore_manual_status
@@ -31,8 +31,7 @@ ButtonType = car.CarState.ButtonEvent.Type
def ioniq6_media_eligible(CP: car.CarParams) -> bool:
"""Only the reviewed Ioniq E-CAN topology has physical media evidence."""
return (CP.carFingerprint == HYUNDAI_CAR.HYUNDAI_IONIQ_6 and config_index(CP) is not None and
CanBus(CP).ECAN in (1, 5))
return CP.carFingerprint == HYUNDAI_CAR.HYUNDAI_IONIQ_6 and CanBus(CP).ECAN == IONIQ6_ECAN_BUS
class Button(StrEnum):
@@ -180,7 +180,6 @@ class ConditionalPlannerHost:
drive_id: int, manual: ManualState | None = None,
owner_context: ConditionalOwnerContext | None = None,
native_plan_valid: bool = False,
cruise_ceiling_mps: float | None = None,
manual_event=None,
ui_event=None,
) -> tuple[HostProposal, SettingsSnapshot | None]:
@@ -290,7 +289,6 @@ class ConditionalPlannerHost:
manual=self.manual_state if manual is None else manual, owner_context=context,
selected_t_follow_s=selected_follow_time(planner),
selected_t_follow_observed_mono_ns=sm.logMonoTime.get('modelV2'),
cruise_ceiling_mps=cruise_ceiling_mps,
settings_owner=self.settings,
)
snapshot = self.settings.current
@@ -406,7 +406,6 @@ class SceneProjector:
selected_t_follow_observed_mono_ns: int | None = None,
owner_context: ConditionalOwnerContext | None = None,
signal_options: CEMOptions | None = None,
cruise_ceiling_mps: float | None = None,
) -> ProjectedScene:
if (
type(now_mono_ns) is not int
@@ -442,9 +441,6 @@ class SceneProjector:
speed = _ego_speed(car) if car is not None and _field(car, 'canValid') is True and _field(car, 'canTimeout') is False else None
cruise_kph = _number(_field(car, 'vCruise'), high=V_CRUISE_UNSET_KPH) if speed is not None else None
cruise = cruise_kph * KPH_TO_MPS if cruise_kph is not None and 0 < cruise_kph < V_CRUISE_UNSET_KPH else None
if cruise_ceiling_mps is not None:
cap = _number(cruise_ceiling_mps, high=V_CRUISE_UNSET_KPH * KPH_TO_MPS)
cruise = min(cruise, cap) if cruise is not None and cap is not None and cap > 0 else None
curvature = _number(_field(controls, 'curvature'), low=-1.0, high=1.0) if controls is not None else None
raw_curve = abs(speed * speed * curvature) >= FROZEN_CURVE_ACCEL_MPS2 if speed is not None and curvature is not None else None
horizon = None
@@ -83,7 +83,6 @@ class ManualSavedJoinTests(unittest.TestCase):
self.assertEqual(self.owner.manual_state.intent, ManualIntent.NONE) # Honda cannot consume Ioniq media.
self.assertEqual(self.owner.card_last_sequence, 0)
self.cp.carFingerprint = HYUNDAI_CAR.HYUNDAI_IONIQ_6
self.cp.safetyConfigs = [{'safetyModel': 'hyundaiCanfd'}]
self.cp.flags = int(HyundaiFlags.CANFD_LKA_STEER_MSG)
self.sample(stamp + 50_000_000, self.receipt(stamp + 50_000_000, 2, button='mode'))
self.assertEqual(self.owner.card_last_sequence, 2)
@@ -114,20 +114,6 @@ def owner_context(stamp: int, *, stop_time=8.0, slower=True, stopped=False):
class TestSceneProjector(unittest.TestCase):
def test_conditional_speed_comparisons_use_the_slc_cap_without_mutating_set(self):
for cap, expected in ((None, 20.), (15., 15.), (25., 20.), (float('nan'), None)):
with self.subTest(cap=cap):
projector = SceneProjector()
sm = FakeSubMaster(serialized_scene(), MONO-5_000_000)
projector.project(sm, CP, now_mono_ns=MONO, now_boot_ns=BOOT, sample_skew_ns=1000)
now = MONO+50_000_000
sm.stamp(now)
sm.payloads.update(serialized_scene(stamp=BOOT+50_000_000))
result = projector.project(sm, CP, now_mono_ns=now, now_boot_ns=BOOT+50_000_000,
sample_skew_ns=1000, safe_mode=False, owner_context=owner_context(now), cruise_ceiling_mps=cap)
self.assertEqual(result.scene.set_speed_mps, expected)
self.assertEqual(sm['carState'].vCruise, 72.)
def test_standstill_filter_undershoot_requires_fresh_physical_zero_speed(self):
cases = ((-1.401298464324817e-45, True, 0., True),
(-0.0008391447481699288, True, 0., True),
@@ -261,7 +261,6 @@ def test_actual_card_alpha_off_short_wheel_gesture_has_lateral_only_authority():
f.setUp()
try:
f.cp.carFingerprint = CAR.HYUNDAI_IONIQ_6
f.cp.safetyConfigs = [{"safetyModel": "hyundaiCanfd"}]
f.cp.flags = int(HyundaiFlags.CANFD_LKA_STEER_MSG)
f.cp.openpilotLongitudinalControl = False
f.sm['carControl'].enabled = f.sm['carControl'].longActive = False

Some files were not shown because too many files have changed in this diff Show More