mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-11 06:53:50 +08:00
Compare commits
2 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 799c5effeb | |||
| 18e52b36d1 |
@@ -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.
@@ -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)
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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,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)
|
||||
@@ -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
@@ -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.
@@ -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}},
|
||||
|
||||
@@ -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
@@ -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:
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
BIN
Binary file not shown.
BIN
Binary file not shown.
@@ -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)
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
Binary file not shown.
Binary file not shown.
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);
|
||||
}
|
||||
@@ -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.
@@ -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",
|
||||
|
||||
BIN
Binary file not shown.
@@ -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
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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
|
||||
@@ -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,
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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']
|
||||
|
||||
+1
-157
@@ -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
Reference in New Issue
Block a user