From 7d39054b6c2dd3b308878defdcfce7216d2ac10c Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sat, 29 Aug 2026 14:17:05 -0500 Subject: [PATCH] gps --- opendbc_repo/opendbc/car/gm/carstate.py | 32 ++++++++++++ opendbc_repo/opendbc/car/gm/tests/test_gm.py | 38 ++++++++++++++ opendbc_repo/opendbc/car/gps.py | 51 +++++++++++++++++++ .../opendbc/car/subaru/carcontroller.py | 4 +- .../opendbc/car/subaru/tests/test_subaru.py | 10 ++-- .../lib/longitudinal_vehicle_tunes.py | 6 +-- .../tests/test_longitudinal_planner.py | 4 +- .../controls/tests/test_starpilot_planner.py | 2 +- 8 files changed, 135 insertions(+), 12 deletions(-) diff --git a/opendbc_repo/opendbc/car/gm/carstate.py b/opendbc_repo/opendbc/car/gm/carstate.py index 699cad02b..707d7e9ac 100644 --- a/opendbc_repo/opendbc/car/gm/carstate.py +++ b/opendbc_repo/opendbc/car/gm/carstate.py @@ -4,6 +4,7 @@ from opendbc.can import CANDefine, CANParser from opendbc.car import Bus, create_button_events, structs from opendbc.car import DT_CTRL from opendbc.car.common.conversions import Conversions as CV +from opendbc.car.gps import get_car_gps_config from opendbc.car.interfaces import CarStateBase from opendbc.car.gm.values import ( ALT_ACCS, @@ -104,6 +105,31 @@ class CarState(CarStateBase): self.lkas_enabled = 0 self.pcm_acc_status = AccState.OFF self.stock_fcw_alert = 0 + self.car_gps_config = get_car_gps_config(CP) + self.car_gps_supported = self.car_gps_config is not None + self.car_gps = None + self._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 not all(timestamps) or max(timestamps) - min(timestamps) > 2_000_000_000: + return + + timestamp_nanos = max(timestamps) + if timestamp_nanos <= self._car_gps_timestamp_nanos: + return + + gps = self.car_gps_config.decoder(*(cp.vl[name] for name in self.car_gps_config.messages)) + if gps is not None: + gps["timestamp_nanos"] = timestamp_nanos + self.car_gps = gps + self._car_gps_timestamp_nanos = timestamp_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: @@ -119,6 +145,8 @@ class CarState(CarStateBase): cam_cp = can_parsers[Bus.cam] loopback_cp = can_parsers[Bus.loopback] + self._update_car_gps(pt_cp) + ret = structs.CarState() volt_like = { @@ -428,6 +456,9 @@ class CarState(CarStateBase): @staticmethod def get_can_parsers(CP): + gps_config = get_car_gps_config(CP) + gps_messages = [(name, 0) for name in gps_config.messages] if gps_config is not None else [] + volt_like = { CAR.CHEVROLET_VOLT, CAR.CHEVROLET_VOLT_2019, @@ -451,6 +482,7 @@ class CarState(CarStateBase): ("PSCMSteeringAngle", 100), ("ECMAcceleratorPos", 80), ("SportMode", 0), + *gps_messages, ] prndl2_rate = 10 if CP.carFingerprint in kaofui_state_cars else 40 diff --git a/opendbc_repo/opendbc/car/gm/tests/test_gm.py b/opendbc_repo/opendbc/car/gm/tests/test_gm.py index cc34107fa..8c8a85c3e 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_gm.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_gm.py @@ -20,6 +20,7 @@ from opendbc.car.gm.carcontroller import ( ) import opendbc.car.gm.interface as gm_interface from opendbc.car.common.conversions import Conversions as CV +from opendbc.car.gps import CHEVROLET_BOLT_GPS_CARS, CHEVROLET_BOLT_GPS_MESSAGES, get_car_gps_config, parse_chevrolet_bolt_can_gps from opendbc.car.gm.fingerprints import FINGERPRINTS from opendbc.car.gm.values import ASCM_INT, CAMERA_ACC_CAR, CAR, CC_ONLY_CAR, DBC, GM_RX_OFFSET, CarControllerParams, CruiseButtons, GMFlags, GMSafetyFlags from opendbc.safety import ALTERNATIVE_EXPERIENCE @@ -65,6 +66,43 @@ class TestGMFingerprint: assert finger.get(required_addr) == 8, required_addr +class TestBoltGps: + @parameterized.expand(CHEVROLET_BOLT_GPS_CARS) + def test_all_bolt_generations_are_registered(self, car_model): + config = get_car_gps_config(SimpleNamespace(carFingerprint=car_model, brand="gm")) + assert config is not None + assert config.messages == CHEVROLET_BOLT_GPS_MESSAGES + gps = parse_chevrolet_bolt_can_gps({ + "GPSLatitude": 145292743.0, + "GPSLongitude": -267520892.0, + }) + assert gps is not None + assert gps["hasFix"] + assert gps["latitude"] == pytest.approx(40.3590953) + assert gps["longitude"] == pytest.approx(-74.3113589) + + def test_invalid_bolt_position_does_not_become_a_fix(self): + gps = parse_chevrolet_bolt_can_gps({"GPSLatitude": 0.0, "GPSLongitude": -2147483648.0}) + assert gps is not None + assert not gps["hasFix"] + assert gps["latitude"] == 0.0 + assert gps["longitude"] == 0.0 + + @parameterized.expand(CHEVROLET_BOLT_GPS_CARS) + def test_gps_message_is_added_to_powertrain_parser(self, car_model): + cp = SimpleNamespace( + brand="gm", + carFingerprint=car_model, + flags=0, + networkLocation=structs.CarParams.NetworkLocation.gateway, + transmissionType=structs.CarParams.TransmissionType.direct, + enableBsm=False, + enableGasInterceptorDEPRECATED=False, + ) + parsers = GMCarState.get_can_parsers(cp) + assert all(message in parsers[Bus.pt].vl for message in CHEVROLET_BOLT_GPS_MESSAGES) + + class TestGMInterface: @parameterized.expand([ CAR.CHEVROLET_BOLT_CC_2017, diff --git a/opendbc_repo/opendbc/car/gps.py b/opendbc_repo/opendbc/car/gps.py index 44f5fd155..271e4cee1 100644 --- a/opendbc_repo/opendbc/car/gps.py +++ b/opendbc_repo/opendbc/car/gps.py @@ -7,6 +7,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 CarGpsSample = dict[str, Any] @@ -90,11 +91,53 @@ def parse_ford_can_gps(nav1: Mapping[str, float], nav2: Mapping[str, float], nav } +def parse_chevrolet_bolt_can_gps(position: Mapping[str, float]) -> CarGpsSample | None: + """Decode the Bolt's OnStar GPS position message.""" + try: + latitude = float(position["GPSLatitude"]) / 3_600_000.0 + longitude = float(position["GPSLongitude"]) / 3_600_000.0 + except (KeyError, TypeError, ValueError): + return None + + coordinates_valid = ( + math.isfinite(latitude) and math.isfinite(longitude) and + -90.0 <= latitude <= 90.0 and -180.0 <= longitude <= 180.0 and + (latitude != 0.0 or longitude != 0.0) + ) + if not coordinates_valid: + latitude = longitude = 0.0 + + return { + "latitude": latitude, + "longitude": longitude, + "altitude": 0.0, + "speed": 0.0, + "bearingDeg": 0.0, + "horizontalAccuracy": 100.0, + "unixTimestampMillis": int(datetime.now(UTC).timestamp() * 1000), + "verticalAccuracy": 100.0, + "bearingAccuracyDeg": 180.0, + "speedAccuracy": 100.0, + "hasFix": coordinates_valid, + "satelliteCount": 0, + "vNED": [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",) + +CHEVROLET_BOLT_GPS_CARS = ( + GM_CAR.CHEVROLET_BOLT_ACC_2022_2023, + GM_CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL, + GM_CAR.CHEVROLET_BOLT_CC_2022_2023, + GM_CAR.CHEVROLET_BOLT_CC_2018_2021, + GM_CAR.CHEVROLET_BOLT_CC_2017, +) CAR_GPS_CONFIGS: dict[str, CarGpsConfig] = { @@ -103,6 +146,14 @@ CAR_GPS_CONFIGS: dict[str, CarGpsConfig] = { messages=FORD_MACH_E_GPS_MESSAGES, decoder=parse_ford_can_gps, ), + **{ + car: CarGpsConfig( + brand="gm", + messages=CHEVROLET_BOLT_GPS_MESSAGES, + decoder=parse_chevrolet_bolt_can_gps, + ) + for car in CHEVROLET_BOLT_GPS_CARS + }, } diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py index c8f1cc264..615b4b1c3 100644 --- a/opendbc_repo/opendbc/car/subaru/carcontroller.py +++ b/opendbc_repo/opendbc/car/subaru/carcontroller.py @@ -32,7 +32,9 @@ _ANGLE_RECLAIM_EXPONENT = 2.5 _ANGLE_MADS_MIN_SPEED = 0.44704 _ANGLE_MADS_MAX_STEER_ANGLE = 120.0 _STOP_START_STARTUP_DELAY_FRAMES = 100 -_STOP_START_STARTUP_DEADLINE_FRAMES = 300 +# StarPilot's first populated toggle message can arrive several seconds after +# the car controller starts while fingerprinting and settings settle. +_STOP_START_STARTUP_DEADLINE_FRAMES = 1000 _STOP_START_PULSE_FRAMES = 30 _STOP_START_PULSE_PERIOD_FRAMES = 5 diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py index bb284144f..585a14c0f 100644 --- a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py +++ b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py @@ -221,14 +221,14 @@ def test_stop_start_inputs_are_captured_for_supported_models(platform): assert car_state.stop_start_state == 3 -@pytest.mark.parametrize("platform, expected_bus", [ - (CAR.SUBARU_OUTBACK_2023, CanBus.alt), - (CAR.SUBARU_LEGACY_2025, CanBus.main), +@pytest.mark.parametrize("platform, expected_bus, start_frame", [ + (CAR.SUBARU_OUTBACK_2023, CanBus.alt, 101), + (CAR.SUBARU_LEGACY_2025, CanBus.main, 401), ]) -def test_stop_start_request_is_bounded_and_uses_live_dashlights(platform, expected_bus): +def test_stop_start_request_is_bounded_and_uses_live_dashlights(platform, expected_bus, start_frame): CP = CarInterface.get_non_essential_params(platform) controller = CarController({}, CP) - controller.frame = 101 + controller.frame = start_frame class TestActuators: steeringAngleDeg = 0.0 diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index 2d3fd418f..746b8b146 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py @@ -21,7 +21,7 @@ HONDA_ACCORD_STOP_GO_MIN_MODEL_PROB = 0.95 HONDA_ACCORD_STOP_GO_ACCEL_RISE_RATE = 4.0 HYUNDAI_ELANTRA_LEAD_FOLLOW_JERK_SCALE = 1.25 GENESIS_GV70_ELECTRIFIED_LEAD_FOLLOW_JERK_SCALE = 1.35 -FORD_LIGHTNING_LEAD_FOLLOW_JERK_SCALE = 1.20 +FORD_LIGHTNING_LEAD_FOLLOW_JERK_SCALE = 1.35 HONDA_CRV_5G_LEAD_FOLLOW_JERK_SCALE = 1.20 GM_SILVERADO_EARLY_FOLLOW_MIN_EGO_SPEED = 18.0 GM_SILVERADO_EARLY_FOLLOW_MAX_DISTANCE = 130.0 @@ -29,10 +29,10 @@ GM_SILVERADO_EARLY_FOLLOW_MIN_MODEL_PROB = 0.85 GM_SILVERADO_EARLY_FOLLOW_MAX_LATERAL_OFFSET = 1.2 DEFAULT_FOLLOW_PREBRAKE_MIN_HEADWAY = 1.25 GM_SILVERADO_FOLLOW_PREBRAKE_MIN_HEADWAY = 1.25 -FORD_LIGHTNING_FOLLOW_PREBRAKE_MIN_HEADWAY = 1.0 +FORD_LIGHTNING_FOLLOW_PREBRAKE_MIN_HEADWAY = 0.75 FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_MIN_HEADWAY_MARGIN = 0.10 FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_FULL_HEADWAY_MARGIN = 0.25 -FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_BIAS_GAIN = 0.65 +FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_BIAS_GAIN = 1.0 HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_MIN_HEADWAY_MARGIN = 0.10 HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_FULL_HEADWAY_MARGIN = 0.35 HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_BIAS_GAIN = 1.25 diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index c78077062..b5da9e32a 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -787,7 +787,7 @@ def test_prebrake_floor_is_vehicle_specific(): honda = SimpleNamespace(brand="honda", carFingerprint=CAR.HONDA_CIVIC) assert get_follow_prebrake_min_headway(silverado, 1.0) == pytest.approx(1.25) - assert get_follow_prebrake_min_headway(lightning, 0.75) == pytest.approx(1.0) + assert get_follow_prebrake_min_headway(lightning, 0.75) == pytest.approx(0.75) assert get_follow_prebrake_min_headway(honda, 1.0) == pytest.approx(1.25) @@ -801,7 +801,7 @@ def test_lightning_stopped_lead_guard_tune_is_vehicle_specific(): assert get_standstill_stopped_lead_guard_max_lead_speed(civic, 0.45) == pytest.approx(0.45) assert get_standstill_gap_settle_max_extra_gap(lightning) == pytest.approx(3.0) assert get_tracked_lead_catchup_headway_margins(lightning) == pytest.approx((0.10, 0.25)) - assert get_tracked_lead_catchup_bias_gain(lightning) == pytest.approx(0.65) + assert get_tracked_lead_catchup_bias_gain(lightning) == pytest.approx(1.0) assert get_tracked_lead_catchup_headway_margins(civic) is None assert get_tracked_lead_catchup_bias_gain(civic) is None diff --git a/selfdrive/controls/tests/test_starpilot_planner.py b/selfdrive/controls/tests/test_starpilot_planner.py index 8ebe49422..53d8c28ca 100644 --- a/selfdrive/controls/tests/test_starpilot_planner.py +++ b/selfdrive/controls/tests/test_starpilot_planner.py @@ -43,7 +43,7 @@ def test_force_stop_jerk_scale_is_platform_specific(): def test_lead_follow_jerk_scale_is_platform_specific(): assert get_lead_follow_jerk_scale(SimpleNamespace(brand="hyundai", carFingerprint="HYUNDAI_ELANTRA_2021")) == 1.25 assert get_lead_follow_jerk_scale(SimpleNamespace(brand="hyundai", carFingerprint="GENESIS_GV70_ELECTRIFIED_1ST_GEN")) == 1.35 - assert get_lead_follow_jerk_scale(SimpleNamespace(brand="ford", carFingerprint="FORD_F_150_LIGHTNING_MK1")) == 1.20 + assert get_lead_follow_jerk_scale(SimpleNamespace(brand="ford", carFingerprint="FORD_F_150_LIGHTNING_MK1")) == 1.35 assert get_lead_follow_jerk_scale(SimpleNamespace(brand="honda", carFingerprint="HONDA_CRV_5G")) == 1.20 assert get_lead_follow_jerk_scale(SimpleNamespace(brand="other", carFingerprint="OTHER_CAR")) == 1.0