diff --git a/opendbc_repo/opendbc/car/gm/carstate.py b/opendbc_repo/opendbc/car/gm/carstate.py index 707d7e9ac..9e0f3c122 100644 --- a/opendbc_repo/opendbc/car/gm/carstate.py +++ b/opendbc_repo/opendbc/car/gm/carstate.py @@ -1,4 +1,5 @@ import copy +import math from cereal import custom from opendbc.can import CANDefine, CANParser from opendbc.car import Bus, create_button_events, structs @@ -109,8 +110,11 @@ class CarState(CarStateBase): self.car_gps_supported = self.car_gps_config is not None self.car_gps = None self._car_gps_timestamp_nanos = 0 + self._prev_gps_lat = None + self._prev_gps_lon = None + self._last_gps_bearing = None - def _update_car_gps(self, cp) -> None: + def _update_car_gps(self, cp, v_ego: float = 0.0) -> None: if self.car_gps_config is None: return @@ -125,6 +129,25 @@ class CarState(CarStateBase): 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 + if gps["hasFix"]: + lat, lon = gps["latitude"], gps["longitude"] + if self._prev_gps_lat is not None and (lat, lon) != (self._prev_gps_lat, self._prev_gps_lon): + d_lat = (lat - self._prev_gps_lat) * 111139.0 + d_lon = (lon - self._prev_gps_lon) * 111139.0 * math.cos(math.radians(lat)) + if math.hypot(d_lat, d_lon) > 1.5 and v_ego > 1.0 and not self.moving_backward: + self._last_gps_bearing = math.degrees(math.atan2(d_lon, d_lat)) % 360.0 + + self._prev_gps_lat, self._prev_gps_lon = lat, lon + + bearing = self._last_gps_bearing if self._last_gps_bearing is not None else 0.0 + gps["speed"] = max(0.0, v_ego) + gps["bearingDeg"] = bearing + gps["bearingAccuracyDeg"] = 5.0 if (v_ego > 1.0 and self._last_gps_bearing is not None) else 180.0 + heading_rad = math.radians(bearing) + gps["vNED"] = [v_ego * math.cos(heading_rad), v_ego * math.sin(heading_rad), 0.0] + else: + self._prev_gps_lat = self._prev_gps_lon = None + self.car_gps = gps self._car_gps_timestamp_nanos = timestamp_nanos @@ -145,8 +168,6 @@ 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 = { @@ -217,6 +238,8 @@ class CarState(CarStateBase): ret.standstill = abs(pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"]) <= STANDSTILL_THRESHOLD and \ abs(pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"]) <= STANDSTILL_THRESHOLD + self._update_car_gps(pt_cp, ret.vEgo) + if pt_cp.vl["ECMPRDNL2"]["ManualMode"] == 1: ret.gearShifter = self.parse_gear_shifter("T") else: diff --git a/opendbc_repo/opendbc/car/gm/tests/test_gm.py b/opendbc_repo/opendbc/car/gm/tests/test_gm.py index 1d0040bde..ca34e609c 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_gm.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_gm.py @@ -88,6 +88,107 @@ class TestBoltGps: assert gps["latitude"] == 0.0 assert gps["longitude"] == 0.0 + def test_bolt_gps_accuracy_metrics(self): + gps = parse_chevrolet_bolt_can_gps({"GPSLatitude": 145292743.0, "GPSLongitude": -267520892.0}) + assert gps is not None + assert gps["horizontalAccuracy"] == 6.0 + assert gps["verticalAccuracy"] == 10.0 + assert gps["speedAccuracy"] == 0.5 + + def test_bolt_gps_heading_and_speed_derivation(self): + cp = SimpleNamespace( + brand="gm", + carFingerprint=CAR.CHEVROLET_BOLT_CC_2018_2021, + flags=0, + networkLocation=structs.CarParams.NetworkLocation.gateway, + transmissionType=structs.CarParams.TransmissionType.direct, + enableBsm=False, + enableGasInterceptorDEPRECATED=False, + pcmCruise=False, + ) + fpcp = custom.StarPilotCarParams.new_message() + cs = GMCarState(cp, fpcp) + + # First position (stationary) + mock_cp = SimpleNamespace( + ts_nanos={"TCICOnStarGPSPosition": {"GPSLatitude": 1_000_000}}, + vl={"TCICOnStarGPSPosition": {"GPSLatitude": 145292743.0, "GPSLongitude": -267520892.0}}, + ) + cs._update_car_gps(mock_cp, v_ego=0.0) + gps = cs.get_car_gps() + assert gps is not None + assert gps["speed"] == 0.0 + assert gps["bearingDeg"] == 0.0 + assert gps["bearingAccuracyDeg"] == 180.0 + + # Move East at 15 m/s + mock_cp.ts_nanos["TCICOnStarGPSPosition"]["GPSLatitude"] = 2_000_000_000 + mock_cp.vl["TCICOnStarGPSPosition"]["GPSLongitude"] = -267520892.0 + 1000.0 # Eastward shift + cs._update_car_gps(mock_cp, v_ego=15.0) + gps = cs.get_car_gps() + assert gps is not None + assert gps["speed"] == 15.0 + assert gps["bearingDeg"] == pytest.approx(90.0, abs=1.0) + assert gps["bearingAccuracyDeg"] == 5.0 + assert gps["vNED"][1] > 0.0 # East velocity positive + + # Stop moving (v_ego=0.0): heading should be retained, not reset to 0 + mock_cp.ts_nanos["TCICOnStarGPSPosition"]["GPSLatitude"] = 3_000_000_000 + cs._update_car_gps(mock_cp, v_ego=0.0) + gps = cs.get_car_gps() + assert gps is not None + assert gps["speed"] == 0.0 + assert gps["bearingDeg"] == pytest.approx(90.0, abs=1.0) + + # Reversing: coordinate changes while moving backward should not flip heading + cs.moving_backward = True + mock_cp.ts_nanos["TCICOnStarGPSPosition"]["GPSLatitude"] = 4_000_000_000 + mock_cp.vl["TCICOnStarGPSPosition"]["GPSLongitude"] = -267520892.0 - 1000.0 # Westward shift + cs._update_car_gps(mock_cp, v_ego=3.0) + gps = cs.get_car_gps() + assert gps is not None + assert gps["bearingDeg"] == pytest.approx(90.0, abs=1.0) + + # Drive True North: verify bearing is 0.0 deg and accuracy is 5.0 deg (not degraded to 180.0) + cs.moving_backward = False + mock_cp.ts_nanos["TCICOnStarGPSPosition"]["GPSLatitude"] = 5_000_000_000 + mock_cp.vl["TCICOnStarGPSPosition"]["GPSLatitude"] = 145292743.0 + 1000.0 # Northward shift + cs._update_car_gps(mock_cp, v_ego=12.0) + gps = cs.get_car_gps() + assert gps is not None + assert gps["bearingDeg"] == pytest.approx(0.0, abs=1.0) + assert gps["bearingAccuracyDeg"] == 5.0 + + # Tunnel / fix loss: invalid coordinates cause hasFix=False and clear previous coordinates + mock_cp.ts_nanos["TCICOnStarGPSPosition"]["GPSLatitude"] = 6_000_000_000 + mock_cp.vl["TCICOnStarGPSPosition"]["GPSLatitude"] = 0.0 + mock_cp.vl["TCICOnStarGPSPosition"]["GPSLongitude"] = 0.0 + cs._update_car_gps(mock_cp, v_ego=20.0) + gps = cs.get_car_gps() + assert gps is not None + assert not gps["hasFix"] + assert cs._prev_gps_lat is None and cs._prev_gps_lon is None + + # Tunnel exit: GPS fix re-acquired 5 km away heading South + # The first sample after fix loss sets initial coordinates without calculating a phantom jump vector + mock_cp.ts_nanos["TCICOnStarGPSPosition"]["GPSLatitude"] = 7_000_000_000 + mock_cp.vl["TCICOnStarGPSPosition"]["GPSLatitude"] = 145292743.0 - 50000.0 + mock_cp.vl["TCICOnStarGPSPosition"]["GPSLongitude"] = -267520892.0 + cs._update_car_gps(mock_cp, v_ego=20.0) + gps = cs.get_car_gps() + assert gps is not None + assert gps["hasFix"] + assert gps["bearingDeg"] == pytest.approx(0.0, abs=1.0) + assert cs._prev_gps_lat is not None + + # Second sample: moving Southward -> bearing smoothly updates to 180 deg + mock_cp.ts_nanos["TCICOnStarGPSPosition"]["GPSLatitude"] = 8_000_000_000 + mock_cp.vl["TCICOnStarGPSPosition"]["GPSLatitude"] = 145292743.0 - 51000.0 + cs._update_car_gps(mock_cp, v_ego=20.0) + gps = cs.get_car_gps() + assert gps is not None + assert gps["bearingDeg"] == pytest.approx(180.0, abs=1.0) + @parameterized.expand(CHEVROLET_BOLT_GPS_CARS) def test_gps_message_is_added_to_powertrain_parser(self, car_model): cp = SimpleNamespace( diff --git a/opendbc_repo/opendbc/car/gps.py b/opendbc_repo/opendbc/car/gps.py index 271e4cee1..750d9ebf8 100644 --- a/opendbc_repo/opendbc/car/gps.py +++ b/opendbc_repo/opendbc/car/gps.py @@ -113,11 +113,11 @@ def parse_chevrolet_bolt_can_gps(position: Mapping[str, float]) -> CarGpsSample "altitude": 0.0, "speed": 0.0, "bearingDeg": 0.0, - "horizontalAccuracy": 100.0, + "horizontalAccuracy": 6.0, "unixTimestampMillis": int(datetime.now(UTC).timestamp() * 1000), - "verticalAccuracy": 100.0, + "verticalAccuracy": 10.0, "bearingAccuracyDeg": 180.0, - "speedAccuracy": 100.0, + "speedAccuracy": 0.5, "hasFix": coordinates_valid, "satelliteCount": 0, "vNED": [0.0, 0.0, 0.0],