Compare commits

..

1 Commits

Author SHA1 Message Date
firestarsdog ee05336586 Implement TripleDipper GPS fallback 2026-09-08 01:05:57 -04:00
55 changed files with 890 additions and 778 deletions
+19 -2
View File
@@ -1,8 +1,25 @@
from __future__ import annotations
from cereal import car
from openpilot.common.params import Params
def get_gps_location_service(params: Params) -> str:
if params.get_bool("UbloxAvailable") or params.get_bool("CarGpsAvailable"):
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"):
return "gpsLocationExternal"
else:
return "gpsLocation"
Binary file not shown.
-1
View File
@@ -18,7 +18,6 @@ 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}},
Binary file not shown.
@@ -852,6 +852,7 @@ 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
+160 -4
View File
@@ -1,5 +1,7 @@
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
@@ -8,12 +10,10 @@ 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,
@@ -38,6 +38,112 @@ BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.D
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)
@@ -109,11 +215,15 @@ class CarState(CarStateBase):
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
@@ -148,12 +258,47 @@ 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 get_car_gps(self):
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:
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:
@@ -239,6 +384,9 @@ 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")
@@ -588,8 +736,16 @@ class CarState(CarStateBase):
("ASCMLKASteeringCmd", 0),
]
return {
parsers = {
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
+2 -2
View File
@@ -408,7 +408,7 @@ class CarInterface(CarInterfaceBase):
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet
ret.steerLimitTimer = 0.4
ret.radarTimeStepDEPRECATED = 0.15 if candidate == CAR.BUICK_LACROSSE else 0.0667
ret.radarTimeStepDEPRECATED = 0.0667 # GM radar runs at 15Hz instead of the standard 20Hz
ret.longitudinalActuatorDelay = 0.5 # large delay to initially start braking
if candidate in (
@@ -440,7 +440,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 = 28 * CV.MPH_TO_MS
ret.minSteerSpeed = 27 * CV.MPH_TO_MS
elif candidate == CAR.CADILLAC_ESCALADE:
ret.minEnableSpeed = -1. # engage speed is decided by pcm
+149 -36
View File
@@ -1,5 +1,6 @@
import pytest
import numpy as np
from datetime import UTC, datetime
from types import SimpleNamespace
from parameterized import parameterized
@@ -8,7 +9,14 @@ from opendbc.can import CANPacker, CANParser
from opendbc.car import Bus, DT_CTRL, structs
from opendbc.car.car_helpers import interfaces
from opendbc.car.gm import gmcan
from opendbc.car.gm.carstate import CarState as GMCarState, get_hard_cruise_buttons, update_auto_hold_drive_timers
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,
)
from opendbc.car.gm.carcontroller import (
VisualAlert,
get_acc_dashboard_always_one,
@@ -95,6 +103,146 @@ 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",
@@ -205,33 +353,6 @@ class TestBoltGps:
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,
@@ -318,14 +439,6 @@ 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),
+21 -2
View File
@@ -6,8 +6,10 @@ 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
from opendbc.car.gm.values import CAR as GM_CAR, DBC as GM_DBC
CarGpsSample = dict[str, Any]
@@ -158,8 +160,25 @@ 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)
return config if config is not None and config.brand == CP.brand else None
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
def car_gps_available(CP) -> bool:
@@ -860,9 +860,7 @@ 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_status_cars = (CAR.HYUNDAI_IONIQ_6, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN)
lfa_longitudinal_active = self.CP.openpilotLongitudinalControl \
if self.CP.carFingerprint in lfa_status_cars else longitudinal_active
lfa_longitudinal_active = longitudinal_active if self.CP.carFingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN else self.CP.openpilotLongitudinalControl
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 \
@@ -892,8 +890,7 @@ class CarController(CarControllerBase):
if angle_lkas_alt:
steering_msg_active = bool(steering_msg_active and drive_gear)
angle_lkas_alt_standstill_handoff = bool(getattr(CS.out, "standstill", False) and not CC.latActive)
forward_stock_lkas = (self.CP.carFingerprint in CANFD_ANGLE_LONGITUDINAL_CAR or
self.CP.carFingerprint == CAR.KIA_SPORTAGE_HEV_2026) and angle_lkas_alt and (
forward_stock_lkas = self.CP.carFingerprint in CANFD_ANGLE_LONGITUDINAL_CAR and angle_lkas_alt and (
angle_lkas_alt_standstill_handoff or not (drive_gear and (CC.latActive or CC.enabled))
)
preserve_stock_lfa_status = preserve_stock_canfd_lfa_status(self.CP.carFingerprint)
@@ -2484,11 +2484,10 @@ 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 = True
CP.openpilotLongitudinalControl = False
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 = {
@@ -2530,6 +2529,7 @@ 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,12 +2537,13 @@ 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 == [("LFA", can_bus.ECAN), ("LKAS", can_bus.ACAN)]
assert steering_names == [("LKAS", can_bus.ACAN)]
controller.frame = 1
cc.longActive = True
@@ -2707,7 +2708,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_forwards_stock_status_when_inactive(self, standstill):
def test_sportage_angle_lkas_alt_keeps_inactive_status_in_drive(self, standstill):
CP = CarParams.new_message()
CP.carFingerprint = CAR.KIA_SPORTAGE_HEV_2026
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.HYBRID | HyundaiFlags.CANFD_ANGLE_STEERING |
@@ -2715,16 +2716,60 @@ class TestHyundaiFingerprint:
CP.openpilotLongitudinalControl = False
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={},
cs = SimpleNamespace(stock_lfa_msg=None, stock_lkas_msg=stock_lkas,
out=SimpleNamespace(standstill=standstill, steeringAngleDeg=0.0,
gearShifter=structs.CarState.GearShifter.drive))
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 not [msg for msg in msgs if msg[0] in (0x110, 0x12A)]
lkas_msgs = [msg for msg in msgs if msg[0] == 0x110]
assert len(lkas_msgs) == 1
parser.update([(1, lkas_msgs)])
assert parser.can_valid
assert parser.vl["LKAS_ALT"]["LKA_StrSnd"] == 2
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):
CP = CarParams.new_message()
+15 -2
View File
@@ -284,14 +284,27 @@ ensure_host_python_extensions() {
}
sync_host_generated_headers() {
if ! command -v capnpc >/dev/null 2>&1; then
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
return
fi
(
cd "${WORK_DIR}"
mkdir -p cereal/gen/cpp
capnpc --src-prefix=cereal \
"${capnpc_cmd}" --src-prefix=cereal \
cereal/log.capnp \
cereal/car.capnp \
cereal/legacy.capnp \
+4
View File
@@ -69,6 +69,10 @@ 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
+94 -30
View File
@@ -43,6 +43,8 @@ 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))
@@ -76,6 +78,22 @@ 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
@@ -85,7 +103,9 @@ 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'])
self.sm = messaging.SubMaster([
'pandaStates', 'carControl', 'onroadEvents', 'radarState', 'longitudinalPlan', 'gpsLocation',
])
self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'])
self.gps_pm = None
@@ -93,6 +113,8 @@ 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()
@@ -141,8 +163,9 @@ 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:
if car_gps_supported or self.gm_gps_supported:
self.gps_pm = messaging.PubMaster(['gpsLocationExternal'])
aol_available = always_on_lateral_available(self.CP)
@@ -349,39 +372,80 @@ 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 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 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 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:
+1 -1
View File
@@ -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._uses_software_cruise()
remembered_resume = resume_prev_button and (self.gm_cc_only or self.redneck_non_pcm)
if self.v_cruise_initialized and (resume_pressed or remembered_resume):
self.v_cruise_kph = self.v_cruise_kph_last
-16
View File
@@ -313,22 +313,6 @@ 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
+169
View File
@@ -0,0 +1,169 @@
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
@@ -275,7 +275,7 @@ GENESIS_G70_FRICTION_JERK_DEADZONE_LAT = 0.30
GENESIS_G70_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.08
GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED = 12.0
GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 3.5
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_MAX = 0.22
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_MAX = 0.16
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_SPEED = 35.0 * CV.MPH_TO_MS
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 8.0 * CV.MPH_TO_MS
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT = 0.35
@@ -284,7 +284,7 @@ GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT_CUTOFF = 1.25
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT_CUTOFF_WIDTH = 0.25
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_JERK = 0.20
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_JERK_WIDTH = 0.12
GENESIS_G70_CENTER_OUTPUT_TAPER_MAX = 0.26
GENESIS_G70_CENTER_OUTPUT_TAPER_MAX = 0.22
GENESIS_G70_CENTER_OUTPUT_TAPER_LAT = 0.30
GENESIS_G70_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.10
GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED = 18.0
@@ -397,27 +397,6 @@ def get_vehicle_min_accel(CP, v_ego):
return float(ACCEL_MIN)
def get_far_lead_coast_cap(lead, v_ego, desired_gap, output_a_target):
if lead is None or not bool(getattr(lead, "status", False)):
return float(output_a_target)
v_ego = float(v_ego)
lead_distance = float(getattr(lead, "dRel", float("inf")))
lead_speed = float(getattr(lead, "vLead", v_ego))
closing_speed = v_ego - lead_speed
if (
v_ego <= 10.0 or
closing_speed <= 0.5 or
lead_distance < FAR_LEAD_COAST_MIN_DISTANCE or
lead_distance <= float(desired_gap) + FAR_LEAD_COAST_MIN_GAP_MARGIN or
lead_distance / max(closing_speed, 0.1) < FAR_LEAD_COAST_MIN_TTC or
max(0.0, -float(getattr(lead, "aLeadK", 0.0))) > FAR_LEAD_COAST_MAX_LEAD_BRAKE
):
return float(output_a_target)
return max(float(output_a_target), -FAR_LEAD_COAST_MAX_DECEL)
# Restored planner constants retained by CEM, stop, and departure paths.
A_CRUISE_MIN = -1.0
# The stop distance runs ~9 m long through the mid-approach, which leaves the obstacle slack
@@ -454,11 +433,6 @@ VEHICLE_FAR_FOLLOW_SLEW_MIN_DISTANCE_TIME = 1.35
VEHICLE_FAR_FOLLOW_SLEW_MIN_HEADWAY = 1.35
VEHICLE_FAR_FOLLOW_SLEW_MIN_TTC = 8.0
VEHICLE_FAR_FOLLOW_SLEW_MAX_LATERAL_OFFSET = 1.5
FAR_LEAD_COAST_MIN_DISTANCE = 45.0
FAR_LEAD_COAST_MIN_TTC = 8.0
FAR_LEAD_COAST_MIN_GAP_MARGIN = 6.0
FAR_LEAD_COAST_MAX_LEAD_BRAKE = 0.35
FAR_LEAD_COAST_MAX_DECEL = 0.20
RADAR_DEPART_CONFLICT_MAX_EGO_SPEED = 1.6
RADAR_DEPART_CONFLICT_MIN_RADAR_LATERAL = 1.5
RADAR_DEPART_CONFLICT_MAX_RADAR_DISTANCE = 18.0
@@ -3095,28 +3069,6 @@ class LongitudinalPlanner:
panic_bypass,
)
far_lead_coast_allowed = (
not experimental_mode and
comfort_lead is not None and
desired_gap is not None and
not output_should_stop and
not vision_low_speed_stop_active and
not close_lead_caps and
not panic_bypass and
not depart_safety_veto and
inside_gap_closing_cap is None and
not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and
not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and
not bool(getattr(sm['starpilotPlan'], 'stopSignConfirmed', False))
)
if far_lead_coast_allowed:
output_a_target = get_far_lead_coast_cap(
comfort_lead,
scene_v_ego,
desired_gap,
output_a_target,
)
if radar_gap_settle_active:
output_a_target = RADAR_STANDSTILL_GAP_SETTLE_ACCEL
output_should_stop = False
+1 -8
View File
@@ -7,7 +7,6 @@ from typing import Any
import capnp
from cereal import messaging, log, car, custom
from cereal.services import SERVICE_LIST
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.params import Params
from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process
@@ -43,11 +42,6 @@ def is_bosch_a_radar_car(CP) -> bool:
return CP.brand == "honda" and CP.carFingerprint in HONDA_BOSCH_A and not CP.radarUnavailable
def has_slow_radar_tracks(CP) -> bool:
radar_ts = float(getattr(CP, "radarTimeStepDEPRECATED", DT_MDL) or DT_MDL)
return not CP.radarUnavailable and radar_ts > 2.0 / SERVICE_LIST["liveTracks"].frequency
# Adjacent-lane stopped-vehicle detector, used as a stop-line hint on red-light
# approaches. The qualifier is the DECELERATION HISTORY, not the current speed: roadside
# furniture and curb-parked cars never show a moving -> stopped transition, so testing
@@ -642,9 +636,8 @@ def main() -> None:
cloudlog.info("radard got CarParams")
# *** setup messaging
ignore_avg_freq = ['liveTracks'] if has_slow_radar_tracks(CP) else None
sm = messaging.SubMaster(['modelV2', 'carState', 'liveTracks'], poll='modelV2',
ignore_avg_freq=ignore_avg_freq, ignore_valid=['starpilotPlan'])
ignore_valid=['starpilotPlan'])
pm = messaging.PubMaster(['radarState'])
radar_ts = float(getattr(CP, "radarTimeStepDEPRECATED", DT_MDL) or DT_MDL)
-10
View File
@@ -14,7 +14,6 @@ from openpilot.selfdrive.controls.radard import (
RadarD,
g90_low_speed_radar_lead_sane,
g90_radar_lead_lateral_sane,
has_slow_radar_tracks,
is_bosch_a_radar_car,
match_vision_to_track,
)
@@ -97,15 +96,6 @@ class TestLeads:
assert bosch_a.lead_prob_filters[0].dt == pytest.approx(DT_MDL)
assert bosch_a.kalman_params.A[0][1] == pytest.approx(HONDA_BOSCH_A_RADAR_TS)
def test_slow_radar_frequency_relaxation_is_scoped(self):
slow_radar = SimpleNamespace(radarTimeStepDEPRECATED=0.15, radarUnavailable=False)
normal_radar = SimpleNamespace(radarTimeStepDEPRECATED=0.1, radarUnavailable=False)
unavailable_radar = SimpleNamespace(radarTimeStepDEPRECATED=0.15, radarUnavailable=True)
assert has_slow_radar_tracks(slow_radar)
assert not has_slow_radar_tracks(normal_radar)
assert not has_slow_radar_tracks(unavailable_radar)
@pytest.mark.skipif(platform.system() == "Darwin", reason="SocketEventHandle requires eventfd")
def test_radar_fault(self):
# if there's no radar-related can traffic, radard should either not respond or respond with an error
@@ -18,13 +18,7 @@ from opendbc.car.toyota.values import CAR as TOYOTA_CAR
import openpilot.selfdrive.controls.lib.longitudinal_planner as longitudinal_planner_module
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
from openpilot.selfdrive.controls.lib.longitudinal_planner import (
LongitudinalPlanner,
get_coast_accel,
get_far_lead_coast_cap,
get_vehicle_min_accel,
should_publish_planner_fcw,
)
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_coast_accel, get_vehicle_min_accel, should_publish_planner_fcw
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
LongitudinalMpc,
build_model_lead_trajectory,
@@ -316,24 +310,6 @@ def test_mpc_panic_bypass_immediately_removes_duplicate_vision_filter():
assert mpc.lead_v_filter.x == pytest.approx(10.0)
def test_far_lead_coast_cap_delays_nonurgent_deceleration():
lead = make_lead(status=True, d_rel=128.0, v_lead=16.7, a_lead=0.2, radar=True)
assert get_far_lead_coast_cap(lead, 26.6, 115.0, -0.43) == pytest.approx(-0.20)
assert get_far_lead_coast_cap(lead, 26.6, 115.0, 0.10) == pytest.approx(0.10)
@pytest.mark.parametrize("d_rel,v_lead,a_lead,desired_gap", [
(50.0, 20.0, 0.2, 45.0), # only a small gap remains
(128.0, 8.0, 0.2, 115.0), # urgent closing time
(128.0, 16.7, -0.5, 115.0), # the lead is braking materially
])
def test_far_lead_coast_cap_preserves_urgent_or_close_deceleration(d_rel, v_lead, a_lead, desired_gap):
lead = make_lead(status=True, d_rel=d_rel, v_lead=v_lead, a_lead=a_lead, radar=True)
assert get_far_lead_coast_cap(lead, 26.6, desired_gap, -0.43) == pytest.approx(-0.43)
def test_hrv_far_follow_output_slew_damps_only_continuous_safe_follow():
v_ego = 24.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_HRV_3G)
@@ -37,7 +37,7 @@ def make_toggles(**overrides):
def test_force_stop_jerk_scale_is_platform_specific():
assert get_force_stop_jerk_scale(SimpleNamespace(carFingerprint="HYUNDAI_ELANTRA_2021")) == 0.80
assert get_force_stop_jerk_scale(SimpleNamespace(carFingerprint="OTHER_CAR")) == 0.32
assert get_force_stop_jerk_scale(SimpleNamespace(carFingerprint="OTHER_CAR")) == 0.20
def test_lead_follow_jerk_scale_is_platform_specific():
@@ -255,3 +255,18 @@ def test_untracked_vision_lead_still_uses_strict_entry_gate():
assert not planner.update_lead_status(16.8)
finally:
planner.shutdown()
def test_gps_location_service_updates_on_carparams(monkeypatch):
planner = make_planner(monkeypatch)
try:
sm = make_sm(planner, frame=1, v_ego=20.0, left_blinker=False)
sm.updated = {"carParams": True}
sm["carParams"] = SimpleNamespace(brand="gm")
sm["gpsLocationExternal"] = SimpleNamespace(latitude=2.0, longitude=2.0, bearingDeg=45.0, hasFix=True)
planner.update(0.0, False, sm, make_toggles())
assert planner.gps_location_service == "gpsLocationExternal"
assert planner.gps_position["latitude"] == 2.0
finally:
planner.shutdown()
@@ -201,8 +201,6 @@
"min": 0.0,
"max": 99.0,
"step": 1.0,
"unit_type": "vehicle_speed",
"metric_max": 150.0,
"parent_key": "LaneChanges",
"settings_tier": "simple"
},
@@ -341,8 +339,6 @@
"min": 0.0,
"max": 99.0,
"step": 1.0,
"unit_type": "vehicle_speed",
"metric_max": 150.0,
"parent_key": "QOLLateral",
"settings_tier": "simple"
},
@@ -1020,8 +1016,6 @@
"min": 0.0,
"max": 99.0,
"step": 1.0,
"unit_type": "vehicle_speed",
"metric_max": 150.0,
"parent_key": "ConditionalExperimental",
"settings_tier": "simple"
},
@@ -1034,8 +1028,6 @@
"min": 0.0,
"max": 99.0,
"step": 1.0,
"unit_type": "vehicle_speed",
"metric_max": 150.0,
"parent_key": "ConditionalExperimental",
"settings_tier": "simple"
},
@@ -1121,8 +1113,6 @@
"min": 0.0,
"max": 99.0,
"step": 1.0,
"unit_type": "vehicle_speed",
"metric_max": 150.0,
"parent_key": "ConditionalExperimental",
"settings_tier": "simple"
},
@@ -1700,10 +1690,6 @@
"ui_type": "numeric",
"min": 1.0,
"max": 99.0,
"step": 1.0,
"precision": 0,
"unit_type": "vehicle_speed",
"metric_max": 150.0,
"parent_key": "QOLLongitudinal",
"settings_tier": "simple"
},
@@ -1715,10 +1701,6 @@
"ui_type": "numeric",
"min": 1.0,
"max": 99.0,
"step": 1.0,
"precision": 0,
"unit_type": "vehicle_speed",
"metric_max": 150.0,
"parent_key": "QOLLongitudinal",
"settings_tier": "simple"
},
@@ -1792,10 +1774,6 @@
"ui_type": "numeric",
"min": 0.0,
"max": 99.0,
"step": 1.0,
"precision": 0,
"unit_type": "vehicle_speed",
"metric_max": 150.0,
"parent_key": "QOLLongitudinal",
"settings_tier": "simple"
},
@@ -1809,8 +1787,6 @@
"max": 30.0,
"step": 0.5,
"precision": 1,
"unit_type": "vehicle_speed",
"imperial_max": 15.0,
"parent_key": "QOLLongitudinal",
"settings_tier": "advanced"
},
@@ -2247,12 +2223,6 @@
"ui_type": "numeric",
"min": -99.0,
"max": 99.0,
"step": 1.0,
"precision": 0,
"unit_type": "vehicle_speed",
"metric_min": -150.0,
"metric_max": 150.0,
"unit_range_index": 0,
"parent_key": "SpeedLimitController",
"settings_tier": "advanced"
},
@@ -2264,12 +2234,6 @@
"ui_type": "numeric",
"min": -99.0,
"max": 99.0,
"step": 1.0,
"precision": 0,
"unit_type": "vehicle_speed",
"metric_min": -150.0,
"metric_max": 150.0,
"unit_range_index": 1,
"parent_key": "SpeedLimitController",
"settings_tier": "advanced"
},
@@ -2281,12 +2245,6 @@
"ui_type": "numeric",
"min": -99.0,
"max": 99.0,
"step": 1.0,
"precision": 0,
"unit_type": "vehicle_speed",
"metric_min": -150.0,
"metric_max": 150.0,
"unit_range_index": 2,
"parent_key": "SpeedLimitController",
"settings_tier": "advanced"
},
@@ -2298,12 +2256,6 @@
"ui_type": "numeric",
"min": -99.0,
"max": 99.0,
"step": 1.0,
"precision": 0,
"unit_type": "vehicle_speed",
"metric_min": -150.0,
"metric_max": 150.0,
"unit_range_index": 3,
"parent_key": "SpeedLimitController",
"settings_tier": "advanced"
},
@@ -2315,12 +2267,6 @@
"ui_type": "numeric",
"min": -99.0,
"max": 99.0,
"step": 1.0,
"precision": 0,
"unit_type": "vehicle_speed",
"metric_min": -150.0,
"metric_max": 150.0,
"unit_range_index": 4,
"parent_key": "SpeedLimitController",
"settings_tier": "advanced"
},
@@ -2332,12 +2278,6 @@
"ui_type": "numeric",
"min": -99.0,
"max": 99.0,
"step": 1.0,
"precision": 0,
"unit_type": "vehicle_speed",
"metric_min": -150.0,
"metric_max": 150.0,
"unit_range_index": 5,
"parent_key": "SpeedLimitController",
"settings_tier": "advanced"
},
@@ -2349,12 +2289,6 @@
"ui_type": "numeric",
"min": -99.0,
"max": 99.0,
"step": 1.0,
"precision": 0,
"unit_type": "vehicle_speed",
"metric_min": -150.0,
"metric_max": 150.0,
"unit_range_index": 6,
"parent_key": "SpeedLimitController",
"settings_tier": "advanced"
},
@@ -2407,8 +2341,6 @@
"min": 0.0,
"max": 99.0,
"step": 1.0,
"unit_type": "vehicle_speed",
"metric_max": 150.0,
"parent_key": "ConditionalChill",
"settings_tier": "advanced"
},
@@ -2421,8 +2353,6 @@
"min": 0.0,
"max": 99.0,
"step": 1.0,
"unit_type": "vehicle_speed",
"metric_max": 150.0,
"parent_key": "ConditionalChill",
"settings_tier": "advanced"
},
@@ -2455,8 +2385,6 @@
"min": 0.0,
"max": 15.0,
"step": 1.0,
"unit_type": "vehicle_speed",
"metric_max": 30.0,
"parent_key": "ConditionalChill",
"settings_tier": "advanced"
},
@@ -2526,7 +2454,6 @@
"min": 5,
"max": 80,
"step": 5,
"unit_type": "vehicle_speed",
"parent_key": "VisionSpeedLimitLowLimitFilter",
"settings_tier": "advanced"
},
@@ -4891,7 +4818,6 @@
"min": 0.0,
"max": 99.0,
"step": 1.0,
"unit_type": "vehicle_speed",
"parent_key": "GalaxyDeveloperMode",
"settings_tier": "advanced"
},
+4
View File
@@ -126,6 +126,10 @@ class StarPilotPlanner:
v_cruise_kph += starpilot_toggles.set_speed_offset
v_cruise = v_cruise_kph * CV.KPH_TO_MS
v_ego = max(sm["carState"].vEgo, 0)
updated = getattr(sm, "updated", None)
if updated is not None and updated.get("carParams", False):
cp = sm["carParams"] if "carParams" in sm else None
self.gps_location_service = get_gps_location_service(self.params, cp)
gps_location = sm[self.gps_location_service]
self.gps_position = {
+3
View File
@@ -310,6 +310,9 @@ def starpilot_thread():
while True:
sm.update()
if sm.updated["carParams"]:
gps_location_service = get_gps_location_service(params, sm["carParams"])
now = datetime.datetime.now(datetime.timezone.utc)
monotonic_now = time.monotonic()
+1 -55
View File
@@ -20,7 +20,6 @@ AUDIO_TEST_HOLD_TIME = 3.0
RECONNECT_INTERVAL_SECONDS = 15.0
RECONNECT_MAX_BACKOFF_SECONDS = 300.0
MANUAL_DISCONNECT_SUPPRESSION_SECONDS = 300.0
CONTROLLER_OFFROAD_DISCONNECT_DELAY_SECONDS = 120.0
class BluetoothController:
@@ -37,9 +36,6 @@ class BluetoothController:
self._last_reconnect = 0.0
self._reconnect_backoff: dict[str, tuple[int, float]] = {}
self._manual_disconnect_until: dict[str, float] = {}
self._offroad_since: float | None = None
self._policy_disconnected: set[str] = set()
self._policy_disconnect_retry_after: dict[str, float] = {}
self._scan_deadline = 0.0
self._audio_test_deadline = 0.0
self._sleep = sleep
@@ -276,65 +272,17 @@ class BluetoothController:
self._client().stop_discovery()
self._scan_deadline = 0.0
def _maintain_controller_offroad_policy(self, status: dict[str, Any], now: float) -> bool:
if not status["offroad"]:
self._offroad_since = None
if self._policy_disconnected:
for address in self._policy_disconnected:
self._reconnect_backoff.pop(address, None)
self._policy_disconnected.clear()
self._policy_disconnect_retry_after.clear()
self._last_reconnect = 0.0
return False
if self._offroad_since is None:
self._offroad_since = now
if not self.params.get_bool("BluetoothDisconnectControllersOffroad"):
if self._policy_disconnected:
self._policy_disconnected.clear()
self._policy_disconnect_retry_after.clear()
self._last_reconnect = 0.0
return False
if now - self._offroad_since < CONTROLLER_OFFROAD_DISCONNECT_DELAY_SECONDS:
return False
for device in status["devices"]:
if not device.get("paired") or not device.get("controller") or not device.get("connected"):
continue
address = str(device["address"]).upper()
if now < self._policy_disconnect_retry_after.get(address, 0.0):
continue
self._policy_disconnected.add(address)
self._policy_disconnect_retry_after[address] = now + RECONNECT_INTERVAL_SECONDS
try:
with self._lock:
self._client().disconnect(address)
except RuntimeError as error:
if "notconnected" not in str(error).replace(" ", "").lower():
self._policy_disconnected.discard(address)
self._policy_disconnect_retry_after.pop(address, None)
cloudlog.warning(f"Bluetooth offroad controller disconnect failed for {address}: {error}")
except Exception as error:
self._policy_disconnected.discard(address)
self._policy_disconnect_retry_after.pop(address, None)
cloudlog.warning(f"Bluetooth offroad controller disconnect failed for {address}: {error}")
return True
def maintain_connections(self) -> None:
while True:
time.sleep(2)
now = time.monotonic()
if not self.params.get_bool("BluetoothEnabled"):
self._maintain_controller_offroad_policy({"offroad": self._offroad(), "devices": []}, now)
continue
try:
status = self.status()
if not status["available"] or not status["powered"]:
continue
now = time.monotonic()
self._maintain_scan(status, now)
suspend_controller_reconnect = self._maintain_controller_offroad_policy(status, now)
if self._pairing_address or now - self._last_reconnect < RECONNECT_INTERVAL_SECONDS:
continue
self._last_reconnect = now
@@ -350,8 +298,6 @@ class BluetoothController:
self._reconnect_backoff.pop(address, None)
for device in candidates:
if device["audio"] or device["controller"]:
if suspend_controller_reconnect and device["controller"]:
continue
address = device["address"].upper()
if now < self._manual_disconnect_until.get(address, 0.0):
continue
@@ -432,48 +432,6 @@ def test_scan_stops_after_timeout():
assert not client.discovering and controller._scan_deadline == 0.0
def test_controller_offroad_disconnect_policy_is_opt_in_and_delayed():
params = FakeParams(IsOffroad=True, BluetoothEnabled=True, BluetoothDisconnectControllersOffroad=False)
client = FakeBlueZ()
controller = BluetoothController(params, lambda: client, FakeRadio())
controller._bluez = client
controller_status = {
"offroad": True,
"devices": [
{**client.device, "name": "Controller", "audio": False, "controller": True, "connected": True},
{**client.device, "address": "AA:BB:CC:DD:EE:FF", "audio": True, "controller": False, "connected": True},
],
}
assert not controller._maintain_controller_offroad_policy(controller_status, 100.0)
params.put_bool("BluetoothDisconnectControllersOffroad", True)
assert not controller._maintain_controller_offroad_policy(controller_status, 219.9)
assert client.actions == []
assert controller._maintain_controller_offroad_policy(controller_status, 220.0)
assert client.actions == [("disconnect", client.device["address"])]
assert client.device["address"].upper() in controller._policy_disconnected
assert controller._maintain_controller_offroad_policy(controller_status, 221.0)
assert client.actions == [("disconnect", client.device["address"])]
def test_controller_offroad_disconnect_policy_reconnects_onroad():
params = FakeParams(IsOffroad=True, BluetoothEnabled=True, BluetoothDisconnectControllersOffroad=True)
controller = BluetoothController(params, FakeBlueZ, FakeRadio())
address = "00:11:22:33:44:55"
controller._offroad_since = 100.0
controller._policy_disconnected.add(address)
controller._reconnect_backoff[address] = (3, 500.0)
controller._last_reconnect = 210.0
assert not controller._maintain_controller_offroad_policy({"offroad": False, "devices": []}, 220.0)
assert controller._offroad_since is None
assert controller._policy_disconnected == set()
assert controller._policy_disconnect_retry_after == {}
assert address not in controller._reconnect_backoff
assert controller._last_reconnect == 0.0
def test_pair_keeps_discovery_until_pair_starts():
params = FakeParams(IsOffroad=True, BluetoothEnabled=True)
client = FakeBlueZ()
+6 -1
View File
@@ -55,7 +55,10 @@ class MapSpeedLogger:
self.gps_location_service = get_gps_location_service(self.params)
self.sm = messaging.SubMaster(["deviceState", "starpilotCarState", "starpilotPlan", self.gps_location_service, "mapdOut", "modelV2"])
self.sm = messaging.SubMaster([
"carParams", "deviceState", "starpilotCarState", "starpilotPlan", "gpsLocation", "gpsLocationExternal",
"mapdOut", "modelV2",
])
@property
def can_make_overpass_request(self):
@@ -252,6 +255,8 @@ class MapSpeedLogger:
return relevant_segments
def log_speed_limit(self):
if self.sm.updated["carParams"]:
self.gps_location_service = get_gps_location_service(self.params, self.sm["carParams"])
if not self.sm.updated[self.gps_location_service]:
return
@@ -3,7 +3,7 @@ import { createBrowserHistory, createRouter } from "/assets/vendor/remix-router-
import { hideSidebar } from "/assets/js/utils.js"
import { DeviceSettings } from "/assets/components/tools/device_settings.js?v=favorite-c4-hint-1"
import { Bluetooth } from "/assets/components/tools/bluetooth.js?v=bluetooth-live-15"
import { WheelControls } from "/assets/components/tools/wheel_controls.js?v=controllers-3"
import { WheelControls } from "/assets/components/tools/wheel_controls.js?v=controllers-2"
import { DoorControl } from "/assets/components/tools/doors.js"
import { ErrorLogs } from "/assets/components/tools/error_logs.js"
import { VehicleFeatures } from "/assets/components/tools/vehicle_features.js"
@@ -342,24 +342,6 @@
margin-bottom: var(--margin-sm);
}
.ds-unit-note {
align-items: center;
background: var(--input-bg);
border: var(--border-style-main);
border-radius: var(--border-radius-base);
color: var(--text-muted);
display: flex;
font-size: var(--font-size-sm);
gap: var(--gap-sm);
margin-bottom: var(--margin-base);
padding: var(--padding-sm) var(--padding-base);
}
.ds-unit-note i,
.ds-unit-note strong {
color: var(--main-fg);
}
/* ――― Empty Filter State ――― */
.ds-empty {
color: var(--text-muted);
@@ -1,5 +1,4 @@
import { html, reactive } from "/assets/vendor/arrow-core.js"
import { formatNumericParamValue, resolveVehicleUnitParam, vehicleSpeedUnit } from "/assets/mobile/js/params.js"
const endpointOptionsCache = {}
const endpointOptionsInflight = {}
@@ -449,6 +448,40 @@ async function fetchLayoutAndParams() {
scheduleSyncInputs()
}
function formatSliderValue(val, stepStr, precisionInt, key) {
if (val === null || val === undefined) return "--"
const v = parseFloat(val)
if (Number.isNaN(v)) return val
if (key === "SwitchbackModeCooldown") {
if (v === 0) return "Off"
return v === 1 ? "1 min" : `${v} min`
}
if (key === "DeviceShutdown") {
return v === 1 ? "1 hour" : `${v} hours`
}
const volumeKeys = [
"BelowSteerSpeedVolume", "DisengageVolume", "EngageVolume", "PromptVolume",
"PromptDistractedVolume", "RefuseVolume",
"WarningImmediateVolume", "WarningSoftVolume",
]
if (key && volumeKeys.includes(key)) {
if (v === 0) return "Muted"
if (v === 101) return "Auto"
return `${v}%`
}
if (precisionInt !== undefined && precisionInt !== null) {
return Number(v.toFixed(precisionInt)).toString()
}
if (!stepStr || !stepStr.includes(".")) return Math.round(v).toString()
const dec = stepStr.split(".")[1].length
return Number(v.toFixed(dec)).toString()
}
function formatReadoutValue(p) {
const raw = state.values[p.key]
const v = parseFloat(raw)
@@ -472,7 +505,6 @@ function formatStepValue(step, precision) {
}
function numericBounds(param) {
param = resolveVehicleUnitParam(param, state.values)
const defaultBounds = {
min: param.min !== undefined ? param.min : (param.data_type === "float" ? 0.0 : 0),
max: param.max !== undefined ? param.max : (param.data_type === "float" ? 100.0 : 100),
@@ -920,7 +952,13 @@ function syncNumericDisplay(param, rawValue) {
const displayEl = document.getElementById(`ds-display-${param.key}`)
if (!displayEl) return
displayEl.textContent = formatNumericParamValue(param, rawValue, state.values)
const bounds = numericBounds(param)
displayEl.textContent = formatSliderValue(
rawValue,
String(bounds.step),
param.precision,
param.key,
)
}
async function updateNumericParam(param, numericValue, options = {}) {
@@ -1273,9 +1311,10 @@ function matchesFilter(p) {
if (!state.filter) return true
if (isGroupParam(p)) return false
const q = state.filter.toLowerCase()
const displayParam = resolveVehicleUnitParam(p, state.values)
return [displayParam.label, displayParam.key, displayParam.description, displayParam.unit, displayParam.unit_search_terms]
.some(value => String(value || "").toLowerCase().includes(q))
const label = String(p.label || "").toLowerCase()
const key = String(p.key || "").toLowerCase()
const description = String(p.description || "").toLowerCase()
return label.includes(q) || key.includes(q) || description.includes(q)
}
function clearSearchFilter() {
@@ -1337,7 +1376,8 @@ function formatFlmValue(param, value) {
if (value === undefined || value === null) return "not set"
if (param.data_type === "bool") return value ? "On" : "Off"
if (param.ui_type === "numeric") {
return formatNumericParamValue(param, value, state.values)
const bounds = numericBounds(param)
return formatSliderValue(value, String(bounds.step), param.precision, param.key)
}
return String(value)
}
@@ -1522,8 +1562,6 @@ function renderSettingRow(p) {
return ""
}
p = resolveVehicleUnitParam(p, state.values)
const isNumeric = p.ui_type === "numeric"
const isSlider = isNumeric && p.control === "slider"
const isText = p.ui_type === "text"
@@ -1566,8 +1604,8 @@ function renderSettingRow(p) {
@input="${(event) => previewSliderParam(p, event.currentTarget.value)}"
@change="${(event) => commitSliderParam(p, event.currentTarget.value)}" />
<div class="ds-slider-scale">
<span>${formatNumericParamValue(p, numericBounds(p).min, state.values)}</span>
<span>${formatNumericParamValue(p, numericBounds(p).max, state.values)}</span>
<span>${formatSliderValue(numericBounds(p).min, String(numericBounds(p).step), p.precision, p.key)}</span>
<span>${formatSliderValue(numericBounds(p).max, String(numericBounds(p).step), p.precision, p.key)}</span>
</div>
<button
class="ds-reset-btn"
@@ -1593,10 +1631,10 @@ function renderSettingRow(p) {
const updating = isNumericUpdating(p.key)
const defaultNumeric = resolveDefaultNumericValue(p, bounds)
const defaultLabel = defaultNumeric !== null
? formatNumericParamValue(p, defaultNumeric, state.values)
? formatSliderValue(defaultNumeric, String(bounds.step), p.precision, p.key)
: "N/A"
const canReset = !updating && defaultNumeric !== null && Math.abs(defaultNumeric - currentNumeric) > epsilon
const stepLabel = p.key === "DeviceShutdown" ? "1 hour" : `${formatStepValue(bounds.step, precision)}${p.unit || ""}`
const stepLabel = p.key === "DeviceShutdown" ? "1 hour" : formatStepValue(bounds.step, precision)
return html`
<div class="ds-stepper">
<button
@@ -1604,7 +1642,7 @@ function renderSettingRow(p) {
disabled="${() => isLocked() || isNumericUpdating(p.key) || !canStepNumericParam(p, -1)}"
@click="${() => stepNumericParam(p, -1)}">-</button>
<div class="ds-stepper-meta">
<span>${formatNumericParamValue(p, bounds.min, state.values)} to ${formatNumericParamValue(p, bounds.max, state.values)}</span>
<span>${formatSliderValue(bounds.min, String(bounds.step), p.precision, p.key)} to ${formatSliderValue(bounds.max, String(bounds.step), p.precision, p.key)}</span>
<span class="ds-step-value">Step: ${stepLabel} per click</span>
<span class="ds-default-value">Default: ${defaultLabel}</span>
<div class="ds-manual-row">
@@ -1752,7 +1790,8 @@ function renderSettingRow(p) {
if (isColor) return formatColorDisplayValue(p)
if (isReadout) return formatReadoutValue(p)
const currentValue = state.sliderPreviewValues[p.key] ?? state.values[p.key]
return currentValue !== undefined ? formatNumericParamValue(p, currentValue, state.values) : ".."
const bounds = numericBounds(p)
return currentValue !== undefined ? formatSliderValue(currentValue, String(bounds.step), p.precision, p.key) : ".."
}}</span>` : ""}
</div>
@@ -1818,11 +1857,6 @@ export function DeviceSettings({ params }) {
<div class="ds-wrapper">
<h2>Toggles</h2>
<div class="ds-unit-note">
<i class="bi bi-speedometer2"></i>
<span>Vehicle-unit speed settings use <strong>${() => vehicleSpeedUnit(state.values)}</strong> and follow the comma's <em>Use Metric System</em> toggle. Each control shows its adjustment step.</span>
</div>
<div class="ds-search-row">
<input
class="ds-search"
@@ -37,44 +37,13 @@
.wheelCard,
.wheelNotice,
.wheelError,
.wheelDeviceSummary,
.wheelPolicy {
.wheelDeviceSummary {
background: var(--sidebar-bg);
border: 1px solid var(--sidebar-border-color);
border-radius: var(--border-radius-lg);
padding: 16px;
}
.wheelPolicy {
display: flex;
align-items: center;
justify-content: space-between;
gap: 20px;
cursor: pointer;
}
.wheelPolicy span,
.wheelPolicy small {
display: block;
}
.wheelPolicy small {
margin-top: 5px;
opacity: 0.68;
}
.wheelPolicy input {
width: 22px;
height: 22px;
flex: 0 0 auto;
accent-color: #8b6cc5;
}
.wheelPolicy:has(input:disabled) {
cursor: not-allowed;
opacity: 0.6;
}
.wheelSlotGrid,
.wheelControllerGrid,
.wheelMappings {
@@ -11,7 +11,6 @@ const state = reactive({
slots: [],
controllerSlots: [],
controllerOptions: [],
disconnectControllersOffroad: false,
speedUnit: "mph",
speedMinimum: 5,
speedMaximum: 90,
@@ -38,7 +37,6 @@ async function refresh() {
state.slots = Array.isArray(payload.slots) ? payload.slots : []
state.controllerSlots = Array.isArray(payload.controller_slots) ? payload.controller_slots : []
state.controllerOptions = Array.isArray(payload.controller_options) ? payload.controller_options : []
state.disconnectControllersOffroad = !!payload.disconnect_controllers_offroad
state.speedUnit = typeof payload.speed_unit === "string" ? payload.speed_unit : "mph"
state.speedMinimum = Number(payload.speed_minimum || 5)
state.speedMaximum = Number(payload.speed_maximum || 90)
@@ -267,16 +265,6 @@ export function WheelControls() {
${() => !state.loading && !state.available && state.mappings.length ? html`<div class="wheelNotice">The wheel control service is starting.</div>` : ""}
${() => state.testing ? testPanel() : ""}
<label class="wheelPolicy">
<span>
<strong>Disconnect controllers when offroad</strong>
<small>After two minutes offroad, paired controllers disconnect to save battery and reconnect when the car starts. Bluetooth and audio-only devices stay connected.</small>
</span>
<input type="checkbox" checked="${() => state.disconnectControllersOffroad}"
disabled="${() => !state.offroad || !!state.busy}"
@change="${event => request("offroad-disconnect", { enabled: event.currentTarget.checked })}" />
</label>
<div class="wheelDeviceSummary">
<div class="wheelDeviceHeading">
<strong>Connected input devices</strong>
@@ -592,24 +592,6 @@ ul { list-style: none; margin: 0; padding: 0; }
transition: transform var(--motion-fast), box-shadow var(--motion-fast);
}
.gx-unit-note {
align-items: center;
background: var(--primary-container);
border: 1px solid var(--primary);
border-radius: var(--radius-md);
color: var(--on-primary-container);
display: flex;
font-size: var(--fs-sm);
gap: var(--sp-2);
margin: 0 0 var(--sp-4);
padding: var(--sp-3) var(--sp-4);
}
.gx-unit-note i {
color: var(--primary);
font-size: 1.2rem;
}
[data-theme="light"] .gx-card {
border: 1px solid rgba(120, 73, 232, 0.22);
}
@@ -767,13 +749,6 @@ ul { list-style: none; margin: 0; padding: 0; }
.gx-slider-row .gx-row__value { text-align: left; min-width: 0; }
.gx-slider-row .gx-slider-reset { align-self: flex-end; }
.gx-slider-meta {
color: var(--text-muted);
display: flex;
font-size: var(--fs-xs);
justify-content: space-between;
}
input[type="range"].gx-slider {
-webkit-appearance: none;
appearance: none;
@@ -1522,4 +1497,4 @@ input[type="color"].gx-color {
.gx-menu-btn { display: inline-flex; }
.gx-back-btn { display: none; }
.gx-content { padding-bottom: var(--sp-6); }
}
}
@@ -1,8 +1,8 @@
import { api, showSnackbar } from "../api.js"
import {
coerceValueByType, formatNumericParamValue, formatReadoutValue, getColorDefault,
coerceValueByType, formatSliderValue, formatReadoutValue, getColorDefault,
normalizeHexColor, numericBounds, numericEpsilon, snapNumericToBoundsAndStep,
resolveVehicleUnitParam, stepPrecision,
stepPrecision,
} from "../params.js"
import { FavoritesEditor } from "./FavoritesEditor.js"
@@ -12,7 +12,6 @@ export const GalaxyToggleCard = {
props: {
param: { type: Object, required: true },
value: { default: undefined },
values: { type: Object, default: () => ({}) },
locked: { type: Boolean, default: false },
manageable: { type: Boolean, default: false },
manageOpen: { type: Boolean, default: false },
@@ -29,9 +28,8 @@ export const GalaxyToggleCard = {
}
},
computed: {
displayParam() { return resolveVehicleUnitParam(this.param, this.values) },
bounds() { return numericBounds(this.displayParam, this.values) },
precision() { return stepPrecision(this.bounds.step, this.displayParam.precision) },
bounds() { return numericBounds(this.param, {}) },
precision() { return stepPrecision(this.bounds.step, this.param.precision) },
epsilon() { return numericEpsilon(this.precision) },
isSlider() { return this.isNumeric },
isNumeric() { return this.param.ui_type === "numeric" },
@@ -40,17 +38,11 @@ export const GalaxyToggleCard = {
currentValue() { return this.preview !== undefined ? this.preview : this.value },
displayValue() {
if (this.isColor) return normalizeHexColor(this.value) ? normalizeHexColor(this.value).toUpperCase() : "Stock"
if (this.isReadout) return formatReadoutValue(this.displayParam, this.value)
return this.value !== undefined && this.value !== null ? formatNumericParamValue(this.displayParam, this.value, this.values) : ".."
if (this.isReadout) return formatReadoutValue(this.param, this.value)
return this.value !== undefined && this.value !== null ? formatSliderValue(this.value, String(this.bounds.step), this.param.precision, this.param.key) : ".."
},
sliderDisplay() {
return this.value !== undefined ? formatNumericParamValue(this.displayParam, this.currentValue, this.values) : ".."
},
sliderRangeDisplay() {
return `${formatNumericParamValue(this.displayParam, this.bounds.min, this.values)} to ${formatNumericParamValue(this.displayParam, this.bounds.max, this.values)}`
},
sliderStepDisplay() {
return formatNumericParamValue(this.displayParam, this.bounds.step, this.values)
return this.value !== undefined ? formatSliderValue(this.currentValue, String(this.bounds.step), this.param.precision, this.param.key) : ".."
},
isColor() { return this.param.ui_type === "color" },
isAction() { return this.param.ui_type === "action" },
@@ -175,10 +167,10 @@ export const GalaxyToggleCard = {
<div>
<div class="gx-row" :class="{ disabled: locked, 'gx-row--favorites': isFavorites, 'gx-row--stack': isSlider || isSelect }">
<div class="gx-row__info">
<span class="gx-row__label">{{ displayParam.label }}
<span v-if="displayParam.settings_tier === 'advanced'" class="gx-chip gx-chip--advanced">Advanced</span>
<span class="gx-row__label">{{ param.label }}
<span v-if="param.settings_tier === 'advanced'" class="gx-chip gx-chip--advanced">Advanced</span>
</span>
<span v-if="displayParam.description" class="gx-row__desc">{{ displayParam.description }}</span>
<span v-if="param.description" class="gx-row__desc">{{ param.description }}</span>
<div v-if="locked" class="gx-row__desc"><strong>Locked:</strong> This setting can only be changed while parked.</div>
</div>
@@ -198,10 +190,6 @@ export const GalaxyToggleCard = {
:value="currentValue" :disabled="locked || updating"
@input="onSliderInput" @change="onSliderCommit" @blur="onSliderBlur"
@touchstart="beginInteract" @mousedown="beginInteract" @keydown="beginInteract" />
<div v-if="displayParam.unit_type" class="gx-slider-meta">
<span>{{ sliderRangeDisplay }}</span>
<span>Step: {{ sliderStepDisplay }}</span>
</div>
<button class="gx-slider-reset" :disabled="locked || updating" @click="resetToDefault">Default</button>
</div>
@@ -1,5 +1,5 @@
import { api } from "../api.js"
import { isSettingVisible, resolveVehicleUnitParam, slugifySectionName, applyParamChange } from "../params.js"
import { isSettingVisible, slugifySectionName, applyParamChange } from "../params.js"
import { SettingTree } from "./SettingTree.js"
import { GalaxySection } from "./GalaxySection.js"
@@ -35,9 +35,7 @@ export const ParamSections = {
matches(p) {
if (!this.search) return true
const q = this.search.toLowerCase()
const displayParam = resolveVehicleUnitParam(p, this.values)
return [displayParam.label, displayParam.key, displayParam.description, displayParam.unit, displayParam.unit_search_terms]
.some((v) => String(v || "").toLowerCase().includes(q))
return [p.label, p.key, p.description].some((v) => String(v || "").toLowerCase().includes(q))
},
async load() {
try {
@@ -30,7 +30,7 @@ export const SettingTree = {
template: `
<template v-for="p in children" :key="p.key">
<div class="gx-tree-node" :class="{ 'gx-tree-node--child': depth > 0 }" :style="'--gx-depth:' + depth">
<GalaxyToggleCard :param="p" :value="values[p.key]" :values="values" :locked="lockReason(p) !== ''"
<GalaxyToggleCard :param="p" :value="values[p.key]" :locked="lockReason(p) !== ''"
:manageable="manageable(p)" :manage-open="manageOpen(p)"
@change="$emit('change', $event)" @manage="$emit('manage', $event)" />
</div>
@@ -12,7 +12,6 @@ export const WheelControls = {
loading: true, busy: "", available: false, offroad: false, learning: false,
devices: [], mappings: [], slots: [], controllerSlots: [], controllerOptions: [],
joystickDevice: "", learningSlot: null, remainingSeconds: 0, testing: false,
disconnectControllersOffroad: false,
lastTested: null, speedUnit: "mph", speedMinimum: 0, speedMaximum: 0, error: "",
}
},
@@ -30,7 +29,6 @@ export const WheelControls = {
this.slots = Array.isArray(p.slots) ? p.slots : []
this.controllerSlots = Array.isArray(p.controller_slots) ? p.controller_slots : []
this.controllerOptions = Array.isArray(p.controller_options) ? p.controller_options : []
this.disconnectControllersOffroad = !!p.disconnect_controllers_offroad
this.joystickDevice = typeof p.joystick_device === "string" ? p.joystick_device : ""
this.learningSlot = Number.isInteger(p.learning_slot) ? p.learning_slot : null
this.remainingSeconds = Number.isFinite(Number(p.remaining_seconds)) ? Number(p.remaining_seconds) : 0
@@ -101,18 +99,6 @@ export const WheelControls = {
<button type="button" class="gx-btn" :disabled="disabled() || !mappings.length" @click="request(testing ? 'test-stop' : 'test')">{{ testing ? 'Stop Testing' : 'Test Buttons' }}</button>
<button type="button" class="gx-btn gx-btn--danger" :disabled="disabled() || !mappings.length" @click="request('clear')">Clear All</button>
</div>
<div class="gx-row" style="margin-bottom:12px;">
<div class="gx-row__info">
<span class="gx-row__label">Disconnect controllers when offroad</span>
<span class="gx-row__desc">After two minutes offroad, paired controllers disconnect to save battery and reconnect when the car starts. Bluetooth and audio-only devices stay connected.</span>
</div>
<label class="gx-switch">
<input type="checkbox" :checked="disconnectControllersOffroad" :disabled="disabled()"
@change="request('offroad-disconnect', { enabled: $event.target.checked })" />
<span class="gx-switch__track"></span>
<span class="gx-switch__thumb"></span>
</label>
</div>
<div v-if="testing && lastTested" style="margin-bottom:12px;">
<span class="gx-chip" :style="lastTested.mapped ? 'background:var(--success);' : 'background:var(--error);'">{{ lastTested.mapped ? 'Successful' : 'Not mapped' }}</span>
<p style="color:var(--text-muted); margin-top:6px;">{{ lastTested.event_name || ('Button ' + lastTested.event_code) }} on {{ lastTested.device_name || 'External input' }} {{ lastTested.mapped ? 'is mapped to slot ' + lastTested.slot : 'has no mapping' }}.</p>
@@ -1,45 +1,4 @@
export const GALAXY_DEVELOPER_MODE_KEY = "GalaxyDeveloperMode"
export const VEHICLE_SPEED_UNIT_TYPE = "vehicle_speed"
const SPEED_OFFSET_RANGES = {
imperial: ["024", "2534", "3544", "4554", "5564", "6574", "7599"],
metric: ["029", "3049", "5059", "6079", "8099", "100119", "120140"],
}
export function usesMetricUnits(values = {}) {
const value = values?.IsMetric
if (value === true || value === 1) return true
return ["1", "true", "yes", "on"].includes(String(value ?? "").trim().toLowerCase())
}
export function vehicleSpeedUnit(values = {}) {
return usesMetricUnits(values) ? "km/h" : "mph"
}
export function resolveVehicleUnitParam(param, values = {}) {
if (!param || param.unit_type !== VEHICLE_SPEED_UNIT_TYPE) return param
const metric = usesMetricUnits(values)
const mode = metric ? "metric" : "imperial"
const resolved = {
...param,
unit: ` ${vehicleSpeedUnit(values)}`,
unit_search_terms: "metric imperial mph km/h vehicle speed units",
}
for (const field of ["min", "max", "step", "precision"]) {
const override = param[`${mode}_${field}`]
if (override !== undefined && override !== null) resolved[field] = override
}
if (Number.isInteger(param.unit_range_index)) {
const range = SPEED_OFFSET_RANGES[mode][param.unit_range_index]
if (range) {
resolved.label = `Speed Offset (${range} ${vehicleSpeedUnit(values)})`
resolved.description = `How much to offset posted speed limits between ${range} ${vehicleSpeedUnit(values)}.`
}
}
return resolved
}
const HIDDEN_SETTING_KEYS = new Set(["HumanAcceleration"])
const RADAR_REQUIRED_KEYS = new Set(["HumanLaneChanges", "RadarTakeoffs"])
@@ -129,8 +88,7 @@ export function countAdvancedHiddenByDeveloperMode(layout, values) {
return count
}
export function numericBounds(param, values = {}) {
param = resolveVehicleUnitParam(param, values)
export function numericBounds(param, values) {
const defaultBounds = {
min: param.min !== undefined ? param.min : (param.data_type === "float" ? 0.0 : 0),
max: param.max !== undefined ? param.max : (param.data_type === "float" ? 100.0 : 100),
@@ -236,13 +194,6 @@ export function formatSliderValue(val, stepStr, precisionInt, key) {
return Number(v.toFixed(dec)).toString()
}
export function formatNumericParamValue(param, value, values = {}) {
const resolved = resolveVehicleUnitParam(param, values)
const bounds = numericBounds(resolved, values)
const formatted = formatSliderValue(value, String(bounds.step), resolved.precision, resolved.key)
return resolved.unit && formatted !== "--" ? `${formatted}${resolved.unit}` : formatted
}
export function formatReadoutValue(p, value) {
const raw = value
const parsed = parseFloat(raw)
@@ -2,7 +2,7 @@ import { api, showSnackbar } from "../api.js"
import { navigate, store } from "../store.js"
import {
applyParamChange, countAdvancedHiddenByDeveloperMode, GALAXY_DEVELOPER_MODE_KEY, isSettingVisible,
resolveVehicleUnitParam, slugifySectionName, vehicleSpeedUnit,
slugifySectionName,
} from "../params.js"
import { SettingTree } from "../components/SettingTree.js"
import { GalaxyToggleCard } from "../components/GalaxyToggleCard.js"
@@ -39,7 +39,6 @@ export const Settings = {
return this.sections.find((s) => s.slug === this.activeSectionSlug) || this.sections[0]
},
hiddenAdvancedCount() { return countAdvancedHiddenByDeveloperMode(this.layout, this.values) },
speedUnit() { return vehicleSpeedUnit(this.values) },
searchActive() { return !!this.searchTerm },
searchTerm: {
get() { return store.search },
@@ -83,9 +82,7 @@ export const Settings = {
matchesFilter(p) {
if (!this.searchTerm) return true
const q = this.searchTerm.toLowerCase()
const displayParam = resolveVehicleUnitParam(p, this.values)
return [displayParam.label, displayParam.key, displayParam.description, displayParam.unit, displayParam.unit_search_terms]
.some((v) => String(v || "").toLowerCase().includes(q))
return [p.label, p.key, p.description].some((v) => String(v || "").toLowerCase().includes(q))
},
selectSection(slug) {
if (slug !== this.activeSectionSlug) navigate("/settings/" + slug)
@@ -120,11 +117,6 @@ export const Settings = {
<div>
<h2 style="margin-top:0;">Toggles</h2>
<div class="gx-unit-note">
<i class="bi bi-speedometer2"></i>
<span>Vehicle-unit speed settings use <strong>{{ speedUnit }}</strong> and follow the comma's <em>Use Metric System</em> toggle. Each control shows its adjustment step.</span>
</div>
<DevModeBanner :hidden-count="hiddenAdvancedCount" :dev-mode-on="devModeOn" />
<div v-if="loading" class="gx-loading">Loading configuration...</div>
@@ -140,7 +132,7 @@ export const Settings = {
<template v-for="section in searchResults" :key="section.slug">
<GalaxySection :title="section.name + ' (' + section.matches.length + ')'" :icon="section.icon || 'bi-search'" :default-open="false">
<template v-for="p in section.matches" :key="p.key">
<GalaxyToggleCard :param="p" :value="values[p.key]" :values="values" :locked="lockReason(p) !== ''"
<GalaxyToggleCard :param="p" :value="values[p.key]" :locked="lockReason(p) !== ''"
@change="onParamChange" />
</template>
</GalaxySection>
@@ -1,6 +1,6 @@
{
"name": "Galaxy",
"short_name": "Galaxy",
"name": "Big Dipper",
"short_name": "Big Dipper",
"description": "Control and configure your openpilot device from anywhere.",
"icons": [
{ "src": "/assets/images/android-chrome-192x192.png", "sizes": "192x192", "type": "image/png", "purpose": "any maskable" },
@@ -8,6 +8,7 @@
{ "src": "/assets/images/apple-touch-icon.png", "sizes": "180x180", "type": "image/png" }
],
"id": "46bf2df73deba8e1512c35de",
"start_url": "/mobile/",
"scope": "/",
"background_color": "#06060f",
"theme_color": "#8b6cc5",
@@ -42,7 +42,7 @@
<link rel="stylesheet" href="/assets/components/tools/update_manager.css">
<link rel="stylesheet" href="/assets/components/tools/device_settings.css?v=favorite-actions-1">
<link rel="stylesheet" href="/assets/components/tools/bluetooth.css?v=bluetooth-6">
<link rel="stylesheet" href="/assets/components/tools/wheel_controls.css?v=controllers-3">
<link rel="stylesheet" href="/assets/components/tools/wheel_controls.css?v=controllers-2">
<link rel="stylesheet" href="/assets/components/tools/galaxy.css">
<link rel="stylesheet" href="/assets/components/tools/sentry.css">
<link rel="stylesheet" href="/assets/components/tools/longitudinal_maneuvers.css">
@@ -51,7 +51,7 @@
<link rel="stylesheet" href="/assets/components/tools/tsk_manager.css">
<script type="module">
import("/assets/components/router.js?v=router-cycle-fix-8").catch((err) => {
import("/assets/components/router.js?v=router-cycle-fix-7").catch((err) => {
console.error("[the_galaxy] bootstrap failed", err);
const target = document.getElementById("app") || document.body;
const pre = document.createElement("pre");
@@ -46,18 +46,6 @@ def test_device_settings_uses_the_params_api_and_layout_json():
assert 'fetch("/assets/components/tools/device_settings_layout.json?v=settings-tier-1"' in source
def test_device_settings_speed_units_follow_the_vehicle():
source = _device_settings()
assert 'from "/assets/mobile/js/params.js"' in source
assert "resolveVehicleUnitParam" in source
assert "formatNumericParamValue" in source
assert "vehicleSpeedUnit(state.values)" in source
assert "unit_search_terms" in source
assert "Use Metric System" in source
assert "per click" in source
def test_lane_center_offset_can_step_below_zero():
source = _device_settings()
@@ -107,41 +107,6 @@ def test_device_shutdown_uses_literal_hours():
assert device_shutdown["step"] == 1
def test_speed_settings_follow_vehicle_units_with_one_unit_steps():
sections = _params_by_section(_layout())
speed_keys = {
"MinimumLaneChangeSpeed", "PauseLateralSpeed",
"CESpeed", "CESpeedLead", "CESignalSpeed",
"CustomCruise", "CustomCruiseLong", "SetSpeedOffset", "PulseGlideSpeedDelta",
"Offset1", "Offset2", "Offset3", "Offset4", "Offset5", "Offset6", "Offset7",
"CCMSpeed", "CCMSpeedLead", "CCMSetSpeedMargin",
"VisionSpeedLimitLowLimitThreshold", "TurnSteeringLimitMuteSpeed",
}
params = {
param["key"]: param
for section in sections.values()
for param in section.values()
if param["key"] in speed_keys
}
assert params.keys() == speed_keys
assert all(param["unit_type"] == "vehicle_speed" for param in params.values())
one_unit_keys = speed_keys - {"PulseGlideSpeedDelta", "VisionSpeedLimitLowLimitThreshold"}
assert all(params[key]["step"] == 1 for key in one_unit_keys)
assert params["PulseGlideSpeedDelta"]["step"] == 0.5
assert params["VisionSpeedLimitLowLimitThreshold"]["step"] == 5
for index in range(7):
offset = params[f"Offset{index + 1}"]
assert offset["unit_range_index"] == index
assert (offset["metric_min"], offset["metric_max"]) == (-150, 150)
assert params["CustomCruise"]["metric_max"] == 150
assert params["CCMSetSpeedMargin"]["metric_max"] == 30
assert params["PulseGlideSpeedDelta"]["imperial_max"] == 15
def test_curve_speed_controller_no_lead_toggle_is_nested_under_csc():
csc_no_lead = _params_by_section(_layout())["Longitudinal (Speed & Following)"]["CurveSpeedControllerNoLead"]
@@ -7,7 +7,6 @@ ROUTER_PATH = REPO_ROOT / "starpilot/system/the_galaxy/assets/components/router.
INDEX_PATH = REPO_ROOT / "starpilot/system/the_galaxy/templates/index.html"
BLUETOOTH_PATH = REPO_ROOT / "starpilot/system/the_galaxy/assets/components/tools/bluetooth.js"
CONTROLLERS_PATH = REPO_ROOT / "starpilot/system/the_galaxy/assets/components/tools/wheel_controls.js"
MOBILE_CONTROLLERS_PATH = REPO_ROOT / "starpilot/system/the_galaxy/assets/mobile/js/components/WheelControls.js"
SIDEBAR_PATH = REPO_ROOT / "starpilot/system/the_galaxy/assets/components/sidebar.js"
MODEL_LAB_PATH = REPO_ROOT / "starpilot/system/the_galaxy/assets/components/tools/model_laboratory.js"
@@ -24,7 +23,7 @@ def test_router_and_settings_cache_bust_is_consistent():
index = INDEX_PATH.read_text(encoding="utf-8")
assert "/assets/components/settings.js?v=router-cycle-fix-5" in router
assert "/assets/components/router.js?v=router-cycle-fix-8" in index
assert "/assets/components/router.js?v=router-cycle-fix-7" in index
def test_bluetooth_actions_use_reactive_disabled_bindings():
@@ -77,16 +76,6 @@ def test_controller_joystick_mode_requires_explicit_device_selection():
assert 'request("joystick", { device_id: device.device_id, enabled: !selected() })' in source
def test_controller_offroad_disconnect_is_opt_in():
source = CONTROLLERS_PATH.read_text(encoding="utf-8")
mobile_source = MOBILE_CONTROLLERS_PATH.read_text(encoding="utf-8")
for frontend in (source, mobile_source):
assert "Disconnect controllers when offroad" in frontend
assert "After two minutes offroad" in frontend
assert "offroad-disconnect" in frontend
def test_controller_page_has_ten_controller_only_action_slots():
source = CONTROLLERS_PATH.read_text(encoding="utf-8")
@@ -231,25 +231,6 @@ def test_wheel_controls_status_includes_favorite_slots(monkeypatch):
"__starpilot_controller_action__:disengage_openpilot",
}
assert response.get_json()["speed_unit"] == "mph"
assert response.get_json()["disconnect_controllers_offroad"] is False
def test_wheel_controls_configures_offroad_controller_disconnect(monkeypatch):
client, fake_params = _params_client(monkeypatch, {"IsOffroad": True}, "mici")
response = client.post("/api/wheel-controls/offroad-disconnect", json={"enabled": True})
assert response.status_code == 200
assert fake_params.get_bool("BluetoothDisconnectControllersOffroad")
def test_wheel_controls_offroad_controller_disconnect_requires_offroad(monkeypatch):
client, fake_params = _params_client(monkeypatch, {"IsOffroad": False}, "mici")
response = client.post("/api/wheel-controls/offroad-disconnect", json={"enabled": True})
assert response.status_code == 409
assert not fake_params.get_bool("BluetoothDisconnectControllersOffroad")
def test_wheel_controls_configures_a_controller_only_action(monkeypatch):
@@ -62,26 +62,6 @@ def test_slug_middleware_service_worker_and_headers(client):
assert response_direct.headers.get("Service-Worker-Allowed") == "/"
def test_mobile_manifest_embeds_device_slug_for_fresh_app_login(client):
galaxy_dir = the_galaxy._get_galaxy_dir()
galaxy_dir.mkdir(parents=True, exist_ok=True)
(galaxy_dir / "glxyslug").write_text("df70390ca648d7c3")
response = client.get("/assets/mobile/manifest.json")
assert response.status_code == 200
assert response.mimetype == "application/manifest+json"
assert response.get_json()["start_url"] == "https://galaxy.firestar.link/df70390ca648d7c3"
assert "no-store" in response.headers.get("Cache-Control", "")
def test_mobile_manifest_falls_back_to_local_mobile_route_without_slug(client):
response = client.get("/assets/mobile/manifest.json")
assert response.status_code == 200
assert response.get_json()["start_url"] == "/mobile/"
def test_404_api_returns_json_not_html(client):
# Non-existent API route without slug
res1 = client.get("/api/nonexistent")
@@ -190,23 +190,6 @@ def test_ui_numeric_toggles_are_sliders_with_default():
assert 'title="Set to zero"' not in card
def test_ui_speed_units_follow_the_vehicle():
params = _read("js/params.js")
card = _read("js/components/GalaxyToggleCard.js")
tree = _read("js/components/SettingTree.js")
settings = _read("js/views/Settings.js")
assert "resolveVehicleUnitParam" in params
assert "formatNumericParamValue" in params
assert "unit_search_terms" in params and "unit_search_terms" in settings
assert "IsMetric" in params
assert ':values="values"' in tree
assert "displayParam" in card and "formatNumericParamValue" in card
assert "sliderStepDisplay" in card and "Step:" in card
assert ':values="values"' in settings
assert "Use Metric System" in settings
def test_ui_centralizes_api_and_uses_composables():
api = _read("js/api.js")
composables = _read("js/composables.js")
@@ -360,7 +343,7 @@ def test_ui_manifest_is_valid_pwa_manifest():
assert manifest["display"] == "standalone"
assert manifest["name"]
assert manifest["icons"]
assert "start_url" not in manifest
assert manifest["start_url"] == "/mobile/"
def test_ui_ported_classic_tools_native_no_embed():
@@ -522,23 +505,6 @@ assert(P.countAdvancedHiddenByDeveloperMode([sec], { GalaxyDeveloperMode: true }
const slider = { key: "DeviceShutdown", data_type: "int", min: 1, max: 30, step: 1 }
assert(P.snapNumericToBoundsAndStep(17.9, P.numericBounds(slider, {}), 0) === 18, "snap")
assert(P.formatSliderValue(6, "1", 0, "DeviceShutdown") === "6 hours", "format")
const speed = {
key: "Offset2", data_type: "float", unit_type: "vehicle_speed",
min: -99, max: 99, step: 1, precision: 0,
metric_min: -150, metric_max: 150, unit_range_index: 1,
}
const imperialSpeed = P.resolveVehicleUnitParam(speed, { IsMetric: false })
assert(imperialSpeed.unit === " mph", "imperial unit")
assert(imperialSpeed.label === "Speed Offset (2534 mph)", "imperial offset band")
assert(P.formatNumericParamValue(speed, 3, { IsMetric: false }) === "3 mph", "imperial value")
const metricSpeed = P.resolveVehicleUnitParam(speed, { IsMetric: true })
assert(metricSpeed.unit === " km/h", "metric unit")
assert(metricSpeed.label === "Speed Offset (3049 km/h)", "metric offset band")
assert(metricSpeed.unit_search_terms.includes("metric"), "metric settings are searchable")
assert(P.numericBounds(speed, { IsMetric: true }).max === 150, "metric bounds")
assert(P.numericBounds(speed, { IsMetric: true }).step === 1, "one km/h per step")
assert(P.formatNumericParamValue(speed, 3, { IsMetric: true }) === "3 km/h", "metric value")
assert(P.usesMetricUnits({ IsMetric: "1" }) === true, "serialized metric bool")
const laneOffset = { key: "LaneCenterOffset", data_type: "float", min: 0, max: 0.3, step: 0.01 }
const laneBounds = P.numericBounds(laneOffset, {})
assert(laneBounds.min === -0.3, "lane offset keeps signed lower bound")
+1 -27
View File
@@ -5015,7 +5015,6 @@ def setup(app):
"/assets/components/settings.js",
"/assets/components/home/home.js",
"/assets/components/home/home.css",
"/assets/mobile/js/params.js",
"/assets/components/tools/device_settings.js",
"/assets/components/tools/device_settings.css",
"/assets/components/tools/device_settings_layout.json",
@@ -5182,7 +5181,6 @@ def setup(app):
status["slots"] = slots
status["controller_slots"] = controller_slots
status["controller_options"] = controller_options
status["disconnect_controllers_offroad"] = params.get_bool("BluetoothDisconnectControllersOffroad")
is_metric = params.get_bool("IsMetric")
speed_minimum, speed_maximum = controller_speed_bounds(is_metric)
status["speed_unit"] = "km/h" if is_metric else "mph"
@@ -5192,16 +5190,13 @@ def setup(app):
@app.route("/api/wheel-controls/<operation>", methods=["POST"])
def wheel_controls_operation(operation):
if operation not in {"action", "learn", "cancel", "delete", "clear", "test", "test-stop", "joystick", "offroad-disconnect"}:
if operation not in {"action", "learn", "cancel", "delete", "clear", "test", "test-stop", "joystick"}:
return jsonify({"error": "Unknown wheel control operation."}), 404
if not params.get_bool("IsOffroad"):
return jsonify({"error": "Wheel controls can only be configured offroad."}), 409
data = request.get_json(silent=True) or {}
try:
if operation == "offroad-disconnect":
params.put_bool("BluetoothDisconnectControllersOffroad", bool(data.get("enabled", False)))
return jsonify({"message": "Offroad controller disconnect updated."}), 200
if operation == "action":
slot_index = int(data.get("slot", -1))
key = str(data.get("key") or "").strip()
@@ -5301,27 +5296,6 @@ def setup(app):
return "Settings catalog not found", 404
return send_file(str(SETTINGS_CATALOG_PATH), mimetype="application/json")
@app.route("/assets/mobile/manifest.json", methods=["GET"])
def mobile_manifest():
manifest_path = Path(app.static_folder) / "mobile" / "manifest.json"
if not manifest_path.is_file():
return jsonify({"error": "Big Dipper manifest not found"}), 404
try:
manifest_data = json.loads(manifest_path.read_text(encoding="utf-8"))
except (OSError, ValueError, json.JSONDecodeError):
return jsonify({"error": "Big Dipper manifest is invalid"}), 500
slug = _read_galaxy_text(_get_galaxy_dir() / "glxyslug")
if re.fullmatch(r"[A-Za-z0-9]{16}", slug):
manifest_data["start_url"] = f"https://galaxy.firestar.link/{slug}"
else:
manifest_data["start_url"] = "/mobile/"
response = jsonify(manifest_data)
response.mimetype = "application/manifest+json"
return _no_store_response(response)
@app.route("/manifest.json", methods=["GET"])
@app.route("/assets/manifest.json", methods=["GET"])
def manifest():
+11 -3
View File
@@ -1,3 +1,5 @@
from __future__ import annotations
import os
import operator
import platform
@@ -6,6 +8,7 @@ import sys
from types import SimpleNamespace
from cereal import car
from openpilot.common.gps import gm_car_params_present
from openpilot.common.params import Params
from opendbc.car.gps import car_gps_available
from openpilot.system.hardware import HARDWARE, PC, TICI
@@ -36,9 +39,12 @@ def update_car_gps_param(params: Params) -> bool | None:
car_params = params.get("CarParams")
if car_params is None:
return None
try:
with car.CarParams.from_bytes(car_params) as parsed_cp:
available = car_gps_available(parsed_cp)
except Exception:
return None
with car.CarParams.from_bytes(car_params) as CP:
available = car_gps_available(CP)
if available != params.get_bool("CarGpsAvailable"):
params.put_bool("CarGpsAvailable", available)
return available
@@ -48,7 +54,9 @@ def ublox(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: S
use_ublox = ublox_available()
if use_ublox != params.get_bool("UbloxAvailable"):
params.put_bool("UbloxAvailable", use_ublox)
return started and use_ublox and car_gps is False
is_gm = gm_car_params_present(params)
# On GM, card arbitrates u-blox and CAN GPS; wait for CarParams.
return started and use_ublox and car_gps is not None and (not car_gps or is_gm)
def joystick(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool:
return started and params.get_bool("JoystickDebugMode")
+28 -19
View File
@@ -4,6 +4,7 @@ import pytest
from cereal import car
from opendbc.car.ford.values import CAR as FORD_CAR
from opendbc.car.gm.values import CAR as GM_CAR
import openpilot.system.manager.process_config as process_config
from openpilot.system.manager.process_config import (
allow_uploads,
@@ -152,25 +153,33 @@ class GpsParams:
self.values[key] = value
def test_ublox_waits_for_current_carparams(monkeypatch):
monkeypatch.setattr("openpilot.system.manager.process_config.ublox_available", lambda: True)
params = GpsParams()
@pytest.mark.parametrize("brand,fingerprint,persisted,live_brand,expected,car_gps", [
("gm", "", False, "gm", False, False),
("mock", "", False, "mock", False, False),
("ford", "", False, "ford", False, False),
("gm", GM_CAR.CHEVROLET_BOLT_CC_2018_2021, False, "gm", False, False),
("gm", GM_CAR.CHEVROLET_BOLT_CC_2018_2021, True, "gm", True, True),
("gm", GM_CAR.CHEVROLET_BOLT_CC_2018_2021, True, "", True, True),
("gm", "unknown GM", True, "gm", True, False),
("mock", "mock", True, "", True, False),
("ford", FORD_CAR.FORD_MUSTANG_MACH_E_MK1, True, "", False, True),
("gm", "", "corrupt", "", False, False),
], ids=["missing-gm", "missing-mock", "missing-ford", "stale-live-cp", "persisted-gm",
"empty-live-cp", "unknown-gm", "non-car-gps", "vehicle-gps", "corrupt-cp"])
def test_ublox_startup(monkeypatch, brand, fingerprint, persisted, live_brand, expected, car_gps):
monkeypatch.setattr(process_config, "ublox_available", lambda: True)
current = car.CarParams.new_message(brand=brand, carFingerprint=fingerprint)
params = GpsParams(current if persisted is True else None)
if persisted == "corrupt":
params.values["CarParams"] = b"invalid_corrupt_data"
live_cp = car.CarParams.new_message(brand=live_brand, carFingerprint=fingerprint if live_brand else "")
assert not ublox(True, params, car.CarParams.new_message(), SimpleNamespace())
assert ublox(True, params, live_cp, SimpleNamespace()) is expected
assert params.get_bool("UbloxAvailable")
@pytest.mark.parametrize("car_gps,expected", [(False, True), (True, False)])
def test_ublox_has_single_external_gps_publisher(monkeypatch, car_gps, expected):
monkeypatch.setattr("openpilot.system.manager.process_config.ublox_available", lambda: True)
CP = car.CarParams.new_message()
if car_gps:
CP.brand = "ford"
CP.carFingerprint = FORD_CAR.FORD_MUSTANG_MACH_E_MK1
else:
CP.brand = "mock"
CP.carFingerprint = "mock"
params = GpsParams(CP)
assert ublox(True, params, car.CarParams.new_message(), SimpleNamespace()) is expected
assert params.get_bool("CarGpsAvailable") is car_gps
assert not ublox(False, params, live_cp, SimpleNamespace())
if persisted is False and fingerprint:
params.values["CarParams"] = current.to_bytes()
assert ublox(True, params, live_cp, SimpleNamespace()) # Same stale live CP, now with current persisted CP.
monkeypatch.setattr(process_config, "ublox_available", lambda: False)
assert not ublox(True, params, live_cp, SimpleNamespace())
+3 -1
View File
@@ -63,7 +63,8 @@ def main() -> NoReturn:
gps_location_service = get_gps_location_service(params)
pm = messaging.PubMaster(['clocks'])
sm = messaging.SubMaster([gps_location_service])
# Subscribe to both services to support dynamic GM GPS routing.
sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal'])
# StarPilot variables
tf = TimezoneFinder() if TimezoneFinder is not None else None
@@ -75,6 +76,7 @@ def main() -> NoReturn:
while True:
sm.update(1000)
gps_location_service = get_gps_location_service(params)
msg = messaging.new_message('clocks')
msg.valid = system_time_valid()
+30
View File
@@ -0,0 +1,30 @@
from types import SimpleNamespace
import pytest
from cereal import car, messaging
from openpilot.system.ubloxd import ubloxd
@pytest.mark.parametrize("brand,service", [("gm", "gpsLocation"), ("mock", "gpsLocationExternal")])
def test_main_routes_every_parsed_fix(monkeypatch, brand, service):
cp = car.CarParams.new_message(brand=brand)
monkeypatch.setattr(ubloxd, "Params", lambda: SimpleNamespace(get=lambda key: cp.to_bytes()))
sent = []
pm = SimpleNamespace(send=lambda service, message: sent.append((service, message)))
monkeypatch.setattr(messaging, "PubMaster", lambda services: pm)
monkeypatch.setattr(messaging, "sub_sock", lambda *args, **kwargs: None)
# Several fixes in one receive cycle must all reach the publisher.
incoming = iter([SimpleNamespace(ubloxRaw=b"", logMonoTime=1)])
monkeypatch.setattr(messaging, "recv_one", lambda sock: next(incoming))
parser = ubloxd.UbloxMsgParser(service)
fixes = [messaging.new_message(service, valid=True) for _ in range(5)]
monkeypatch.setattr(parser.framer, "add_data", lambda *args: range(5))
monkeypatch.setattr(parser, "parse_frame", lambda index: (parser.gps_service, fixes[index]))
def make_parser(actual_service):
assert actual_service == service
return parser
monkeypatch.setattr(ubloxd, "UbloxMsgParser", make_parser)
with pytest.raises(StopIteration):
ubloxd.main()
assert sent == [(service, fix) for fix in fixes]
+14 -7
View File
@@ -6,8 +6,11 @@ import numpy as np
from collections import defaultdict
from dataclasses import dataclass
from cereal import car
from cereal import log
from cereal import messaging
from openpilot.common.gps import gm_car_params_present
from openpilot.common.params import Params
from openpilot.system.ubloxd.generated.ubx import Ubx
from openpilot.system.ubloxd.generated.gps import Gps
from openpilot.system.ubloxd.generated.glonass import Glonass
@@ -103,7 +106,8 @@ class UbloxMsgParser:
11: 64, 12: 128, 13: 256, 14: 512, 15: 1024,
}
def __init__(self) -> None:
def __init__(self, gps_service: str = 'gpsLocationExternal') -> None:
self.gps_service = gps_service
self.framer = UbxFramer()
self.caches = EphemerisCaches(
gps_subframes=defaultdict(dict),
@@ -159,10 +163,10 @@ class UbloxMsgParser:
return self._gen_nav_sat(body)
return None
# NAV-PVT -> gpsLocationExternal
# NAV-PVT -> the selected canonical GPS service
def _gen_nav_pvt(self, msg: Ubx.NavPvt) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder]:
dat = messaging.new_message('gpsLocationExternal', valid=True)
gps = dat.gpsLocationExternal
dat = messaging.new_message(self.gps_service, valid=True)
gps = getattr(dat, self.gps_service)
gps.source = log.GpsLocationData.SensorSource.ublox
gps.flags = msg.flags
gps.hasFix = (msg.flags % 2) == 1
@@ -191,7 +195,7 @@ class UbloxMsgParser:
gps.verticalAccuracy = msg.v_acc * 1e-03
gps.speedAccuracy = msg.s_acc * 1e-03
gps.bearingAccuracyDeg = msg.head_acc * 1e-05
return ('gpsLocationExternal', dat)
return (self.gps_service, dat)
# RXM-SFRBX dispatch to GPS or GLONASS ephemeris
def _gen_rxm_sfrbx(self, msg) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder] | None:
@@ -493,8 +497,11 @@ class UbloxMsgParser:
def main():
parser = UbloxMsgParser()
pm = messaging.PubMaster(['ubloxGnss', 'gpsLocationExternal'])
params = Params()
gps_service = 'gpsLocation' if gm_car_params_present(params) else 'gpsLocationExternal'
parser = UbloxMsgParser(gps_service)
pm = messaging.PubMaster(['ubloxGnss', gps_service])
sock = messaging.sub_sock('ubloxRaw', timeout=100, conflate=False)
while True: