mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-10 08:13:57 +08:00
Potato Ole
This commit is contained in:
firestar5683
+3
-2
@@ -4,7 +4,7 @@
|
||||
|
||||
A supported vehicle is one that just works when you install a comma device. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified.
|
||||
|
||||
# 453 Supported Cars
|
||||
# 454 Supported Cars
|
||||
|
||||
|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|<a href="##"><img width=2000></a>Hardware Needed<br> |Video|Setup Video|
|
||||
|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
|
||||
@@ -264,7 +264,8 @@ A supported vehicle is one that just works when you install a comma device. All
|
||||
|Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|6 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai G connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Forte 2019-21">Buy Here</a></sub></details>|||
|
||||
|Kia|Forte 2022-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Forte 2022-23">Buy Here</a></sub></details>|||
|
||||
|Kia|K4 (with HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai R connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K4 (with HDA II) 2025">Buy Here</a></sub></details>|||
|
||||
|Kia|K4 (without HDA II) 2025-26|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K4 (without HDA II) 2025-26">Buy Here</a></sub></details>|||
|
||||
|Kia|K4 (without HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K4 (without HDA II) 2025">Buy Here</a></sub></details>|||
|
||||
|Kia|K4 (without HDA II) 2026|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K4 (without HDA II) 2026">Buy Here</a></sub></details>|||
|
||||
|Kia|K5 2021-24|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K5 2021-24">Buy Here</a></sub></details>|||
|
||||
|Kia|K5 (without HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai M connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K5 (without HDA II) 2025">Buy Here</a></sub></details>|||
|
||||
|Kia|K5 Hybrid 2020-22|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K5 Hybrid 2020-22">Buy Here</a></sub></details>|||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
<!--- AUTOGENERATED FROM selfdrive/car/CARS_template.md, DO NOT EDIT. --->
|
||||
|
||||
# Support Information for 518 Known Cars
|
||||
# Support Information for 519 Known Cars
|
||||
|
||||
|Make|Model|Package|Support Level|
|
||||
|---|---|---|:---:|
|
||||
@@ -287,6 +287,7 @@
|
||||
|Kia|Forte Non-SCC 2021|No Smart Cruise Control (Non-SCC)|[Community](community)|
|
||||
|Kia|K4 (with HDA II) 2025|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|
||||
|Kia|K4 (without HDA II) 2025|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|
||||
|Kia|K4 (without HDA II) 2026|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|
||||
|Kia|K5 2021-24|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|
||||
|Kia|K5 (without HDA II) 2025|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|
||||
|Kia|K5 Hybrid 2020-22|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|
||||
|
||||
@@ -8,6 +8,7 @@ from typing import Any
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.ford.values import CAR as FORD_CAR
|
||||
from opendbc.car.gm.values import CAR as GM_CAR
|
||||
from opendbc.car.volkswagen.values import CAR as VOLKSWAGEN_CAR
|
||||
|
||||
|
||||
CarGpsSample = dict[str, Any]
|
||||
@@ -124,12 +125,66 @@ def parse_chevrolet_bolt_can_gps(position: Mapping[str, float]) -> CarGpsSample
|
||||
}
|
||||
|
||||
|
||||
def parse_volkswagen_taos_can_gps(position: Mapping[str, float], motion: Mapping[str, float],
|
||||
altitude: Mapping[str, float], status: Mapping[str, float]) -> CarGpsSample | None:
|
||||
packet_ids = (position["GNSS_Nachrichtenpaket_ID1"], motion["GNSS_Nachrichtenpaket_ID2"],
|
||||
altitude["GNSS_Nachrichtenpaket_ID4"], status["GNSS_Nachrichtenpaket_ID5"])
|
||||
if len(set(packet_ids)) != 1:
|
||||
return None
|
||||
|
||||
timestamp_ms = int(status["GNSS_UTC_Zeit"]) * 1000
|
||||
if timestamp_ms == 0:
|
||||
return None
|
||||
|
||||
latitude = float(position["GNSS_LatitudeMagnitude"])
|
||||
longitude = float(position["GNSS_LongitudeMagnitude"])
|
||||
coordinates_valid = (math.isfinite(latitude) and math.isfinite(longitude) and
|
||||
0.0 <= latitude <= 90.0 and 0.0 <= longitude <= 180.0 and (latitude != 0.0 or longitude != 0.0))
|
||||
if coordinates_valid:
|
||||
latitude *= -1.0 if position["GNSS_LatitudeSouth"] else 1.0
|
||||
longitude *= -1.0 if position["GNSS_LongitudeWest"] else 1.0
|
||||
else:
|
||||
latitude = longitude = 0.0
|
||||
|
||||
satellite_count = int(status["GNSS_Genutzte_Satelliten"])
|
||||
hemisphere_validated = position["GNSS_LatitudeSouth"] == 0 and position["GNSS_LongitudeWest"] == 1
|
||||
has_fix = (coordinates_valid and hemisphere_validated and position["GNSS_PositionStatus"] == 3 and status["GNSS_Empfaenger_Status"] == 1 and
|
||||
bool(status["GNSS_GPS_in_Nutzung"] or status["GNSS_GLONASS_in_Nutzung"]) and 4 <= satellite_count <= 31)
|
||||
|
||||
speed = float(motion["GNSS_Speed"])
|
||||
bearing = float(motion["GNSS_Bearing"])
|
||||
speed_valid = math.isfinite(speed) and 0.0 <= speed <= 127.5
|
||||
bearing_valid = math.isfinite(bearing) and 0.0 <= bearing < 360.0
|
||||
speed = speed if has_fix and speed_valid else 0.0
|
||||
bearing = bearing if has_fix and bearing_valid else 0.0
|
||||
height = float(altitude["GNSS_Ortung_Hoehe"])
|
||||
height_valid = math.isfinite(height) and -500.0 <= height <= 7686.0
|
||||
heading_rad = math.radians(bearing)
|
||||
|
||||
return {
|
||||
"latitude": latitude,
|
||||
"longitude": longitude,
|
||||
"altitude": height if has_fix and height_valid else 0.0,
|
||||
"speed": speed,
|
||||
"bearingDeg": bearing,
|
||||
"horizontalAccuracy": 20.0,
|
||||
"unixTimestampMillis": timestamp_ms,
|
||||
"verticalAccuracy": 50.0 if height_valid else 500.0,
|
||||
"bearingAccuracyDeg": 10.0 if has_fix and bearing_valid and speed > 1.0 else 180.0,
|
||||
"speedAccuracy": 1.5 if speed_valid and bearing_valid else 100.0,
|
||||
"hasFix": has_fix,
|
||||
"satelliteCount": satellite_count if 0 <= satellite_count <= 31 else 0,
|
||||
"vNED": [speed * math.cos(heading_rad), speed * math.sin(heading_rad), 0.0] if bearing_valid else [0.0, 0.0, 0.0],
|
||||
}
|
||||
|
||||
|
||||
FORD_MACH_E_GPS_MESSAGES = (
|
||||
"APIMGPS_Data_Nav_1_FD1",
|
||||
"APIMGPS_Data_Nav_2_FD1",
|
||||
"APIMGPS_Data_Nav_3_FD1",
|
||||
)
|
||||
CHEVROLET_BOLT_GPS_MESSAGES = ("TCICOnStarGPSPosition",)
|
||||
VOLKSWAGEN_TAOS_GPS_MESSAGES = ("GNSS_01", "GNSS_02", "GNSS_04", "GNSS_05")
|
||||
|
||||
CHEVROLET_BOLT_GPS_CARS = (
|
||||
GM_CAR.CHEVROLET_BOLT_ACC_2022_2023,
|
||||
@@ -141,6 +196,11 @@ CHEVROLET_BOLT_GPS_CARS = (
|
||||
|
||||
|
||||
CAR_GPS_CONFIGS: dict[str, CarGpsConfig] = {
|
||||
VOLKSWAGEN_CAR.VOLKSWAGEN_TAOS_MK1: CarGpsConfig(
|
||||
brand="volkswagen",
|
||||
messages=VOLKSWAGEN_TAOS_GPS_MESSAGES,
|
||||
decoder=parse_volkswagen_taos_can_gps,
|
||||
),
|
||||
FORD_CAR.FORD_MUSTANG_MACH_E_MK1: CarGpsConfig(
|
||||
brand="ford",
|
||||
messages=FORD_MACH_E_GPS_MESSAGES,
|
||||
|
||||
@@ -1863,6 +1863,37 @@ class TestHyundaiFingerprint:
|
||||
assert exact
|
||||
assert matches == {candidate}
|
||||
|
||||
@pytest.mark.parametrize("alpha_long", (False, True))
|
||||
@pytest.mark.parametrize("alt_buttons", (False, True))
|
||||
@pytest.mark.parametrize("signal, value, button_type", (
|
||||
("LDA_BTN", 1, ButtonType.lkas),
|
||||
("ADAPTIVE_CRUISE_MAIN_BTN", 1, ButtonType.mainCruise),
|
||||
("CRUISE_BUTTONS", Buttons.RES_ACCEL, ButtonType.accelCruise),
|
||||
("CRUISE_BUTTONS", Buttons.SET_DECEL, ButtonType.decelCruise),
|
||||
("CRUISE_BUTTONS", Buttons.CANCEL, ButtonType.cancel),
|
||||
))
|
||||
def test_k4_2025_2026_physical_button_events(self, alpha_long, alt_buttons, signal, value, button_type):
|
||||
toggles = get_test_toggles()
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
if not alt_buttons:
|
||||
fingerprint[0][0x1CF] = 8
|
||||
CP = CarInterface.get_params(CAR.KIA_K4_2025, fingerprint, [], alpha_long, False, False, toggles)
|
||||
FPCP = CarInterface.get_starpilot_params(CAR.KIA_K4_2025, fingerprint, [], CP, toggles)
|
||||
car_state = CarState(CP, FPCP)
|
||||
parsers = car_state.get_can_parsers(CP)
|
||||
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
|
||||
|
||||
def update(button_value, frame):
|
||||
msg = packer.make_can_msg(car_state.cruise_btns_msg_canfd, CanBus(CP).ECAN, {signal: button_value})
|
||||
parsers[Bus.pt].update([(frame * 10_000_000, [msg])])
|
||||
return car_state.update(parsers, toggles)[0].buttonEvents
|
||||
|
||||
assert not update(0, 1)
|
||||
pressed = update(value, 2)
|
||||
assert [(event.type, event.pressed) for event in pressed] == [(button_type, True)]
|
||||
released = update(0, 3)
|
||||
assert [(event.type, event.pressed) for event in released] == [(button_type, False)]
|
||||
|
||||
@pytest.mark.parametrize("camera_fw", [
|
||||
b'\xf1\x00CL4 MFC AT CAN LHD 1.00 1.02 99210-GG000 240708',
|
||||
b'\xf1\x00CL4 MFC AT USA LHD 1.00 1.02 99210-GG000 240708',
|
||||
|
||||
@@ -605,7 +605,8 @@ class CAR(Platforms):
|
||||
)
|
||||
KIA_K4_2025 = HyundaiCanFDPlatformConfig(
|
||||
[
|
||||
HyundaiCarDocs("Kia K4 (without HDA II) 2025-26", car_parts=CarParts.common([CarHarness.hyundai_a])),
|
||||
HyundaiCarDocs("Kia K4 (without HDA II) 2025", car_parts=CarParts.common([CarHarness.hyundai_a])),
|
||||
HyundaiCarDocs("Kia K4 (without HDA II) 2026", car_parts=CarParts.common([CarHarness.hyundai_a])),
|
||||
HyundaiCarDocs("Kia K4 (with HDA II) 2025", car_parts=CarParts.common([CarHarness.hyundai_r])),
|
||||
],
|
||||
CarSpecs(mass=2987 * CV.LB_TO_KG, wheelbase=2.72, steerRatio=13.4),
|
||||
|
||||
@@ -3,6 +3,7 @@ from opendbc.can import CANParser
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.gps import get_car_gps_config
|
||||
from opendbc.car.volkswagen.values import DBC, CanBus, NetworkLocation, TransmissionType, GearShifter, \
|
||||
CarControllerParams, VolkswagenFlags
|
||||
|
||||
@@ -24,6 +25,32 @@ class CarState(CarStateBase):
|
||||
self.travel_assist_available = False
|
||||
self.curvature_meas = 0.
|
||||
self.klr_stock_values = {}
|
||||
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._car_gps_timestamp_nanos = 0
|
||||
|
||||
def _update_car_gps(self, cp) -> None:
|
||||
if self.car_gps_config is None:
|
||||
return
|
||||
|
||||
timestamps = [max(cp.ts_nanos[name].values(), default=0) for name in self.car_gps_config.messages]
|
||||
if min(timestamps) > self._car_gps_timestamp_nanos and max(timestamps) - min(timestamps) <= 2_000_000_000:
|
||||
gps = self.car_gps_config.decoder(*(cp.vl[name] for name in self.car_gps_config.messages))
|
||||
if gps is not None and (self.car_gps is None or not gps["hasFix"] or
|
||||
gps["unixTimestampMillis"] > self.car_gps["unixTimestampMillis"]):
|
||||
timestamp_nanos = max(timestamps)
|
||||
gps["timestamp_nanos"] = timestamp_nanos
|
||||
self.car_gps = gps
|
||||
self._car_gps_timestamp_nanos = timestamp_nanos
|
||||
|
||||
if self.car_gps is not None and cp._last_update_nanos - self._car_gps_timestamp_nanos > 2_500_000_000:
|
||||
if self.car_gps["hasFix"]:
|
||||
self.car_gps = {**self.car_gps, "hasFix": False, "speed": 0.0, "vNED": [0.0, 0.0, 0.0],
|
||||
"bearingAccuracyDeg": 180.0, "timestamp_nanos": cp._last_update_nanos}
|
||||
|
||||
def get_car_gps(self):
|
||||
return self.car_gps
|
||||
|
||||
def update_button_enable(self, buttonEvents: list[structs.CarState.ButtonEvent]):
|
||||
if not self.CP.pcmCruise:
|
||||
@@ -51,6 +78,7 @@ class CarState(CarStateBase):
|
||||
pt_cp = can_parsers[Bus.pt]
|
||||
cam_cp = can_parsers[Bus.cam]
|
||||
ext_cp = pt_cp if self.CP.networkLocation == NetworkLocation.fwdCamera else cam_cp
|
||||
self._update_car_gps(pt_cp)
|
||||
|
||||
if self.CP.flags & VolkswagenFlags.PQ:
|
||||
return self.update_pq(pt_cp, cam_cp, ext_cp)
|
||||
@@ -427,6 +455,9 @@ class CarState(CarStateBase):
|
||||
|
||||
# manually configure some optional and variable-rate/edge-triggered messages
|
||||
pt_messages, cam_messages = [], []
|
||||
gps_config = get_car_gps_config(CP)
|
||||
if gps_config is not None:
|
||||
pt_messages += [(name, 0) for name in gps_config.messages]
|
||||
|
||||
if not CP.flags & VolkswagenFlags.MLB:
|
||||
pt_messages += [
|
||||
|
||||
@@ -0,0 +1,196 @@
|
||||
import pytest
|
||||
|
||||
from opendbc.can import CANPacker, CANParser
|
||||
from opendbc.car import Bus
|
||||
from opendbc.car.gps import VOLKSWAGEN_TAOS_GPS_MESSAGES, get_car_gps_config, parse_volkswagen_taos_can_gps
|
||||
from opendbc.car.volkswagen.carstate import CarState
|
||||
from opendbc.car.volkswagen.interface import CarInterface
|
||||
from opendbc.car.volkswagen.values import CAR, DBC
|
||||
|
||||
|
||||
# Synthetic frames, not route data: 40 N / 110 W, 12.5 m/s westbound,
|
||||
# altitude 300 m, UTC 1,800,000,000, ten satellites, packet ID 2.
|
||||
GPS_FRAMES = (
|
||||
(0x36F, bytes.fromhex("02688909e09da31d"), 0),
|
||||
(0x374, bytes.fromhex("02c0a83200000000"), 0),
|
||||
(0x378, bytes.fromhex("0221415290010000"), 0),
|
||||
(0x37B, bytes.fromhex("00d2496b7f4c0900"), 0),
|
||||
)
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def gps_state():
|
||||
cp = CarInterface.get_non_essential_params(CAR.VOLKSWAGEN_TAOS_MK1)
|
||||
state = CarState(cp, None)
|
||||
parsers = state.get_can_parsers(cp)
|
||||
return state, parsers
|
||||
|
||||
|
||||
def decode_frames(frames=GPS_FRAMES):
|
||||
parser = CANParser("vw_mqb", [(name, 0) for name in VOLKSWAGEN_TAOS_GPS_MESSAGES], 0)
|
||||
parser.update([(1_000_000_000, list(frames))])
|
||||
return parser, [parser.vl[name] for name in VOLKSWAGEN_TAOS_GPS_MESSAGES]
|
||||
|
||||
|
||||
def test_synthetic_gps_frame_decode():
|
||||
parser, values = decode_frames()
|
||||
gps = parse_volkswagen_taos_can_gps(*values)
|
||||
assert parser.can_valid
|
||||
assert gps["hasFix"]
|
||||
assert gps["latitude"] == pytest.approx(40.0)
|
||||
assert gps["longitude"] == pytest.approx(-110.0)
|
||||
assert gps["speed"] == 12.5
|
||||
assert gps["bearingDeg"] == 270.0
|
||||
assert gps["altitude"] == 300.0
|
||||
assert gps["unixTimestampMillis"] == 1_800_000_000_000
|
||||
assert gps["satelliteCount"] == 10
|
||||
assert gps["vNED"] == pytest.approx([0.0, -12.5, 0.0])
|
||||
|
||||
|
||||
def test_unvalidated_hemisphere_is_not_a_fix():
|
||||
frames = ((0x36F, bytes.fromhex("02688929e09da319"), 0), *GPS_FRAMES[1:])
|
||||
_, values = decode_frames(frames)
|
||||
gps = parse_volkswagen_taos_can_gps(*values)
|
||||
assert not gps["hasFix"]
|
||||
assert gps["vNED"] == [0.0, 0.0, 0.0]
|
||||
|
||||
|
||||
@pytest.mark.parametrize("message,signal,value", (
|
||||
(0, "GNSS_LatitudeMagnitude", 91.0),
|
||||
(0, "GNSS_LongitudeMagnitude", 181.0),
|
||||
(0, "GNSS_LatitudeMagnitude", float("nan")),
|
||||
(0, "GNSS_PositionStatus", 0),
|
||||
(0, "GNSS_PositionStatus", 1),
|
||||
(0, "GNSS_PositionStatus", 2),
|
||||
(3, "GNSS_Empfaenger_Status", 0),
|
||||
(3, "GNSS_Genutzte_Satelliten", 0),
|
||||
(3, "GNSS_Genutzte_Satelliten", 3),
|
||||
))
|
||||
def test_invalid_solution_is_not_a_fix(message, signal, value):
|
||||
_, values = decode_frames()
|
||||
values[message][signal] = value
|
||||
gps = parse_volkswagen_taos_can_gps(*values)
|
||||
assert not gps["hasFix"]
|
||||
assert gps["speed"] == 0.0
|
||||
assert gps["vNED"] == [0.0, 0.0, 0.0]
|
||||
|
||||
|
||||
def test_no_constellation_or_null_position_is_not_a_fix():
|
||||
_, values = decode_frames()
|
||||
values[3]["GNSS_GPS_in_Nutzung"] = values[3]["GNSS_GLONASS_in_Nutzung"] = 0
|
||||
assert not parse_volkswagen_taos_can_gps(*values)["hasFix"]
|
||||
values[3]["GNSS_GPS_in_Nutzung"] = 1
|
||||
values[0]["GNSS_LatitudeMagnitude"] = values[0]["GNSS_LongitudeMagnitude"] = 0
|
||||
assert not parse_volkswagen_taos_can_gps(*values)["hasFix"]
|
||||
|
||||
|
||||
def test_initial_clock_and_mixed_epochs_are_rejected():
|
||||
_, values = decode_frames()
|
||||
values[3]["GNSS_UTC_Zeit"] = 0
|
||||
assert parse_volkswagen_taos_can_gps(*values) is None
|
||||
values[3]["GNSS_UTC_Zeit"] = 1_800_000_000
|
||||
values[1]["GNSS_Nachrichtenpaket_ID2"] = 1
|
||||
assert parse_volkswagen_taos_can_gps(*values) is None
|
||||
|
||||
|
||||
def test_invalid_motion_and_altitude_have_unknown_accuracy():
|
||||
_, values = decode_frames()
|
||||
values[1]["GNSS_Speed"] = 127.75
|
||||
values[1]["GNSS_Bearing"] = 409.5
|
||||
values[2]["GNSS_Ortung_Hoehe"] = 7690.0
|
||||
gps = parse_volkswagen_taos_can_gps(*values)
|
||||
assert gps["hasFix"]
|
||||
assert gps["speed"] == gps["bearingDeg"] == gps["altitude"] == 0.0
|
||||
assert gps["bearingAccuracyDeg"] == 180.0
|
||||
assert gps["speedAccuracy"] == 100.0
|
||||
assert gps["verticalAccuracy"] == 500.0
|
||||
assert gps["vNED"] == [0.0, 0.0, 0.0]
|
||||
|
||||
|
||||
def test_unknown_course_does_not_report_accurate_zero_velocity():
|
||||
_, values = decode_frames()
|
||||
values[1]["GNSS_Bearing"] = 409.5
|
||||
gps = parse_volkswagen_taos_can_gps(*values)
|
||||
assert gps["hasFix"]
|
||||
assert gps["speed"] == 12.5
|
||||
assert gps["speedAccuracy"] == 100.0
|
||||
assert gps["vNED"] == [0.0, 0.0, 0.0]
|
||||
|
||||
|
||||
def test_gps_is_only_enabled_for_the_taos():
|
||||
for model in (CAR.VOLKSWAGEN_TAOS_MK1, CAR.VOLKSWAGEN_GOLF_MK7, CAR.VOLKSWAGEN_ID4_MK1):
|
||||
cp = CarInterface.get_non_essential_params(model)
|
||||
state = CarState(cp, None)
|
||||
config = get_car_gps_config(cp)
|
||||
expected = model == CAR.VOLKSWAGEN_TAOS_MK1
|
||||
assert state.car_gps_supported is expected
|
||||
assert (config is not None) is expected
|
||||
if expected:
|
||||
assert config.messages == VOLKSWAGEN_TAOS_GPS_MESSAGES
|
||||
cp.brand = "mock"
|
||||
assert get_car_gps_config(cp) is None
|
||||
|
||||
|
||||
def test_carstate_update_reads_gps(gps_state):
|
||||
state, parsers = gps_state
|
||||
parsers[Bus.pt].update([(1_000_000_000, list(GPS_FRAMES))])
|
||||
state.update(parsers, None)
|
||||
gps = state.get_car_gps()
|
||||
assert gps["hasFix"]
|
||||
assert gps["timestamp_nanos"] == 1_000_000_000
|
||||
assert gps["longitude"] == pytest.approx(-110.0)
|
||||
|
||||
|
||||
def test_partial_startup_and_missing_gps_are_optional(gps_state):
|
||||
state, parsers = gps_state
|
||||
parser = parsers[Bus.pt]
|
||||
packer = CANPacker(DBC[state.CP.carFingerprint][Bus.pt])
|
||||
blink = packer.make_can_msg("Blinkmodi_02", parser.bus, {})
|
||||
for time in (1_000_000_000, 2_000_000_000):
|
||||
parser.update([(time, [blink])])
|
||||
state._update_car_gps(parser)
|
||||
assert state.get_car_gps() is None
|
||||
assert parser.can_valid
|
||||
parser.update([(3_000_000_000, list(GPS_FRAMES[:3]))])
|
||||
state._update_car_gps(parser)
|
||||
assert state.get_car_gps() is None
|
||||
assert all(parser.message_states[address].ignore_alive for address, _, _ in GPS_FRAMES)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("dropout", ("missing_position", "frozen_clock", "missing_all"))
|
||||
def test_gps_dropout_and_recovery(gps_state, dropout):
|
||||
state, parsers = gps_state
|
||||
parser = parsers[Bus.pt]
|
||||
parser.update([(1_000_000_000, list(GPS_FRAMES))])
|
||||
state._update_car_gps(parser)
|
||||
assert state.get_car_gps()["hasFix"]
|
||||
|
||||
frames = list(GPS_FRAMES)
|
||||
if dropout == "missing_position":
|
||||
frames = frames[1:]
|
||||
elif dropout == "missing_all":
|
||||
frames = []
|
||||
parser.update([(4_000_000_000, frames)])
|
||||
state._update_car_gps(parser)
|
||||
assert not state.get_car_gps()["hasFix"]
|
||||
assert state.get_car_gps()["timestamp_nanos"] == 4_000_000_000
|
||||
assert state.get_car_gps()["speed"] == 0.0
|
||||
|
||||
packer = CANPacker("vw_mqb")
|
||||
status = {**parser.vl["GNSS_05"], "GNSS_UTC_Zeit": 1_800_000_004}
|
||||
parser.update([(5_000_000_000, [*GPS_FRAMES[:3], packer.make_can_msg("GNSS_05", parser.bus, status)])])
|
||||
state._update_car_gps(parser)
|
||||
assert state.get_car_gps()["hasFix"]
|
||||
assert state.get_car_gps()["unixTimestampMillis"] == 1_800_000_004_000
|
||||
|
||||
|
||||
def test_invalid_receiver_status_clears_existing_fix(gps_state):
|
||||
state, parsers = gps_state
|
||||
parser = parsers[Bus.pt]
|
||||
parser.update([(1_000_000_000, list(GPS_FRAMES))])
|
||||
state._update_car_gps(parser)
|
||||
status = {**parser.vl["GNSS_05"], "GNSS_Empfaenger_Status": 0}
|
||||
packer = CANPacker("vw_mqb")
|
||||
parser.update([(2_000_000_000, [*GPS_FRAMES[:3], packer.make_can_msg("GNSS_05", parser.bus, status)])])
|
||||
state._update_car_gps(parser)
|
||||
assert not state.get_car_gps()["hasFix"]
|
||||
@@ -1610,6 +1610,36 @@ BO_ 391 Motor_EV_01: 8 Motor_MQB_BEV
|
||||
SG_ MO_HVEM_Eskalation : 54|1@1+ (1.0,0.0) [0.0|1] "" XXX
|
||||
SG_ MO_HVEM_MaxLeistung : 55|9@1+ (50,0) [0|25450] "Unit_Watt" XXX
|
||||
|
||||
BO_ 879 GNSS_01: 8 XXX
|
||||
SG_ GNSS_Nachrichtenpaket_ID1 : 0|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ GNSS_LatitudeMagnitude : 2|27@1+ (0.000001,0) [0|90] "deg" XXX
|
||||
SG_ GNSS_LatitudeSouth : 29|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ GNSS_LongitudeMagnitude : 30|28@1+ (0.000001,0) [0|180] "deg" XXX
|
||||
SG_ GNSS_LongitudeWest : 58|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ GNSS_PositionStatus : 59|2@1+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 884 GNSS_02: 8 XXX
|
||||
SG_ GNSS_Nachrichtenpaket_ID2 : 0|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ GNSS_Bearing : 12|12@1+ (0.1,0) [0|359.9] "deg" XXX
|
||||
SG_ GNSS_Speed : 24|9@1+ (0.25,0) [0|127.5] "m/s" XXX
|
||||
|
||||
BO_ 888 GNSS_04: 8 XXX
|
||||
SG_ GNSS_Nachrichtenpaket_ID4 : 0|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ GNSS_Ortung_Zeit_in_GPSWoche : 2|30@1+ (1,0) [0|604800001] "ms" XXX
|
||||
SG_ GNSS_Ortung_Hoehe : 32|12@1+ (2,-500) [-500|7686] "m" XXX
|
||||
|
||||
BO_ 891 GNSS_05: 8 XXX
|
||||
SG_ GNSS_UTC_Zeit : 0|32@1+ (1,0) [1|4294967295] "s" XXX
|
||||
SG_ GNSS_Empfaenger_Status : 32|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ GNSS_GPS_in_Nutzung : 33|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ GNSS_GLONASS_in_Nutzung : 34|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ GNSS_Empfangbare_Satelliten : 35|5@1+ (1,0) [0|31] "" XXX
|
||||
SG_ GNSS_Sichtbare_Satelliten : 40|5@1+ (1,0) [0|31] "" XXX
|
||||
SG_ GNSS_Genutzte_Satelliten : 45|5@1+ (1,0) [0|31] "" XXX
|
||||
SG_ GNSS_Nachrichtenpaket_ID5 : 50|2@1+ (1,0) [0|3] "" XXX
|
||||
|
||||
CM_ BO_ 879 "Taos coordinate layout decoded from CAN/GPS comparison. Only north/west coordinates and position status 3 have been validated.";
|
||||
CM_ BO_ 884 "Taos course and ground speed decoded from CAN/GPS comparison. Remaining fields are not decoded.";
|
||||
CM_ SG_ 134 LWI_Lenkradwinkel "Steering angle WITH variable ratio effect included";
|
||||
CM_ SG_ 159 EPS_HCA_Status "Status of Heading Control Assist feature";
|
||||
CM_ SG_ 159 EPS_Lenkmoment "Steering input by driver, torque";
|
||||
|
||||
@@ -366,7 +366,7 @@ GENESIS_G70_OUTPUT_SMOOTHING_CENTER_RC = 0.18
|
||||
GENESIS_G70_OUTPUT_SMOOTHING_CURVE_RC = 0.10
|
||||
GENESIS_G70_OUTPUT_SMOOTHING_RELEASE_RC = 0.03
|
||||
GENESIS_G70_OUTPUT_SMOOTHING_OVERSHOOT = 0.08
|
||||
GENESIS_G70_CENTER_MEASUREMENT_DAMPING_MAX = 0.06
|
||||
GENESIS_G70_CENTER_MEASUREMENT_DAMPING_MAX = 0.09
|
||||
GENESIS_G70_CENTER_MEASUREMENT_DAMPING_SPEED_BP = [50.0 * CV.MPH_TO_MS, 60.0 * CV.MPH_TO_MS]
|
||||
GENESIS_G70_CENTER_MEASUREMENT_DAMPING_LAT_BP = [0.15, 0.35]
|
||||
GENESIS_G70_CENTER_MEASUREMENT_DAMPING_JERK_BP = [0.20, 0.50]
|
||||
@@ -1336,6 +1336,11 @@ KONA_EV_2022_CENTER_FRICTION_THRESHOLD_LAT = 0.20
|
||||
KONA_EV_2022_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.05
|
||||
KONA_EV_2022_CENTER_FRICTION_THRESHOLD_SPEED = 18.0
|
||||
KONA_EV_2022_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 2.5
|
||||
KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_GAIN = 0.30
|
||||
KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_SPEED_ONSET = 100.0 / 3.6
|
||||
KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_SPEED_FULL = 120.0 / 3.6
|
||||
KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_LAT_FADE_START = 0.80
|
||||
KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_LAT_FADE_END = 1.50
|
||||
KONA_EV_2022_CENTER_OUTPUT_TAPER_MAX = 0.08
|
||||
KONA_EV_2022_CENTER_OUTPUT_TAPER_LAT = 0.20
|
||||
KONA_EV_2022_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.05
|
||||
@@ -2061,9 +2066,20 @@ def _kona_ev_2022_center_weights(desired_lateral_accel: float, v_ego: float) ->
|
||||
|
||||
def get_kona_ev_2022_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0) -> float:
|
||||
speed_weight, center_weight = _kona_ev_2022_center_weights(desired_lateral_accel, v_ego)
|
||||
return get_standard_friction_threshold(v_ego) * (
|
||||
base_threshold = get_standard_friction_threshold(v_ego) * (
|
||||
1.0 + KONA_EV_2022_CENTER_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight
|
||||
)
|
||||
high_speed_weight = float(np.interp(
|
||||
v_ego,
|
||||
[KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_SPEED_ONSET, KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_SPEED_FULL],
|
||||
[0.0, 1.0],
|
||||
))
|
||||
curve_weight = float(np.interp(
|
||||
abs(desired_lateral_accel),
|
||||
[KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_LAT_FADE_START, KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_LAT_FADE_END],
|
||||
[1.0, 0.0],
|
||||
))
|
||||
return base_threshold + KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_GAIN * high_speed_weight * curve_weight
|
||||
|
||||
|
||||
def get_kona_ev_2022_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
|
||||
|
||||
@@ -10,6 +10,7 @@ import openpilot.selfdrive.controls.lib.latcontrol_pid as latcontrol_pid
|
||||
import openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes as latcontrol_vehicle_tunes
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.interfaces import CarInterfaceBase
|
||||
from opendbc.car.lateral import get_friction
|
||||
from opendbc.car.chrysler.values import CAR as CHRYSLER
|
||||
from opendbc.car.honda.values import CAR as HONDA, HondaFlags
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
@@ -1043,14 +1044,14 @@ class TestLatControl:
|
||||
assert output == pytest.approx(-0.123)
|
||||
|
||||
@pytest.mark.parametrize("mph,desired,measured,jerk,expected", [
|
||||
(65.0, 0.0, 0.10, 0.0, 0.06),
|
||||
(65.0, 0.0, 0.10, 0.0, 0.09),
|
||||
(50.0, 0.0, 0.10, 0.0, 0.0),
|
||||
(55.0, 0.0, 0.10, 0.0, 0.03),
|
||||
(65.0, 0.25, 0.10, 0.0, 0.03),
|
||||
(65.0, 0.0, 0.25, 0.0, 0.03),
|
||||
(55.0, 0.0, 0.10, 0.0, 0.045),
|
||||
(65.0, 0.25, 0.10, 0.0, 0.045),
|
||||
(65.0, 0.0, 0.25, 0.0, 0.045),
|
||||
(65.0, 0.35, 0.10, 0.0, 0.0),
|
||||
(65.0, 0.0, 0.35, 0.0, 0.0),
|
||||
(65.0, 0.0, 0.10, 0.35, 0.03),
|
||||
(65.0, 0.0, 0.10, 0.35, 0.045),
|
||||
(65.0, 0.0, 0.10, 0.50, 0.0),
|
||||
])
|
||||
def test_genesis_g70_center_measurement_damping_gates(self, mph, desired, measured, jerk, expected):
|
||||
@@ -1069,7 +1070,7 @@ class TestLatControl:
|
||||
CS.steeringAngleDeg = -direction * 0.5
|
||||
output, _, moving_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles)
|
||||
assert moving_log.d * direction < 0.0
|
||||
assert abs(moving_log.d) <= 0.15
|
||||
assert abs(moving_log.d) <= 0.225
|
||||
assert abs(output) <= controller.steer_max
|
||||
|
||||
for _ in range(150):
|
||||
@@ -1516,6 +1517,83 @@ class TestLatControl:
|
||||
assert lac_log.active
|
||||
assert tapered_output == pytest.approx(base_output * 0.5)
|
||||
|
||||
@pytest.mark.parametrize("kph,desired,increment", [
|
||||
(0.0, 0.0, 0.0),
|
||||
(60.0, 0.0, 0.0),
|
||||
(100.0, 0.0, 0.0),
|
||||
(100.0, 0.65, 0.0),
|
||||
(110.0, 0.0, 0.15),
|
||||
(110.0, 0.65, 0.15),
|
||||
(120.0, 0.0, 0.30),
|
||||
(120.0, 0.65, 0.30),
|
||||
(120.0, -0.65, 0.30),
|
||||
(120.0, 1.15, 0.15),
|
||||
(120.0, -1.15, 0.15),
|
||||
(120.0, 1.5, 0.0),
|
||||
(140.0, 0.0, 0.30),
|
||||
(140.0, -2.0, 0.0),
|
||||
])
|
||||
def test_kona_ev_2022_high_speed_friction_ramp(self, monkeypatch, kph, desired, increment):
|
||||
threshold = get_kona_ev_2022_friction_threshold(kph / 3.6, desired)
|
||||
monkeypatch.setattr(latcontrol_vehicle_tunes, "KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_GAIN", 0.0)
|
||||
baseline = get_kona_ev_2022_friction_threshold(kph / 3.6, desired)
|
||||
|
||||
assert threshold == pytest.approx(baseline + increment)
|
||||
assert baseline <= threshold <= baseline + 0.30
|
||||
|
||||
def test_kona_ev_2022_high_speed_friction_preserves_large_error_authority(self):
|
||||
controller, _, _, _, _ = self._build_torque_controller(HYUNDAI.HYUNDAI_KONA_EV_2022)
|
||||
torque_params = controller.torque_params
|
||||
threshold = get_kona_ev_2022_friction_threshold(120.0 / 3.6, 0.65)
|
||||
base_threshold = get_standard_friction_threshold(120.0 / 3.6)
|
||||
|
||||
for direction in (-1.0, 1.0):
|
||||
small = get_friction(direction * 0.10, 0.0, threshold, torque_params)
|
||||
baseline_small = get_friction(direction * 0.10, 0.0, base_threshold, torque_params)
|
||||
assert abs(small) == pytest.approx(abs(baseline_small) * 0.5, abs=0.0001)
|
||||
assert small * direction > 0.0
|
||||
assert get_friction(direction * 1.0, 0.0, threshold, torque_params) == pytest.approx(
|
||||
get_friction(direction * 1.0, 0.0, base_threshold, torque_params),
|
||||
)
|
||||
|
||||
def test_kona_ev_2022_high_speed_friction_update_path(self, monkeypatch):
|
||||
controller, VM, CS, params, toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_KONA_EV_2022)
|
||||
CS.vEgo = 120.0 / 3.6
|
||||
CS.steeringAngleDeg = 0.0
|
||||
for _ in range(60):
|
||||
output, _, state = controller.update(True, CS, VM, params, False, 0.0001, False, 0.28, None, None, toggles)
|
||||
threshold = controller.starpilot_lateral_state.frictionThreshold
|
||||
|
||||
monkeypatch.setattr(latcontrol_vehicle_tunes, "KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_GAIN", 0.0)
|
||||
baseline, base_VM, base_CS, base_params, base_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_KONA_EV_2022)
|
||||
base_CS.vEgo = CS.vEgo
|
||||
base_CS.steeringAngleDeg = CS.steeringAngleDeg
|
||||
for _ in range(60):
|
||||
base_output, _, base_state = baseline.update(
|
||||
True, base_CS, base_VM, base_params, False, 0.0001, False, 0.28, None, None, base_toggles,
|
||||
)
|
||||
|
||||
assert state.active and base_state.active
|
||||
assert threshold - baseline.starpilot_lateral_state.frictionThreshold == pytest.approx(0.30)
|
||||
assert state.desiredLateralAccel == pytest.approx(base_state.desiredLateralAccel)
|
||||
assert state.p == pytest.approx(base_state.p)
|
||||
assert abs(state.f) < abs(base_state.f)
|
||||
assert abs(output) < abs(base_output)
|
||||
assert controller.steer_max == baseline.steer_max
|
||||
|
||||
@pytest.mark.parametrize("platform", [HYUNDAI.HYUNDAI_KONA_EV, HYUNDAI.HYUNDAI_KONA, HYUNDAI.HYUNDAI_IONIQ_6])
|
||||
def test_kona_ev_2022_high_speed_cleanup_does_not_apply_to_other_platforms(self, monkeypatch, platform):
|
||||
def unexpected_kona_threshold(*_args):
|
||||
raise AssertionError("Kona EV 2022 threshold called for another platform")
|
||||
|
||||
monkeypatch.setattr(latcontrol_torque, "get_kona_ev_2022_friction_threshold", unexpected_kona_threshold)
|
||||
controller, VM, CS, params, toggles = self._build_torque_controller(platform, force_torque=True)
|
||||
CS.vEgo = 120.0 / 3.6
|
||||
_, _, state = controller.update(True, CS, VM, params, False, 0.0001, False, 0.0, None, None, toggles)
|
||||
|
||||
assert state.active
|
||||
assert not controller.is_kona_ev_2022
|
||||
|
||||
def test_ioniq_5_center_taper_curve(self):
|
||||
assert get_ioniq_5_center_taper_scale(0.0, 25.0) < get_ioniq_5_center_taper_scale(0.0, 10.0)
|
||||
assert get_ioniq_5_center_taper_scale(0.0, 25.0) < get_ioniq_5_center_taper_scale(0.20, 25.0) <= 1.0
|
||||
|
||||
@@ -43,6 +43,8 @@ MACH_E_LOW_SPEED_TURN_IN_FADE_SPEED = 15.0
|
||||
MACH_E_TURN_IN_MIN_CURVATURE = 0.002
|
||||
MACH_E_TURN_IN_FULL_CURVATURE = 0.008
|
||||
MACH_E_TURN_IN_LAG_CURVATURE = 0.006
|
||||
MACH_E_TURN_IN_PREVIEW_HOLD_SECONDS = 0.10
|
||||
MACH_E_TURN_IN_PREVIEW_HOLD_MIN_LAG = 0.0005
|
||||
MACH_E_UNWIND_LOOKAHEAD_EXTRA = 0.80
|
||||
MACH_E_UNWIND_FULL_LAG_CURVATURE = 0.0005
|
||||
MACH_E_UNWIND_PREVIEW_LAG_CURVATURE = 0.002
|
||||
@@ -181,6 +183,7 @@ class FordLateralController:
|
||||
self.path_angle_last = 0.0
|
||||
self.path_angle_driver_cooldown = 0.0
|
||||
self.desired_curvature_last = 0.0
|
||||
self.turn_in_preview_hold_timer = 0.0
|
||||
self._frame = 0
|
||||
self._update_params()
|
||||
|
||||
@@ -356,12 +359,31 @@ class FordLateralController:
|
||||
lag_weight = max(lag_weight, preview_lag_weight)
|
||||
return predicted + speed_weight * curvature_weight * lag_weight * (preview - predicted)
|
||||
|
||||
def _turn_in_preview_weight(self, desired: float, preview: float, current: float) -> float:
|
||||
def _turn_in_preview_plateau(self, desired: float, current: float, v_ego: float,
|
||||
steering_pressed: bool, lane_change: bool) -> float:
|
||||
if (self.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or not self.CP.flags & FordFlags.CANFD or
|
||||
not MACH_E_LOW_SPEED_TURN_IN_START_SPEED <= v_ego < MACH_E_LOW_SPEED_TURN_IN_FADE_SPEED or
|
||||
steering_pressed or lane_change or desired * self.desired_curvature_last < 0.0 or
|
||||
abs(desired) < abs(self.desired_curvature_last)):
|
||||
self.turn_in_preview_hold_timer = 0.0
|
||||
return 0.0
|
||||
if abs(desired) > abs(self.desired_curvature_last):
|
||||
self.turn_in_preview_hold_timer = MACH_E_TURN_IN_PREVIEW_HOLD_SECONDS
|
||||
return 0.0
|
||||
self.turn_in_preview_hold_timer = max(0.0, self.turn_in_preview_hold_timer - STEER_DT)
|
||||
if (self.turn_in_preview_hold_timer <= 1e-9 or
|
||||
np.sign(desired) * (desired - current) <= MACH_E_TURN_IN_PREVIEW_HOLD_MIN_LAG):
|
||||
return 0.0
|
||||
return float(np.interp(v_ego, [2.0, 3.0, 14.0, 15.0], [0.0, 1.0, 1.0, 0.0]))
|
||||
|
||||
def _turn_in_preview_weight(self, desired: float, preview: float, current: float,
|
||||
allow_plateau: float = 0.0) -> float:
|
||||
if self.CP.carFingerprint not in FORD_CONSERVATIVE_PREVIEW_CARS:
|
||||
return 0.0
|
||||
if desired * preview <= 0.0 or desired * self.desired_curvature_last < 0.0:
|
||||
return 0.0
|
||||
if abs(desired) <= abs(self.desired_curvature_last):
|
||||
if (abs(desired) < abs(self.desired_curvature_last) or
|
||||
(abs(desired) == abs(self.desired_curvature_last) and not allow_plateau)):
|
||||
return 0.0
|
||||
|
||||
target = max(abs(desired), abs(preview))
|
||||
@@ -375,7 +397,8 @@ class FordLateralController:
|
||||
(target - direction * current) / MACH_E_TURN_IN_LAG_CURVATURE,
|
||||
0.0, 1.0,
|
||||
))
|
||||
return curvature_weight * lag_weight
|
||||
plateau_weight = allow_plateau if abs(desired) == abs(self.desired_curvature_last) else 1.0
|
||||
return curvature_weight * lag_weight * plateau_weight
|
||||
|
||||
@staticmethod
|
||||
def _turn_in_lookahead_extra(v_ego: float) -> float:
|
||||
@@ -552,6 +575,7 @@ class FordLateralController:
|
||||
self.path_angle_last = 0.0
|
||||
self.path_angle_driver_cooldown = 0.0
|
||||
self.desired_curvature_last = 0.0
|
||||
self.turn_in_preview_hold_timer = 0.0
|
||||
return FordLateralResult()
|
||||
|
||||
desired = float(actuators.curvature)
|
||||
@@ -565,6 +589,7 @@ class FordLateralController:
|
||||
self.curvature_last = 0.0
|
||||
self.path_angle_last = 0.0
|
||||
self.desired_curvature_last = 0.0
|
||||
self.turn_in_preview_hold_timer = 0.0
|
||||
return FordLateralResult(active=not (
|
||||
manual_turn and self.CP.carFingerprint in FORD_MANUAL_TURN_LATCH_CARS))
|
||||
|
||||
@@ -573,6 +598,8 @@ class FordLateralController:
|
||||
predicted = self._predicted_curvature(v_ego, lookahead)
|
||||
allow_opposite_preview = False
|
||||
if self.CP.carFingerprint in FORD_CONSERVATIVE_PREVIEW_CARS:
|
||||
turn_in_plateau = self._turn_in_preview_plateau(
|
||||
desired, current, v_ego, bool(CS.out.steeringPressed), self._lane_change()[0])
|
||||
turn_in_predicted = self._predicted_curvature(v_ego, lookahead + MACH_E_TURN_IN_LOOKAHEAD_EXTRA)
|
||||
direction_change_predicted = turn_in_predicted
|
||||
direction_change_weight = 0.0
|
||||
@@ -610,12 +637,12 @@ class FordLateralController:
|
||||
turn_in_lookahead_extra = self._turn_in_lookahead_extra(v_ego)
|
||||
if (turn_in_lookahead_extra > MACH_E_TURN_IN_LOOKAHEAD_EXTRA and
|
||||
desired * self.desired_curvature_last >= 0.0 and
|
||||
abs(desired) > abs(self.desired_curvature_last)):
|
||||
(abs(desired) > abs(self.desired_curvature_last) or turn_in_plateau)):
|
||||
low_speed_turn_in_predicted = self._predicted_curvature(v_ego, lookahead + turn_in_lookahead_extra)
|
||||
if (desired * low_speed_turn_in_predicted > 0.0 and
|
||||
abs(low_speed_turn_in_predicted) > abs(turn_in_predicted)):
|
||||
turn_in_predicted = low_speed_turn_in_predicted
|
||||
turn_in_weight = self._turn_in_preview_weight(desired, turn_in_predicted, current)
|
||||
turn_in_weight = self._turn_in_preview_weight(desired, turn_in_predicted, current, turn_in_plateau)
|
||||
if turn_in_weight > 0.0:
|
||||
turn_in_target = float(np.copysign(max(abs(desired), abs(turn_in_predicted)), desired))
|
||||
predicted = float(np.interp(turn_in_weight, [0.0, 1.0], [predicted, turn_in_target]))
|
||||
|
||||
@@ -105,6 +105,97 @@ def test_mach_e_unwind_lag_ramps_continuously(controller, monkeypatch):
|
||||
assert controller._unwind_preview(0.010, -0.009, 0.011, 10.0) == -0.009
|
||||
|
||||
|
||||
@pytest.mark.parametrize("sign", (-1, 1))
|
||||
def test_mach_e_turn_in_preview_holds_one_repeated_sample(controller, sign):
|
||||
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
controller.desired_curvature_last = sign * 0.003
|
||||
assert not controller._turn_in_preview_plateau(sign * 0.004, sign * 0.001, 6.0, False, False)
|
||||
controller.desired_curvature_last = sign * 0.004
|
||||
assert controller._turn_in_preview_plateau(sign * 0.004, sign * 0.001, 6.0, False, False)
|
||||
assert controller._turn_in_preview_weight(sign * 0.004, sign * 0.03, sign * 0.001, True) == 1.0
|
||||
assert not controller._turn_in_preview_plateau(sign * 0.004, sign * 0.001, 6.0, False, False)
|
||||
assert controller._turn_in_preview_weight(sign * 0.004, sign * 0.03, sign * 0.001) == 0.0
|
||||
|
||||
|
||||
@pytest.mark.parametrize("sign", (-1, 1))
|
||||
@pytest.mark.parametrize("current", (0.0036, 0.004, 0.005))
|
||||
def test_mach_e_turn_in_plateau_requires_tracking_lag(controller, sign, current):
|
||||
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
controller.desired_curvature_last = sign * 0.004
|
||||
controller.turn_in_preview_hold_timer = 0.1
|
||||
assert not controller._turn_in_preview_plateau(sign * 0.004, sign * current, 6.0, False, False)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("desired", (0.003, -0.004, 0.0))
|
||||
def test_mach_e_turn_in_plateau_resets_on_unwind_and_reversal(controller, desired):
|
||||
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
controller.desired_curvature_last = 0.004
|
||||
controller.turn_in_preview_hold_timer = 0.1
|
||||
assert not controller._turn_in_preview_plateau(desired, 0.001, 6.0, False, False)
|
||||
assert controller.turn_in_preview_hold_timer == 0.0
|
||||
|
||||
|
||||
@pytest.mark.parametrize("fingerprint,flags,speed,driver,lane_change", (
|
||||
(CAR.FORD_EDGE_MK2, FordFlags.CANFD, 6.0, False, False),
|
||||
(CAR.FORD_EXPLORER_MK6, FordFlags.CANFD, 6.0, False, False),
|
||||
(CAR.FORD_F_150_MK14, FordFlags.CANFD, 6.0, False, False),
|
||||
(CAR.FORD_MUSTANG_MACH_E_MK1, 0, 6.0, False, False),
|
||||
(CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, 1.99, False, False),
|
||||
(CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, 15.0, False, False),
|
||||
(CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, 6.0, True, False),
|
||||
(CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, 6.0, False, True),
|
||||
))
|
||||
def test_turn_in_plateau_preserves_other_fords_and_handoffs(controller, fingerprint, flags, speed, driver, lane_change):
|
||||
controller.CP.carFingerprint = fingerprint
|
||||
controller.CP.flags = flags
|
||||
controller.desired_curvature_last = 0.004
|
||||
controller.turn_in_preview_hold_timer = 0.1
|
||||
assert not controller._turn_in_preview_plateau(0.004, 0.001, speed, driver, lane_change)
|
||||
assert controller.turn_in_preview_hold_timer == 0.0
|
||||
|
||||
|
||||
@pytest.mark.parametrize("sign", (-1, 1))
|
||||
def test_mach_e_repeated_turn_in_sample_preserves_extended_preview(controller, monkeypatch, sign):
|
||||
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
controller.sm["liveDelay"].lateralDelay = 0.4
|
||||
controller.desired_curvature_last = sign * 0.003
|
||||
controller.curvature_last = sign * 0.012
|
||||
monkeypatch.setattr(controller, "_predicted_curvature",
|
||||
lambda _v, t: sign * (0.002 if t < 1.0 else 0.01 if t < 1.5 else 0.03))
|
||||
state = car_state(speed=6.0, curvature=sign * 0.001)
|
||||
cc = SimpleNamespace(latActive=True, enabled=False)
|
||||
actuators = SimpleNamespace(curvature=sign * 0.004)
|
||||
first = controller.update(cc, state, actuators)
|
||||
repeated = controller.update(cc, state, actuators)
|
||||
assert sign * repeated.curvature >= sign * first.curvature
|
||||
assert sign * repeated.curvature == pytest.approx(0.0144)
|
||||
expired = controller.update(cc, state, actuators)
|
||||
assert sign * expired.curvature < sign * repeated.curvature
|
||||
assert repeated.path_angle == 0.0
|
||||
controller.update(SimpleNamespace(latActive=False), state, actuators)
|
||||
assert controller.turn_in_preview_hold_timer == 0.0
|
||||
controller.update(cc, state, actuators)
|
||||
assert controller.turn_in_preview_hold_timer > 0.0
|
||||
controller.update(cc, car_state(speed=0.0), actuators)
|
||||
assert controller.turn_in_preview_hold_timer == 0.0
|
||||
|
||||
|
||||
@pytest.mark.parametrize("speed,weight", ((1.99, 0.0), (2.0, 0.0), (2.5, 0.5), (3.0, 1.0),
|
||||
(14.0, 1.0), (14.5, 0.5), (15.0, 0.0), (16.0, 0.0)))
|
||||
def test_mach_e_turn_in_plateau_speed_boundaries_are_continuous(controller, speed, weight):
|
||||
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
controller.desired_curvature_last = 0.004
|
||||
controller.turn_in_preview_hold_timer = 0.1
|
||||
actual_weight = controller._turn_in_preview_plateau(0.004, 0.001, speed, False, False)
|
||||
assert actual_weight == pytest.approx(weight)
|
||||
assert controller._turn_in_preview_weight(0.004, 0.03, 0.001, actual_weight) == pytest.approx(weight)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("speed,desired,requested,current,driver,lane_change,expected", (
|
||||
(12.0, 0.012, 0.012, 0.004, False, False, 0.006),
|
||||
(12.0, -0.012, -0.012, 0.004, False, False, 0.006),
|
||||
|
||||
@@ -41,11 +41,26 @@ def test_galaxy_does_not_assign_a_regional_label_to_ambiguous_ev6_fingerprint():
|
||||
|
||||
def test_galaxy_lists_2026_k4_under_existing_non_hda2_platform():
|
||||
kia_models = the_galaxy._extract_fingerprint_models_for_make("kia")
|
||||
assert {"value": "KIA_K4_2025", "label": "Kia K4 (without HDA II) 2025-26"} in kia_models
|
||||
assert {"value": "KIA_K4_2025", "label": "Kia K4 (without HDA II) 2025"} in kia_models
|
||||
assert {"value": "KIA_K4_2025", "label": "Kia K4 (without HDA II) 2026"} in kia_models
|
||||
assert {"value": "KIA_K4_2025", "label": "Kia K4 (with HDA II) 2025"} in kia_models
|
||||
assert {"value": "KIA_K4_2025", "label": "Kia K4 (with HDA II) 2025-26"} not in kia_models
|
||||
|
||||
|
||||
def test_manual_2026_k4_selection_keeps_the_shared_platform(monkeypatch):
|
||||
client, params = _params_client(monkeypatch, {}, "pc")
|
||||
monkeypatch.setattr(api_server, "_get_param_type_info", lambda: ({"CarModel"}, {"CarModel": str}))
|
||||
monkeypatch.setattr(api_server, "update_starpilot_toggles", lambda: None)
|
||||
|
||||
label = "Kia K4 (without HDA II) 2026"
|
||||
options = client.get("/api/fingerprints/models?make=Kia").get_json()
|
||||
assert {"value": "KIA_K4_2025", "label": label} in options
|
||||
response = client.put("/api/params", json={"key": "CarModel", "value": "KIA_K4_2025", "label": label})
|
||||
assert response.status_code == 200
|
||||
assert params.values["CarModel"] == "KIA_K4_2025"
|
||||
assert params.values["CarModelName"] == label
|
||||
|
||||
|
||||
def test_manual_fingerprint_api_keeps_the_saved_value_and_label_consistent(monkeypatch):
|
||||
client, params = _params_client(monkeypatch, {}, "pc")
|
||||
monkeypatch.setattr(api_server, "_get_param_type_info", lambda: ({"CarModel"}, {"CarModel": str}))
|
||||
|
||||
@@ -4,6 +4,8 @@ import pytest
|
||||
|
||||
from cereal import car
|
||||
from opendbc.car.ford.values import CAR as FORD_CAR
|
||||
from opendbc.car.volkswagen.values import CAR as VOLKSWAGEN_CAR
|
||||
from openpilot.common.gps import get_gps_location_service
|
||||
import openpilot.system.manager.process_config as process_config
|
||||
from openpilot.system.manager.process_config import (
|
||||
allow_uploads,
|
||||
@@ -174,3 +176,15 @@ def test_ublox_has_single_external_gps_publisher(monkeypatch, car_gps, expected)
|
||||
|
||||
assert ublox(True, params, car.CarParams.new_message(), SimpleNamespace()) is expected
|
||||
assert params.get_bool("CarGpsAvailable") is car_gps
|
||||
|
||||
|
||||
def test_taos_uses_the_car_gps_service(monkeypatch):
|
||||
monkeypatch.setattr("openpilot.system.manager.process_config.ublox_available", lambda: True)
|
||||
CP = car.CarParams.new_message()
|
||||
CP.brand = "volkswagen"
|
||||
CP.carFingerprint = VOLKSWAGEN_CAR.VOLKSWAGEN_TAOS_MK1
|
||||
params = GpsParams(CP)
|
||||
|
||||
assert not ublox(True, params, CP, SimpleNamespace())
|
||||
assert params.get_bool("CarGpsAvailable")
|
||||
assert get_gps_location_service(params) == "gpsLocationExternal"
|
||||
|
||||
Reference in New Issue
Block a user