GM/Bolt CAN GPS Accuracy

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:

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(

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],