mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-10 10:13:45 +08:00
Compare commits
25 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 95a7f3b398 | |||
| 22707891bd | |||
| 962d8b6719 | |||
| 0f3bdb34b3 | |||
| c2921c1a8f | |||
| a19beda327 | |||
| b7775991bf | |||
| eedd73e522 | |||
| 50e2c21dbd | |||
| 54c3fb13f3 | |||
| 7f3bd61292 | |||
| dec4a0884a | |||
| 4edc8ab86a | |||
| b3a14cb48d | |||
| f47322cbee | |||
| a08065f282 | |||
| a33bec1ca4 | |||
| ca3d8a3816 | |||
| 2360ff9b0f | |||
| 0976fd804d | |||
| 2504441a4e | |||
| 0b5ccb31e1 | |||
| b91ea3e1da | |||
| 1588f7041a | |||
| bcf152e6f7 |
+2
-19
@@ -1,25 +1,8 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.params import Params
|
||||
|
||||
|
||||
def gm_car_params_present(params: Params, CP: car.CarParams | None = None) -> bool:
|
||||
if CP is not None and getattr(CP, "brand", None):
|
||||
return CP.brand == "gm"
|
||||
try:
|
||||
raw_car_params = params.get("CarParams")
|
||||
if raw_car_params is None:
|
||||
return False
|
||||
with car.CarParams.from_bytes(raw_car_params) as parsed_cp:
|
||||
return parsed_cp.brand == "gm"
|
||||
except Exception:
|
||||
return False
|
||||
|
||||
|
||||
def get_gps_location_service(params: Params, CP: car.CarParams | None = None) -> str:
|
||||
# GM arbitrates device/PPS/OnStar through gpsLocationExternal.
|
||||
if gm_car_params_present(params, CP) or params.get_bool("UbloxAvailable") or params.get_bool("CarGpsAvailable"):
|
||||
def get_gps_location_service(params: Params) -> str:
|
||||
if params.get_bool("UbloxAvailable") or params.get_bool("CarGpsAvailable"):
|
||||
return "gpsLocationExternal"
|
||||
else:
|
||||
return "gpsLocation"
|
||||
|
||||
Binary file not shown.
@@ -18,6 +18,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"BootCount", {PERSISTENT, INT}},
|
||||
{"BluetoothAudioAddress", {PERSISTENT, STRING}},
|
||||
{"BluetoothAudioTestActive", {CLEAR_ON_MANAGER_START | DONT_LOG, BOOL}},
|
||||
{"BluetoothDisconnectControllersOffroad", {PERSISTENT, BOOL, "0"}},
|
||||
{"BluetoothEnabled", {PERSISTENT, BOOL, "0"}},
|
||||
{"CalibrationParams", {PERSISTENT, BYTES}},
|
||||
{"CameraDebugExpGain", {CLEAR_ON_MANAGER_START, STRING}},
|
||||
@@ -109,6 +110,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"LocationFilterInitialState", {PERSISTENT, BYTES}},
|
||||
{"LongitudinalManeuverMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
|
||||
{"LongitudinalPersonality", {PERSISTENT, INT, std::to_string(static_cast<int>(cereal::LongitudinalPersonality::STANDARD))}},
|
||||
{"LongitudinalPersonalityProfiles", {PERSISTENT | DONT_LOG, JSON, "{}", "{}"}},
|
||||
{"NetworkMetered", {PERSISTENT, BOOL}},
|
||||
{"ObdMultiplexingChanged", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}},
|
||||
{"ObdMultiplexingEnabled", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}},
|
||||
@@ -316,7 +318,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"DeveloperSidebarMetric7", {PERSISTENT, INT, "7", "0", 3}},
|
||||
{"DeveloperUI", {PERSISTENT, BOOL, "0", "0", 3}},
|
||||
{"GalaxyDeveloperMode", {PERSISTENT | DONT_LOG, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
|
||||
{"GalaxyMobileDefault", {PERSISTENT | DONT_LOG, BOOL, "0", "0", 0, SETTINGS_ADVANCED}},
|
||||
{"GalaxyMobileDefault", {PERSISTENT | DONT_LOG, BOOL, "1", "1", 0, SETTINGS_SIMPLE}},
|
||||
{"DeveloperWidgets", {PERSISTENT, BOOL, "1", "0", 3}},
|
||||
{"DeviceManagement", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
|
||||
{"DeviceShutdown", {PERSISTENT, INT, "6", "6", 1, SETTINGS_SIMPLE}},
|
||||
@@ -463,7 +465,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"LeadDepartingAlert", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
|
||||
{"LeadDetectionThreshold", {PERSISTENT, INT, "35", "50", 3}},
|
||||
{"LeadIndicator", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"LeadInfo", {PERSISTENT, BOOL, "1", "0", 3}},
|
||||
{"LeadInfo", {PERSISTENT, BOOL, "0", "0", 3}},
|
||||
{"LeadInfoMode", {PERSISTENT, INT, "2", "2", 3}},
|
||||
{"LKASButtonControl", {PERSISTENT, INT, "5", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"LockDoors", {PERSISTENT, BOOL, "1", "0", 0}},
|
||||
{"LockDoorsTimer", {PERSISTENT, INT, "0", "0", 0}},
|
||||
@@ -608,6 +611,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"RelaxedJerkDeceleration", {PERSISTENT, FLOAT, "100.0", "100.0", 3}},
|
||||
{"RelaxedJerkSpeed", {PERSISTENT, FLOAT, "100.0", "100.0", 3}},
|
||||
{"RelaxedJerkSpeedDecrease", {PERSISTENT, FLOAT, "100.0", "100.0", 3}},
|
||||
{"ReverseCruise", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
|
||||
{"RivianAngleControl", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"RivianAngleSaturated", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL, "0", "0"}},
|
||||
{"RivianToiRecoveryFailed", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL, "0", "0"}},
|
||||
|
||||
Binary file not shown.
@@ -5,7 +5,7 @@ import threading
|
||||
import time
|
||||
import uuid
|
||||
|
||||
from openpilot.common.params import Params, ParamKeyFlag, UnknownKeyName
|
||||
from openpilot.common.params import Params, ParamKeyFlag, ParamKeyType, UnknownKeyName
|
||||
|
||||
class TestParams:
|
||||
def setup_method(self):
|
||||
@@ -128,6 +128,31 @@ class TestParams:
|
||||
assert self.params.get("LiveParameters") is None
|
||||
assert self.params.get("LiveParameters", return_default=True) is None
|
||||
|
||||
def test_longitudinal_personality_profiles_json_round_trip(self):
|
||||
key = "LongitudinalPersonalityProfiles"
|
||||
value = {
|
||||
"schemaVersion": 1,
|
||||
"enabled": False,
|
||||
"axes": {
|
||||
"acceleration": {
|
||||
"speed": {"unit": "mph", "values": [0.0, 11.184681, 22.369363, 33.554044, 44.738726, 55.923407, 89.477452]},
|
||||
"value": {"unit": "m/s^2", "meaning": "maximum_requested_acceleration"},
|
||||
},
|
||||
"braking": {
|
||||
"speed": {"unit": "mph", "values": [0.0, 11.184681, 22.369363, 33.554044, 44.738726, 55.923407, 89.477452]},
|
||||
"value": {"unit": "m/s^2", "meaning": "cruise_slc_deceleration_magnitude"},
|
||||
},
|
||||
"following": {"speed": {"unit": "mph", "values": [0, 10, 20, 30, 40, 50, 60, 70, 80, 90]}, "value": {"unit": "s", "meaning": "base_time_headway"}},
|
||||
},
|
||||
"profiles": {},
|
||||
}
|
||||
self.params.remove(key)
|
||||
|
||||
assert self.params.get_type(key) == ParamKeyType.JSON
|
||||
assert self.params.get(key) is None
|
||||
self.params.put(key, value)
|
||||
assert self.params.get(key) == value
|
||||
|
||||
def test_params_get_type(self):
|
||||
# json
|
||||
self.params.put("ApiCache_DriveStats", {"a": 0})
|
||||
|
||||
@@ -194,7 +194,7 @@ def get_checksum_state(dbc_name: str) -> ChecksumState | None:
|
||||
return ChecksumState(4, 2, 3, 5, False, SignalType.HONDA_CHECKSUM, honda_checksum)
|
||||
elif dbc_name.startswith(("toyota_", "lexus_")):
|
||||
return ChecksumState(8, -1, 7, -1, False, SignalType.TOYOTA_CHECKSUM, toyota_checksum)
|
||||
elif dbc_name.startswith("hyundai_canfd_generated"):
|
||||
elif dbc_name.startswith(("hyundai_canfd_generated", "hyundai_radar_210_21f_generated")):
|
||||
return ChecksumState(16, -1, 0, -1, True, SignalType.HKG_CAN_FD_CHECKSUM, hkg_can_fd_checksum)
|
||||
elif dbc_name.startswith(("vw_mqb", "vw_mqbevo", "vw_meb")):
|
||||
return ChecksumState(8, 4, 0, 0, True, SignalType.VOLKSWAGEN_MQB_MEB_CHECKSUM, volkswagen_mqb_meb_checksum)
|
||||
|
||||
@@ -6,7 +6,7 @@ from opendbc.car.lateral import apply_driver_steer_torque_limits
|
||||
from opendbc.car.gm import gmcan
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.gm.values import (
|
||||
ASCM_INT, CAMERA_ACC_CAR, CAR, CC_ONLY_CAR, CC_REGEN_PADDLE_CAR, DBC, EV_CAR, SDGM_CAR, AccState, CanBus, CarControllerParams,
|
||||
ASCM_INT, CAMERA_ACC_CAR, CAR, CC_ONLY_CAR, CC_REGEN_PADDLE_CAR, DBC, EV_CAR, GM_AUTO_HOLD_CARS, SDGM_CAR, AccState, CanBus, CarControllerParams,
|
||||
CruiseButtons, GMFlags, GMSafetyFlags,
|
||||
)
|
||||
from opendbc.car.interfaces import CarControllerBase
|
||||
@@ -309,7 +309,7 @@ def supports_volt_auto_hold(CP, auto_hold_enabled: bool):
|
||||
auto_hold_enabled and
|
||||
getattr(CP, "openpilotLongitudinalControl", False) and
|
||||
stock_hold_safety_ready and
|
||||
CP.carFingerprint in AUTO_HOLD_VOLT_CARS
|
||||
CP.carFingerprint in GM_AUTO_HOLD_CARS
|
||||
)
|
||||
|
||||
|
||||
@@ -852,7 +852,6 @@ class CarController(CarControllerBase):
|
||||
CAR.CHEVROLET_VOLT_CC,
|
||||
CAR.CHEVROLET_MALIBU_CC,
|
||||
CAR.CHEVROLET_MALIBU_HYBRID_CC,
|
||||
CAR.BUICK_LACROSSE,
|
||||
}
|
||||
|
||||
if (self.CP.enableGasInterceptorDEPRECATED and self.CP.carFingerprint in CC_REGEN_PADDLE_CAR and
|
||||
|
||||
@@ -1,7 +1,5 @@
|
||||
import copy
|
||||
import math
|
||||
from datetime import UTC, datetime, timedelta
|
||||
from collections.abc import Mapping
|
||||
from cereal import custom
|
||||
from opendbc.can import CANDefine, CANParser
|
||||
from opendbc.car import Bus, create_button_events, structs
|
||||
@@ -10,10 +8,12 @@ from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.gps import get_car_gps_config
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from opendbc.car.gm.values import (
|
||||
ALT_ACCS,
|
||||
ASCM_INT,
|
||||
CAMERA_ACC_CAR,
|
||||
CAR,
|
||||
CC_ONLY_CAR,
|
||||
CC_REGEN_PADDLE_CAR,
|
||||
DBC,
|
||||
AccState,
|
||||
CanBus,
|
||||
@@ -32,118 +32,13 @@ STANDSTILL_THRESHOLD = 10 * 0.0311
|
||||
VOLT_EBCM_BRAKE_PRESSED_THRESHOLD = 6 / 0xd0
|
||||
AUTO_HOLD_MIN_DRIVE_TIME_S = 3.0
|
||||
AUTO_HOLD_REGEN_RELEASE_COOLDOWN_S = 1.0
|
||||
ACC_STARTUP_FAULT_GRACE_PERIOD_S = 5.0
|
||||
|
||||
BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise,
|
||||
CruiseButtons.MAIN: ButtonType.mainCruise, CruiseButtons.CANCEL: ButtonType.cancel}
|
||||
HARD_BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise}
|
||||
NORMAL_CRUISE_BUTTONS = (CruiseButtons.RES_ACCEL, CruiseButtons.DECEL_SET)
|
||||
|
||||
# Optional ~10 Hz CT6 PPS GPS messages on the powertrain bus.
|
||||
PPS_GPS_MESSAGES = (
|
||||
"PPS_ElevHdSpd_FO",
|
||||
"PPS_PosLat_FO",
|
||||
"PPS_PosLong_FO",
|
||||
"PPS_Time_FO",
|
||||
"PPS_QualMetrics_FO",
|
||||
)
|
||||
# PPS_SigAcqTime_FO is omitted: its validity bit stays 1 even during valid fixes.
|
||||
|
||||
|
||||
def pps_checksum_ok(data: bytes) -> bool:
|
||||
"""Validate the 11-bit checksum used by the observed PPS frames."""
|
||||
if len(data) < 2:
|
||||
return False
|
||||
received = ((data[-2] & 0x07) << 8) | data[-1]
|
||||
expected = sum(data[:-2]) + (data[-2] >> 3) + 0x4C
|
||||
return (expected & 0x7FF) == received
|
||||
|
||||
|
||||
def decode_gm_pps_gps(values: Mapping[str, Mapping[str, float]], raw: Mapping[str, bytes],
|
||||
timestamp_nanos: int) -> dict | None:
|
||||
"""Decode one coherent PPS bundle into the existing car-GPS sample shape."""
|
||||
if any(not pps_checksum_ok(raw.get(name, b"")) for name in PPS_GPS_MESSAGES):
|
||||
return None
|
||||
|
||||
try:
|
||||
pos_lat = values["PPS_PosLat_FO"]
|
||||
pos_long = values["PPS_PosLong_FO"]
|
||||
timestamp_values = values["PPS_Time_FO"]
|
||||
quality = values["PPS_QualMetrics_FO"]
|
||||
if (int(pos_lat.get("PPSLatV", 1)) != 0 or
|
||||
int(pos_long.get("PPSLongV", 1)) != 0 or
|
||||
int(quality.get("PPS2DAbsPosErrEstmtV", 1)) != 0 or
|
||||
int(quality.get("PPSMdV", 1)) != 0 or
|
||||
int(quality.get("PPSPstnDilPrcsV", 1)) != 0 or
|
||||
int(timestamp_values.get("PPSTmdayV", 1)) != 0 or
|
||||
int(timestamp_values.get("PPSCldrDayV", 1)) != 0 or
|
||||
int(timestamp_values.get("PPSCldrYrV", 1)) != 0):
|
||||
return None
|
||||
# Reject mode 6 (dead reckoning only without GNSS).
|
||||
if int(quality["PPSMd"]) == 6:
|
||||
return None
|
||||
|
||||
latitude = float(pos_lat["PPSLat"]) / 3_600_000.0
|
||||
longitude = float(pos_long["PPSLong"]) / 3_600_000.0
|
||||
if not (math.isfinite(latitude) and math.isfinite(longitude) and
|
||||
-90.0 <= latitude <= 90.0 and -180.0 <= longitude <= 180.0 and
|
||||
(latitude != 0.0 or longitude != 0.0)):
|
||||
return None
|
||||
|
||||
year = int(timestamp_values["PPSCldrYr"])
|
||||
day_of_year = int(timestamp_values["PPSCldrDay"])
|
||||
millis_of_day = int(timestamp_values["PPSTmday"])
|
||||
if not 2014 <= year <= 2141 or day_of_year < 1 or not 0 <= millis_of_day < 86_400_000:
|
||||
return None
|
||||
timestamp = datetime(year, 1, 1, tzinfo=UTC) + timedelta(days=day_of_year - 1, milliseconds=millis_of_day)
|
||||
if timestamp.year != year:
|
||||
return None
|
||||
|
||||
elev = values["PPS_ElevHdSpd_FO"]
|
||||
speed = float(elev["PPSVel"]) * CV.KPH_TO_MS
|
||||
if int(elev.get("PPSVelV", 1)) != 0 or not math.isfinite(speed) or not 0.0 <= speed <= 200.0:
|
||||
speed = 0.0
|
||||
heading = float(elev["PPSHedng"])
|
||||
if (int(elev.get("PPSHedngV", 1)) != 0 or
|
||||
not math.isfinite(heading) or not 0.0 <= heading < 360.0):
|
||||
heading = 0.0
|
||||
|
||||
altitude = float(elev["PPSElvtn"]) / 100.0
|
||||
if int(elev.get("PPSElvtnV", 1)) != 0 or not math.isfinite(altitude):
|
||||
altitude = 0.0
|
||||
|
||||
horizontal_accuracy = float(quality["PPS2DAbsPosErrEstmt"])
|
||||
if not math.isfinite(horizontal_accuracy) or horizontal_accuracy < 0.0:
|
||||
horizontal_accuracy = 0.0
|
||||
|
||||
vertical_accuracy = float(quality["PPS3DAbsPosErrEstmt"])
|
||||
if int(quality.get("PPS3DAbsPosErrEstmtV", 1)) != 0 or not math.isfinite(vertical_accuracy) or vertical_accuracy < 0.0:
|
||||
vertical_accuracy = 0.0
|
||||
|
||||
bearing_accuracy = float(quality["PPSAbsHdngErrEstmt"])
|
||||
if int(quality.get("PPSAbsHdngErrEstmtV", 1)) != 0 or not math.isfinite(bearing_accuracy) or bearing_accuracy < 0.0:
|
||||
bearing_accuracy = 180.0
|
||||
except (KeyError, TypeError, ValueError, OverflowError, AttributeError):
|
||||
return None
|
||||
|
||||
heading_rad = math.radians(heading)
|
||||
return {
|
||||
"timestamp_nanos": timestamp_nanos,
|
||||
"latitude": latitude,
|
||||
"longitude": longitude,
|
||||
"altitude": altitude,
|
||||
"speed": speed,
|
||||
"bearingDeg": heading,
|
||||
"horizontalAccuracy": horizontal_accuracy,
|
||||
"unixTimestampMillis": round(timestamp.timestamp() * 1000),
|
||||
"verticalAccuracy": vertical_accuracy,
|
||||
"bearingAccuracyDeg": bearing_accuracy,
|
||||
# Velocity error units are undocumented in DBC; omit conversion.
|
||||
"speedAccuracy": 0.0,
|
||||
"hasFix": True,
|
||||
"satelliteCount": 0,
|
||||
"vNED": [speed * math.cos(heading_rad), speed * math.sin(heading_rad), 0.0],
|
||||
}
|
||||
|
||||
|
||||
def get_hard_cruise_buttons(steering_button_msg: dict) -> int:
|
||||
return steering_button_msg.get("ACCButtonsHard", CruiseButtons.INIT)
|
||||
@@ -173,6 +68,24 @@ def update_auto_hold_drive_timers(in_drive_for_hold: bool, moving_for_hold: bool
|
||||
return auto_hold_drive_time, one_pedal_drive_time
|
||||
|
||||
|
||||
def update_startup_acc_fault_suppression(car_fingerprint: str, system_power_mode: int,
|
||||
previous_system_power_mode: int, timer: float,
|
||||
acc_state: int, friction_brake_unavailable: bool) -> tuple[float, bool]:
|
||||
if car_fingerprint != CAR.BUICK_LACROSSE:
|
||||
return 0.0, False
|
||||
|
||||
if system_power_mode == 2 and previous_system_power_mode != 2:
|
||||
timer = ACC_STARTUP_FAULT_GRACE_PERIOD_S
|
||||
elif system_power_mode != 2:
|
||||
timer = 0.0
|
||||
|
||||
if timer <= 0.0 or acc_state != AccState.FAULTED:
|
||||
return 0.0, False
|
||||
|
||||
timer = max(timer - DT_CTRL, 0.0)
|
||||
return timer, timer > 0.0 and not friction_brake_unavailable
|
||||
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP, FPCP):
|
||||
super().__init__(CP, FPCP)
|
||||
@@ -211,19 +124,17 @@ class CarState(CarStateBase):
|
||||
self.lkas_previously_enabled = 0
|
||||
self.lkas_enabled = 0
|
||||
self.pcm_acc_status = AccState.OFF
|
||||
self.system_power_mode = 0
|
||||
self.startup_acc_fault_suppression_timer = 0.0
|
||||
self.stock_fcw_alert = 0
|
||||
self.car_gps_config = get_car_gps_config(CP)
|
||||
self.car_gps_supported = self.car_gps_config is not None
|
||||
self.car_gps = None
|
||||
self.onstar_gps = None
|
||||
self._car_gps_timestamp_nanos = 0
|
||||
self._prev_gps_lat = None
|
||||
self._prev_gps_lon = None
|
||||
self._last_gps_bearing = None
|
||||
|
||||
self.pps_gps = None
|
||||
self._pps_gps_timestamp_nanos = 0
|
||||
|
||||
def _update_car_gps(self, cp, v_ego: float = 0.0) -> None:
|
||||
if self.car_gps_config is None:
|
||||
return
|
||||
@@ -258,47 +169,12 @@ class CarState(CarStateBase):
|
||||
else:
|
||||
self._prev_gps_lat = self._prev_gps_lon = None
|
||||
|
||||
self.onstar_gps = gps
|
||||
self.car_gps = gps
|
||||
self._car_gps_timestamp_nanos = timestamp_nanos
|
||||
|
||||
def _update_pps_gps(self, cp) -> None:
|
||||
"""Decode a complete, checksum-valid PPS burst when one is available."""
|
||||
timestamps = [max(cp.ts_nanos[name].values(), default=0) for name in PPS_GPS_MESSAGES]
|
||||
if not all(timestamps):
|
||||
return
|
||||
|
||||
timestamp_nanos = max(timestamps)
|
||||
if timestamp_nanos <= self._pps_gps_timestamp_nanos:
|
||||
return
|
||||
if timestamp_nanos - min(timestamps) > 100_000_000:
|
||||
return
|
||||
|
||||
vl = cp.vl
|
||||
try:
|
||||
first_id = int(vl["PPS_ElevHdSpd_FO"]["PPSElvHedngSpdBrstID"])
|
||||
if not (first_id == int(vl["PPS_PosLat_FO"]["PPSLatBrstID"]) ==
|
||||
int(vl["PPS_PosLong_FO"]["PPSLongBrstID"]) ==
|
||||
int(vl["PPS_Time_FO"]["PPSTmBrstID"]) ==
|
||||
int(vl["PPS_QualMetrics_FO"]["PPSPosQltyMtcBrstID"])):
|
||||
return
|
||||
except (KeyError, ValueError, TypeError, OverflowError):
|
||||
return
|
||||
|
||||
values = {name: cp.vl[name] for name in PPS_GPS_MESSAGES}
|
||||
raw = {name: cp.vl_raw[name] for name in PPS_GPS_MESSAGES}
|
||||
self.pps_gps = decode_gm_pps_gps(values, raw, timestamp_nanos)
|
||||
self._pps_gps_timestamp_nanos = timestamp_nanos
|
||||
|
||||
def get_car_gps(self) -> dict | None:
|
||||
def get_car_gps(self):
|
||||
return self.car_gps
|
||||
|
||||
def get_car_gps_sources(self) -> dict[str, dict | None]:
|
||||
return {
|
||||
"pps": self.pps_gps,
|
||||
"onstar": self.onstar_gps,
|
||||
}
|
||||
|
||||
def update_button_enable(self, buttonEvents: list[structs.CarState.ButtonEvent]):
|
||||
if not self.CP.pcmCruise:
|
||||
for b in buttonEvents:
|
||||
@@ -384,9 +260,6 @@ class CarState(CarStateBase):
|
||||
abs(pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"]) <= STANDSTILL_THRESHOLD
|
||||
|
||||
self._update_car_gps(pt_cp, ret.vEgo)
|
||||
pps_cp = can_parsers.get(Bus.adas)
|
||||
if pps_cp is not None:
|
||||
self._update_pps_gps(pps_cp)
|
||||
|
||||
if pt_cp.vl["ECMPRDNL2"]["ManualMode"] == 1:
|
||||
ret.gearShifter = self.parse_gear_shifter("T")
|
||||
@@ -498,8 +371,18 @@ class CarState(CarStateBase):
|
||||
|
||||
ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0
|
||||
ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1
|
||||
ret.accFaulted = (pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED or
|
||||
pt_cp.vl["EBCMFrictionBrakeStatus"]["FrictionBrakeUnavailable"] == 1)
|
||||
acc_state = pt_cp.vl["AcceleratorPedal2"]["CruiseState"]
|
||||
friction_brake_unavailable = pt_cp.vl["EBCMFrictionBrakeStatus"]["FrictionBrakeUnavailable"] == 1
|
||||
self.startup_acc_fault_suppression_timer, suppress_startup_acc_fault = update_startup_acc_fault_suppression(
|
||||
self.CP.carFingerprint,
|
||||
int(pt_cp.vl["BCMGeneralPlatformStatus"]["SystemPowerMode"]),
|
||||
self.system_power_mode,
|
||||
self.startup_acc_fault_suppression_timer,
|
||||
acc_state,
|
||||
friction_brake_unavailable,
|
||||
)
|
||||
self.system_power_mode = int(pt_cp.vl["BCMGeneralPlatformStatus"]["SystemPowerMode"])
|
||||
ret.accFaulted = (acc_state == AccState.FAULTED and not suppress_startup_acc_fault) or friction_brake_unavailable
|
||||
|
||||
ret.cruiseState.enabled = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] != AccState.OFF
|
||||
ret.cruiseState.standstill = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.STANDSTILL
|
||||
@@ -736,16 +619,8 @@ class CarState(CarStateBase):
|
||||
("ASCMLKASteeringCmd", 0),
|
||||
]
|
||||
|
||||
parsers = {
|
||||
return {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, CanBus.POWERTRAIN),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, CanBus.CAMERA),
|
||||
Bus.loopback: CANParser(DBC[CP.carFingerprint][Bus.pt], loopback_messages, CanBus.LOOPBACK),
|
||||
}
|
||||
if getattr(CP, "brand", None) == "gm":
|
||||
# Optional CT6 PPS parser on Bus.adas; non-PPS vehicles remain CAN-valid.
|
||||
parsers[Bus.adas] = CANParser(
|
||||
"cadillac_ct6_object",
|
||||
[(name, 0) for name in PPS_GPS_MESSAGES],
|
||||
CanBus.POWERTRAIN,
|
||||
)
|
||||
return parsers
|
||||
|
||||
@@ -31,6 +31,8 @@ BOLT_CC_DIRECTION_MEMORY_S = 1.5
|
||||
VOLT_CC_CARS = {
|
||||
CAR.CHEVROLET_VOLT_CC,
|
||||
}
|
||||
VOLT_CC_TARGET_DEADBAND_MPH = 5.0
|
||||
VOLT_CC_ACCEL_DEADBAND_MS2 = 0.15
|
||||
|
||||
|
||||
def malibu_phase_map_for_button(button):
|
||||
@@ -344,7 +346,15 @@ def _create_volt_cc_spam_command(CS, actuators, ms_convert):
|
||||
speed_setpoint = int(round(CS.out.cruiseState.speed * ms_convert))
|
||||
ego_speed = CS.out.vEgo * ms_convert
|
||||
|
||||
if accel == 0.0:
|
||||
v_cruise_kph = float(getattr(CS.out, "vCruise", 0.0))
|
||||
if 0.0 < v_cruise_kph < 255.0:
|
||||
is_metric = ms_convert == CV.MS_TO_KPH
|
||||
target_setpoint = v_cruise_kph if is_metric else v_cruise_kph * CV.KPH_TO_MPH
|
||||
target_deadband = VOLT_CC_TARGET_DEADBAND_MPH * (CV.MPH_TO_KPH if is_metric else 1.0)
|
||||
if abs(target_setpoint - speed_setpoint) <= target_deadband:
|
||||
return CruiseButtons.INIT, float("inf")
|
||||
|
||||
if abs(accel) <= VOLT_CC_ACCEL_DEADBAND_MS2:
|
||||
return CruiseButtons.INIT, float("inf")
|
||||
|
||||
if accel < 0.0:
|
||||
|
||||
@@ -15,6 +15,7 @@ from opendbc.car.gm.values import (
|
||||
CC_ONLY_CAR,
|
||||
CC_REGEN_PADDLE_CAR,
|
||||
EV_CAR,
|
||||
GM_AUTO_HOLD_CARS,
|
||||
SDGM_CAR,
|
||||
CarControllerParams,
|
||||
CanBus,
|
||||
@@ -408,7 +409,7 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet
|
||||
|
||||
ret.steerLimitTimer = 0.4
|
||||
ret.radarTimeStepDEPRECATED = 0.0667 # GM radar runs at 15Hz instead of the standard 20Hz
|
||||
ret.radarTimeStepDEPRECATED = 0.15 if candidate == CAR.BUICK_LACROSSE else 0.0667
|
||||
ret.longitudinalActuatorDelay = 0.5 # large delay to initially start braking
|
||||
|
||||
if candidate in (
|
||||
@@ -440,7 +441,7 @@ class CarInterface(CarInterfaceBase):
|
||||
elif candidate in (CAR.BUICK_LACROSSE, CAR.BUICK_LACROSSE_ASCM, CAR.BUICK_LACROSSE_ASCM_19US):
|
||||
CarInterfaceBase.configure_torque_tune(CAR.BUICK_LACROSSE, ret.lateralTuning)
|
||||
if candidate == CAR.BUICK_LACROSSE_ASCM_19US:
|
||||
ret.minSteerSpeed = 27 * CV.MPH_TO_MS
|
||||
ret.minSteerSpeed = 28 * CV.MPH_TO_MS
|
||||
|
||||
elif candidate == CAR.CADILLAC_ESCALADE:
|
||||
ret.minEnableSpeed = -1. # engage speed is decided by pcm
|
||||
@@ -710,18 +711,19 @@ class CarInterface(CarInterfaceBase):
|
||||
if remote_start_boots_comma:
|
||||
ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.FLAG_GM_REMOTE_START_BOOTS_COMMA.value
|
||||
|
||||
volt_stock_friction_brake_safety = (
|
||||
gm_stock_friction_brake_safety = (
|
||||
ret.openpilotLongitudinalControl and
|
||||
(gm_auto_hold or volt_one_pedal_mode) and
|
||||
candidate in {
|
||||
CAR.CHEVROLET_VOLT,
|
||||
CAR.CHEVROLET_VOLT_2019,
|
||||
CAR.CHEVROLET_VOLT_ASCM,
|
||||
CAR.CHEVROLET_VOLT_CAMERA,
|
||||
}
|
||||
(
|
||||
(gm_auto_hold and candidate in GM_AUTO_HOLD_CARS) or
|
||||
(volt_one_pedal_mode and candidate in {
|
||||
CAR.CHEVROLET_VOLT,
|
||||
CAR.CHEVROLET_VOLT_2019,
|
||||
CAR.CHEVROLET_VOLT_ASCM,
|
||||
CAR.CHEVROLET_VOLT_CAMERA,
|
||||
})
|
||||
)
|
||||
)
|
||||
if volt_stock_friction_brake_safety:
|
||||
# Reuse the paddle-scheduler safety bit as a Volt stock friction-brake
|
||||
if gm_stock_friction_brake_safety:
|
||||
# marker on non-pedal paths. Auto hold and one-pedal can run while OP
|
||||
# longitudinal is configured but not currently active, so the bit must
|
||||
# be present regardless of the current long-control mode. Do not expose
|
||||
|
||||
@@ -431,6 +431,15 @@ def test_volt_auto_hold_requires_toggle_supported_non_cc_only_volt_and_stock_saf
|
||||
),
|
||||
True,
|
||||
)
|
||||
assert supports_volt_auto_hold(
|
||||
SimpleNamespace(
|
||||
carFingerprint=CAR.BUICK_LACROSSE,
|
||||
openpilotLongitudinalControl=True,
|
||||
networkLocation=CarParams.NetworkLocation.gateway,
|
||||
safetyConfigs=stock_safety,
|
||||
),
|
||||
True,
|
||||
)
|
||||
assert not supports_volt_auto_hold(
|
||||
SimpleNamespace(
|
||||
carFingerprint=CAR.CHEVROLET_VOLT,
|
||||
|
||||
@@ -1,6 +1,5 @@
|
||||
import pytest
|
||||
import numpy as np
|
||||
from datetime import UTC, datetime
|
||||
from types import SimpleNamespace
|
||||
from parameterized import parameterized
|
||||
|
||||
@@ -11,11 +10,9 @@ from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.gm import gmcan
|
||||
from opendbc.car.gm.carstate import (
|
||||
CarState as GMCarState,
|
||||
PPS_GPS_MESSAGES,
|
||||
decode_gm_pps_gps,
|
||||
get_hard_cruise_buttons,
|
||||
pps_checksum_ok,
|
||||
update_auto_hold_drive_timers,
|
||||
update_startup_acc_fault_suppression,
|
||||
)
|
||||
from opendbc.car.gm.carcontroller import (
|
||||
VisualAlert,
|
||||
@@ -103,146 +100,6 @@ class TestBoltGps:
|
||||
assert gps["verticalAccuracy"] == 10.0
|
||||
assert gps["speedAccuracy"] == 0.5
|
||||
|
||||
|
||||
class TestPpsGps:
|
||||
_frames = [
|
||||
(0x260, bytes.fromhex("10ddac000d831277"), 0),
|
||||
(0x261, bytes.fromhex("08386fce09ca"), 0),
|
||||
(0x262, bytes.fromhex("6d98820341de"), 0),
|
||||
(0x264, bytes.fromhex("0018f90578b5eaac"), 0),
|
||||
(0x265, bytes.fromhex("1a0000800a0258fd"), 0),
|
||||
]
|
||||
|
||||
def test_observed_bundle_checksum_and_conversion(self):
|
||||
parser = CANParser("cadillac_ct6_object", [(name, 0) for name in PPS_GPS_MESSAGES], 0)
|
||||
parser.update([(1_000_000_000, self._frames)])
|
||||
|
||||
assert all(pps_checksum_ok(parser.vl_raw[name]) for name in PPS_GPS_MESSAGES)
|
||||
gps = decode_gm_pps_gps(
|
||||
{name: parser.vl[name] for name in PPS_GPS_MESSAGES},
|
||||
{name: parser.vl_raw[name] for name in PPS_GPS_MESSAGES},
|
||||
1_000_000_000,
|
||||
)
|
||||
assert gps is not None
|
||||
assert gps["hasFix"]
|
||||
assert gps["latitude"] == pytest.approx(38.3101, abs=1e-4)
|
||||
assert gps["longitude"] == pytest.approx(-85.7701, abs=1e-4)
|
||||
assert gps["altitude"] == pytest.approx(106.9)
|
||||
assert gps["bearingDeg"] == pytest.approx(56.748)
|
||||
assert gps["horizontalAccuracy"] == pytest.approx(1.0)
|
||||
assert gps["unixTimestampMillis"] == 1788655668655
|
||||
|
||||
@pytest.fixture
|
||||
def bundle(self):
|
||||
parser = CANParser("cadillac_ct6_object", [(name, 0) for name in PPS_GPS_MESSAGES], 0)
|
||||
parser.update([(1_000_000_000, self._frames)])
|
||||
return ({name: dict(parser.vl[name]) for name in PPS_GPS_MESSAGES},
|
||||
{name: parser.vl_raw[name] for name in PPS_GPS_MESSAGES})
|
||||
|
||||
@pytest.mark.parametrize("year,day,date", [
|
||||
(2025, 1, "2025-01-01"), (2025, 365, "2025-12-31"), (2025, 366, None),
|
||||
(2024, 366, "2024-12-31"), (2024, 367, None), (2025, 0, None),
|
||||
])
|
||||
def test_one_based_day_of_year(self, bundle, year, day, date):
|
||||
values, raw = bundle
|
||||
values["PPS_Time_FO"].update(PPSCldrYr=year, PPSCldrDay=day, PPSTmday=1234)
|
||||
gps = decode_gm_pps_gps(values, raw, 1_000_000_000)
|
||||
if date is None:
|
||||
assert gps is None
|
||||
else:
|
||||
expected = int(datetime.fromisoformat(date).replace(tzinfo=UTC).timestamp() * 1000) + 1234
|
||||
assert gps["unixTimestampMillis"] == expected
|
||||
|
||||
@pytest.mark.parametrize("bad_data", [b"", b"\x01"])
|
||||
def test_checksum_short_input(self, bad_data):
|
||||
assert not pps_checksum_ok(bad_data)
|
||||
|
||||
@pytest.mark.parametrize("lat,lon,valid", [(0.0, 10.0 * 3_600_000, True), (10.0 * 3_600_000, 0.0, True), (0.0, 0.0, False)])
|
||||
def test_coordinate_axes(self, bundle, lat, lon, valid):
|
||||
values, raw = bundle
|
||||
values["PPS_PosLat_FO"]["PPSLat"] = lat
|
||||
values["PPS_PosLong_FO"]["PPSLong"] = lon
|
||||
gps = decode_gm_pps_gps(values, raw, 1_000_000_000)
|
||||
if valid:
|
||||
assert gps is not None
|
||||
assert gps["hasFix"]
|
||||
else:
|
||||
assert gps is None
|
||||
|
||||
def test_get_car_gps_sources_shape(self):
|
||||
cs = GMCarState.__new__(GMCarState)
|
||||
cs.pps_gps = {"hasFix": True}
|
||||
cs.onstar_gps = None
|
||||
sources = cs.get_car_gps_sources()
|
||||
assert sources == {"pps": {"hasFix": True}, "onstar": None}
|
||||
|
||||
@pytest.mark.parametrize("message,signal,value", [
|
||||
("PPS_PosLat_FO", "PPSLatV", 1),
|
||||
("PPS_PosLong_FO", "PPSLongV", 1),
|
||||
("PPS_QualMetrics_FO", "PPS2DAbsPosErrEstmtV", 1),
|
||||
("PPS_PosLat_FO", "PPSLat", float("nan")),
|
||||
("PPS_PosLong_FO", "PPSLong", 181 * 3_600_000),
|
||||
("PPS_QualMetrics_FO", "PPSMd", 6),
|
||||
("PPS_Time_FO", "PPSTmdayV", 1),
|
||||
])
|
||||
def test_unusable_position_rejected(self, bundle, message, signal, value):
|
||||
values, raw = bundle
|
||||
values[message][signal] = value
|
||||
assert decode_gm_pps_gps(values, raw, 1_000_000_000) is None
|
||||
|
||||
@pytest.mark.parametrize("invalidity", ["checksum", "position-validity"])
|
||||
def test_burst_cache_and_explicit_invalidation(self, invalidity):
|
||||
parser = CANParser("cadillac_ct6_object", [(name, 0) for name in PPS_GPS_MESSAGES], 0)
|
||||
cs = GMCarState.__new__(GMCarState)
|
||||
cs.pps_gps = None
|
||||
cs._pps_gps_timestamp_nanos = 0
|
||||
parser.update([(1_000_000_000, self._frames[:-1])])
|
||||
cs._update_pps_gps(parser)
|
||||
assert cs.pps_gps is None # Incomplete startup burst.
|
||||
parser.update([(1_000_000_000, self._frames[-1:])])
|
||||
cs._update_pps_gps(parser)
|
||||
good = cs.pps_gps
|
||||
assert good is not None
|
||||
# No complete new burst: keep its original timestamp for freshness.
|
||||
parser.update([(2_000_000_000, self._frames[:1])])
|
||||
cs._update_pps_gps(parser)
|
||||
assert cs.pps_gps is good
|
||||
parser.update([(2_100_000_000, self._frames)])
|
||||
parser.vl["PPS_PosLat_FO"]["PPSLatBrstID"] = int(parser.vl["PPS_PosLat_FO"]["PPSLatBrstID"]) ^ 1
|
||||
cs._update_pps_gps(parser)
|
||||
assert cs.pps_gps is good # A mismatched burst must not refresh the fix.
|
||||
bad_frames = ([(addr, data[:-1] + bytes([data[-1] ^ 1]), bus) for addr, data, bus in self._frames]
|
||||
if invalidity == "checksum" else self._frames)
|
||||
parser.update([(3_000_000_000, bad_frames)])
|
||||
if invalidity == "position-validity":
|
||||
parser.vl["PPS_PosLat_FO"]["PPSLatV"] = 1
|
||||
cs._update_pps_gps(parser)
|
||||
assert cs.pps_gps is None
|
||||
parser.update([(4_000_000_000, self._frames)])
|
||||
cs._update_pps_gps(parser)
|
||||
assert cs.pps_gps is not None
|
||||
|
||||
@pytest.mark.parametrize("speed_bad,heading_bad,elevation_bad,vertical_bad,bearing_bad", [
|
||||
(0, 0, 0, 0, 0), (1, 0, 0, 0, 0), (0, 1, 0, 0, 0), (0, 0, 1, 0, 0),
|
||||
(0, 0, 0, 1, 0), (0, 0, 0, 0, 1), (1, 1, 1, 1, 1),
|
||||
], ids=["valid", "speed", "heading", "elevation", "vertical-accuracy", "bearing-accuracy", "all-invalid"])
|
||||
def test_optional_field_fallbacks(self, bundle, speed_bad, heading_bad, elevation_bad, vertical_bad, bearing_bad):
|
||||
values, raw = bundle
|
||||
values["PPS_ElevHdSpd_FO"].update(PPSVel=36, PPSVelV=speed_bad, PPSHedng=90, PPSHedngV=heading_bad,
|
||||
PPSElvtn=12345, PPSElvtnV=elevation_bad)
|
||||
values["PPS_QualMetrics_FO"].update(PPS2DAbsPosErrEstmt=3.2, PPS3DAbsPosErrEstmt=4.5, PPS3DAbsPosErrEstmtV=vertical_bad,
|
||||
PPSAbsHdngErrEstmt=6.0, PPSAbsHdngErrEstmtV=bearing_bad)
|
||||
gps = decode_gm_pps_gps(values, raw, 1_000_000_000)
|
||||
assert gps is not None
|
||||
assert gps["hasFix"]
|
||||
assert gps["speed"] == pytest.approx(0.0 if speed_bad else 10.0)
|
||||
assert gps["bearingDeg"] == (0 if heading_bad else 90)
|
||||
assert gps["altitude"] == pytest.approx(0.0 if elevation_bad else 123.45)
|
||||
assert gps["vNED"] == pytest.approx([gps["speed"], 0, 0] if heading_bad else [0, gps["speed"], 0])
|
||||
assert gps["horizontalAccuracy"] == pytest.approx(3.2)
|
||||
assert gps["verticalAccuracy"] == pytest.approx(0.0 if vertical_bad else 4.5)
|
||||
assert gps["bearingAccuracyDeg"] == pytest.approx(180.0 if bearing_bad else 6.0)
|
||||
|
||||
def test_bolt_gps_heading_and_speed_derivation(self):
|
||||
cp = SimpleNamespace(
|
||||
brand="gm",
|
||||
@@ -352,7 +209,78 @@ class TestPpsGps:
|
||||
assert all(message in parsers[Bus.pt].vl for message in CHEVROLET_BOLT_GPS_MESSAGES)
|
||||
|
||||
|
||||
class TestGMCarState:
|
||||
def test_lacrosse_startup_acc_fault_is_suppressed(self):
|
||||
timer, suppressed = update_startup_acc_fault_suppression(
|
||||
CAR.BUICK_LACROSSE, 2, 0, 0.0, 3, False,
|
||||
)
|
||||
|
||||
assert suppressed
|
||||
assert timer == pytest.approx(5.0 - DT_CTRL)
|
||||
|
||||
timer, suppressed = update_startup_acc_fault_suppression(
|
||||
CAR.BUICK_LACROSSE, 2, 2, timer, 0, False,
|
||||
)
|
||||
|
||||
assert timer == 0.0
|
||||
assert not suppressed
|
||||
|
||||
def test_lacrosse_persistent_acc_fault_is_reported_after_startup(self):
|
||||
timer, suppressed = update_startup_acc_fault_suppression(
|
||||
CAR.BUICK_LACROSSE, 2, 0, 0.0, 3, False,
|
||||
)
|
||||
|
||||
for _ in range(int(5.0 / DT_CTRL)):
|
||||
timer, suppressed = update_startup_acc_fault_suppression(
|
||||
CAR.BUICK_LACROSSE, 2, 2, timer, 3, False,
|
||||
)
|
||||
|
||||
assert timer == 0.0
|
||||
assert not suppressed
|
||||
|
||||
def test_lacrosse_brake_unavailable_fault_is_never_suppressed(self):
|
||||
_, suppressed = update_startup_acc_fault_suppression(
|
||||
CAR.BUICK_LACROSSE, 2, 0, 0.0, 3, True,
|
||||
)
|
||||
|
||||
assert not suppressed
|
||||
|
||||
def test_startup_acc_fault_suppression_is_scoped_to_lacrosse(self):
|
||||
_, suppressed = update_startup_acc_fault_suppression(
|
||||
CAR.BUICK_REGAL, 2, 0, 0.0, 3, False,
|
||||
)
|
||||
|
||||
assert not suppressed
|
||||
|
||||
|
||||
class TestGMInterface:
|
||||
def test_lacrosse_obd_and_ascm_integrations_remain_separate(self):
|
||||
obd_params = interfaces[CAR.BUICK_LACROSSE].get_params(
|
||||
CAR.BUICK_LACROSSE,
|
||||
_empty_fingerprint(),
|
||||
[],
|
||||
alpha_long=False,
|
||||
is_release=False,
|
||||
docs=False,
|
||||
starpilot_toggles=_test_starpilot_toggles(),
|
||||
)
|
||||
ascm_params = interfaces[CAR.BUICK_LACROSSE_ASCM].get_params(
|
||||
CAR.BUICK_LACROSSE_ASCM,
|
||||
_empty_fingerprint(),
|
||||
[],
|
||||
alpha_long=False,
|
||||
is_release=False,
|
||||
docs=False,
|
||||
starpilot_toggles=_test_starpilot_toggles(),
|
||||
)
|
||||
|
||||
assert obd_params.networkLocation == structs.CarParams.NetworkLocation.gateway
|
||||
assert obd_params.openpilotLongitudinalControl
|
||||
assert obd_params.radarTimeStepDEPRECATED == pytest.approx(0.15)
|
||||
assert ascm_params.networkLocation == structs.CarParams.NetworkLocation.fwdCamera
|
||||
assert not ascm_params.openpilotLongitudinalControl
|
||||
assert ascm_params.radarTimeStepDEPRECATED == pytest.approx(0.0667)
|
||||
|
||||
@parameterized.expand([
|
||||
CAR.CHEVROLET_BOLT_CC_2017,
|
||||
CAR.CHEVROLET_BOLT_CC_2018_2021,
|
||||
@@ -439,6 +367,14 @@ class TestGMInterface:
|
||||
|
||||
assert car_params.minSteerSpeed == pytest.approx(7 * CV.MPH_TO_MS)
|
||||
|
||||
def test_lacrosse_2019_ascm_min_steer_speed_is_28_mph(self):
|
||||
car_model = CAR.BUICK_LACROSSE_ASCM_19US
|
||||
CarInterface = interfaces[car_model]
|
||||
car_params = CarInterface.get_params(car_model, _empty_fingerprint(), [], alpha_long=False, is_release=False, docs=False,
|
||||
starpilot_toggles=_test_starpilot_toggles())
|
||||
|
||||
assert car_params.minSteerSpeed == pytest.approx(28 * CV.MPH_TO_MS)
|
||||
|
||||
@parameterized.expand([
|
||||
("interceptor", True),
|
||||
("ascm_int", False),
|
||||
@@ -634,6 +570,43 @@ class TestGMInterface:
|
||||
assert car_params.openpilotLongitudinalControl
|
||||
assert car_params.safetyConfigs[0].safetyParam & GMSafetyFlags.FLAG_GM_PANDA_PADDLE_SCHED.value
|
||||
|
||||
def test_buick_lacrosse_auto_hold_sets_stock_hold_safety_bit_with_op_long_enabled(self):
|
||||
params = Params()
|
||||
try:
|
||||
params.put_bool("GMAutoHold", True)
|
||||
car_params = interfaces[CAR.BUICK_LACROSSE].get_params(
|
||||
CAR.BUICK_LACROSSE,
|
||||
_empty_fingerprint(),
|
||||
[],
|
||||
alpha_long=False,
|
||||
is_release=False,
|
||||
docs=False,
|
||||
starpilot_toggles=_test_starpilot_toggles(),
|
||||
)
|
||||
finally:
|
||||
params.remove("GMAutoHold")
|
||||
|
||||
assert car_params.openpilotLongitudinalControl
|
||||
assert car_params.safetyConfigs[0].safetyParam & GMSafetyFlags.FLAG_GM_PANDA_PADDLE_SCHED.value
|
||||
|
||||
def test_buick_lacrosse_auto_hold_is_off_when_toggle_is_disabled(self):
|
||||
params = Params()
|
||||
try:
|
||||
params.put_bool("GMAutoHold", False)
|
||||
car_params = interfaces[CAR.BUICK_LACROSSE].get_params(
|
||||
CAR.BUICK_LACROSSE,
|
||||
_empty_fingerprint(),
|
||||
[],
|
||||
alpha_long=False,
|
||||
is_release=False,
|
||||
docs=False,
|
||||
starpilot_toggles=_test_starpilot_toggles(),
|
||||
)
|
||||
finally:
|
||||
params.remove("GMAutoHold")
|
||||
|
||||
assert not car_params.safetyConfigs[0].safetyParam & GMSafetyFlags.FLAG_GM_PANDA_PADDLE_SCHED.value
|
||||
|
||||
def test_volt_auto_hold_does_not_set_stock_hold_safety_bit_with_op_long_disabled(self):
|
||||
CarInterface = interfaces[CAR.CHEVROLET_VOLT_ASCM]
|
||||
fingerprint = _empty_fingerprint()
|
||||
@@ -960,6 +933,63 @@ class TestGMCarController:
|
||||
|
||||
assert len(msgs) == 1
|
||||
|
||||
def test_volt_cc_redneck_holds_when_stock_setpoint_is_within_target_deadband(self):
|
||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||
controller = SimpleNamespace(frame=int(2.0 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||
cs = SimpleNamespace(
|
||||
CP=SimpleNamespace(
|
||||
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
||||
flags=GMFlags.NO_CAMERA.value,
|
||||
networkLocation=structs.CarParams.NetworkLocation.gateway,
|
||||
minEnableSpeed=0.0,
|
||||
),
|
||||
buttons_counter=2,
|
||||
out=SimpleNamespace(
|
||||
vEgo=100.0 * CV.KPH_TO_MS,
|
||||
cruiseState=SimpleNamespace(speed=99.0 * CV.KPH_TO_MS),
|
||||
vCruise=100.0,
|
||||
),
|
||||
)
|
||||
|
||||
msgs = gmcan.create_gm_cc_spam_command(
|
||||
packer, controller, cs, SimpleNamespace(accel=0.5), SimpleNamespace(is_metric=True),
|
||||
)
|
||||
|
||||
assert msgs == []
|
||||
assert controller.apply_speed == 99
|
||||
|
||||
def test_volt_cc_redneck_catches_up_when_target_exceeds_deadband(self):
|
||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||
controller = SimpleNamespace(frame=int(0.5 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||
cs = SimpleNamespace(
|
||||
CP=SimpleNamespace(
|
||||
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
||||
flags=GMFlags.NO_CAMERA.value,
|
||||
networkLocation=structs.CarParams.NetworkLocation.gateway,
|
||||
minEnableSpeed=0.0,
|
||||
),
|
||||
buttons_counter=2,
|
||||
out=SimpleNamespace(
|
||||
vEgo=90.0 * CV.KPH_TO_MS,
|
||||
cruiseState=SimpleNamespace(speed=90.0 * CV.KPH_TO_MS),
|
||||
vCruise=100.0,
|
||||
),
|
||||
)
|
||||
|
||||
msgs = gmcan.create_gm_cc_spam_command(
|
||||
packer, controller, cs, SimpleNamespace(accel=0.5), SimpleNamespace(is_metric=True),
|
||||
)
|
||||
|
||||
assert msgs == []
|
||||
|
||||
controller.frame = int(0.7 / DT_CTRL)
|
||||
msgs = gmcan.create_gm_cc_spam_command(
|
||||
packer, controller, cs, SimpleNamespace(accel=0.5), SimpleNamespace(is_metric=True),
|
||||
)
|
||||
|
||||
assert len(msgs) == 1
|
||||
assert controller.apply_speed == 91
|
||||
|
||||
def test_volt_cc_no_camera_redneck_spam_stays_on_powertrain_bus(self):
|
||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||
controller = SimpleNamespace(frame=int(0.3 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||
|
||||
@@ -533,6 +533,14 @@ EV_CAR = {
|
||||
CAR.CHEVROLET_MALIBU_HYBRID_CC,
|
||||
}
|
||||
|
||||
GM_AUTO_HOLD_CARS = {
|
||||
CAR.CHEVROLET_VOLT,
|
||||
CAR.CHEVROLET_VOLT_2019,
|
||||
CAR.CHEVROLET_VOLT_ASCM,
|
||||
CAR.CHEVROLET_VOLT_CAMERA,
|
||||
CAR.BUICK_LACROSSE,
|
||||
}
|
||||
|
||||
# We're integrated at the camera with VOACC on these cars (instead of ASCM w/ OBD-II harness)
|
||||
CAMERA_ACC_CAR = {
|
||||
CAR.CHEVROLET_BOLT_ACC_2022_2023,
|
||||
|
||||
@@ -6,10 +6,8 @@ from collections.abc import Callable, Mapping
|
||||
from typing import Any
|
||||
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.can.dbc import DBC as DBC_FILE
|
||||
from opendbc.car import Bus
|
||||
from opendbc.car.ford.values import CAR as FORD_CAR
|
||||
from opendbc.car.gm.values import CAR as GM_CAR, DBC as GM_DBC
|
||||
from opendbc.car.gm.values import CAR as GM_CAR
|
||||
|
||||
|
||||
CarGpsSample = dict[str, Any]
|
||||
@@ -160,25 +158,8 @@ CAR_GPS_CONFIGS: dict[str, CarGpsConfig] = {
|
||||
|
||||
|
||||
def get_car_gps_config(CP) -> CarGpsConfig | None:
|
||||
cp_brand = getattr(CP, "brand", None)
|
||||
config = CAR_GPS_CONFIGS.get(CP.carFingerprint)
|
||||
if config is not None and config.brand == cp_brand:
|
||||
return config
|
||||
|
||||
# Enable OnStar GPS for GM cars whose powertrain DBC defines it.
|
||||
if cp_brand == "gm":
|
||||
try:
|
||||
dbc_name = GM_DBC[CP.carFingerprint][Bus.pt]
|
||||
if "TCICOnStarGPSPosition" in DBC_FILE(dbc_name).name_to_msg:
|
||||
return CarGpsConfig(
|
||||
brand="gm",
|
||||
messages=CHEVROLET_BOLT_GPS_MESSAGES,
|
||||
decoder=parse_chevrolet_bolt_can_gps,
|
||||
)
|
||||
except (KeyError, OSError, TypeError, RuntimeError):
|
||||
pass
|
||||
|
||||
return None
|
||||
return config if config is not None and config.brand == CP.brand else None
|
||||
|
||||
|
||||
def car_gps_available(CP) -> bool:
|
||||
|
||||
@@ -11,7 +11,7 @@ from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.hyundai import hyundaicanfd, hyundaican
|
||||
from opendbc.car.hyundai.hyundaicanfd import CanBus
|
||||
from opendbc.car.hyundai.values import HyundaiFlags, HyundaiSafetyFlags, HyundaiStarPilotFlags, Buttons, CarControllerParams, CAR, CANFD_ANGLE_LONGITUDINAL_CAR, \
|
||||
CANFD_RADAR_LIVE_LONGITUDINAL_CAR, CANFD_ALT_BUTTONS_RESUME_CAR, kia_ev6_gt_line_longitudinal_tuning, \
|
||||
CANFD_RADAR_ECU_KEEPALIVE_CAR, CANFD_ALT_BUTTONS_RESUME_CAR, kia_ev6_gt_line_longitudinal_tuning, \
|
||||
KIA_EV6_GT_LINE_LONG_TUNING_TESTING_GROUND_ID
|
||||
from opendbc.car.interfaces import CarControllerBase
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
@@ -860,7 +860,13 @@ class CarController(CarControllerBase):
|
||||
|
||||
lka_steering = self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING
|
||||
longitudinal_active = bool(self.long_active_ecu and getattr(CC, "longActive", False))
|
||||
lfa_longitudinal_active = longitudinal_active if self.CP.carFingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN else self.CP.openpilotLongitudinalControl
|
||||
lfa_status_cars = (
|
||||
CAR.HYUNDAI_IONIQ_6,
|
||||
CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN,
|
||||
CAR.KIA_EV6,
|
||||
)
|
||||
lfa_longitudinal_active = self.CP.openpilotLongitudinalControl \
|
||||
if self.CP.carFingerprint in lfa_status_cars else longitudinal_active
|
||||
lka_steering_long = lka_steering and lfa_longitudinal_active
|
||||
ccnc_non_hda2 = self.CP.flags & HyundaiFlags.CCNC and not lka_steering
|
||||
use_egmp_dynamic_long_tuning = egmp_dynamic_longitudinal_tuning(self.CP) and self.long_active_ecu and \
|
||||
@@ -992,7 +998,7 @@ class CarController(CarControllerBase):
|
||||
# The front radar treats ADAS_DRV's 0x100 broadcast as its host heartbeat
|
||||
# and stops publishing object tracks when it disappears.
|
||||
radar_heartbeat_step = 1 if ccnc_angle_long else 4
|
||||
if self.CP.carFingerprint in CANFD_RADAR_LIVE_LONGITUDINAL_CAR and self.frame % radar_heartbeat_step == 0:
|
||||
if self.CP.carFingerprint in CANFD_RADAR_ECU_KEEPALIVE_CAR and self.frame % radar_heartbeat_step == 0:
|
||||
can_sends.append(hyundaicanfd.create_accelerator_brake_alt_spoof(0, self.frame // radar_heartbeat_step,
|
||||
CS.out.brakePressed, CS.out.gasPressed,
|
||||
self.CP.carFingerprint))
|
||||
|
||||
@@ -8,6 +8,7 @@ from opendbc.car.hyundai.values import HyundaiFlags, CAR, CarControllerParams, \
|
||||
CANFD_SECURITYACCESS_CAR, \
|
||||
CANFD_ANGLE_LONGITUDINAL_CAR, \
|
||||
CANFD_RADAR_LIVE_LONGITUDINAL_CAR, \
|
||||
CANFD_RADAR_ECU_KEEPALIVE_CAR, \
|
||||
RADAR_LIVE_LONGITUDINAL_CAR, \
|
||||
UNSUPPORTED_LONGITUDINAL_CAR, HyundaiSafetyFlags, \
|
||||
LEGACY_LONGITUDINAL_CAR, \
|
||||
@@ -27,6 +28,15 @@ from openpilot.starpilot.common.testing_grounds import testing_ground
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
Ecu = structs.CarParams.Ecu
|
||||
|
||||
|
||||
def get_communication_control_request(car_fingerprint):
|
||||
if car_fingerprint in CANFD_RADAR_ECU_KEEPALIVE_CAR:
|
||||
return bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX,
|
||||
uds.MESSAGE_TYPE.NORMAL])
|
||||
|
||||
return bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, 0x80 | uds.CONTROL_TYPE.DISABLE_RX_DISABLE_TX,
|
||||
uds.MESSAGE_TYPE.NORMAL])
|
||||
|
||||
# Cancel button can sometimes be ACC pause/resume button, main button can also enable on some cars
|
||||
ENABLE_BUTTONS = (ButtonType.accelCruise, ButtonType.decelCruise, ButtonType.cancel, ButtonType.mainCruise)
|
||||
|
||||
@@ -353,14 +363,7 @@ class CarInterface(CarInterfaceBase):
|
||||
params = Params()
|
||||
|
||||
if communication_control is None:
|
||||
if CP.carFingerprint in CANFD_RADAR_LIVE_LONGITUDINAL_CAR:
|
||||
# Don't use 0x80 suppress bit so we can read the ECU response.
|
||||
# Use ENABLE_RX_DISABLE_TX (0x01) so the ECU can still receive from rear radars for BSM
|
||||
# while blocking SCC TX.
|
||||
communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX, uds.MESSAGE_TYPE.NORMAL])
|
||||
else:
|
||||
# 0x80 silences response for other cars (original behavior)
|
||||
communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, 0x80 | uds.CONTROL_TYPE.DISABLE_RX_DISABLE_TX, uds.MESSAGE_TYPE.NORMAL])
|
||||
communication_control = get_communication_control_request(CP.carFingerprint)
|
||||
|
||||
ecu_log(f"=== init() called: opLong={CP.openpilotLongitudinalControl}, flags=0x{CP.flags:x}, safetyParam={CP.safetyConfigs[-1].safetyParam} ===")
|
||||
|
||||
|
||||
@@ -19,6 +19,9 @@ MRR30_RADAR_START_ADDR = 0x210
|
||||
MRR30_RADAR_MSG_COUNT = 16
|
||||
MRR35_RADAR_START_ADDR = 0x3A5
|
||||
MRR35_RADAR_MSG_COUNT = 32
|
||||
GV70_RADAR_START_ADDR = 0x210
|
||||
GV70_RADAR_MSG_COUNT = 16
|
||||
GV70_RADAR_DBC = "hyundai_radar_210_21f_generated"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
@@ -30,6 +33,7 @@ class RadarTrackConfig:
|
||||
frequency: int = 50
|
||||
parser_msg_count: int | None = None
|
||||
expected_length: int | None = None
|
||||
dbc_name: str | None = None
|
||||
|
||||
@property
|
||||
def can_parser_msg_count(self) -> int:
|
||||
@@ -47,6 +51,10 @@ RADAR_TRACK_CONFIGS = {
|
||||
|
||||
|
||||
def get_radar_track_config(car_fingerprint, flags: int = 0) -> RadarTrackConfig | None:
|
||||
if car_fingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN:
|
||||
return RadarTrackConfig(GV70_RADAR_START_ADDR, GV70_RADAR_MSG_COUNT, "gv70_210", bus=0,
|
||||
frequency=20, expected_length=32, dbc_name=GV70_RADAR_DBC)
|
||||
|
||||
radar_dbc = DBC[car_fingerprint].get(Bus.radar)
|
||||
if car_fingerprint == CAR.GENESIS_G90 and radar_dbc == HYUNDAI_MANDO_FRONT_RADAR_DBC:
|
||||
return RadarTrackConfig(RADAR_START_ADDR, G90_RADAR_MSG_COUNT, "mando", parser_msg_count=RADAR_MSG_COUNT)
|
||||
@@ -65,6 +73,10 @@ def radar_tracks_available(radar_config: RadarTrackConfig | None, fingerprint) -
|
||||
if radar_config is None:
|
||||
return False
|
||||
|
||||
if radar_config.radar_type == "gv70_210":
|
||||
return all(fingerprint[radar_config.bus].get(addr) == radar_config.expected_length
|
||||
for addr in range(radar_config.start_addr, radar_config.start_addr + radar_config.msg_count))
|
||||
|
||||
msg_len = fingerprint[radar_config.bus].get(radar_config.start_addr)
|
||||
if msg_len is None:
|
||||
return False
|
||||
@@ -78,7 +90,8 @@ def get_radar_can_parser(CP, radar_config):
|
||||
|
||||
messages = [(f"RADAR_TRACK_{addr:x}", radar_config.frequency)
|
||||
for addr in range(radar_config.start_addr, radar_config.start_addr + radar_config.can_parser_msg_count)]
|
||||
return CANParser(DBC[CP.carFingerprint][Bus.radar], messages, radar_config.bus)
|
||||
dbc_name = radar_config.dbc_name or DBC[CP.carFingerprint][Bus.radar]
|
||||
return CANParser(dbc_name, messages, radar_config.bus)
|
||||
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
@@ -223,6 +236,27 @@ class RadarInterface(RadarInterfaceBase):
|
||||
del self.pts[track_key]
|
||||
continue
|
||||
|
||||
if radar_type == "gv70_210":
|
||||
for i in ("1", "2"):
|
||||
track_key = addr * 2 + int(i) - 1
|
||||
valid = msg[f"{i}_STATE"] in (3, 4)
|
||||
if valid:
|
||||
pt = self.pts.get(track_key)
|
||||
if pt is None:
|
||||
pt = structs.RadarData.RadarPoint()
|
||||
pt.trackId = self.track_id
|
||||
self.track_id += 1
|
||||
self.pts[track_key] = pt
|
||||
pt.measured = True
|
||||
pt.dRel = msg[f"{i}_LONG_DIST"]
|
||||
pt.yRel = msg[f"{i}_LAT_DIST"]
|
||||
pt.vRel = msg[f"{i}_REL_SPEED"]
|
||||
pt.aRel = msg[f"{i}_REL_ACCEL"]
|
||||
pt.yvRel = msg[f"{i}_REL_LAT_SPEED"]
|
||||
elif track_key in self.pts:
|
||||
del self.pts[track_key]
|
||||
continue
|
||||
|
||||
if radar_type == "mrrevo14f":
|
||||
for i in ("1", "2"):
|
||||
track_key = addr * 2 + int(i) - 1
|
||||
|
||||
@@ -24,16 +24,17 @@ from opendbc.car.hyundai.carcontroller import CarController, CANCEL_BUTTON_DELAY
|
||||
clear_ioniq_6_torque_when_request_inactive
|
||||
from opendbc.car.hyundai.carstate import CarState, decode_canfd_camera_lead, decode_ioniq_6_blindspot_radar_state, \
|
||||
get_canfd_cruise_available
|
||||
from opendbc.car.hyundai.interface import CarInterface, KIA_EV9_ACCEL_MAX
|
||||
from opendbc.car.hyundai.interface import CarInterface, KIA_EV9_ACCEL_MAX, get_communication_control_request
|
||||
from opendbc.car.hyundai import hyundaican, hyundaicanfd
|
||||
from opendbc.car.hyundai.hyundaicanfd import CanBus, hkg_can_fd_checksum
|
||||
from opendbc.car.hyundai.radar_interface import MRREVO14F_RADAR_START_ADDR, MRR30_RADAR_START_ADDR, MRR35_RADAR_START_ADDR, \
|
||||
RADAR_START_ADDR, get_radar_track_config
|
||||
RADAR_START_ADDR, RadarInterface, get_radar_track_config, radar_tracks_available
|
||||
from opendbc.car.hyundai.values import CAMERA_SCC_CAR, CANFD_CAR, CAN_GEARS, CAR, CHECKSUM, DATE_FW_ECUS, DATELESS_FUZZY_CARS, \
|
||||
HYBRID_CAR, EV_CAR, FW_QUERY_CONFIG, LEGACY_SAFETY_MODE_CAR, CANFD_FUZZY_WHITELIST, \
|
||||
UNSUPPORTED_LONGITUDINAL_CAR, PLATFORM_CODE_ECUS, HYUNDAI_VERSION_REQUEST_LONG, \
|
||||
LEGACY_LONGITUDINAL_CAR, DBC, HyundaiFlags, get_platform_codes, HyundaiSafetyFlags, \
|
||||
HyundaiStarPilotFlags, HyundaiStarPilotSafetyFlags, Buttons, CarControllerParams, kia_ev6_gt_line_longitudinal_tuning
|
||||
HyundaiStarPilotFlags, HyundaiStarPilotSafetyFlags, Buttons, CarControllerParams, \
|
||||
CANFD_RADAR_ECU_KEEPALIVE_CAR, CANFD_RADAR_LIVE_LONGITUDINAL_CAR, kia_ev6_gt_line_longitudinal_tuning
|
||||
|
||||
LongCtrlState = CarControl.Actuators.LongControlState
|
||||
from opendbc.car.hyundai.fingerprints import FW_VERSIONS
|
||||
@@ -129,6 +130,15 @@ def get_test_toggles() -> SimpleNamespace:
|
||||
|
||||
|
||||
class TestHyundaiFingerprint:
|
||||
def test_ev6_uses_stock_hda2_communication_control_path(self):
|
||||
stock_request = bytes([0x28, 0x83, 0x01])
|
||||
radar_keepalive_request = bytes([0x28, 0x01, 0x01])
|
||||
|
||||
assert CAR.KIA_EV6 in CANFD_RADAR_LIVE_LONGITUDINAL_CAR
|
||||
assert CAR.KIA_EV6 not in CANFD_RADAR_ECU_KEEPALIVE_CAR
|
||||
assert get_communication_control_request(CAR.KIA_EV6) == stock_request
|
||||
assert get_communication_control_request(CAR.HYUNDAI_IONIQ_6) == radar_keepalive_request
|
||||
|
||||
def test_carnival_hev_low_speed_torque_rate_limits(self):
|
||||
CP = CarInterface.get_params(CAR.KIA_CARNIVAL_HEV_4TH_GEN, gen_empty_fingerprint(), [],
|
||||
False, False, False, None)
|
||||
@@ -426,6 +436,42 @@ class TestHyundaiFingerprint:
|
||||
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CANFD_LKA_STEERING
|
||||
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.EV_GAS
|
||||
|
||||
gv70_radar_config = get_radar_track_config(CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN)
|
||||
assert gv70_radar_config.radar_type == "gv70_210"
|
||||
for addr in range(gv70_radar_config.start_addr, gv70_radar_config.start_addr + gv70_radar_config.msg_count):
|
||||
gv70_fingerprint[gv70_radar_config.bus][addr] = gv70_radar_config.expected_length
|
||||
assert radar_tracks_available(gv70_radar_config, gv70_fingerprint)
|
||||
|
||||
CP = CarInterface.get_params(CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN, gv70_fingerprint, gv70_car_fw,
|
||||
True, False, False, None)
|
||||
assert not CP.radarUnavailable
|
||||
|
||||
radar = RadarInterface(CP)
|
||||
packer = CANPacker(gv70_radar_config.dbc_name)
|
||||
messages = []
|
||||
for addr in range(gv70_radar_config.start_addr, gv70_radar_config.start_addr + gv70_radar_config.msg_count):
|
||||
message = packer.make_can_msg(f"RADAR_TRACK_{addr:x}", 0, {
|
||||
"1_STATE": 3,
|
||||
"1_LONG_DIST": 25.0,
|
||||
"1_LAT_DIST": 0.5,
|
||||
"1_REL_SPEED": -2.0,
|
||||
"1_REL_LAT_SPEED": 0.1,
|
||||
"1_REL_ACCEL": -0.2,
|
||||
})
|
||||
data = bytearray(message[1])
|
||||
checksum = hkg_can_fd_checksum(addr, None, data)
|
||||
data[0] = checksum & 0xff
|
||||
data[1] = (checksum >> 8) & 0xff
|
||||
messages.append((message[0], bytes(data), message[2]))
|
||||
radar_data = radar.update([(1, messages)])
|
||||
assert radar_data is not None
|
||||
assert len(radar_data.points) == 16
|
||||
assert radar_data.points[0].dRel == pytest.approx(25.0)
|
||||
|
||||
other_config = get_radar_track_config(CAR.HYUNDAI_IONIQ_5)
|
||||
assert other_config.radar_type == "mrr30"
|
||||
assert other_config.dbc_name is None
|
||||
|
||||
for candidate in HYUNDAI_NON_SCC_CARS:
|
||||
CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], True, False, False, None)
|
||||
assert bool(CP.flags & HyundaiFlags.NON_SCC)
|
||||
@@ -2484,10 +2530,11 @@ class TestHyundaiFingerprint:
|
||||
CP = CarParams.new_message()
|
||||
CP.carFingerprint = CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN
|
||||
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_LKA_STEERING)
|
||||
CP.openpilotLongitudinalControl = False
|
||||
CP.openpilotLongitudinalControl = True
|
||||
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
controller.frame = 1
|
||||
controller.long_active_ecu = True
|
||||
can_bus = CanBus(CP)
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS", 0)], can_bus.ACAN)
|
||||
stock_lkas = {
|
||||
@@ -2529,7 +2576,6 @@ class TestHyundaiFingerprint:
|
||||
assert parser.vl["LKAS"]["STEER_MODE"] == 0
|
||||
assert parser.vl["LKAS"]["NEW_SIGNAL_2"] == 0
|
||||
|
||||
CP.openpilotLongitudinalControl = True
|
||||
lfa_parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LFA", 0)], can_bus.ECAN)
|
||||
lfa_msgs = hyundaicanfd.create_steering_messages(controller.packer, CP, can_bus, True, True, 0, 0.0)
|
||||
assert [(controller.packer.dbc.addr_to_msg[addr].name, bus) for addr, _, bus in lfa_msgs] == [("LFA", can_bus.ECAN), ("LKAS", can_bus.ACAN)]
|
||||
@@ -2537,13 +2583,12 @@ class TestHyundaiFingerprint:
|
||||
assert lfa_parser.can_valid
|
||||
assert lfa_parser.vl["LFA"]["DAMP_FACTOR"] == 100
|
||||
|
||||
controller.long_active_ecu = True
|
||||
cc.longActive = False
|
||||
inactive_msgs = controller.create_canfd_msgs(0, True, 0.44, 0.0, 0.0, 0.0, False,
|
||||
cc.hudControl, cs, cc, get_test_toggles(), lka_icon=2, lfa_icon=2)
|
||||
steering_names = [(controller.packer.dbc.addr_to_msg[addr].name, bus) for addr, _, bus in inactive_msgs
|
||||
if controller.packer.dbc.addr_to_msg[addr].name in ("LFA", "LKAS")]
|
||||
assert steering_names == [("LKAS", can_bus.ACAN)]
|
||||
assert steering_names == [("LFA", can_bus.ECAN), ("LKAS", can_bus.ACAN)]
|
||||
|
||||
controller.frame = 1
|
||||
cc.longActive = True
|
||||
@@ -2553,15 +2598,14 @@ class TestHyundaiFingerprint:
|
||||
if controller.packer.dbc.addr_to_msg[addr].name in ("LFA", "LKAS")]
|
||||
assert steering_names == [("LFA", can_bus.ECAN), ("LKAS", can_bus.ACAN)]
|
||||
|
||||
def test_ioniq_6_keeps_lfa_status_when_longitudinal_is_inactive(self):
|
||||
@pytest.mark.parametrize("car", [CAR.HYUNDAI_IONIQ_6, CAR.KIA_EV6])
|
||||
def test_egmp_keeps_lfa_status_when_longitudinal_is_inactive(self, car):
|
||||
CP = CarParams.new_message()
|
||||
CP.carFingerprint = CAR.HYUNDAI_IONIQ_6
|
||||
CP.carFingerprint = car
|
||||
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_LKA_STEERING)
|
||||
CP.openpilotLongitudinalControl = True
|
||||
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
controller.frame = 1
|
||||
controller.long_active_ecu = False
|
||||
cc = SimpleNamespace(
|
||||
enabled=False, latActive=False, longActive=False,
|
||||
actuators=SimpleNamespace(longControlState=LongCtrlState.off),
|
||||
@@ -2569,12 +2613,15 @@ class TestHyundaiFingerprint:
|
||||
)
|
||||
cs = SimpleNamespace(
|
||||
stock_lfa_msg=None, stock_lkas_msg=None,
|
||||
left_blindspot_from_radar=False, right_blindspot_from_radar=False,
|
||||
out=SimpleNamespace(gearShifter=structs.CarState.GearShifter.park),
|
||||
)
|
||||
|
||||
msgs = controller.create_canfd_msgs(0, False, 0.0, 0.0, 0.0, 0.0, False,
|
||||
cc.hudControl, cs, cc, get_test_toggles(), lka_icon=1, lfa_icon=1)
|
||||
assert any(addr == 0x12A for addr, _, _ in msgs)
|
||||
controller.frame = 1
|
||||
for controller.long_active_ecu in (False, True):
|
||||
msgs = controller.create_canfd_msgs(0, False, 0.0, 0.0, 0.0, 0.0, False,
|
||||
cc.hudControl, cs, cc, get_test_toggles(), lka_icon=1, lfa_icon=1)
|
||||
assert any(addr == 0x12A for addr, _, _ in msgs)
|
||||
|
||||
def test_gv70_electrified_longitudinal_uses_hda2_scc_contract(self):
|
||||
CP = CarParams.new_message()
|
||||
@@ -2708,7 +2755,7 @@ class TestHyundaiFingerprint:
|
||||
assert len([msg for msg in msgs if msg[0] == 0x110]) == expected_lkas_msgs
|
||||
|
||||
@pytest.mark.parametrize("standstill", [False, True])
|
||||
def test_sportage_angle_lkas_alt_keeps_inactive_status_in_drive(self, standstill):
|
||||
def test_sportage_angle_lkas_alt_publishes_inactive_status(self, standstill):
|
||||
CP = CarParams.new_message()
|
||||
CP.carFingerprint = CAR.KIA_SPORTAGE_HEV_2026
|
||||
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.HYBRID | HyundaiFlags.CANFD_ANGLE_STEERING |
|
||||
@@ -2717,44 +2764,10 @@ class TestHyundaiFingerprint:
|
||||
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
can_bus = CanBus(CP)
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS_ALT", 0)], can_bus.ACAN)
|
||||
stock_lkas = {
|
||||
"CHECKSUM": 1234,
|
||||
"COUNTER": 42,
|
||||
"LKA_OptUsmSta": 2,
|
||||
"LKA_MODE": 2,
|
||||
"LKA_RcgSta": 3,
|
||||
"LKA_AVAILABLE": 3,
|
||||
"LKA_LHLnWrnSta": 3,
|
||||
"LKA_RHLnWrnSta": 3,
|
||||
"LKA_WARNING": 1,
|
||||
"LKA_HndsoffSnd": 1,
|
||||
"LKA_StrSnd": 1,
|
||||
"LKA_SysIndReq": 4,
|
||||
"LKA_ICON": 2,
|
||||
"FCA_SYSWARN": 1,
|
||||
"StrTqReqVal": 17,
|
||||
"TORQUE_REQUEST": 17,
|
||||
"ActToiSta": 3,
|
||||
"STEER_REQ": 1,
|
||||
"ToiFltSta": 3,
|
||||
"LFA_BUTTON": 1,
|
||||
"LKA_SysWrn": 15,
|
||||
"LKA_ASSIST": 1,
|
||||
"Damping_Gain": 0,
|
||||
"STEER_MODE": 5,
|
||||
"NEW_SIGNAL_2": 0,
|
||||
"LKAS_ANGLE_ACTIVE": 2,
|
||||
"LKA_UsmMod": 3,
|
||||
"HAS_LANE_SAFETY": 1,
|
||||
"ADAS_StrAnglReqVal": 12.3,
|
||||
"ADAS_ACIAnglTqRedcGainVal": 0.42,
|
||||
"DAMP_FACTOR": 0,
|
||||
}
|
||||
cc = SimpleNamespace(enabled=False, latActive=False,
|
||||
actuators=SimpleNamespace(longControlState=LongCtrlState.off),
|
||||
leftBlinker=False, rightBlinker=False, hudControl=SimpleNamespace())
|
||||
cs = SimpleNamespace(stock_lfa_msg=None, stock_lkas_msg=stock_lkas,
|
||||
cs = SimpleNamespace(stock_lfa_msg=None, stock_lkas_msg={},
|
||||
out=SimpleNamespace(standstill=standstill, steeringAngleDeg=0.0,
|
||||
gearShifter=structs.CarState.GearShifter.drive))
|
||||
|
||||
@@ -2763,12 +2776,12 @@ class TestHyundaiFingerprint:
|
||||
lkas_msgs = [msg for msg in msgs if msg[0] == 0x110]
|
||||
assert len(lkas_msgs) == 1
|
||||
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS_ALT", 0)], can_bus.ACAN)
|
||||
parser.update([(1, lkas_msgs)])
|
||||
assert parser.can_valid
|
||||
assert parser.vl["LKAS_ALT"]["LKA_StrSnd"] == 2
|
||||
assert parser.vl["LKAS_ALT"]["LKA_ICON"] == 1
|
||||
assert parser.vl["LKAS_ALT"]["LKA_SysIndReq"] == 1
|
||||
assert parser.vl["LKAS_ALT"]["LKA_RcgSta"] == 0
|
||||
assert parser.vl["LKAS_ALT"]["LKA_AVAILABLE"] == 0
|
||||
assert parser.vl["LKAS_ALT"]["LKAS_ANGLE_ACTIVE"] == 1
|
||||
|
||||
def test_ev9_inactive_angle_steering_does_not_suppress_stock_lfa(self):
|
||||
|
||||
@@ -1218,7 +1218,9 @@ CANFD_ALT_BUTTONS_RESUME_CAR = {CAR.KIA_CARNIVAL_2025, CAR.KIA_CARNIVAL_HEV_4TH_
|
||||
CANFD_CORNER_RADAR_BSM_CAR = {CAR.HYUNDAI_IONIQ_6, CAR.HYUNDAI_IONIQ_5_PE, CAR.KIA_EV9}
|
||||
CANFD_RADAR_LIVE_LONGITUDINAL_CAR = {
|
||||
CAR.HYUNDAI_IONIQ_5, CAR.HYUNDAI_IONIQ_5_PE, CAR.HYUNDAI_IONIQ_6, CAR.KIA_EV6, CAR.KIA_EV9, CAR.GENESIS_GV60_EV_1ST_GEN,
|
||||
CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN,
|
||||
}
|
||||
CANFD_RADAR_ECU_KEEPALIVE_CAR = CANFD_RADAR_LIVE_LONGITUDINAL_CAR - {CAR.KIA_EV6}
|
||||
RADAR_LIVE_LONGITUDINAL_CAR = CANFD_RADAR_LIVE_LONGITUDINAL_CAR | {
|
||||
CAR.HYUNDAI_IONIQ,
|
||||
CAR.HYUNDAI_KONA_EV_2022,
|
||||
|
||||
@@ -77,7 +77,7 @@ class CarController(CarControllerBase):
|
||||
self.angle_bus = CanBus.angle_for_cp(CP)
|
||||
self.status_bus = CanBus.camera if CP.flags & SubaruFlags.D_PLATFORM_CAMERA else CanBus.main
|
||||
|
||||
if CP.flags & SubaruFlags.LKAS_ANGLE:
|
||||
if CP.flags & SubaruFlags.LKAS_ANGLE and CP.carFingerprint != CAR.SUBARU_OUTBACK_2023:
|
||||
self.VM = VehicleModel(get_safety_CP())
|
||||
|
||||
self.prev_close_distance = 0
|
||||
@@ -302,7 +302,7 @@ class CarController(CarControllerBase):
|
||||
lkas_available = CC.latActive and (not mads_only or mads_only_ok) and \
|
||||
CS.out.gearShifter == structs.CarState.GearShifter.drive and not CS.out.standstill
|
||||
|
||||
manual_handoff = self._legacy_2025_manual_handoff(CS, lkas_available)
|
||||
manual_handoff = self._legacy_2025_manual_handoff(CS, CC.latActive)
|
||||
lkas_active = lkas_available and not manual_handoff
|
||||
|
||||
steer_target = self._legacy_2025_reclaim_target(CC.actuators.steeringAngleDeg) if lkas_active else CC.actuators.steeringAngleDeg
|
||||
@@ -325,14 +325,14 @@ class CarController(CarControllerBase):
|
||||
lkas_available = CC.latActive and (not mads_only or mads_only_ok) and \
|
||||
CS.out.gearShifter == structs.CarState.GearShifter.drive and not CS.out.standstill
|
||||
|
||||
manual_handoff = self._angle_manual_handoff(CS, lkas_available)
|
||||
manual_handoff = self._angle_manual_handoff(CS, CC.latActive)
|
||||
lkas_active = lkas_available and not manual_handoff
|
||||
|
||||
if lkas_active and not self.angle_lkas_active:
|
||||
self.apply_steer_last = CS.out.steeringAngleDeg
|
||||
|
||||
steer_target = self._angle_reclaim_target(CC.actuators.steeringAngleDeg) if lkas_active else CC.actuators.steeringAngleDeg
|
||||
if self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023:
|
||||
if self.CP.carFingerprint in (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023):
|
||||
apply_steer = apply_std_steer_angle_limits(
|
||||
steer_target,
|
||||
self.apply_steer_last,
|
||||
@@ -361,7 +361,7 @@ class CarController(CarControllerBase):
|
||||
lkas_available = CC.latActive and (not mads_only or mads_only_ok) and \
|
||||
getattr(CS.out, "gearShifter", structs.CarState.GearShifter.drive) == structs.CarState.GearShifter.drive and \
|
||||
not getattr(CS.out, "standstill", False)
|
||||
manual_handoff = self._angle_manual_handoff(CS, lkas_available)
|
||||
manual_handoff = self._angle_manual_handoff(CS, CC.latActive)
|
||||
lat_active = lkas_available and not self.driver_override and not manual_handoff
|
||||
if lat_active and not self.angle_lkas_active:
|
||||
self.apply_steer_last = CS.out.steeringAngleDeg
|
||||
|
||||
@@ -85,14 +85,14 @@ class CarState(CarStateBase):
|
||||
|
||||
if self.CP.flags & SubaruFlags.LKAS_ANGLE:
|
||||
ret.steeringAngleDeg = cp_angle.vl["Steering_2"]["Steering_Angle"]
|
||||
steering_updated = len(cp_angle.vl_all["Steering_2"]["Steering_Angle"]) > 0
|
||||
steering_counter = cp_angle.vl["Steering_2"]["COUNTER"]
|
||||
else:
|
||||
ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"]
|
||||
steering_updated = len(cp.vl_all["Steering_Torque"]["Steering_Angle"]) > 0
|
||||
steering_counter = cp.vl["Steering_Torque"].get("COUNTER", 0)
|
||||
|
||||
if not (self.CP.flags & SubaruFlags.PREGLOBAL):
|
||||
# ideally we get this from the car, but unclear if it exists. diagnostic software doesn't even have it
|
||||
ret.steeringRateDeg = self.angle_rate_calulator.update(ret.steeringAngleDeg, steering_updated)
|
||||
ret.steeringRateDeg = self.angle_rate_calulator.update(ret.steeringAngleDeg, steering_counter)
|
||||
|
||||
ret.steeringTorque = cp_angle.vl["Steering_Torque"]["Steer_Torque_Sensor"]
|
||||
ret.steeringTorqueEps = cp_angle.vl["Steering_Torque"]["Steer_Torque_Output"]
|
||||
|
||||
@@ -42,7 +42,7 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM_CAMERA.value
|
||||
if candidate in SUBARU_STOP_START_CARS:
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.STOP_START_BUTTON.value
|
||||
if candidate in (CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023):
|
||||
if candidate in (CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023):
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.FIXED_ANGLE_LIMITS.value
|
||||
|
||||
ret.steerLimitTimer = 0.4
|
||||
|
||||
@@ -244,7 +244,7 @@ def test_outback_2023_uses_d_platform_bus_layout():
|
||||
assert CP.flags & SubaruFlags.D_PLATFORM
|
||||
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM
|
||||
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.STOP_START_BUTTON
|
||||
assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS)
|
||||
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.FIXED_ANGLE_LIMITS
|
||||
assert CanBus.main_for_cp(CP) == CanBus.alt
|
||||
assert CanBus.angle_for_cp(CP) == CanBus.main
|
||||
assert parsers[Bus.pt].bus == CanBus.alt
|
||||
@@ -546,6 +546,25 @@ def test_ascent_2023_uses_gen2_angle_bus_layout():
|
||||
assert controller.status_bus == CanBus.main
|
||||
|
||||
|
||||
def test_ascent_steering_rate_retains_last_can_sample():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
|
||||
car_state = CarState(CP, None)
|
||||
parsers = car_state.get_can_parsers(CP)
|
||||
toggles = SimpleNamespace(subaru_sng=False)
|
||||
|
||||
parsers[Bus.pt].vl["Steering_2"]["Steering_Angle"] = 1.0
|
||||
parsers[Bus.pt].vl["Steering_2"]["COUNTER"] = 1
|
||||
car_state.update(parsers, toggles)
|
||||
|
||||
parsers[Bus.pt].vl["Steering_2"]["Steering_Angle"] = 2.0
|
||||
parsers[Bus.pt].vl["Steering_2"]["COUNTER"] = 2
|
||||
state, _ = car_state.update(parsers, toggles)
|
||||
assert state.steeringRateDeg == pytest.approx(50.0)
|
||||
|
||||
state, _ = car_state.update(parsers, toggles)
|
||||
assert state.steeringRateDeg == pytest.approx(50.0)
|
||||
|
||||
|
||||
def test_other_angle_platforms_keep_existing_bus_layout():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_CROSSTREK_2025)
|
||||
parsers = CarState.get_can_parsers(CP)
|
||||
@@ -622,8 +641,9 @@ def test_angle_controller_blocks_low_speed_mads_engagement():
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
||||
|
||||
|
||||
def test_ascent_angle_controller_uses_fixed_angle_rate_limits():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
|
||||
@pytest.mark.parametrize("platform", (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023))
|
||||
def test_angle_controller_uses_fixed_angle_rate_limits(platform):
|
||||
CP = CarInterface.get_non_essential_params(platform)
|
||||
controller = CarController({}, CP)
|
||||
CC = SimpleNamespace(enabled=True, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-14.88))
|
||||
CS = SimpleNamespace(out=SimpleNamespace(
|
||||
@@ -706,11 +726,20 @@ def test_ascent_angle_controller_blocks_parking_lot_aol_engagement():
|
||||
CS.out.steeringRateDeg = 0.0
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(2, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
|
||||
for i in range(8):
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(3 + i, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(11, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
||||
|
||||
CS.out.gearShifter = structs.CarState.GearShifter.reverse
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(3, [msg])])
|
||||
parser.update([(12, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
|
||||
|
||||
|
||||
@@ -108,6 +108,7 @@ non_tested_cars = [
|
||||
TOYOTA.TOYOTA_RAV4H,
|
||||
|
||||
# No recorded routes yet
|
||||
VOLVO.VOLVO_V40,
|
||||
VOLVO.VOLVO_XC40_RECHARGE,
|
||||
VOLVO.VOLVO_S60_RECHARGE,
|
||||
VOLVO.POLESTAR_2,
|
||||
|
||||
@@ -145,6 +145,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
|
||||
"PORSCHE_MACAN_MK1" = [2.0, 2.0, 0.2]
|
||||
"VOLVO_XC40_RECHARGE" = [1.5, 1.5, 0.1]
|
||||
"VOLVO_S60_RECHARGE" = [1.5, 1.5, 0.1]
|
||||
"VOLVO_V40" = [1.5, 1.5, 0.1]
|
||||
|
||||
# Dashcam or fallback configured as ideal car
|
||||
"MOCK" = [10.0, 10, 0.0]
|
||||
|
||||
@@ -40,6 +40,8 @@ TOYOTA_NO_LEAD_CRUISE_SIGN_FLIP_MIN_SET_SPEED_ERROR = 0.35 # m/s
|
||||
TOYOTA_RAV4_LAUNCH_PEDAL_BLEND_SPEED = 5.0 # m/s
|
||||
TOYOTA_RAV4_LAUNCH_PEDAL_SCALE = 0.11
|
||||
TOYOTA_RAV4_LOW_SPEED_PEDAL_SCALE = 0.23
|
||||
TOYOTA_AUTO_HOLD_ACCEL = -1.0
|
||||
TOYOTA_AUTO_HOLD_ACTIVATION_FRAMES = 100
|
||||
|
||||
# LKA limits
|
||||
# EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
|
||||
@@ -309,23 +311,24 @@ class CarController(CarControllerBase):
|
||||
|
||||
self.last_standstill = CS.out.standstill
|
||||
|
||||
def create_auto_brake_hold_messages(self, CS: structs.CarState, brake_hold_allowed_timer: int = 100):
|
||||
can_sends = []
|
||||
brake_hold_allowed = (CS.out.standstill and CS.out.cruiseState.available and
|
||||
def update_auto_hold_state(self, CS: structs.CarState, cancel_requested: bool = False,
|
||||
activation_frames: int = TOYOTA_AUTO_HOLD_ACTIVATION_FRAMES):
|
||||
brake_hold_allowed = (not cancel_requested and CS.out.standstill and CS.out.cruiseState.available and
|
||||
not CS.out.gasPressed and not CS.out.cruiseState.enabled and
|
||||
CS.out.gearShifter not in (PARK, REVERSE))
|
||||
|
||||
if brake_hold_allowed and not self.brake_hold_active and CS.out.brakePressed:
|
||||
self._brake_hold_counter += 1
|
||||
self.brake_hold_active = self._brake_hold_counter > brake_hold_allowed_timer
|
||||
self.brake_hold_active = self._brake_hold_counter > activation_frames
|
||||
elif not brake_hold_allowed:
|
||||
self._brake_hold_counter = 0
|
||||
self.brake_hold_active = False
|
||||
|
||||
if self.frame % 2 == 0:
|
||||
can_sends.append(toyotacan.create_brake_hold_command(self.packer, self.frame, CS.pre_collision_2, self.brake_hold_active))
|
||||
return self.brake_hold_active
|
||||
|
||||
return can_sends
|
||||
def reset_auto_hold_state(self):
|
||||
self._brake_hold_counter = 0
|
||||
self.brake_hold_active = False
|
||||
|
||||
def update(self, CC, CS, now_nanos, starpilot_toggles):
|
||||
actuators = CC.actuators
|
||||
@@ -423,10 +426,9 @@ class CarController(CarControllerBase):
|
||||
|
||||
self._update_standstill_request(CC, CS, actuators, starpilot_toggles)
|
||||
if supports_toyota_auto_hold(self.CP, getattr(starpilot_toggles, "toyota_auto_hold", False)):
|
||||
can_sends.extend(self.create_auto_brake_hold_messages(CS))
|
||||
elif self.brake_hold_active:
|
||||
self._brake_hold_counter = 0
|
||||
self.brake_hold_active = False
|
||||
self.update_auto_hold_state(CS, pcm_cancel_cmd)
|
||||
else:
|
||||
self.reset_auto_hold_state()
|
||||
|
||||
interceptor_gas_cmd = self._compute_interceptor_gas_cmd(CC, CS)
|
||||
|
||||
@@ -534,6 +536,11 @@ class CarController(CarControllerBase):
|
||||
|
||||
pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX))
|
||||
|
||||
if self.brake_hold_active:
|
||||
pcm_accel_cmd = TOYOTA_AUTO_HOLD_ACCEL
|
||||
self.permit_braking = True
|
||||
self.standstill_req = True
|
||||
|
||||
main_accel_cmd = 0. if self.CP.flags & ToyotaFlags.SECOC.value else pcm_accel_cmd
|
||||
can_sends.append(toyotacan.create_accel_command(self.packer, main_accel_cmd, pcm_cancel_cmd, self.permit_braking, self.standstill_req, lead,
|
||||
CS.acc_type, fcw_alert, self.distance_button,
|
||||
|
||||
@@ -90,8 +90,6 @@ class CarState(CarStateBase):
|
||||
self.has_can_filter = self.FPCP.flags & ToyotaStarPilotFlags.RADAR_CAN_FILTER.value
|
||||
self.has_SDSU = self.FPCP.flags & ToyotaStarPilotFlags.SMART_DSU.value
|
||||
self.has_ZSS = self.FPCP.flags & ToyotaStarPilotFlags.ZSS.value
|
||||
self.auto_brake_hold = bool(self.CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value)
|
||||
self.pre_collision_2 = {}
|
||||
|
||||
def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
|
||||
cp = can_parsers[Bus.pt]
|
||||
@@ -227,9 +225,6 @@ class CarState(CarStateBase):
|
||||
if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:
|
||||
self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"])
|
||||
|
||||
if self.auto_brake_hold:
|
||||
self.pre_collision_2 = copy.copy(cp_cam.vl["PRE_COLLISION_2"])
|
||||
|
||||
if self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR:
|
||||
self.pcm_follow_distance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"]
|
||||
|
||||
@@ -314,9 +309,6 @@ class CarState(CarStateBase):
|
||||
if CP.carFingerprint in DISTANCE_BUTTON_CAR:
|
||||
pt_messages.append(("PCM_CRUISE_4", 1))
|
||||
|
||||
if CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value:
|
||||
cam_messages.append(("PRE_COLLISION_2", 50))
|
||||
|
||||
return {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, 2),
|
||||
|
||||
@@ -164,8 +164,8 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.GAS_INTERCEPTOR.value
|
||||
|
||||
toyota_auto_hold = Params(return_defaults=True).get_bool("ToyotaAutoHold")
|
||||
if toyota_auto_hold and candidate in TOYOTA_AUTO_HOLD_CARS:
|
||||
ret.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||
if toyota_auto_hold and ret.openpilotLongitudinalControl and candidate in TOYOTA_AUTO_HOLD_CARS:
|
||||
ret.alternativeExperience |= ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD
|
||||
ret.flags |= ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||
|
||||
if not ret.openpilotLongitudinalControl:
|
||||
|
||||
@@ -196,7 +196,8 @@ class TestToyotaInterfaces:
|
||||
params.put_bool("ToyotaAutoHold", True)
|
||||
car_params = CarInterface.get_params(
|
||||
candidate,
|
||||
{bus: {} for bus in range(8)},
|
||||
{bus: ({0x2FF: 8} if candidate in (CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H) and bus == 0 else {})
|
||||
for bus in range(8)},
|
||||
[],
|
||||
alpha_long=False,
|
||||
is_release=False,
|
||||
@@ -207,12 +208,13 @@ class TestToyotaInterfaces:
|
||||
params.remove("ToyotaAutoHold")
|
||||
|
||||
assert car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||
assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||
assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD
|
||||
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||
|
||||
can_parsers = CarState.get_can_parsers(car_params)
|
||||
car_state = CarState(car_params, SimpleNamespace(flags=0))
|
||||
car_state.update(can_parsers, SimpleNamespace(cluster_offset=1.0))
|
||||
assert "PRE_COLLISION_2" in can_parsers[Bus.cam].vl
|
||||
assert "PRE_COLLISION_2" not in can_parsers[Bus.cam].vl
|
||||
|
||||
@pytest.mark.parametrize("candidate", [CAR.TOYOTA_CAMRY_TSS2, CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H])
|
||||
def test_auto_hold_is_disabled_by_default(self, candidate):
|
||||
@@ -229,7 +231,7 @@ class TestToyotaInterfaces:
|
||||
)
|
||||
|
||||
assert not car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD
|
||||
|
||||
def test_prius_openpilot_long_uses_hybrid_long_defaults(self):
|
||||
car_params = CarInterface.get_params(
|
||||
@@ -744,6 +746,8 @@ class TestToyotaCarController:
|
||||
controller.standstill_req = standstill_req
|
||||
controller.last_standstill = last_standstill
|
||||
controller.accel = 0.0
|
||||
controller.brake_hold_active = False
|
||||
controller._brake_hold_counter = 0
|
||||
return controller
|
||||
|
||||
@staticmethod
|
||||
@@ -806,9 +810,6 @@ class TestToyotaCarController:
|
||||
def test_toyota_auto_hold_latches_after_brake_press_until_gas(self):
|
||||
controller = self._make_controller()
|
||||
controller.packer = CANPacker(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt])
|
||||
controller.frame = 0
|
||||
controller.brake_hold_active = False
|
||||
controller._brake_hold_counter = 0
|
||||
|
||||
cs = SimpleNamespace(
|
||||
out=SimpleNamespace(
|
||||
@@ -818,28 +819,22 @@ class TestToyotaCarController:
|
||||
brakePressed=True,
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
),
|
||||
pre_collision_2={},
|
||||
)
|
||||
|
||||
controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
|
||||
controller.update_auto_hold_state(cs, activation_frames=0)
|
||||
assert controller.brake_hold_active
|
||||
|
||||
cs.out.brakePressed = False
|
||||
controller.frame = 2
|
||||
controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
|
||||
controller.update_auto_hold_state(cs, activation_frames=0)
|
||||
assert controller.brake_hold_active
|
||||
|
||||
cs.out.gasPressed = True
|
||||
controller.frame = 4
|
||||
controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
|
||||
controller.update_auto_hold_state(cs, activation_frames=0)
|
||||
assert not controller.brake_hold_active
|
||||
|
||||
def test_toyota_auto_hold_does_not_trigger_without_brake_press(self):
|
||||
controller = self._make_controller()
|
||||
controller.packer = CANPacker(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt])
|
||||
controller.frame = 0
|
||||
controller.brake_hold_active = False
|
||||
controller._brake_hold_counter = 0
|
||||
cs = SimpleNamespace(
|
||||
out=SimpleNamespace(
|
||||
standstill=True,
|
||||
@@ -848,10 +843,9 @@ class TestToyotaCarController:
|
||||
brakePressed=False,
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
),
|
||||
pre_collision_2={},
|
||||
)
|
||||
|
||||
controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
|
||||
controller.update_auto_hold_state(cs, activation_frames=0)
|
||||
assert not controller.brake_hold_active
|
||||
|
||||
def test_prius_resume_request_releases_standstill_latch(self):
|
||||
@@ -991,12 +985,9 @@ class TestToyotaCarController:
|
||||
assert parser.can_valid
|
||||
assert parser.vl["ACC_CONTROL"]["ALLOW_LONG_PRESS"] == 1
|
||||
|
||||
def test_auto_brake_hold_sends_modified_pre_collision_after_timer(self):
|
||||
def test_auto_hold_uses_acc_control_brake_path(self):
|
||||
controller = self._make_controller()
|
||||
controller.packer = CANPacker(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt])
|
||||
controller.frame = 0
|
||||
controller.brake_hold_active = False
|
||||
controller._brake_hold_counter = 0
|
||||
cs = SimpleNamespace(
|
||||
out=SimpleNamespace(
|
||||
standstill=True,
|
||||
@@ -1005,16 +996,19 @@ class TestToyotaCarController:
|
||||
brakePressed=True,
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
),
|
||||
pre_collision_2={},
|
||||
)
|
||||
|
||||
can_sends = controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
|
||||
controller.update_auto_hold_state(cs, activation_frames=0)
|
||||
can_sends = [toyotacan.create_accel_command(
|
||||
controller.packer, -1.0, False, True, True, False, 1, False, 0, False,
|
||||
)]
|
||||
|
||||
parser = CANParser(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt], [("PRE_COLLISION_2", 0)], 0)
|
||||
parser = CANParser(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt], [("ACC_CONTROL", 0)], 0)
|
||||
parser.update([(1, can_sends)])
|
||||
assert controller.brake_hold_active
|
||||
assert parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -1.0
|
||||
assert parser.vl["PRE_COLLISION_2"]["PBRTRGR"] == 1
|
||||
assert parser.vl["ACC_CONTROL"]["ACCEL_CMD"] == -1.0
|
||||
assert parser.vl["ACC_CONTROL"]["PERMIT_BRAKING"] == 1
|
||||
assert parser.vl["ACC_CONTROL"]["RELEASE_STANDSTILL"] == 0
|
||||
|
||||
def test_interceptor_stop_and_go_holds_small_launch_at_standstill(self):
|
||||
controller = self._make_controller()
|
||||
|
||||
@@ -89,38 +89,6 @@ def create_pcs_commands(packer, accel, active, mass):
|
||||
return [msg1, msg2]
|
||||
|
||||
|
||||
def create_brake_hold_command(packer, frame, pre_collision_2, brake_hold_active):
|
||||
values = {s: pre_collision_2[s] for s in [
|
||||
"DSS1GDRV",
|
||||
"DS1STAT2",
|
||||
"DS1STBK2",
|
||||
"PCSWAR",
|
||||
"PCSALM",
|
||||
"PCSOPR",
|
||||
"PCSABK",
|
||||
"PBATRGR",
|
||||
"PPTRGR",
|
||||
"IBTRGR",
|
||||
"CLEXTRGR",
|
||||
"IRLT_REQ",
|
||||
"BRKHLD",
|
||||
"AVSTRGR",
|
||||
"VGRSTRGR",
|
||||
"PREFILL",
|
||||
"PBRTRGR",
|
||||
"PCSDIS",
|
||||
"PBPREPMP",
|
||||
] if s in pre_collision_2}
|
||||
|
||||
if brake_hold_active:
|
||||
values = {
|
||||
"DSS1GDRV": 0x3FF,
|
||||
"PBRTRGR": frame % 730 < 727,
|
||||
}
|
||||
|
||||
return packer.make_can_msg("PRE_COLLISION_2", 0, values)
|
||||
|
||||
|
||||
def create_acc_cancel_command(packer):
|
||||
values = {
|
||||
"GAS_RELEASED": 0,
|
||||
|
||||
@@ -1,3 +1,5 @@
|
||||
from collections import deque
|
||||
|
||||
import numpy as np
|
||||
|
||||
from opendbc.can.packer import CANPacker
|
||||
@@ -5,16 +7,20 @@ from opendbc.car import Bus
|
||||
from opendbc.car.interfaces import CarControllerBase
|
||||
from opendbc.car.lateral import apply_std_steer_angle_limits
|
||||
from opendbc.car.volvo.helpers import LCA3CounterSync
|
||||
from opendbc.car.volvo.volvocan import (create_lca_message, create_pscm_message, create_lca_3_message, create_lca_2_message, create_lca_4_message,
|
||||
create_lca_5_message, create_lca_6_message, create_lca_7_message, create_pscm_related_message)
|
||||
from opendbc.car.volvo.values import CarControllerParams
|
||||
from opendbc.car.volvo.volvocan import (create_c1_cancel, create_c1_pscm_message, create_c1_steering_control, create_lca_message,
|
||||
create_pscm_message, create_lca_3_message, create_lca_2_message, create_lca_4_message, create_lca_5_message,
|
||||
create_lca_6_message, create_lca_7_message, create_pscm_related_message)
|
||||
from opendbc.car.volvo.values import CAR, CarControllerParams, VolvoC1PlatformConfig
|
||||
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_names, CP):
|
||||
super().__init__(dbc_names, CP)
|
||||
self.packer = CANPacker(dbc_names[Bus.party])
|
||||
self.is_c1 = isinstance(CAR(CP.carFingerprint).config, VolvoC1PlatformConfig)
|
||||
self.packer = CANPacker(dbc_names[Bus.pt] if self.is_c1 else dbc_names[Bus.party])
|
||||
self.apply_angle_last = 0.0 # Track last applied steering angle
|
||||
self.c1_torque_samples = deque(maxlen=CarControllerParams.C1_N_ZERO_TORQUE)
|
||||
self.c1_recovery_until = -1
|
||||
|
||||
self.gear_acc = 60
|
||||
self.lca_4_acc = 0 # Bresenham accumulator for 29 Hz
|
||||
@@ -62,6 +68,9 @@ class CarController(CarControllerBase):
|
||||
self.lca_auth_drv_mag_filt = 0.0
|
||||
|
||||
def update(self, CC, CS, now_nanos, starpilot_toggles):
|
||||
if self.is_c1:
|
||||
return self._update_c1(CC, CS)
|
||||
|
||||
can_sends = []
|
||||
actuators = CC.actuators
|
||||
|
||||
@@ -285,3 +294,50 @@ class CarController(CarControllerBase):
|
||||
self.frame += 1
|
||||
self.last_lat_active = CC.latActive
|
||||
return new_actuators, can_sends
|
||||
|
||||
def _update_c1(self, CC, CS):
|
||||
can_sends = []
|
||||
actuators = CC.actuators
|
||||
|
||||
if self.frame % 2 == 0: # stock FSM1 and PSCM1 messages are 50 Hz
|
||||
requested_active = CC.latActive and CS.out.vEgo > self.CP.minSteerSpeed
|
||||
recovering = requested_active and self.frame < self.c1_recovery_until
|
||||
|
||||
if not requested_active:
|
||||
self.c1_torque_samples.clear()
|
||||
self.c1_recovery_until = -1
|
||||
elif recovering:
|
||||
self.c1_torque_samples.clear()
|
||||
else:
|
||||
if self.c1_recovery_until >= 0:
|
||||
self.c1_recovery_until = -1
|
||||
self.c1_torque_samples.clear()
|
||||
self.c1_torque_samples.append(CS.c1_lka_torque)
|
||||
if (len(self.c1_torque_samples) == CarControllerParams.C1_N_ZERO_TORQUE and
|
||||
all(torque == 0 for torque in self.c1_torque_samples)):
|
||||
self.c1_recovery_until = self.frame + 100
|
||||
self.c1_torque_samples.clear()
|
||||
recovering = True
|
||||
|
||||
lat_active = requested_active and not recovering
|
||||
desired_angle = float(np.clip(
|
||||
actuators.steeringAngleDeg,
|
||||
CS.out.steeringAngleDeg - CarControllerParams.C1_ANGLE_ERROR,
|
||||
CS.out.steeringAngleDeg + CarControllerParams.C1_ANGLE_ERROR,
|
||||
))
|
||||
apply_angle = apply_std_steer_angle_limits(
|
||||
desired_angle, self.apply_angle_last, CS.out.vEgoRaw,
|
||||
CS.out.steeringAngleDeg, lat_active, CarControllerParams.C1_ANGLE_LIMITS,
|
||||
)
|
||||
|
||||
can_sends.append(create_c1_pscm_message(self.packer, CS.c1_msg_pscm))
|
||||
can_sends.append(create_c1_steering_control(self.packer, apply_angle, lat_active))
|
||||
self.apply_angle_last = apply_angle
|
||||
|
||||
if CC.cruiseControl.cancel and self.frame % 10 == 0:
|
||||
can_sends.append(create_c1_cancel(self.packer))
|
||||
|
||||
new_actuators = actuators.as_builder()
|
||||
new_actuators.steeringAngleDeg = self.apply_angle_last
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
|
||||
@@ -1,8 +1,9 @@
|
||||
from cereal import custom
|
||||
from opendbc.car import structs, Bus
|
||||
from opendbc.car import Bus, ButtonType, create_button_events, structs
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.car.volvo.values import DBC, VolvoSPAPlatformConfig, CAR
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from opendbc.car.volvo.values import CAR, DBC, VolvoC1PlatformConfig, VolvoSPAPlatformConfig
|
||||
|
||||
GearShifter = structs.CarState.GearShifter
|
||||
TransmissionType = structs.CarParams.TransmissionType
|
||||
@@ -16,6 +17,7 @@ STEERING_PRESSED_THRESHOLD = 2
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP, FPCP):
|
||||
super().__init__(CP, FPCP)
|
||||
self.is_c1 = isinstance(CAR(CP.carFingerprint).config, VolvoC1PlatformConfig)
|
||||
self.is_spa = isinstance(CAR(CP.carFingerprint).config, VolvoSPAPlatformConfig)
|
||||
self.gas_pressed_prev = False
|
||||
self.dispatch_lca_2_msg = False
|
||||
@@ -34,8 +36,22 @@ class CarState(CarStateBase):
|
||||
self.msg_lca_4 = {}
|
||||
self.msg_lca_6 = {}
|
||||
self.msg_lca_7 = {}
|
||||
self.c1_msg_pscm = {}
|
||||
self.c1_lka_torque = 0
|
||||
self.c1_button_states = {
|
||||
"ACCOnOffBtn": False,
|
||||
"ACCStopBtn": False,
|
||||
"ACCSetBtn": False,
|
||||
"ACCResumeBtn": False,
|
||||
"ACCMinusBtn": False,
|
||||
"TimeGapIncreaseBtn": False,
|
||||
"TimeGapDecreaseBtn": False,
|
||||
}
|
||||
|
||||
def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
|
||||
if self.is_c1:
|
||||
return self._update_c1(can_parsers)
|
||||
|
||||
cp_main = can_parsers[Bus.main]
|
||||
cp_pt = can_parsers[Bus.pt]
|
||||
cp_party = can_parsers[Bus.party]
|
||||
@@ -137,8 +153,83 @@ class CarState(CarStateBase):
|
||||
fp_ret = custom.StarPilotCarState.new_message()
|
||||
return ret, fp_ret
|
||||
|
||||
def _update_c1(self, can_parsers):
|
||||
cp = can_parsers[Bus.pt]
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
ret = structs.CarState()
|
||||
|
||||
ret.vEgoRaw = cp.vl["VehicleSpeed1"]["VehicleSpeed"] * CV.KPH_TO_MS
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.standstill = ret.vEgoRaw < 0.1
|
||||
|
||||
ret.steeringAngleDeg = cp.vl["PSCM1"]["SteeringAngleServo"]
|
||||
ret.steeringTorque = cp.vl["PSCM1"]["LKATorque"]
|
||||
ret.steeringPressed = False
|
||||
|
||||
ret.gasPressed = cp.vl["PedalandBrake"]["AccPedal"] > 5.0
|
||||
ret.brakePressed = bool(cp.vl["PedalandBrake"]["BrakePedalActive2"] or
|
||||
cp.vl["PedalandBrake"]["BrakePedalActive"])
|
||||
|
||||
ret.gearShifter = {
|
||||
0: GearShifter.park,
|
||||
1: GearShifter.reverse,
|
||||
2: GearShifter.neutral,
|
||||
3: GearShifter.drive,
|
||||
}.get(int(cp.vl["TCM0"]["GearShifter"]), GearShifter.unknown)
|
||||
|
||||
ret.cruiseState.available = bool(cp_cam.vl["FSM0"]["ACCStatusOnOff"])
|
||||
ret.cruiseState.enabled = bool(cp_cam.vl["FSM0"]["ACCStatusActive"])
|
||||
ret.cruiseState.speed = cp.vl["ACC"]["SpeedTargetACC"] * CV.KPH_TO_MS
|
||||
ret.cruiseState.nonAdaptive = False
|
||||
ret.cruiseState.standstill = ret.standstill
|
||||
|
||||
turn_signal = int(cp.vl["MiscCarInfo"]["TurnSignal"])
|
||||
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(
|
||||
50, turn_signal == 1, turn_signal == 3)
|
||||
ret.doorOpen = False
|
||||
ret.seatbeltUnlatched = False
|
||||
|
||||
button_types = {
|
||||
"ACCOnOffBtn": ButtonType.mainCruise,
|
||||
"ACCStopBtn": ButtonType.cancel,
|
||||
"ACCSetBtn": ButtonType.setCruise,
|
||||
"ACCResumeBtn": ButtonType.resumeCruise,
|
||||
"ACCMinusBtn": ButtonType.decelCruise,
|
||||
"TimeGapIncreaseBtn": ButtonType.gapAdjustCruise,
|
||||
"TimeGapDecreaseBtn": ButtonType.gapAdjustCruise,
|
||||
}
|
||||
button_events = []
|
||||
for signal, button_type in button_types.items():
|
||||
pressed = bool(cp.vl["CCButtons"][signal])
|
||||
button_events.extend(create_button_events(pressed, self.c1_button_states[signal], {True: button_type}))
|
||||
self.c1_button_states[signal] = pressed
|
||||
ret.buttonEvents = button_events
|
||||
|
||||
self.c1_msg_pscm = cp.vl["PSCM1"]
|
||||
self.c1_lka_torque = int(cp.vl["PSCM1"]["LKATorque"])
|
||||
|
||||
fp_ret = custom.StarPilotCarState.new_message()
|
||||
return ret, fp_ret
|
||||
|
||||
@staticmethod
|
||||
def get_can_parsers(CP):
|
||||
if isinstance(CAR(CP.carFingerprint).config, VolvoC1PlatformConfig):
|
||||
return {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [
|
||||
("VehicleSpeed1", 50),
|
||||
("CCButtons", 100),
|
||||
("PSCM1", 50),
|
||||
("PedalandBrake", 100),
|
||||
("TCM0", 10),
|
||||
("ACC", 17),
|
||||
("MiscCarInfo", 25),
|
||||
], 0),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.cam], [
|
||||
("FSM0", 100),
|
||||
("FSM1", 50),
|
||||
], 2),
|
||||
}
|
||||
|
||||
return {
|
||||
Bus.main: CANParser(DBC[CP.carFingerprint][Bus.main], [], 0),
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 1),
|
||||
|
||||
@@ -3,6 +3,14 @@
|
||||
from opendbc.car.volvo.values import CAR
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.VOLVO_V40: [
|
||||
# V40 2017
|
||||
{8: 8, 16: 8, 48: 8, 64: 8, 85: 8, 101: 8, 112: 8, 114: 8, 117: 8, 128: 8, 176: 8, 192: 8, 208: 8, 224: 8, 240: 8, 245: 8, 256: 8, 272: 8, 288: 8, 291: 8, 293: 8, 304: 8, 325: 8, 336: 8, 352: 8, 424: 8, 432: 8, 437: 8, 464: 8, 472: 8, 480: 8, 528: 8, 608: 8, 624: 8, 640: 8, 648: 8, 652: 8, 656: 8, 657: 8, 681: 8, 693: 8, 704: 8, 707: 8, 709: 8, 816: 8, 832: 8, 848: 8, 853: 8, 864: 8, 880: 8, 912: 8, 928: 8, 943: 8, 944: 8, 968: 8, 970: 8, 976: 8, 992: 8, 997: 8, 1024: 8, 1029: 8, 1061: 8, 1072: 8, 1409: 8},
|
||||
# V40 2015
|
||||
{8: 8, 16: 8, 64: 8, 85: 8, 101: 8, 112: 8, 114: 8, 117: 8, 128: 8, 176: 8, 192: 8, 224: 8, 240: 8, 245: 8, 256: 8, 272: 8, 288: 8, 291: 8, 293: 8, 304: 8, 325: 8, 336: 8, 424: 8, 432: 8, 437: 8, 464: 8, 472: 8, 480: 8, 528: 8, 608: 8, 648: 8, 652: 8, 656: 8, 657: 8, 681: 8, 693: 8, 704: 8, 707: 8, 709: 8, 816: 8, 832: 8, 864: 8, 880: 8, 912: 8, 928: 8, 943: 8, 944: 8, 968: 8, 970: 8, 976: 8, 992: 8, 997: 8, 1024: 8, 1029: 8, 1061: 8, 1072: 8, 1409: 8},
|
||||
# V40 2014
|
||||
{8: 8, 16: 8, 64: 8, 85: 8, 101: 8, 112: 8, 114: 8, 117: 8, 128: 8, 176: 8, 192: 8, 224: 8, 240: 8, 245: 8, 256: 8, 272: 8, 288: 8, 291: 8, 293: 8, 304: 8, 325: 8, 336: 8, 424: 8, 432: 8, 437: 8, 464: 8, 472: 8, 480: 8, 528: 8, 608: 8, 648: 8, 652: 8, 657: 8, 681: 8, 693: 8, 704: 8, 707: 8, 709: 8, 816: 8, 864: 8, 880: 8, 912: 8, 928: 8, 943: 8, 944: 8, 968: 8, 970: 8, 976: 8, 992: 8, 997: 8, 1024: 8, 1029: 8, 1072: 8, 1409: 8},
|
||||
],
|
||||
CAR.VOLVO_XC40_RECHARGE: [{
|
||||
21: 8, 22: 8, 23: 8, 26: 8, 69: 8, 85: 8, 87: 8, 88: 8, 96: 8, 103: 8, 104: 8, 105: 8, 128: 8, 144: 8, 146: 8, 147: 8, 151: 8, 304: 8, 336: 8, 339: 8, 341: 8, 394: 8, 395: 8, 640: 8, 656: 8, 666: 8, 773: 8, 778: 8, 1072: 8, 1120: 8, 1302: 8, 1336: 8, 1407: 8, 1422: 8, 1423: 8, 1424: 8, 1425: 8, 1426: 8
|
||||
}],
|
||||
|
||||
@@ -2,11 +2,10 @@ from opendbc.car import structs, get_safety_config
|
||||
from opendbc.car.interfaces import CarInterfaceBase
|
||||
from opendbc.car.volvo.carcontroller import CarController
|
||||
from opendbc.car.volvo.carstate import CarState
|
||||
from opendbc.car.volvo.values import VolvoSPAPlatformConfig, CAR
|
||||
from opendbc.car.volvo.values import CAR, VolvoC1PlatformConfig, VolvoSafetyFlags, VolvoSPAPlatformConfig
|
||||
|
||||
TransmissionType = structs.CarParams.TransmissionType
|
||||
|
||||
VOLVO_FLAG_SPA = 1
|
||||
SAFETY_VOLVO = structs.CarParams.SafetyModel.volvo
|
||||
|
||||
|
||||
@@ -18,17 +17,20 @@ class CarInterface(CarInterfaceBase):
|
||||
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
|
||||
ret.brand = 'volvo'
|
||||
|
||||
platform = CAR(candidate).config
|
||||
safety_param = 0
|
||||
if isinstance(CAR(candidate).config, VolvoSPAPlatformConfig):
|
||||
safety_param = VOLVO_FLAG_SPA
|
||||
if isinstance(platform, VolvoSPAPlatformConfig):
|
||||
safety_param = VolvoSafetyFlags.SPA.value
|
||||
elif isinstance(platform, VolvoC1PlatformConfig):
|
||||
safety_param = VolvoSafetyFlags.C1.value
|
||||
ret.safetyConfigs = [get_safety_config(SAFETY_VOLVO, safety_param)]
|
||||
#ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.noOutput)]
|
||||
|
||||
ret.dashcamOnly = False
|
||||
|
||||
ret.steerActuatorDelay = 0.3
|
||||
ret.steerActuatorDelay = 0.2 if isinstance(platform, VolvoC1PlatformConfig) else 0.3
|
||||
ret.steerLimitTimer = 0.1
|
||||
ret.steerAtStandstill = True
|
||||
ret.steerAtStandstill = not isinstance(platform, VolvoC1PlatformConfig)
|
||||
|
||||
# Use angle-based steering control for Volvo CMA platform
|
||||
ret.steerControlType = structs.CarParams.SteerControlType.angle
|
||||
@@ -39,4 +41,7 @@ class CarInterface(CarInterfaceBase):
|
||||
|
||||
ret.pcmCruise = True
|
||||
|
||||
if isinstance(platform, VolvoC1PlatformConfig):
|
||||
ret.transmissionType = TransmissionType.automatic
|
||||
|
||||
return ret
|
||||
|
||||
@@ -0,0 +1,57 @@
|
||||
import pytest
|
||||
|
||||
from cereal import custom
|
||||
from opendbc.can.packer import CANPacker
|
||||
from opendbc.car import Bus, ButtonType, CanData, structs
|
||||
from opendbc.car.volvo.carstate import CarState
|
||||
from opendbc.car.volvo.interface import CarInterface
|
||||
from opendbc.car.volvo.values import CAR, DBC
|
||||
|
||||
|
||||
def _can_data(msg):
|
||||
address, data, bus = msg
|
||||
return CanData(address, data, bus)
|
||||
|
||||
|
||||
def test_c1_carstate_decodes_vehicle_and_cruise_signals():
|
||||
cp = CarInterface.get_non_essential_params(CAR.VOLVO_V40)
|
||||
cs = CarState(cp, custom.StarPilotCarParams.new_message())
|
||||
parsers = CarState.get_can_parsers(cp)
|
||||
packer = CANPacker(DBC[cp.carFingerprint][Bus.pt])
|
||||
|
||||
messages = [
|
||||
packer.make_can_msg("VehicleSpeed1", 0, {"VehicleSpeed": 72}),
|
||||
packer.make_can_msg("CCButtons", 0, {"ACCStopBtn": 1, "ACCSetBtn": 1}),
|
||||
packer.make_can_msg("PSCM1", 0, {"SteeringAngleServo": -12.5, "LKATorque": 7}),
|
||||
packer.make_can_msg("PedalandBrake", 0, {"AccPedal": 6, "BrakePedalActive2": 1}),
|
||||
packer.make_can_msg("TCM0", 0, {"GearShifter": 3}),
|
||||
packer.make_can_msg("ACC", 0, {"SpeedTargetACC": 100}),
|
||||
packer.make_can_msg("MiscCarInfo", 0, {"TurnSignal": 1}),
|
||||
packer.make_can_msg("FSM0", 2, {"ACCStatusOnOff": 1, "ACCStatusActive": 1}),
|
||||
packer.make_can_msg("FSM1", 2, {}),
|
||||
]
|
||||
packets = [(1_000_000, [_can_data(msg) for msg in messages])]
|
||||
for parser in parsers.values():
|
||||
parser.update(packets)
|
||||
|
||||
ret, _ = cs.update(parsers, None)
|
||||
assert ret.vEgoRaw == pytest.approx(20.0)
|
||||
assert ret.steeringAngleDeg == pytest.approx(-12.5, abs=0.05)
|
||||
assert ret.steeringTorque == 7
|
||||
assert ret.gasPressed and ret.brakePressed
|
||||
assert ret.gearShifter == structs.CarState.GearShifter.drive
|
||||
assert ret.cruiseState.available and ret.cruiseState.enabled
|
||||
assert ret.cruiseState.speed == pytest.approx(100 / 3.6)
|
||||
assert ret.leftBlinker and not ret.rightBlinker
|
||||
assert len(ret.buttonEvents) == 2
|
||||
assert any(event.type == ButtonType.cancel and event.pressed for event in ret.buttonEvents)
|
||||
assert any(event.type == ButtonType.setCruise and event.pressed for event in ret.buttonEvents)
|
||||
|
||||
release = packer.make_can_msg("CCButtons", 0, {})
|
||||
packets = [(2_000_000, [_can_data(release)])]
|
||||
for parser in parsers.values():
|
||||
parser.update(packets)
|
||||
ret, _ = cs.update(parsers, None)
|
||||
assert len(ret.buttonEvents) == 2
|
||||
assert any(event.type == ButtonType.cancel and not event.pressed for event in ret.buttonEvents)
|
||||
assert any(event.type == ButtonType.setCruise and not event.pressed for event in ret.buttonEvents)
|
||||
@@ -4,7 +4,8 @@ from types import SimpleNamespace
|
||||
from opendbc.car.volvo.carcontroller import CarController
|
||||
from opendbc.car.volvo.helpers import checksum_lca_5_message
|
||||
from opendbc.car.volvo.interface import CarInterface
|
||||
from opendbc.car.volvo.values import DBC
|
||||
from opendbc.car.volvo.values import CAR, DBC
|
||||
from opendbc.car.volvo.volvocan import create_c1_checksum
|
||||
|
||||
|
||||
def _zero_message():
|
||||
@@ -67,3 +68,70 @@ def test_controller_relays_stock_lca5_angle_when_inactive():
|
||||
if raw & (1 << 14):
|
||||
raw -= 1 << 15
|
||||
assert abs(raw * 0.05596 - 12.0) < 0.1
|
||||
|
||||
|
||||
def _c1_state():
|
||||
return SimpleNamespace(
|
||||
out=SimpleNamespace(steeringAngleDeg=10.0, vEgo=12.0, vEgoRaw=12.0),
|
||||
c1_lka_torque=5,
|
||||
c1_msg_pscm=_zero_message(),
|
||||
)
|
||||
|
||||
|
||||
def test_c1_controller_emits_checked_steering_and_pscm_relay():
|
||||
cp = CarInterface.get_non_essential_params(CAR.VOLVO_V40)
|
||||
controller = CarController(DBC[cp.carFingerprint], cp)
|
||||
cc = SimpleNamespace(
|
||||
latActive=True,
|
||||
actuators=_Actuators(),
|
||||
cruiseControl=SimpleNamespace(cancel=False),
|
||||
)
|
||||
|
||||
actuators, can_sends = controller.update(cc, _c1_state(), 0, None)
|
||||
assert [(msg[0], msg[2]) for msg in can_sends] == [(0x125, 2), (0xD0, 0)]
|
||||
|
||||
fsm = can_sends[1][1]
|
||||
assert fsm[7] & 0x3 == 3
|
||||
assert fsm[6] == create_c1_checksum(fsm)
|
||||
assert 0.0 < actuators.steeringAngleDeg <= 2.0
|
||||
|
||||
|
||||
def test_c1_controller_sends_only_cancel_button():
|
||||
cp = CarInterface.get_non_essential_params(CAR.VOLVO_V40)
|
||||
controller = CarController(DBC[cp.carFingerprint], cp)
|
||||
cc = SimpleNamespace(
|
||||
latActive=False,
|
||||
actuators=_Actuators(),
|
||||
cruiseControl=SimpleNamespace(cancel=True),
|
||||
)
|
||||
|
||||
_, can_sends = controller.update(cc, _c1_state(), 0, None)
|
||||
buttons = next(msg for msg in can_sends if msg[0] == 0x10)
|
||||
assert buttons[2] == 0
|
||||
assert buttons[1][7] == 0x10
|
||||
assert buttons[1][6] == 0
|
||||
|
||||
|
||||
def test_c1_controller_temporarily_drops_steering_on_zero_torque_fault():
|
||||
cp = CarInterface.get_non_essential_params(CAR.VOLVO_V40)
|
||||
controller = CarController(DBC[cp.carFingerprint], cp)
|
||||
cs = _c1_state()
|
||||
cs.c1_lka_torque = 0
|
||||
cc = SimpleNamespace(
|
||||
latActive=True,
|
||||
actuators=_Actuators(),
|
||||
cruiseControl=SimpleNamespace(cancel=False),
|
||||
)
|
||||
|
||||
directions = []
|
||||
for _ in range(23):
|
||||
_, can_sends = controller.update(cc, cs, 0, None)
|
||||
directions.extend(msg[1][7] & 0x3 for msg in can_sends if msg[0] == 0xD0)
|
||||
|
||||
assert directions[:-1] == [3] * 11
|
||||
assert directions[-1] == 0
|
||||
|
||||
while controller.frame <= 122:
|
||||
_, can_sends = controller.update(cc, cs, 0, None)
|
||||
fsm = next(msg for msg in can_sends if msg[0] == 0xD0)
|
||||
assert fsm[1][7] & 0x3 == 3
|
||||
|
||||
@@ -1,13 +1,23 @@
|
||||
from dataclasses import dataclass, field
|
||||
from enum import IntFlag
|
||||
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms
|
||||
from opendbc.car.lateral import AngleSteeringLimits
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.docs_definitions import CarDocs, CarHarness, CarParts
|
||||
from opendbc.car.fw_query_definitions import FwQueryConfig
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
# C1 support is adapted from the original dragonpilot V40 port:
|
||||
# https://github.com/dragonpilot/dragonpilot/commit/773dce507082d931236b64dca8024dce9625446f
|
||||
|
||||
|
||||
class VolvoSafetyFlags(IntFlag):
|
||||
SPA = 1
|
||||
C1 = 2
|
||||
|
||||
|
||||
class CarControllerParams:
|
||||
STEER_STEP = 1 # 100 Hz LCA command frequency (controlsd runs at 100 Hz)
|
||||
@@ -81,6 +91,19 @@ class CarControllerParams:
|
||||
([0., 5., 25.], [5., 2., .3]), # rate down limits at different speeds
|
||||
)
|
||||
|
||||
C1_STEER_NO = 0
|
||||
C1_STEER = 3
|
||||
C1_N_ZERO_TORQUE = 12
|
||||
C1_ANGLE_ERROR = 20.0
|
||||
C1_ANGLE_DELTA_BP = [0., 8.33, 13.89, 19.44, 25., 30.55, 36.1]
|
||||
C1_ANGLE_DELTA_UP = [2., 1.2, .25, .20, .15, .10, .10]
|
||||
C1_ANGLE_DELTA_DOWN = [2., 1.2, .25, .20, .15, .10, .10]
|
||||
C1_ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
|
||||
359.9,
|
||||
(C1_ANGLE_DELTA_BP, C1_ANGLE_DELTA_UP),
|
||||
(C1_ANGLE_DELTA_BP, C1_ANGLE_DELTA_DOWN),
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class VolvoCarDocs(CarDocs):
|
||||
@@ -105,7 +128,26 @@ class VolvoSPAPlatformConfig(PlatformConfig):
|
||||
})
|
||||
|
||||
|
||||
@dataclass
|
||||
class VolvoC1PlatformConfig(PlatformConfig):
|
||||
dbc_dict: DbcDict = field(default_factory=lambda: {
|
||||
Bus.pt: 'volvo_v40_2017_pt',
|
||||
Bus.cam: 'volvo_v40_2017_pt',
|
||||
})
|
||||
|
||||
|
||||
class CAR(Platforms):
|
||||
VOLVO_V40 = VolvoC1PlatformConfig(
|
||||
[VolvoCarDocs("Volvo V40 2013-19")],
|
||||
CarSpecs(
|
||||
mass=1610,
|
||||
wheelbase=2.647,
|
||||
steerRatio=14.7,
|
||||
centerToFrontRatio=0.44,
|
||||
minSteerSpeed=1.0 * CV.KPH_TO_MS,
|
||||
),
|
||||
)
|
||||
|
||||
VOLVO_XC40_RECHARGE = VolvoCMAPlatformConfig(
|
||||
[VolvoCarDocs("Volvo XC40 Recharge 2021-23")],
|
||||
CarSpecs(
|
||||
|
||||
@@ -2,6 +2,47 @@ from opendbc.car.volvo.helpers import (checksum_lca_2_message, checksum_2_0x69_m
|
||||
checksum_2_pscm_related_message, checksum_lca_5_message)
|
||||
from opendbc.car.carlog import carlog
|
||||
|
||||
|
||||
def create_c1_pscm_message(packer, msg_pscm: dict):
|
||||
values = {
|
||||
"LKATorque": 0,
|
||||
"SteeringAngleServo": msg_pscm["SteeringAngleServo"],
|
||||
"byte0": msg_pscm["byte0"],
|
||||
"byte3": msg_pscm["byte3"],
|
||||
"byte4": msg_pscm["byte4"],
|
||||
"byte7": msg_pscm["byte7"],
|
||||
"LKAActive": int(msg_pscm["LKAActive"]) & 0xD,
|
||||
}
|
||||
return packer.make_can_msg("PSCM1", 2, values)
|
||||
|
||||
|
||||
def create_c1_checksum(data: bytes) -> int:
|
||||
angle_raw = ((data[4] & 0x3F) << 8) | data[5]
|
||||
direction = data[7] & 0x3
|
||||
checksum_sum = (data[3] + direction + angle_raw + (angle_raw >> 8)) & 0xFF
|
||||
return checksum_sum ^ 0xFF
|
||||
|
||||
|
||||
def create_c1_steering_control(packer, apply_angle: float, lat_active: bool):
|
||||
values = {
|
||||
"SET_X_E3": 0xE3,
|
||||
"SET_X_B4": 0xB4,
|
||||
"SET_X_08": 0x08,
|
||||
"TrqLim": 0,
|
||||
"LKAAngleReq": apply_angle,
|
||||
"LKASteerDirection": 3 if lat_active else 0,
|
||||
"SET_X_25": 0x25,
|
||||
"SET_X_02": 0x02,
|
||||
}
|
||||
data = packer.make_can_msg("FSM1", 0, values)[1]
|
||||
values["Checksum"] = create_c1_checksum(data)
|
||||
return packer.make_can_msg("FSM1", 0, values)
|
||||
|
||||
|
||||
def create_c1_cancel(packer):
|
||||
return packer.make_can_msg("CCButtons", 0, {"ACCStopBtn": 1})
|
||||
|
||||
|
||||
def create_lca_message(packer, lat_active: bool, apply_angle: float, msg_lca: dict,
|
||||
authority_pos: int = 614, authority_neg: int = -614,
|
||||
overrides: dict | None = None):
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1497,7 +1497,7 @@ BO_ 913 BCM_PO_11: 8 Vector__XXX
|
||||
SG_ BCM_Door_Dri_Status : 5|1@0+ (1,0) [0|1] "" PT_ESC_ABS
|
||||
SG_ BCM_Shift_R_MT_SW_Status : 39|2@0+ (1,0) [0|3] "" PT_ESC_ABS
|
||||
SG_ LDA_BTN : 4|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ RAY_LKAS_BTN : 0|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ RAY_LKAS_BTN : 4|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 1426 LABEL11: 8 XXX
|
||||
SG_ CC_React : 34|1@1+ (1,0) [0|1] "" XXX
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -11,3 +11,4 @@ class ALTERNATIVE_EXPERIENCE:
|
||||
|
||||
ALWAYS_ON_LATERAL = 32
|
||||
GM_REMAP_CANCEL_TO_DISTANCE = 64
|
||||
TOYOTA_AUTO_HOLD = 128
|
||||
|
||||
@@ -339,6 +339,7 @@ extern bool gm_remote_start_boots_comma;
|
||||
|
||||
#define ALT_EXP_ALWAYS_ON_LATERAL 32
|
||||
#define ALT_EXP_GM_REMAP_CANCEL_TO_DISTANCE 64
|
||||
#define ALT_EXP_TOYOTA_AUTO_HOLD 128
|
||||
|
||||
extern int alternative_experience;
|
||||
|
||||
|
||||
@@ -2,6 +2,8 @@
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
#define TOYOTA_AUTO_HOLD_ACCEL -1000 // -1.0 m/s^2 in ACC_CONTROL units
|
||||
|
||||
// Stock longitudinal
|
||||
#define TOYOTA_BASE_TX_MSGS \
|
||||
{0x191, 0, 8, .check_relay = true}, {0x412, 0, 8, .check_relay = true}, {0x1D2, 0, 8, .check_relay = false}, /* LKAS + LTA + PCM cancel cmd */ \
|
||||
@@ -277,7 +279,15 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
|
||||
// SecOC cars move accel to 0x183. Only allow inactive accel on 0x343 to match stock behavior
|
||||
violation = desired_accel != TOYOTA_LONG_LIMITS.inactive_accel;
|
||||
}
|
||||
violation |= longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS);
|
||||
|
||||
bool toyota_auto_hold =
|
||||
!toyota_stock_longitudinal &&
|
||||
((alternative_experience & ALT_EXP_TOYOTA_AUTO_HOLD) != 0) &&
|
||||
!vehicle_moving && !gas_pressed && acc_main_on &&
|
||||
(desired_accel == TOYOTA_AUTO_HOLD_ACCEL) &&
|
||||
GET_BIT(msg, 30U) && !GET_BIT(msg, 31U) && !GET_BIT(msg, 24U);
|
||||
|
||||
violation |= !toyota_auto_hold && longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS);
|
||||
|
||||
// only ACC messages that cancel are allowed when openpilot is not controlling longitudinal
|
||||
if (toyota_stock_longitudinal) {
|
||||
@@ -394,12 +404,7 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
|
||||
tx = false;
|
||||
}
|
||||
|
||||
// Auto brake hold replaces the camera AEB message only while stopped.
|
||||
if ((msg->addr == 0x344U) && ((alternative_experience & ALT_EXP_ALLOW_AEB) != 0)) {
|
||||
if (vehicle_moving || gas_pressed || !acc_main_on) {
|
||||
tx = false;
|
||||
}
|
||||
} else if ((msg->addr == 0x344U) && toyota_stock_longitudinal) {
|
||||
if ((msg->addr == 0x344U) && toyota_stock_longitudinal) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
@@ -566,21 +571,11 @@ static safety_config toyota_init(uint16_t param) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
static bool toyota_fwd_hook(int bus_num, int addr) {
|
||||
bool block_msg = false;
|
||||
if (bus_num == 2) {
|
||||
block_msg = (addr == 0x344) && ((alternative_experience & ALT_EXP_ALLOW_AEB) != 0) &&
|
||||
!vehicle_moving && !gas_pressed && acc_main_on;
|
||||
}
|
||||
return block_msg;
|
||||
}
|
||||
|
||||
const safety_hooks toyota_hooks = {
|
||||
.init = toyota_init,
|
||||
.rx = toyota_rx_hook,
|
||||
.rx_all = toyota_rx_all_hook,
|
||||
.tx = toyota_tx_hook,
|
||||
.fwd = toyota_fwd_hook,
|
||||
.get_checksum = toyota_get_checksum,
|
||||
.compute_checksum = toyota_compute_checksum,
|
||||
.get_quality_flag_valid = toyota_get_quality_flag_valid,
|
||||
|
||||
@@ -2,9 +2,11 @@
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// safetyParam: 0 = CMA (XC40 Recharge), 1 = SPA (S60 Recharge, Polestar 2)
|
||||
// safetyParam: 0 = CMA (XC40 Recharge), 1 = SPA (S60 Recharge, Polestar 2),
|
||||
// 2 = C1 (V40)
|
||||
// Polestar 2 is technically CMA, but appears to use SPA DBC for CAN 1 bus
|
||||
#define VOLVO_FLAG_SPA 1U
|
||||
#define VOLVO_FLAG_C1 2U
|
||||
|
||||
// Volvo CAN message addresses shared between CMA and SPA
|
||||
#define VOLVO_LCA_STEER 0x58U // TX from VCU1 to PSCM, LCA steering command (0x58)
|
||||
@@ -24,6 +26,15 @@
|
||||
#define VOLVO_LCA_6 0x97U // TX LCA_6 message
|
||||
#define VOLVO_LCA_7 0x92U // TX LCA_7 message
|
||||
|
||||
// C1-specific addresses (V40). The V40 powertrain bus is bus 0 and its
|
||||
// forward-camera bus is bus 2; bus 1 is unused by this port.
|
||||
#define VOLVO_C1_BUTTONS 0x10U
|
||||
#define VOLVO_C1_FSM_0 0x30U
|
||||
#define VOLVO_C1_FSM_1 0xD0U
|
||||
#define VOLVO_C1_PSCM_1 0x125U
|
||||
#define VOLVO_C1_PEDAL_AND_BRAKE 0x55U
|
||||
#define VOLVO_C1_SPEED 0x150U
|
||||
|
||||
// CMA-specific PT bus addresses
|
||||
#define VOLVO_CMA_BUS1_SPEED 0x70U // RX vehicle speed
|
||||
#define VOLVO_CMA_ECM_1 0x250U // RX accelerator pedal position
|
||||
@@ -44,6 +55,10 @@
|
||||
#define VOLVO_MAX_ANGLE_CAN 9650
|
||||
#define VOLVO_RELAY_ANGLE_TOLERANCE 54 // approximately 3 degrees
|
||||
|
||||
#define VOLVO_C1_ANGLE_DEG_TO_CAN 22.753128f
|
||||
#define VOLVO_C1_MAX_ANGLE_CAN 8189
|
||||
#define VOLVO_C1_RELAY_ANGLE_TOLERANCE 2
|
||||
|
||||
|
||||
// CAN bus definitions for Volvo
|
||||
// Using same naming as carstate.py for consistency: main, pt, party
|
||||
@@ -54,6 +69,7 @@
|
||||
// Runtime addresses set by volvo_init based on safetyParam
|
||||
static uint16_t volvo_ecm_1_addr;
|
||||
static uint16_t volvo_bus1_cruise_control_addr;
|
||||
static bool volvo_c1;
|
||||
|
||||
static int volvo_be_15(const CANPacket_t *msg, uint8_t byte) {
|
||||
return (int)(((uint16_t)(msg->data[byte] & 0x7FU) << 8U) | msg->data[byte + 1U]);
|
||||
@@ -67,6 +83,21 @@ static int volvo_lca_5_angle(const CANPacket_t *msg) {
|
||||
return to_signed(volvo_be_15(msg, 6U), 15);
|
||||
}
|
||||
|
||||
static int volvo_c1_pscm_angle(const CANPacket_t *msg) {
|
||||
return (int)(((uint16_t)msg->data[5] << 8U) | msg->data[6]) - 32768;
|
||||
}
|
||||
|
||||
static int volvo_c1_fsm_angle(const CANPacket_t *msg) {
|
||||
return (int)(((uint16_t)(msg->data[4] & 0x3FU) << 8U) | msg->data[5]) - 8192;
|
||||
}
|
||||
|
||||
static uint8_t volvo_c1_fsm_checksum(const CANPacket_t *msg) {
|
||||
const uint16_t angle_raw = ((uint16_t)(msg->data[4] & 0x3FU) << 8U) | msg->data[5];
|
||||
const uint8_t direction = msg->data[7] & 0x3U;
|
||||
const uint8_t checksum_sum = (msg->data[3] + direction + angle_raw + (angle_raw >> 8U)) & 0xFFU;
|
||||
return checksum_sum ^ 0xFFU;
|
||||
}
|
||||
|
||||
static const AngleSteeringLimits VOLVO_ANGLE_STEERING_LIMITS = {
|
||||
.max_angle = VOLVO_MAX_ANGLE_CAN,
|
||||
.angle_deg_to_can = VOLVO_ANGLE_DEG_TO_CAN,
|
||||
@@ -81,8 +112,51 @@ static const AngleSteeringLimits VOLVO_ANGLE_STEERING_LIMITS = {
|
||||
.frequency = 50U,
|
||||
};
|
||||
|
||||
static const AngleSteeringLimits VOLVO_C1_ANGLE_STEERING_LIMITS = {
|
||||
.max_angle = VOLVO_C1_MAX_ANGLE_CAN,
|
||||
.angle_deg_to_can = VOLVO_C1_ANGLE_DEG_TO_CAN,
|
||||
.angle_rate_up_lookup = {
|
||||
{7.0f, 17.0f, 36.0f},
|
||||
{2.0f, 0.25f, 0.1f},
|
||||
},
|
||||
.angle_rate_down_lookup = {
|
||||
{7.0f, 17.0f, 36.0f},
|
||||
{2.0f, 0.25f, 0.1f},
|
||||
},
|
||||
.max_angle_error = 455, // 20 degrees
|
||||
.angle_error_min_speed = 0.0f,
|
||||
.frequency = 50U,
|
||||
.enforce_angle_error = true,
|
||||
};
|
||||
|
||||
static void volvo_rx_hook(const CANPacket_t *msg) {
|
||||
|
||||
if (volvo_c1) {
|
||||
if (msg->bus == VOLVO_MAIN_BUS) {
|
||||
if (msg->addr == VOLVO_C1_PSCM_1) {
|
||||
update_sample(&angle_meas, volvo_c1_pscm_angle(msg));
|
||||
}
|
||||
|
||||
if (msg->addr == VOLVO_C1_SPEED) {
|
||||
const uint16_t speed_raw = ((uint16_t)msg->data[6] << 8U) | msg->data[7];
|
||||
const float speed = ((float)speed_raw * 0.01f) / 3.6f;
|
||||
vehicle_moving = speed > 0.1f;
|
||||
UPDATE_VEHICLE_SPEED(speed);
|
||||
}
|
||||
|
||||
if (msg->addr == VOLVO_C1_PEDAL_AND_BRAKE) {
|
||||
const uint16_t gas_raw = ((uint16_t)(msg->data[1] & 0x3U) << 8U) | msg->data[2];
|
||||
gas_pressed = gas_raw > 50U; // DBC factor 0.1: greater than 5 percent
|
||||
brake_pressed = GET_BIT(msg, 24U) || GET_BIT(msg, 38U);
|
||||
}
|
||||
}
|
||||
|
||||
if ((msg->bus == VOLVO_PARTY_BUS) && (msg->addr == VOLVO_C1_FSM_0)) {
|
||||
pcm_cruise_check(GET_BIT(msg, 58U));
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// Main bus (bus 0) messages
|
||||
if (msg->bus == VOLVO_MAIN_BUS) {
|
||||
// Update brake pedal and cruise state from BCM2
|
||||
@@ -158,6 +232,33 @@ static void volvo_rx_hook(const CANPacket_t *msg) {
|
||||
static bool volvo_tx_hook(const CANPacket_t *msg) {
|
||||
bool tx = true;
|
||||
|
||||
if (volvo_c1) {
|
||||
if (msg->addr == VOLVO_C1_FSM_1) {
|
||||
const int desired_angle = volvo_c1_fsm_angle(msg);
|
||||
const uint8_t direction = msg->data[7] & 0x3U;
|
||||
const bool steer_control_enabled = direction != 0U;
|
||||
tx &= SAFETY_ABS(desired_angle) <= VOLVO_C1_MAX_ANGLE_CAN;
|
||||
tx &= !steer_angle_cmd_checks(desired_angle, steer_control_enabled, VOLVO_C1_ANGLE_STEERING_LIMITS);
|
||||
tx &= (direction == 0U) || (direction == 3U);
|
||||
tx &= (msg->data[0] == 0xE3U) && (msg->data[1] == 0xB4U) && (msg->data[2] == 0x08U);
|
||||
tx &= (msg->data[3] == 0x80U) && ((msg->data[4] & 0xC0U) == 0x80U) && ((msg->data[7] & 0xFCU) == 0x94U);
|
||||
tx &= msg->data[6] == volvo_c1_fsm_checksum(msg);
|
||||
}
|
||||
|
||||
if (msg->addr == VOLVO_C1_PSCM_1) {
|
||||
const int relayed_angle = volvo_c1_pscm_angle(msg);
|
||||
const int measured_max = angle_meas.max + VOLVO_C1_RELAY_ANGLE_TOLERANCE;
|
||||
const int measured_min = angle_meas.min - VOLVO_C1_RELAY_ANGLE_TOLERANCE;
|
||||
tx &= !safety_max_limit_check(relayed_angle, measured_max, measured_min);
|
||||
}
|
||||
|
||||
// Only ACC cancel (byte 7 bit 4) may be synthesized.
|
||||
if (msg->addr == VOLVO_C1_BUTTONS) {
|
||||
tx &= ((msg->data[7] & 0xEFU) == 0U) && (msg->data[6] == 0U);
|
||||
}
|
||||
return tx;
|
||||
}
|
||||
|
||||
// LCA_5 carries the actual angle command used by the controller. The stock
|
||||
// LCA frame also contains an angle-shaped field, but the imported controller
|
||||
// deliberately leaves that field at the observed vehicle value.
|
||||
@@ -255,6 +356,22 @@ static bool volvo_tx_hook(const CANPacket_t *msg) {
|
||||
|
||||
static safety_config volvo_init(uint16_t param) {
|
||||
bool spa = GET_FLAG(param, VOLVO_FLAG_SPA);
|
||||
volvo_c1 = GET_FLAG(param, VOLVO_FLAG_C1);
|
||||
|
||||
if (volvo_c1) {
|
||||
static const CanMsg VOLVO_C1_TX_MSGS[] = {
|
||||
{VOLVO_C1_FSM_1, VOLVO_MAIN_BUS, 8, .check_relay = true},
|
||||
{VOLVO_C1_PSCM_1, VOLVO_PARTY_BUS, 8, .check_relay = true},
|
||||
{VOLVO_C1_BUTTONS, VOLVO_MAIN_BUS, 8, .check_relay = false},
|
||||
};
|
||||
static RxCheck volvo_c1_rx_checks[] = {
|
||||
{.msg = {{VOLVO_C1_PSCM_1, VOLVO_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{VOLVO_C1_FSM_0, VOLVO_PARTY_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{VOLVO_C1_PEDAL_AND_BRAKE, VOLVO_MAIN_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{VOLVO_C1_SPEED, VOLVO_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
return BUILD_SAFETY_CFG(volvo_c1_rx_checks, VOLVO_C1_TX_MSGS);
|
||||
}
|
||||
|
||||
// Set PT bus addresses based on platform
|
||||
volvo_ecm_1_addr = spa ? VOLVO_SPA_ECM_1 : VOLVO_CMA_ECM_1;
|
||||
|
||||
@@ -417,6 +417,18 @@ class TestSubaruDPlatformAngleSafety(TestSubaruStockLongitudinalSafetyBase, Test
|
||||
return self.packer.make_can_msg_safety("Steering_2", SUBARU_MAIN_BUS, {"Steering_Angle": angle})
|
||||
|
||||
|
||||
class TestSubaruDPlatformFixedAngleSafety(TestSubaruDPlatformAngleSafety):
|
||||
FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.D_PLATFORM | \
|
||||
SubaruSafetyFlags.FIXED_ANGLE_LIMITS
|
||||
STEER_ANGLE_MAX = 545
|
||||
ANGLE_RATE_BP = [0., 5., 35.]
|
||||
ANGLE_RATE_UP = [5., .8, .15]
|
||||
ANGLE_RATE_DOWN = [5., .8, .15]
|
||||
|
||||
def test_rt_limits(self):
|
||||
raise unittest.SkipTest("Breakpoint angle limits do not enforce a real-time message frequency")
|
||||
|
||||
|
||||
class TestSubaruDPlatformStopStartSafety(TestSubaruDPlatformAngleSafety):
|
||||
FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.D_PLATFORM | \
|
||||
SubaruSafetyFlags.STOP_START_BUTTON
|
||||
|
||||
@@ -97,29 +97,55 @@ class TestToyotaSafetyBase(common.CarSafetyTest, common.LongitudinalAccelSafetyT
|
||||
msg = libsafety_py.make_CANPacket(0x283, 0, bytes(dat))
|
||||
self.assertEqual(not bad and not stock_longitudinal, self._tx(msg))
|
||||
|
||||
def test_auto_brake_hold_aeb_replacement_only_at_standstill(self):
|
||||
self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALLOW_AEB)
|
||||
hold_msg = libsafety_py.make_CANPacket(0x344, 0, b"\xfd\x80\x00\x00\x00\x00\x00\xcc")
|
||||
def test_auto_hold_acc_control_is_narrowly_allowed_only_at_standstill(self):
|
||||
if (not self.LONGITUDINAL or
|
||||
self.safety.get_current_safety_param() & (ToyotaSafetyFlags.STOCK_LONGITUDINAL.value | ToyotaSafetyFlags.SECOC.value)):
|
||||
raise unittest.SkipTest("Toyota Auto Hold requires non-SecOC openpilot longitudinal control")
|
||||
|
||||
self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD)
|
||||
hold_msg = self.packer.make_can_msg_safety("ACC_CONTROL", 0, {
|
||||
"ACCEL_CMD": -1.0,
|
||||
"PERMIT_BRAKING": 1,
|
||||
"RELEASE_STANDSTILL": 0,
|
||||
"CANCEL_REQ": 0,
|
||||
})
|
||||
|
||||
self._rx(self._speed_msg(0))
|
||||
self._rx(self._toggle_aol(True))
|
||||
self._rx(self._user_gas_msg(False))
|
||||
self.safety.set_controls_allowed(False)
|
||||
self.assertTrue(self._tx(hold_msg))
|
||||
self.assertEqual(-1, self.safety.safety_fwd_hook(2, 0x344))
|
||||
|
||||
self.assertFalse(self._tx(self.packer.make_can_msg_safety("ACC_CONTROL", 0, {
|
||||
"ACCEL_CMD": -1.1,
|
||||
"PERMIT_BRAKING": 1,
|
||||
"RELEASE_STANDSTILL": 0,
|
||||
})))
|
||||
|
||||
self._rx(self._speed_msg(1.0))
|
||||
self.assertFalse(self._tx(hold_msg))
|
||||
self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x344))
|
||||
|
||||
self._rx(self._speed_msg(0))
|
||||
self._rx(self._user_gas_msg(True))
|
||||
self.assertFalse(self._tx(hold_msg))
|
||||
self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x344))
|
||||
|
||||
self._rx(self._user_gas_msg(False))
|
||||
self._rx(self._toggle_aol(False))
|
||||
self.assertFalse(self._tx(hold_msg))
|
||||
self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x344))
|
||||
|
||||
def test_auto_hold_acc_control_is_blocked_without_toyota_hold_toggle(self):
|
||||
hold_msg = self.packer.make_can_msg_safety("ACC_CONTROL", 0, {
|
||||
"ACCEL_CMD": -1.0,
|
||||
"PERMIT_BRAKING": 1,
|
||||
"RELEASE_STANDSTILL": 0,
|
||||
"CANCEL_REQ": 0,
|
||||
})
|
||||
self._rx(self._speed_msg(0))
|
||||
self._rx(self._toggle_aol(True))
|
||||
self._rx(self._user_gas_msg(False))
|
||||
self.safety.set_controls_allowed(False)
|
||||
self.safety.set_alternative_experience(0)
|
||||
self.assertFalse(self._tx(hold_msg))
|
||||
|
||||
# Only allow LTA msgs with no actuation
|
||||
def test_lta_steer_cmd(self):
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Safety tests for Volvo CMA/SPA.
|
||||
Safety tests for Volvo C1/CMA/SPA.
|
||||
|
||||
The safety mode lives at ``opendbc/safety/modes/volvo.h`` and is parameterized
|
||||
by ``safetyParam``:
|
||||
@@ -8,14 +8,13 @@ by ``safetyParam``:
|
||||
- ``safetyParam == 0`` → CMA platform (Volvo XC40 Recharge)
|
||||
- ``safetyParam == VOLVO_FLAG_SPA`` → SPA platform (Volvo S60 Recharge,
|
||||
Polestar 2)
|
||||
- ``safetyParam == VOLVO_FLAG_C1`` → C1 platform (Volvo V40)
|
||||
|
||||
The two platforms share LCA/PSCM/etc. addresses on the main and party buses
|
||||
but use *different* PT-bus addresses and signal scales for ECM_1 and
|
||||
BUS1_CRUISE_CONTROL. Vehicle speed is read from main-bus SPEED on both, so it
|
||||
is not platform-dependent. This test file exercises both platforms through the
|
||||
same generic ``CarSafetyTest`` harness so that any future divergence between
|
||||
``carstate.py`` and ``volvo.h`` — e.g. a threshold drifting out of sync — is
|
||||
caught on a laptop instead of in the car.
|
||||
CMA and SPA share LCA/PSCM/etc. addresses on the main and party buses but use
|
||||
different PT-bus addresses and signal scales. C1 uses the V40's legacy CAN
|
||||
layout and its own safety allowlist. The tests exercise all three through the
|
||||
generic ``CarSafetyTest`` harness so divergence between ``carstate.py`` and
|
||||
``volvo.h`` is caught before running in a car.
|
||||
|
||||
Companion to: ``opendbc/car/volvo/carstate.py`` (must agree on thresholds).
|
||||
"""
|
||||
@@ -25,13 +24,16 @@ import re
|
||||
import unittest
|
||||
|
||||
from opendbc.car.volvo.interface import SAFETY_VOLVO
|
||||
from opendbc.car.volvo.values import VolvoSafetyFlags
|
||||
from opendbc.car.volvo.volvocan import create_c1_checksum, create_c1_steering_control
|
||||
from opendbc.safety.tests.libsafety import libsafety_py
|
||||
import opendbc.safety.tests.common as common
|
||||
from opendbc.safety.tests.common import CANPackerSafety
|
||||
|
||||
|
||||
# Must match VOLVO_FLAG_SPA in opendbc/safety/modes/volvo.h
|
||||
VOLVO_FLAG_SPA = 1
|
||||
# Must match the flags in opendbc/safety/modes/volvo.h
|
||||
VOLVO_FLAG_SPA = VolvoSafetyFlags.SPA.value
|
||||
VOLVO_FLAG_C1 = VolvoSafetyFlags.C1.value
|
||||
|
||||
# Must match VOLVO_SPEED_TO_MS in volvo.h and SPEED_TO_MS in carstate.py
|
||||
VOLVO_SPEED_TO_MS = 0.003977
|
||||
@@ -332,5 +334,124 @@ class TestVolvoSPA(TestVolvoSafetyBase):
|
||||
"BUS1_CRUISE_CONTROL", VOLVO_PT_BUS, values)
|
||||
|
||||
|
||||
class TestVolvoC1(common.CarSafetyTest, common.AngleSteeringSafetyTest):
|
||||
TX_MSGS = [[0xD0, VOLVO_MAIN_BUS], [0x125, VOLVO_PARTY_BUS], [0x10, VOLVO_MAIN_BUS]]
|
||||
RELAY_MALFUNCTION_ADDRS = {
|
||||
VOLVO_MAIN_BUS: (0xD0,),
|
||||
VOLVO_PARTY_BUS: (0x125,),
|
||||
}
|
||||
FWD_BLACKLISTED_ADDRS = {
|
||||
VOLVO_MAIN_BUS: [0x125],
|
||||
VOLVO_PARTY_BUS: [0xD0],
|
||||
}
|
||||
STANDSTILL_THRESHOLD = 0.1
|
||||
GAS_PRESSED_THRESHOLD = 5.0
|
||||
|
||||
STEER_ANGLE_MAX = 359.9
|
||||
STEER_ANGLE_TEST_MAX = 350.0
|
||||
DEG_TO_CAN = 1 / 0.04395
|
||||
ANGLE_RATE_BP = [7.0, 17.0, 36.0]
|
||||
ANGLE_RATE_UP = [2.0, 0.25, 0.1]
|
||||
ANGLE_RATE_DOWN = [2.0, 0.25, 0.1]
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerSafety("volvo_v40_2017_pt")
|
||||
self.safety = libsafety_py.libsafety
|
||||
self.safety.set_safety_hooks(SAFETY_VOLVO, VOLVO_FLAG_C1)
|
||||
self.safety.init_tests()
|
||||
|
||||
def _angle_cmd_msg(self, angle: float, enabled: bool, increment_timer: bool = True):
|
||||
values = {
|
||||
"SET_X_E3": 0xE3,
|
||||
"SET_X_B4": 0xB4,
|
||||
"SET_X_08": 0x08,
|
||||
"LKAAngleReq": angle,
|
||||
"LKASteerDirection": 3 if enabled else 0,
|
||||
"TrqLim": 0,
|
||||
"SET_X_25": 0x25,
|
||||
"SET_X_02": 0x02,
|
||||
}
|
||||
|
||||
def fix_checksum(msg):
|
||||
address, data, bus = msg
|
||||
data = bytearray(data)
|
||||
data[6] = create_c1_checksum(data)
|
||||
return address, data, bus
|
||||
|
||||
return self.packer.make_can_msg_safety("FSM1", VOLVO_MAIN_BUS, values, fix_checksum)
|
||||
|
||||
def _angle_meas_msg(self, angle: float):
|
||||
return self.packer.make_can_msg_safety(
|
||||
"PSCM1", VOLVO_MAIN_BUS, {"SteeringAngleServo": angle})
|
||||
|
||||
def _speed_msg(self, speed):
|
||||
return self.packer.make_can_msg_safety(
|
||||
"VehicleSpeed1", VOLVO_MAIN_BUS, {"VehicleSpeed": speed * 3.6})
|
||||
|
||||
def _speed_msg_2(self, speed):
|
||||
return None
|
||||
|
||||
def _user_brake_msg(self, brake):
|
||||
return self.packer.make_can_msg_safety(
|
||||
"PedalandBrake", VOLVO_MAIN_BUS, {"BrakePedalActive2": bool(brake)})
|
||||
|
||||
def _user_gas_msg(self, gas):
|
||||
return self.packer.make_can_msg_safety(
|
||||
"PedalandBrake", VOLVO_MAIN_BUS, {"AccPedal": gas})
|
||||
|
||||
def _pcm_status_msg(self, enable):
|
||||
return self.packer.make_can_msg_safety(
|
||||
"FSM0", VOLVO_PARTY_BUS, {"ACCStatusActive": bool(enable)})
|
||||
|
||||
def test_cancel_button_only(self):
|
||||
allowed = self.packer.make_can_msg_safety(
|
||||
"CCButtons", VOLVO_MAIN_BUS, {"ACCStopBtn": 1})
|
||||
self.assertTrue(self._tx(allowed))
|
||||
|
||||
for signal in ("ACCOnOffBtn", "ACCSetBtn", "ACCResumeBtn", "ACCMinusBtn",
|
||||
"TimeGapIncreaseBtn", "TimeGapDecreaseBtn"):
|
||||
msg = self.packer.make_can_msg_safety("CCButtons", VOLVO_MAIN_BUS, {signal: 1})
|
||||
self.assertFalse(self._tx(msg), signal)
|
||||
|
||||
def test_pscm_relay_cannot_invent_angle(self):
|
||||
for _ in range(common.MAX_SAMPLE_VALS):
|
||||
self._rx(self._angle_meas_msg(10))
|
||||
valid = self.packer.make_can_msg_safety(
|
||||
"PSCM1", VOLVO_PARTY_BUS, {"SteeringAngleServo": 10})
|
||||
invalid = self.packer.make_can_msg_safety(
|
||||
"PSCM1", VOLVO_PARTY_BUS, {"SteeringAngleServo": 20})
|
||||
self.assertTrue(self._tx(valid))
|
||||
self.assertFalse(self._tx(invalid))
|
||||
|
||||
def test_pscm_relay_preserves_full_lock_angle(self):
|
||||
for angle in (-720, 500):
|
||||
for _ in range(common.MAX_SAMPLE_VALS):
|
||||
self._rx(self._angle_meas_msg(angle))
|
||||
relayed = self.packer.make_can_msg_safety(
|
||||
"PSCM1", VOLVO_PARTY_BUS, {"SteeringAngleServo": angle})
|
||||
self.assertTrue(self._tx(relayed), angle)
|
||||
|
||||
def test_steering_static_fields_and_checksum(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
self._reset_angle_measurement(0)
|
||||
self._reset_speed_measurement(10)
|
||||
self._set_prev_desired_angle(0)
|
||||
valid = self._angle_cmd_msg(0, True)
|
||||
self.assertTrue(self._tx(valid))
|
||||
|
||||
for byte_index in (0, 1, 2, 3, 4, 6, 7):
|
||||
invalid = self._angle_cmd_msg(0, True)
|
||||
invalid[0].data[byte_index] ^= 0x4 if byte_index in (4, 7) else 0x1
|
||||
self.assertFalse(self._tx(invalid), byte_index)
|
||||
|
||||
def test_controller_steering_message_is_allowed(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
self._reset_angle_measurement(0)
|
||||
self._reset_speed_measurement(10)
|
||||
self._set_prev_desired_angle(0)
|
||||
address, data, bus = create_c1_steering_control(self.packer, 0, True)
|
||||
self.assertTrue(self._tx(libsafety_py.make_CANPacket(address, bus, data)))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,2 +1,2 @@
|
||||
extern const uint8_t gitversion[19];
|
||||
const uint8_t gitversion[19] = "DEV-c03d06b4-DEBUG";
|
||||
const uint8_t gitversion[19] = "DEV-eedd73e5-DEBUG";
|
||||
|
||||
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.
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.
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.
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.
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.
@@ -1 +1 @@
|
||||
DEV-c03d06b4-DEBUG
|
||||
DEV-eedd73e5-DEBUG
|
||||
@@ -284,32 +284,22 @@ ensure_host_python_extensions() {
|
||||
}
|
||||
|
||||
sync_host_generated_headers() {
|
||||
local capnp_bin=""
|
||||
local candidate=""
|
||||
for candidate in "${HOST_VENV}"/lib/python*/site-packages/capnproto/install/bin; do
|
||||
if [[ -x "${candidate}/capnpc" ]]; then
|
||||
capnp_bin="${candidate}"
|
||||
break
|
||||
fi
|
||||
done
|
||||
|
||||
local capnpc_cmd="capnpc"
|
||||
if [[ -n "${capnp_bin}" ]]; then
|
||||
capnpc_cmd="${capnp_bin}/capnpc"
|
||||
export PATH="${capnp_bin}:${PATH}"
|
||||
elif ! command -v capnpc >/dev/null 2>&1; then
|
||||
local capnpc="${ROOT_DIR}/.venv/bin/capnpc"
|
||||
local capnpc_cpp
|
||||
capnpc_cpp="$(find "${ROOT_DIR}/.venv/lib" -path '*/capnproto/install/bin/capnpc-c++' -type f -print -quit)"
|
||||
if [[ ! -x "${capnpc}" || ! -x "${capnpc_cpp}" ]]; then
|
||||
return
|
||||
fi
|
||||
|
||||
(
|
||||
cd "${WORK_DIR}"
|
||||
mkdir -p cereal/gen/cpp
|
||||
"${capnpc_cmd}" --src-prefix=cereal \
|
||||
"${capnpc}" --src-prefix=cereal \
|
||||
cereal/log.capnp \
|
||||
cereal/car.capnp \
|
||||
cereal/legacy.capnp \
|
||||
cereal/custom.capnp \
|
||||
-o c++:cereal/gen/cpp/
|
||||
-o "${capnpc_cpp}:cereal/gen/cpp/"
|
||||
)
|
||||
}
|
||||
|
||||
|
||||
@@ -69,10 +69,6 @@ def build_compile_env(*, supercombo: bool = False) -> dict[str, str]:
|
||||
int(str(env.get(key)), 0)
|
||||
except (TypeError, ValueError):
|
||||
env[key] = default
|
||||
if supercombo:
|
||||
# Unified supercombo artifacts must use upstream compile defaults. The
|
||||
# legacy QCOM tuning causes a reproducible HCQ timeline failure here.
|
||||
env.pop("QCOM_PRIORITY", None)
|
||||
return env
|
||||
|
||||
|
||||
@@ -88,9 +84,14 @@ def wait_for_external_gpu() -> None:
|
||||
|
||||
|
||||
def external_gpu_compile_command(command: list[str]) -> list[str]:
|
||||
"""Pin USB-GPU compilation to AGNOS' isolated CPU without changing host builds."""
|
||||
"""Pin USB-GPU compilation when AGNOS exposes the isolated CPU."""
|
||||
if sys.platform == "linux" and platform.machine() == "aarch64":
|
||||
return ["taskset", "-c", "7", *command]
|
||||
try:
|
||||
available_cpus = os.sched_getaffinity(0)
|
||||
if 7 in available_cpus:
|
||||
return ["taskset", "-c", "7", *command]
|
||||
except (AttributeError, OSError):
|
||||
pass
|
||||
return command
|
||||
|
||||
|
||||
|
||||
@@ -19,6 +19,7 @@ import shlex
|
||||
import shutil
|
||||
import subprocess
|
||||
import sys
|
||||
import tempfile
|
||||
import urllib.error
|
||||
import urllib.parse
|
||||
import urllib.request
|
||||
@@ -619,6 +620,70 @@ def update_manifest(repo: Path, info: ReleaseInfo, result: dict, manifest_versio
|
||||
return path
|
||||
|
||||
|
||||
def manifest_models(payload: object) -> list[dict]:
|
||||
models = payload.get("models") if isinstance(payload, dict) else payload
|
||||
if not isinstance(models, list) or not models:
|
||||
raise ReleaseError("Unsupported or empty Hugging Face manifest")
|
||||
if any(not isinstance(model, dict) or not str(model.get("id") or "").strip() for model in models):
|
||||
raise ReleaseError("Hugging Face manifest contains an invalid model entry")
|
||||
model_ids = [str(model["id"]).strip() for model in models]
|
||||
if len(model_ids) != len(set(model_ids)):
|
||||
raise ReleaseError("Hugging Face manifest contains duplicate model IDs")
|
||||
return models
|
||||
|
||||
|
||||
def accelerator_artifact_map(payload: object) -> dict[tuple[str, str], dict]:
|
||||
artifacts: dict[tuple[str, str], dict] = {}
|
||||
for model in manifest_models(payload):
|
||||
model_id = str(model.get("id") or "").strip()
|
||||
model_artifacts = model.get("accelerator_artifacts")
|
||||
if not model_id or not isinstance(model_artifacts, dict):
|
||||
continue
|
||||
for accelerator, metadata in model_artifacts.items():
|
||||
if isinstance(metadata, dict):
|
||||
artifacts[(model_id, str(accelerator))] = metadata
|
||||
return artifacts
|
||||
|
||||
|
||||
def validate_manifest_update(before: object, after: object, replacing_model_id: str) -> None:
|
||||
before_by_id = {str(model["id"]).strip(): model for model in manifest_models(before)}
|
||||
after_by_id = {str(model["id"]).strip(): model for model in manifest_models(after)}
|
||||
before_ids = set(before_by_id)
|
||||
after_ids = set(after_by_id)
|
||||
expected_ids = before_ids | {replacing_model_id}
|
||||
if after_ids != expected_ids:
|
||||
missing = sorted(expected_ids - after_ids)
|
||||
unexpected = sorted(after_ids - expected_ids)
|
||||
raise ReleaseError(
|
||||
"Refusing to publish a manifest with an unexpected model set"
|
||||
+ (f"; missing: {', '.join(missing)}" if missing else "")
|
||||
+ (f"; unexpected: {', '.join(unexpected)}" if unexpected else "")
|
||||
)
|
||||
|
||||
before_artifacts = accelerator_artifact_map(before)
|
||||
after_artifacts = accelerator_artifact_map(after)
|
||||
regressions = [
|
||||
f"{model_id}:{accelerator}"
|
||||
for (model_id, accelerator), metadata in before_artifacts.items()
|
||||
if model_id != replacing_model_id and after_artifacts.get((model_id, accelerator)) != metadata
|
||||
]
|
||||
if regressions:
|
||||
raise ReleaseError(
|
||||
"Refusing to publish a manifest that removes or changes existing accelerator metadata for: "
|
||||
+ ", ".join(sorted(regressions))
|
||||
)
|
||||
|
||||
unrelated_changes = sorted(
|
||||
model_id for model_id, model in before_by_id.items()
|
||||
if model_id != replacing_model_id and after_by_id[model_id] != model
|
||||
)
|
||||
if unrelated_changes:
|
||||
raise ReleaseError(
|
||||
"Refusing to publish a manifest that changes unrelated model entries: "
|
||||
+ ", ".join(unrelated_changes)
|
||||
)
|
||||
|
||||
|
||||
def find_hf() -> str:
|
||||
candidates = [shutil.which("hf"), str(Path.home() / ".local/bin/hf")]
|
||||
for candidate in candidates:
|
||||
@@ -633,15 +698,42 @@ def hf_copy(source: Path, bucket: str, remote_path: str) -> None:
|
||||
run([hf, "buckets", "cp", str(source), destination, "--format", "quiet"])
|
||||
|
||||
|
||||
def refresh_huggingface_manifest(manifest: Path, bucket: str) -> dict:
|
||||
remote_path = f"manifests/{manifest.name}"
|
||||
source = f"hf://buckets/{bucket}/{remote_path}"
|
||||
manifest.parent.mkdir(parents=True, exist_ok=True)
|
||||
with tempfile.TemporaryDirectory(prefix=".model-release-", dir=manifest.parent) as temporary_dir:
|
||||
candidate = Path(temporary_dir) / manifest.name
|
||||
run([find_hf(), "buckets", "cp", source, str(candidate), "--format", "quiet"])
|
||||
try:
|
||||
payload = json.loads(candidate.read_text(encoding="utf-8"))
|
||||
except (OSError, json.JSONDecodeError) as error:
|
||||
raise ReleaseError(f"Invalid live Hugging Face manifest: {error}") from error
|
||||
accelerator_artifact_map(payload)
|
||||
candidate.replace(manifest)
|
||||
return payload
|
||||
|
||||
|
||||
def prepare_huggingface_manifest(manifest: Path, info: ReleaseInfo, result: dict,
|
||||
bucket: str, manifest_version: str) -> Path:
|
||||
live_payload = refresh_huggingface_manifest(manifest, bucket)
|
||||
updated_manifest = update_manifest(manifest.parent, info, result, manifest_version)
|
||||
updated_payload = json.loads(updated_manifest.read_text(encoding="utf-8"))
|
||||
validate_manifest_update(live_payload, updated_payload, info.model_id)
|
||||
return updated_manifest
|
||||
|
||||
|
||||
def upload_huggingface(info: ReleaseInfo, result: dict, workspace: Path, bucket: str, manifest_version: str,
|
||||
manifest: Path, upload_onnx: bool, source: Path) -> None:
|
||||
manifest: Path, upload_onnx: bool, source: Path) -> Path:
|
||||
artifact_dir = Path(result["path"])
|
||||
for filename in result["files"]:
|
||||
hf_copy(artifact_dir / filename, bucket, f"models/{manifest_version}/{info.model_id}/{filename}")
|
||||
if upload_onnx:
|
||||
hf_copy(source, bucket, f"onnx/{info.model_id}/{source.name}")
|
||||
manifest = prepare_huggingface_manifest(manifest, info, result, bucket, manifest_version)
|
||||
hf_copy(manifest, bucket, f"manifests/{manifest.name}")
|
||||
print(f"Hugging Face upload complete: {bucket}/models/{manifest_version}/{info.model_id}/")
|
||||
return manifest
|
||||
|
||||
|
||||
def git_output(repo: Path, args: list[str]) -> str:
|
||||
@@ -777,9 +869,9 @@ def main() -> int:
|
||||
result = remote_compile(info, source, ip, workspace, args.keep_device_files)
|
||||
resources_repo = args.resources_repo.expanduser().resolve()
|
||||
check_resources_repo(resources_repo, args.resources_branch)
|
||||
manifest = update_manifest(resources_repo, info, result, args.manifest_version)
|
||||
upload_huggingface(info, result, workspace, args.hf_bucket, args.manifest_version,
|
||||
manifest, not args.no_onnx_upload, source)
|
||||
manifest = resources_repo / f"model_names_{args.manifest_version}.json"
|
||||
manifest = upload_huggingface(info, result, workspace, args.hf_bucket, args.manifest_version,
|
||||
manifest, not args.no_onnx_upload, source)
|
||||
push_github(info, result, resources_repo, args.manifest_version, manifest, args.resources_branch, args.force)
|
||||
print("\nRelease complete.")
|
||||
print(f" local artifact: {result['path']}")
|
||||
|
||||
@@ -3,8 +3,20 @@ from __future__ import annotations
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from scripts import model_release
|
||||
from scripts.model_release import parse_lfs_pointer, parse_pasted_release, runtime_file, update_manifest
|
||||
from scripts.model_release import (
|
||||
ReleaseError,
|
||||
parse_lfs_pointer,
|
||||
parse_pasted_release,
|
||||
prepare_huggingface_manifest,
|
||||
refresh_huggingface_manifest,
|
||||
runtime_file,
|
||||
update_manifest,
|
||||
upload_huggingface,
|
||||
validate_manifest_update,
|
||||
)
|
||||
|
||||
|
||||
RELEASE_TEXT = """
|
||||
@@ -95,3 +107,138 @@ def test_update_manifest_replaces_one_entry(tmp_path: Path):
|
||||
assert entry["artifact_size"] == 123
|
||||
assert entry["artifact_chunk_count"] == 2
|
||||
assert entry["uses_external_gpu"]
|
||||
|
||||
|
||||
def test_prepare_manifest_refreshes_live_copy_before_adding_model(tmp_path: Path, monkeypatch):
|
||||
manifest = tmp_path / "model_names_v25.json"
|
||||
manifest.write_text(json.dumps({"models": [{"id": "stale"}]}) + "\n")
|
||||
live_payload = {
|
||||
"models": [{
|
||||
"id": "small-model",
|
||||
"model_lab_eligible": True,
|
||||
"accelerator_artifacts": {
|
||||
"chestnut": {
|
||||
"artifact_filename": "small-model_driving_chestnut_tinygrad.pkl",
|
||||
"artifact_sha256": "b" * 64,
|
||||
"execution_device": "AMD",
|
||||
},
|
||||
},
|
||||
}],
|
||||
}
|
||||
|
||||
def fake_refresh(path, bucket):
|
||||
assert bucket == "StarPilot-Driving/StarPilot-Resources"
|
||||
path.write_text(json.dumps(live_payload) + "\n")
|
||||
return live_payload
|
||||
|
||||
monkeypatch.setattr(model_release, "refresh_huggingface_manifest", fake_refresh)
|
||||
info = parse_pasted_release(RELEASE_TEXT, "bmrlnapv4", "v16")
|
||||
prepared = prepare_huggingface_manifest(
|
||||
manifest,
|
||||
info,
|
||||
{"size": 123, "sha256": "a" * 64, "chunk_count": 2},
|
||||
"StarPilot-Driving/StarPilot-Resources",
|
||||
"v25",
|
||||
)
|
||||
|
||||
models = {entry["id"]: entry for entry in json.loads(prepared.read_text())["models"]}
|
||||
assert set(models) == {"small-model", "bmrlnapv4"}
|
||||
assert models["small-model"] == live_payload["models"][0]
|
||||
assert models["bmrlnapv4"]["artifact_sha256"] == "a" * 64
|
||||
|
||||
|
||||
def test_manifest_guard_rejects_unrelated_accelerator_metadata_regression():
|
||||
chestnut = {"execution_device": "AMD", "artifact_sha256": "a" * 64}
|
||||
before = {"models": [
|
||||
{"id": "keep", "accelerator_artifacts": {"chestnut": chestnut}},
|
||||
{"id": "replace", "accelerator_artifacts": {"chestnut": chestnut}},
|
||||
]}
|
||||
after = {"models": [{"id": "keep"}, {"id": "replace"}]}
|
||||
|
||||
with pytest.raises(ReleaseError, match="keep:chestnut"):
|
||||
validate_manifest_update(before, after, "replace")
|
||||
|
||||
validate_manifest_update(
|
||||
{"models": [{"id": "replace", "accelerator_artifacts": {"chestnut": chestnut}}]},
|
||||
{"models": [{"id": "replace"}]},
|
||||
"replace",
|
||||
)
|
||||
|
||||
with pytest.raises(ReleaseError, match="unrelated model entries: keep"):
|
||||
validate_manifest_update(
|
||||
{"models": [{"id": "keep", "model_lab_eligible": True}]},
|
||||
{"models": [{"id": "keep", "model_lab_eligible": False}, {"id": "replace"}]},
|
||||
"replace",
|
||||
)
|
||||
|
||||
|
||||
def test_refresh_manifest_is_atomic_and_uses_hugging_face_bucket(tmp_path: Path, monkeypatch):
|
||||
manifest = tmp_path / "model_names_v25.json"
|
||||
manifest.write_text(json.dumps({"models": [{"id": "stale"}]}) + "\n")
|
||||
live_payload = {"models": [{"id": "live"}]}
|
||||
commands = []
|
||||
|
||||
monkeypatch.setattr(model_release, "find_hf", lambda: "/usr/bin/hf")
|
||||
|
||||
def fake_run(command, **kwargs):
|
||||
commands.append(command)
|
||||
Path(command[4]).write_text(json.dumps(live_payload) + "\n")
|
||||
|
||||
monkeypatch.setattr(model_release, "run", fake_run)
|
||||
assert refresh_huggingface_manifest(manifest, "owner/resources") == live_payload
|
||||
assert json.loads(manifest.read_text()) == live_payload
|
||||
assert commands[0][0:4] == [
|
||||
"/usr/bin/hf",
|
||||
"buckets",
|
||||
"cp",
|
||||
"hf://buckets/owner/resources/manifests/model_names_v25.json",
|
||||
]
|
||||
|
||||
|
||||
def test_refresh_manifest_keeps_local_copy_when_live_json_is_invalid(tmp_path: Path, monkeypatch):
|
||||
manifest = tmp_path / "model_names_v25.json"
|
||||
original = {"models": [{"id": "safe"}]}
|
||||
manifest.write_text(json.dumps(original) + "\n")
|
||||
monkeypatch.setattr(model_release, "find_hf", lambda: "/usr/bin/hf")
|
||||
monkeypatch.setattr(model_release, "run", lambda command, **kwargs: Path(command[4]).write_text("{"))
|
||||
|
||||
with pytest.raises(ReleaseError, match="Invalid live Hugging Face manifest"):
|
||||
refresh_huggingface_manifest(manifest, "owner/resources")
|
||||
assert json.loads(manifest.read_text()) == original
|
||||
|
||||
|
||||
def test_upload_refreshes_manifest_after_artifacts(tmp_path: Path, monkeypatch):
|
||||
artifact_dir = tmp_path / "artifacts"
|
||||
artifact_dir.mkdir()
|
||||
manifest = tmp_path / "resources" / "model_names_v25.json"
|
||||
manifest.parent.mkdir()
|
||||
manifest.write_text(json.dumps({"models": [{"id": "old"}]}) + "\n")
|
||||
source = tmp_path / "model.onnx"
|
||||
calls = []
|
||||
info = parse_pasted_release(RELEASE_TEXT, "bmrlnapv4", "v16")
|
||||
result = {
|
||||
"path": str(artifact_dir),
|
||||
"files": ["bmrlnapv4_driving_tinygrad.pkl"],
|
||||
"size": 123,
|
||||
"sha256": "a" * 64,
|
||||
"chunk_count": 0,
|
||||
}
|
||||
|
||||
monkeypatch.setattr(model_release, "hf_copy", lambda source, bucket, remote: calls.append(("copy", remote)))
|
||||
|
||||
def fake_prepare(path, release_info, release_result, bucket, version):
|
||||
calls.append(("prepare", path.name))
|
||||
return path
|
||||
|
||||
monkeypatch.setattr(model_release, "prepare_huggingface_manifest", fake_prepare)
|
||||
returned = upload_huggingface(
|
||||
info, result, tmp_path, "owner/resources", "v25", manifest, True, source,
|
||||
)
|
||||
|
||||
assert returned == manifest
|
||||
assert calls == [
|
||||
("copy", "models/v25/bmrlnapv4/bmrlnapv4_driving_tinygrad.pkl"),
|
||||
("copy", "onnx/bmrlnapv4/model.onnx"),
|
||||
("prepare", "model_names_v25.json"),
|
||||
("copy", "manifests/model_names_v25.json"),
|
||||
]
|
||||
|
||||
+30
-94
@@ -43,8 +43,6 @@ REDNECK_DECREASE_LOOKAHEAD_POINTS = 10
|
||||
SLC_SOURCE_NONE = "None"
|
||||
EventName = log.OnroadEvent.EventName
|
||||
|
||||
GM_GPS_SOURCES = (("device", 2.0), ("pps", 1.0), ("onstar", 2.5))
|
||||
|
||||
# forward
|
||||
carlog.addHandler(ForwardingHandler(cloudlog))
|
||||
|
||||
@@ -78,22 +76,6 @@ def can_comm_callbacks(logcan: messaging.SubSocket, sendcan: messaging.PubSocket
|
||||
return can_recv, can_send
|
||||
|
||||
|
||||
def _gps_sample_is_healthy(sample: dict) -> bool:
|
||||
if not sample.get("hasFix", False):
|
||||
return False
|
||||
try:
|
||||
latitude = float(sample["latitude"])
|
||||
longitude = float(sample["longitude"])
|
||||
return (math.isfinite(latitude) and math.isfinite(longitude) and
|
||||
-90.0 <= latitude <= 90.0 and -180.0 <= longitude <= 180.0 and
|
||||
(latitude != 0.0 or longitude != 0.0) and
|
||||
math.isfinite(float(sample["altitude"])) and
|
||||
math.isfinite(float(sample["speed"])) and
|
||||
math.isfinite(float(sample["bearingDeg"])))
|
||||
except (KeyError, TypeError, ValueError):
|
||||
return False
|
||||
|
||||
|
||||
class Car:
|
||||
CI: CarInterfaceBase
|
||||
RI: RadarInterfaceBase
|
||||
@@ -103,9 +85,7 @@ class Car:
|
||||
|
||||
def __init__(self, CI=None, RI=None) -> None:
|
||||
self.can_sock = messaging.sub_sock('can', timeout=20)
|
||||
self.sm = messaging.SubMaster([
|
||||
'pandaStates', 'carControl', 'onroadEvents', 'radarState', 'longitudinalPlan', 'gpsLocation',
|
||||
])
|
||||
self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents', 'radarState', 'longitudinalPlan'])
|
||||
self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'])
|
||||
self.gps_pm = None
|
||||
|
||||
@@ -113,8 +93,6 @@ class Car:
|
||||
self._last_car_gps_timestamp_nanos = 0
|
||||
self._last_car_gps_received_monotonic = 0.0
|
||||
self._last_car_gps_publish_monotonic = 0.0
|
||||
self._gm_gps = dict.fromkeys(source for source, _ in GM_GPS_SOURCES)
|
||||
self._gm_gps_had_fix = False
|
||||
|
||||
self.CC_prev = car.CarControl.new_message()
|
||||
self.CS_prev = car.CarState.new_message()
|
||||
@@ -163,9 +141,8 @@ class Car:
|
||||
self.RI = RI
|
||||
|
||||
car_gps_supported = bool(getattr(self.CI.CS, 'car_gps_supported', False))
|
||||
self.gm_gps_supported = self.CP.brand == "gm"
|
||||
self.params.put_bool("CarGpsAvailable", car_gps_supported)
|
||||
if car_gps_supported or self.gm_gps_supported:
|
||||
if car_gps_supported:
|
||||
self.gps_pm = messaging.PubMaster(['gpsLocationExternal'])
|
||||
|
||||
aol_available = always_on_lateral_available(self.CP)
|
||||
@@ -372,80 +349,39 @@ class Car:
|
||||
FPCS = self.starpilot_card.update(CS, FPCS, self.sm, self.starpilot_toggles)
|
||||
return CS, RD, FPCS
|
||||
|
||||
def _publish_gm_gps(self, now: float) -> None:
|
||||
if self.sm.updated.get("gpsLocation", False):
|
||||
self._gm_gps["device"] = (self.sm["gpsLocation"].to_dict(), now) if self.sm.valid["gpsLocation"] else None
|
||||
|
||||
for source, sample in self.CI.CS.get_car_gps_sources().items():
|
||||
previous = self._gm_gps[source]
|
||||
if sample is None:
|
||||
self._gm_gps[source] = None
|
||||
elif previous is None or sample["timestamp_nanos"] > previous[0]["timestamp_nanos"]:
|
||||
self._gm_gps[source] = (sample.copy(), now)
|
||||
|
||||
# Device > PPS > OnStar, with each source's own freshness window.
|
||||
selected = None
|
||||
for source, timeout in GM_GPS_SOURCES:
|
||||
candidate = self._gm_gps[source]
|
||||
if candidate is not None and now - candidate[1] <= timeout and _gps_sample_is_healthy(candidate[0]):
|
||||
selected = candidate[0]
|
||||
break
|
||||
|
||||
if selected is None:
|
||||
if not self._gm_gps_had_fix:
|
||||
return
|
||||
elif self._gm_gps_had_fix and (now - self._last_car_gps_publish_monotonic < 0.2):
|
||||
return
|
||||
|
||||
gps_send = messaging.new_message('gpsLocationExternal', valid=selected is not None)
|
||||
if selected is not None:
|
||||
gps_send.gpsLocationExternal = {key: value for key, value in selected.items() if key != "timestamp_nanos"}
|
||||
if source != "device":
|
||||
gps_send.gpsLocationExternal.source = "car"
|
||||
else:
|
||||
# Clear subscribers' cached fix once, then let the service become stale.
|
||||
gps_send.gpsLocationExternal.hasFix = False
|
||||
assert self.gps_pm is not None
|
||||
self.gps_pm.send('gpsLocationExternal', gps_send)
|
||||
self._gm_gps_had_fix = selected is not None
|
||||
self._last_car_gps_publish_monotonic = now
|
||||
|
||||
def state_publish(self, CS: car.CarState, RD: structs.RadarDataT | None, FPCS: custom.StarPilotCarState):
|
||||
"""carState and carParams publish loop"""
|
||||
|
||||
get_car_gps = getattr(self.CI.CS, 'get_car_gps', None)
|
||||
car_gps = get_car_gps() if get_car_gps is not None else None
|
||||
now = time.monotonic()
|
||||
if self.gm_gps_supported:
|
||||
self._publish_gm_gps(now)
|
||||
else:
|
||||
get_car_gps = getattr(self.CI.CS, 'get_car_gps', None)
|
||||
car_gps = get_car_gps() if get_car_gps is not None else None
|
||||
if car_gps is not None and car_gps['timestamp_nanos'] > self._last_car_gps_timestamp_nanos:
|
||||
self._last_car_gps_timestamp_nanos = car_gps['timestamp_nanos']
|
||||
self._last_car_gps_received_monotonic = now
|
||||
if car_gps is not None and car_gps['timestamp_nanos'] > self._last_car_gps_timestamp_nanos:
|
||||
self._last_car_gps_timestamp_nanos = car_gps['timestamp_nanos']
|
||||
self._last_car_gps_received_monotonic = now
|
||||
|
||||
if car_gps is not None and self._last_car_gps_received_monotonic > 0.0 and \
|
||||
now - self._last_car_gps_received_monotonic <= 2.5 and \
|
||||
now - self._last_car_gps_publish_monotonic >= 0.2:
|
||||
gps_send = messaging.new_message('gpsLocationExternal', valid=True)
|
||||
gps = gps_send.gpsLocationExternal
|
||||
gps.flags = 0
|
||||
gps.latitude = car_gps['latitude']
|
||||
gps.longitude = car_gps['longitude']
|
||||
gps.altitude = car_gps['altitude']
|
||||
gps.speed = car_gps['speed']
|
||||
gps.bearingDeg = car_gps['bearingDeg']
|
||||
gps.horizontalAccuracy = car_gps['horizontalAccuracy']
|
||||
gps.unixTimestampMillis = car_gps['unixTimestampMillis']
|
||||
gps.source = log.GpsLocationData.SensorSource.car
|
||||
gps.vNED = car_gps['vNED']
|
||||
gps.verticalAccuracy = car_gps['verticalAccuracy']
|
||||
gps.bearingAccuracyDeg = car_gps['bearingAccuracyDeg']
|
||||
gps.speedAccuracy = car_gps['speedAccuracy']
|
||||
gps.hasFix = car_gps['hasFix']
|
||||
gps.satelliteCount = car_gps['satelliteCount']
|
||||
assert self.gps_pm is not None
|
||||
self.gps_pm.send('gpsLocationExternal', gps_send)
|
||||
self._last_car_gps_publish_monotonic = now
|
||||
if car_gps is not None and self._last_car_gps_received_monotonic > 0.0 and \
|
||||
now - self._last_car_gps_received_monotonic <= 2.5 and \
|
||||
now - self._last_car_gps_publish_monotonic >= 0.2:
|
||||
gps_send = messaging.new_message('gpsLocationExternal', valid=True)
|
||||
gps = gps_send.gpsLocationExternal
|
||||
gps.flags = 0
|
||||
gps.latitude = car_gps['latitude']
|
||||
gps.longitude = car_gps['longitude']
|
||||
gps.altitude = car_gps['altitude']
|
||||
gps.speed = car_gps['speed']
|
||||
gps.bearingDeg = car_gps['bearingDeg']
|
||||
gps.horizontalAccuracy = car_gps['horizontalAccuracy']
|
||||
gps.unixTimestampMillis = car_gps['unixTimestampMillis']
|
||||
gps.source = log.GpsLocationData.SensorSource.car
|
||||
gps.vNED = car_gps['vNED']
|
||||
gps.verticalAccuracy = car_gps['verticalAccuracy']
|
||||
gps.bearingAccuracyDeg = car_gps['bearingAccuracyDeg']
|
||||
gps.speedAccuracy = car_gps['speedAccuracy']
|
||||
gps.hasFix = car_gps['hasFix']
|
||||
gps.satelliteCount = car_gps['satelliteCount']
|
||||
assert self.gps_pm is not None
|
||||
self.gps_pm.send('gpsLocationExternal', gps_send)
|
||||
self._last_car_gps_publish_monotonic = now
|
||||
|
||||
# carParams - logged every 50 seconds (> 1 per segment)
|
||||
if self.sm.frame % int(50. / DT_CTRL) == 0:
|
||||
|
||||
@@ -214,7 +214,7 @@ class VCruiseHelper:
|
||||
|
||||
engage_floor_kph = max(V_CRUISE_MIN, 7.0 * CV.MPH_TO_KPH)
|
||||
resume_pressed = any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents)
|
||||
remembered_resume = resume_prev_button and (self.gm_cc_only or self.redneck_non_pcm)
|
||||
remembered_resume = resume_prev_button and self._uses_software_cruise()
|
||||
|
||||
if self.v_cruise_initialized and (resume_pressed or remembered_resume):
|
||||
self.v_cruise_kph = self.v_cruise_kph_last
|
||||
|
||||
@@ -313,6 +313,22 @@ class TestVCruiseHelper:
|
||||
assert V_CRUISE_MIN <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX
|
||||
assert self.v_cruise_helper.v_cruise_initialized
|
||||
|
||||
def test_resume_keeps_previous_software_cruise_speed(self):
|
||||
engage_cs = car.CarState(vEgo=75 * CV.MPH_TO_MS)
|
||||
self.v_cruise_helper.initialize_v_cruise(engage_cs, experimental_mode=False, resume_prev_button=False,
|
||||
starpilot_toggles=self.starpilot_toggles)
|
||||
|
||||
disabled_cs = car.CarState(cruiseState={"available": True})
|
||||
self.v_cruise_helper.update_v_cruise(disabled_cs, enabled=False, is_metric=False,
|
||||
speed_limit_changed=False, starpilot_toggles=self.starpilot_toggles)
|
||||
|
||||
resume_cs = car.CarState(vEgo=22 * CV.MPH_TO_MS)
|
||||
self.v_cruise_helper.initialize_v_cruise(resume_cs, experimental_mode=False, resume_prev_button=True,
|
||||
starpilot_toggles=self.starpilot_toggles)
|
||||
|
||||
assert self.v_cruise_helper.v_cruise_kph == pytest.approx(75 * CV.MPH_TO_KPH)
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(75 * CV.MPH_TO_KPH)
|
||||
|
||||
def test_initialize_v_cruise_matches_speed_limit(self):
|
||||
self.reset_cruise_speed_state()
|
||||
self.starpilot_toggles.set_speed_limit = True
|
||||
@@ -483,39 +499,33 @@ class TestVCruiseHelper:
|
||||
assert self.v_cruise_helper.v_cruise_kph == pytest.approx(initial_v_cruise_kph + IMPERIAL_INCREMENT)
|
||||
|
||||
@pytest.mark.parametrize("openpilot_longitudinal", [False, True])
|
||||
def test_pcm_cruise_uses_pcm_speed(self, openpilot_longitudinal):
|
||||
def test_pcm_cruise_always_tracks_pcm_speed(self, openpilot_longitudinal):
|
||||
CP = car.CarParams(pcmCruise=True, openpilotLongitudinalControl=openpilot_longitudinal)
|
||||
helper = VCruiseHelper(CP)
|
||||
toggles = SimpleNamespace(cruise_increase=5, cruise_increase_long=1, set_speed_limit=False)
|
||||
pcm_speed_kph = 72.0
|
||||
pcm_cluster_speed_kph = 71.0
|
||||
|
||||
helper.initialize_v_cruise(car.CarState(vEgo=40 * CV.KPH_TO_MS), False, False, toggles)
|
||||
assert not helper.v_cruise_initialized
|
||||
|
||||
cs = car.CarState(
|
||||
cruiseState={
|
||||
"available": True,
|
||||
"speed": pcm_speed_kph * CV.KPH_TO_MS,
|
||||
"speedCluster": pcm_cluster_speed_kph * CV.KPH_TO_MS,
|
||||
},
|
||||
samples = (
|
||||
(72.0, 71.0, None),
|
||||
(25.0, 25.0, {"type": ButtonType.decelCruise, "pressed": True}),
|
||||
(65.0, 65.0, {"type": ButtonType.decelCruise, "pressed": False}),
|
||||
(90.0, 90.0, {"type": ButtonType.accelCruise, "pressed": True}),
|
||||
(5.0, 5.0, {"type": ButtonType.accelCruise, "pressed": False}),
|
||||
)
|
||||
|
||||
helper.update_v_cruise(cs, True, True, False, toggles)
|
||||
assert helper.v_cruise_kph == pytest.approx(pcm_speed_kph)
|
||||
assert helper.v_cruise_cluster_kph == pytest.approx(pcm_cluster_speed_kph)
|
||||
|
||||
next_pcm_speed_kph = 74.0
|
||||
next_cs = car.CarState(
|
||||
cruiseState={
|
||||
"available": True,
|
||||
"speed": next_pcm_speed_kph * CV.KPH_TO_MS,
|
||||
"speedCluster": next_pcm_speed_kph * CV.KPH_TO_MS,
|
||||
},
|
||||
buttonEvents=[{"type": ButtonType.accelCruise, "pressed": False}],
|
||||
)
|
||||
helper.update_v_cruise(next_cs, True, True, False, toggles)
|
||||
assert helper.v_cruise_kph == pytest.approx(next_pcm_speed_kph)
|
||||
for pcm_speed_kph, pcm_cluster_speed_kph, button_event in samples:
|
||||
cs = car.CarState(
|
||||
cruiseState={
|
||||
"available": True,
|
||||
"speed": pcm_speed_kph * CV.KPH_TO_MS,
|
||||
"speedCluster": pcm_cluster_speed_kph * CV.KPH_TO_MS,
|
||||
},
|
||||
buttonEvents=[] if button_event is None else [button_event],
|
||||
)
|
||||
helper.update_v_cruise(cs, True, True, False, toggles)
|
||||
assert helper.v_cruise_kph == pytest.approx(pcm_speed_kph)
|
||||
assert helper.v_cruise_cluster_kph == pytest.approx(pcm_cluster_speed_kph)
|
||||
|
||||
|
||||
class TestVCruiseHelperRedneck:
|
||||
|
||||
@@ -1,169 +0,0 @@
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
from cereal import messaging
|
||||
from openpilot.selfdrive.car.card import Car, GM_GPS_SOURCES
|
||||
|
||||
|
||||
def sample():
|
||||
gps = messaging.new_message("gpsLocation", valid=True).gpsLocation
|
||||
gps.hasFix = True
|
||||
gps.latitude = 38.3
|
||||
gps.longitude = -85.7
|
||||
gps.source = "ublox"
|
||||
return dict(gps.to_dict(), timestamp_nanos=1)
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def selector():
|
||||
card = Car.__new__(Car)
|
||||
card._gm_gps = dict.fromkeys(source for source, _ in GM_GPS_SOURCES)
|
||||
card._gm_gps_had_fix = False
|
||||
card._last_car_gps_publish_monotonic = 0.0
|
||||
card.gps_pm = SimpleNamespace(sent=[])
|
||||
card.gps_pm.send = lambda service, message: card.gps_pm.sent.append((service, message))
|
||||
card.sm = SimpleNamespace(updated={"gpsLocation": False})
|
||||
card.CI = SimpleNamespace(CS=SimpleNamespace(get_car_gps_sources=dict))
|
||||
return card
|
||||
|
||||
|
||||
@pytest.mark.parametrize("states,winner", [
|
||||
(("good", None, None), "device"),
|
||||
(("bad", "good", None), "pps"),
|
||||
(("stale", "good", None), "pps"),
|
||||
(("bad", "bad", "good"), "onstar"),
|
||||
(("good", "good", "good"), "device"),
|
||||
(("bad", "stale", None), None),
|
||||
(("stale", "stale", "good"), "onstar"),
|
||||
(("bad", "bad", "bad"), None),
|
||||
(("stale", "stale", "stale"), None),
|
||||
])
|
||||
def test_priority(selector, states, winner):
|
||||
for index, ((source, timeout), state) in enumerate(zip(GM_GPS_SOURCES, states, strict=True)):
|
||||
if state is not None:
|
||||
fix = sample() | {"latitude": 38.0 + index, "hasFix": state != "bad"}
|
||||
selector._gm_gps[source] = (fix, 10.0 - timeout - 0.01 if state == "stale" else 10.0)
|
||||
selector._publish_gm_gps(10.0)
|
||||
if winner is None:
|
||||
assert not selector.gps_pm.sent
|
||||
else:
|
||||
assert selector.gps_pm.sent[-1][1].gpsLocationExternal.latitude == selector._gm_gps[winner][0]["latitude"]
|
||||
|
||||
|
||||
@pytest.mark.parametrize("source,timeout", [("device", 2.0), ("pps", 1.0), ("onstar", 2.5)])
|
||||
def test_freshness_and_recovery(selector, source, timeout):
|
||||
selector._gm_gps[source] = (sample(), 10.0)
|
||||
selector._publish_gm_gps(10.0 + timeout)
|
||||
assert selector.gps_pm.sent[-1][1].valid
|
||||
selector._publish_gm_gps(10.01 + timeout)
|
||||
assert not selector.gps_pm.sent[-1][1].valid
|
||||
selector._gm_gps[source] = (sample(), 20.0)
|
||||
selector._publish_gm_gps(20.0)
|
||||
assert selector.gps_pm.sent[-1][1].valid
|
||||
|
||||
|
||||
@pytest.mark.parametrize("source", ["device", "pps"])
|
||||
def test_upward_recovery(selector, source):
|
||||
selector._gm_gps["onstar"] = (sample() | {"latitude": 40.0}, 10.0)
|
||||
selector._publish_gm_gps(10.0)
|
||||
assert selector.gps_pm.sent[-1][1].gpsLocationExternal.latitude == 40.0
|
||||
selector._gm_gps[source] = (sample(), 10.21)
|
||||
selector._publish_gm_gps(10.21)
|
||||
assert selector.gps_pm.sent[-1][1].gpsLocationExternal.latitude == 38.3
|
||||
|
||||
|
||||
@pytest.mark.parametrize("changes,expected", [
|
||||
({"latitude": float("nan")}, False), ({"longitude": float("inf")}, False),
|
||||
({"latitude": 91}, False), ({"longitude": -181}, False), ({"longitude": 181}, False),
|
||||
({"latitude": 0, "longitude": 0}, False), ({"speed": float("nan")}, False),
|
||||
({"latitude": 0, "longitude": 10}, True), ({"latitude": 10, "longitude": 0}, True),
|
||||
({"horizontalAccuracy": 500.0}, True),
|
||||
])
|
||||
def test_health(selector, changes, expected):
|
||||
selector._gm_gps = {source: (sample() | {"latitude": 40.0}, 10.0) for source, _ in GM_GPS_SOURCES}
|
||||
selector._gm_gps["device"] = (sample() | changes, 10.0)
|
||||
selector._publish_gm_gps(10.0)
|
||||
gps = selector.gps_pm.sent[-1][1].gpsLocationExternal
|
||||
assert gps.latitude == (selector._gm_gps["device"][0]["latitude"] if expected else 40.0)
|
||||
assert gps.source == ("ublox" if expected else "car")
|
||||
|
||||
|
||||
@pytest.mark.parametrize("source", [source for source, _ in GM_GPS_SOURCES])
|
||||
def test_selected_fix_fields_are_preserved(selector, source):
|
||||
fix = sample() | {"altitude": 123.0, "speed": 10.0, "vNED": [0.0, 10.0, 0.0], "bearingDeg": 90.0,
|
||||
"horizontalAccuracy": 3.0, "verticalAccuracy": 5.0, "bearingAccuracyDeg": 2.0}
|
||||
if source == "device":
|
||||
selector._gm_gps[source] = (fix, 10.0)
|
||||
else:
|
||||
selector.CI.CS.get_car_gps_sources = lambda: {source: fix}
|
||||
selector._publish_gm_gps(10.0)
|
||||
service, message = selector.gps_pm.sent[-1]
|
||||
assert service == "gpsLocationExternal"
|
||||
assert message.valid
|
||||
expected = {key: value for key, value in fix.items() if key != "timestamp_nanos"}
|
||||
expected["source"] = "ublox" if source == "device" else "car"
|
||||
assert message.gpsLocationExternal.to_dict() == expected
|
||||
|
||||
|
||||
@pytest.mark.parametrize("source", ["pps", "onstar"])
|
||||
def test_can_cache_freshness_and_explicit_invalidation(selector, source):
|
||||
sources = {"pps": sample(), "onstar": sample() | {"latitude": 40.0}}
|
||||
selector.CI.CS.get_car_gps_sources = lambda: sources
|
||||
selector._publish_gm_gps(10.0)
|
||||
for timestamp in (1, 0):
|
||||
sources[source] = sources[source] | {"timestamp_nanos": timestamp}
|
||||
selector._publish_gm_gps(10.5)
|
||||
assert selector._gm_gps[source][1] == 10.0
|
||||
sources[source] = sources[source] | {"timestamp_nanos": 2}
|
||||
selector._publish_gm_gps(10.6)
|
||||
assert selector._gm_gps[source][1] == 10.6
|
||||
sources[source] = None
|
||||
selector._publish_gm_gps(10.9)
|
||||
assert selector._gm_gps[source] is None
|
||||
assert selector.gps_pm.sent[-1][1].gpsLocationExternal.latitude == (40.0 if source == "pps" else 38.3)
|
||||
|
||||
|
||||
def test_no_source_publication_transition(selector):
|
||||
selector._publish_gm_gps(10.0)
|
||||
assert not selector.gps_pm.sent
|
||||
selector._gm_gps["device"] = (sample(), 10.0)
|
||||
selector._publish_gm_gps(10.0)
|
||||
valid = selector.gps_pm.sent[-1][1]
|
||||
assert valid.valid and valid.gpsLocationExternal.hasFix
|
||||
assert valid.gpsLocationExternal.latitude == 38.3
|
||||
selector._publish_gm_gps(10.1)
|
||||
assert len(selector.gps_pm.sent) == 1
|
||||
selector._publish_gm_gps(12.1)
|
||||
invalid = selector.gps_pm.sent[-1][1]
|
||||
assert not invalid.valid and not invalid.gpsLocationExternal.hasFix
|
||||
for now in (12.12, 12.13, 12.14):
|
||||
selector._publish_gm_gps(now)
|
||||
assert len(selector.gps_pm.sent) == 2
|
||||
selector._gm_gps["device"] = (sample(), 12.15)
|
||||
selector._publish_gm_gps(12.15) # Recovery bypasses the 200 ms healthy cadence.
|
||||
assert len(selector.gps_pm.sent) == 3
|
||||
assert selector.gps_pm.sent[-1][1].gpsLocationExternal.hasFix
|
||||
for now in (15.0, 20.0, 30.0):
|
||||
selector._publish_gm_gps(now)
|
||||
assert len(selector.gps_pm.sent) == 4 # One more loss marker, then sustained silence.
|
||||
|
||||
|
||||
def test_device_updates_honor_event_validity(selector):
|
||||
message = messaging.new_message("gpsLocation", valid=True)
|
||||
message.gpsLocation = {key: value for key, value in sample().items() if key != "timestamp_nanos"}
|
||||
class DeviceMessages(dict):
|
||||
updated = {"gpsLocation": True}
|
||||
valid = {"gpsLocation": True}
|
||||
selector.sm = DeviceMessages(gpsLocation=message.gpsLocation)
|
||||
selector._publish_gm_gps(10.0)
|
||||
assert selector.gps_pm.sent[-1][1].valid
|
||||
selector.sm.updated["gpsLocation"] = False
|
||||
selector._publish_gm_gps(10.05)
|
||||
assert selector._gm_gps["device"][1] == 10.0
|
||||
selector.sm.updated["gpsLocation"] = True
|
||||
selector.sm.valid["gpsLocation"] = False
|
||||
selector._publish_gm_gps(10.1)
|
||||
assert selector._gm_gps["device"] is None
|
||||
assert len(selector.gps_pm.sent) == 2 # Loss bypasses the healthy cadence too.
|
||||
assert not selector.gps_pm.sent[-1][1].gpsLocationExternal.hasFix
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user