From 6de77049b3c3dc77c7275e9ff724a96f9e72a19d Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 6 Oct 2026 21:41:21 -0500 Subject: [PATCH] Potato Ole --- docs/CARS.md | 5 +- opendbc_repo/docs/CARS.md | 3 +- opendbc_repo/opendbc/car/gps.py | 60 ++++++ .../opendbc/car/hyundai/tests/test_hyundai.py | 31 +++ opendbc_repo/opendbc/car/hyundai/values.py | 3 +- .../opendbc/car/volkswagen/carstate.py | 31 +++ .../opendbc/car/volkswagen/tests/test_gps.py | 196 ++++++++++++++++++ opendbc_repo/opendbc/dbc/vw_mqb.dbc | 30 +++ .../controls/lib/latcontrol_vehicle_tunes.py | 20 +- selfdrive/controls/tests/test_latcontrol.py | 90 +++++++- starpilot/car/ford/lateral.py | 37 +++- starpilot/car/ford/tests/test_lateral.py | 91 ++++++++ .../tests/test_fingerprint_catalog.py | 17 +- system/manager/test/test_process_config.py | 14 ++ 14 files changed, 610 insertions(+), 18 deletions(-) create mode 100644 opendbc_repo/opendbc/car/volkswagen/tests/test_gps.py diff --git a/docs/CARS.md b/docs/CARS.md index 55801eaa27..2523d07af0 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -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|Hardware Needed
 |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[1](#footnotes)|6 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 Hyundai G connector
- 1 OBD-C cable (2 ft)
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
||| |Kia|Forte 2022-23|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai E connector
- 1 OBD-C cable (2 ft)
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
||| |Kia|K4 (with HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai R connector
- 1 OBD-C cable (2 ft)
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
||| -|Kia|K4 (without HDA II) 2025-26|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai A connector
- 1 OBD-C cable (2 ft)
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
||| +|Kia|K4 (without HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai A connector
- 1 OBD-C cable (2 ft)
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
||| +|Kia|K4 (without HDA II) 2026|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai A connector
- 1 OBD-C cable (2 ft)
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
||| |Kia|K5 2021-24|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai A connector
- 1 OBD-C cable (2 ft)
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
||| |Kia|K5 (without HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai M connector
- 1 OBD-C cable (2 ft)
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
||| |Kia|K5 Hybrid 2020-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai A connector
- 1 OBD-C cable (2 ft)
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
||| diff --git a/opendbc_repo/docs/CARS.md b/opendbc_repo/docs/CARS.md index 3eb7d0fd05..9be564215f 100644 --- a/opendbc_repo/docs/CARS.md +++ b/opendbc_repo/docs/CARS.md @@ -1,6 +1,6 @@ -# 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)| diff --git a/opendbc_repo/opendbc/car/gps.py b/opendbc_repo/opendbc/car/gps.py index 750d9ebf8a..33a046c2b7 100644 --- a/opendbc_repo/opendbc/car/gps.py +++ b/opendbc_repo/opendbc/car/gps.py @@ -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, diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index eec601dfd9..fd8f8236be 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -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', diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index fe4d7dcabb..78e8d2f99a 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -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), diff --git a/opendbc_repo/opendbc/car/volkswagen/carstate.py b/opendbc_repo/opendbc/car/volkswagen/carstate.py index 55e47859b3..9129d48833 100644 --- a/opendbc_repo/opendbc/car/volkswagen/carstate.py +++ b/opendbc_repo/opendbc/car/volkswagen/carstate.py @@ -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 += [ diff --git a/opendbc_repo/opendbc/car/volkswagen/tests/test_gps.py b/opendbc_repo/opendbc/car/volkswagen/tests/test_gps.py new file mode 100644 index 0000000000..796c4e31a8 --- /dev/null +++ b/opendbc_repo/opendbc/car/volkswagen/tests/test_gps.py @@ -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"] diff --git a/opendbc_repo/opendbc/dbc/vw_mqb.dbc b/opendbc_repo/opendbc/dbc/vw_mqb.dbc index d1ecb0041a..f5cda04a9a 100644 --- a/opendbc_repo/opendbc/dbc/vw_mqb.dbc +++ b/opendbc_repo/opendbc/dbc/vw_mqb.dbc @@ -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"; diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index ac3711fc5b..c5ff4ef391 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -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: diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index e01ced1c47..9a578222c9 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -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 diff --git a/starpilot/car/ford/lateral.py b/starpilot/car/ford/lateral.py index 70611ed9b0..5215e1abf4 100644 --- a/starpilot/car/ford/lateral.py +++ b/starpilot/car/ford/lateral.py @@ -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])) diff --git a/starpilot/car/ford/tests/test_lateral.py b/starpilot/car/ford/tests/test_lateral.py index b70215195d..13f54812d3 100644 --- a/starpilot/car/ford/tests/test_lateral.py +++ b/starpilot/car/ford/tests/test_lateral.py @@ -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), diff --git a/starpilot/system/the_galaxy/tests/test_fingerprint_catalog.py b/starpilot/system/the_galaxy/tests/test_fingerprint_catalog.py index 8fbb718cdb..28f74972f7 100644 --- a/starpilot/system/the_galaxy/tests/test_fingerprint_catalog.py +++ b/starpilot/system/the_galaxy/tests/test_fingerprint_catalog.py @@ -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})) diff --git a/system/manager/test/test_process_config.py b/system/manager/test/test_process_config.py index 2088385bd3..6251c67ebd 100644 --- a/system/manager/test/test_process_config.py +++ b/system/manager/test/test_process_config.py @@ -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"