Refactor GPS data handling to use 'liveLocationKalman'

CHANGELOGS.md

@@ -48,6 +48,10 @@ sunnypilot - 0.9.5.1 (2023-10-xx)
 * FIXED: Retain hotspot/tethering state was not consistently saved
 * FIXED: Map stuck in "Map Loading" if comma Prime is active
 * FIXED: UI elements adjustments
+* FIXED: OpenStreetMap implementation on C3X devices
+  * M-TSC
+  * Altitude (ALT.) display on Developer UI
+  * Current street name on top of driving screen when "OSM Debug UI" is enabled
 * Kia Seltos Non-SCC 2023-24 support thanks to Moodkiller and jeroid_!
 
 sunnypilot - 0.9.4.1 (2023-08-11)

selfdrive/mapd/mapd.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 import json
+import math
 import threading
 from traceback import print_exception
 import numpy as np
@@ -36,6 +37,16 @@ def excepthook(args):
 
 threading.excepthook = excepthook
 
+class GPSData:
+  def __init__(self, unixTimestampMillis, latitude, longitude, speed, bearingDeg, accuracy, bearingAccuracyDeg):
+    self.unixTimestampMillis = unixTimestampMillis
+    self.latitude = latitude
+    self.longitude = longitude
+    self.speed = speed
+    self.bearingDeg = bearingDeg
+    self.accuracy = accuracy
+    self.bearingAccuracyDeg = bearingAccuracyDeg
+
 
 class MapD():
   def __init__(self):
@@ -80,27 +91,37 @@ class MapD():
     self._op_enabled = hud_control.speedVisible
 
   def update_gps(self, sm):
-    sock = 'gpsLocationExternal'
+    sock = 'liveLocationKalman'
     if not sm.updated[sock] or not sm.valid[sock]:
       return
 
     log = sm[sock]
-    self.last_gps = log
+
 
     # ignore the message if the fix is invalid
-    if log.flags % 2 == 0:
+    if not log.positionGeodetic.valid:
       return
 
     self.last_gps_fix_timestamp = log.unixTimestampMillis  # Unix TS. Milliseconds since January 1, 1970.
-    self.location_rad = np.radians(np.array([log.latitude, log.longitude], dtype=float))
-    self.location_deg = (log.latitude, log.longitude)
-    self.bearing_rad = np.radians(log.bearingDeg, dtype=float)
-    self.gps_speed = log.speed
-    self.location_stdev = log.accuracy  # log accuracies are presumably 1 standard deviation.
+    self.location_rad = np.radians(np.array([log.positionGeodetic.value[0], log.positionGeodetic.value[1]], dtype=float))
+    self.location_deg = (log.positionGeodetic.value[0], log.positionGeodetic.value[1])
+    self.bearing_rad = math.degrees(log.calibratedOrientationNED.value[2])
+    self.gps_speed = log.velocityCalibrated.value[2]
+    self.location_stdev = 1  # hardcoding, this is not available in liveLocationKalman
+
+    self.last_gps = GPSData(
+      unixTimestampMillis=self.last_gps_fix_timestamp,
+      latitude=self.location_deg[0],
+      longitude=self.location_deg[1],
+      speed=self.gps_speed,
+      bearingDeg=self.bearing_rad,
+      accuracy=self.location_stdev,  # assuming this is equivalent information
+      bearingAccuracyDeg=1.  # you'll need to assign this if available
+    )
 
     _debug('Mapd: ********* Got GPS fix'
            + f'Pos: {self.location_deg} +/- {self.location_stdev * 2.} mts.\n'
-           + f'Bearing: {log.bearingDeg} +/- {log.bearingAccuracyDeg * 2.} deg.\n'
+           + f'Bearing: {log.calibratedOrientationNED.value[2]} +/- {1. * 2.} deg.\n'
            + f'timestamp: {strftime("%d-%m-%y %H:%M:%S", gmtime(self.last_gps_fix_timestamp * 1e-3))}'
            + '*******', log_to_cloud=False)
 
@@ -221,8 +242,8 @@ class MapD():
     current_road_name = self.route.current_road_name
 
     map_data_msg = messaging.new_message('liveMapDataSP')
-    map_data_msg.valid = sm.all_alive(service_list=['gpsLocationExternal']) and \
-                         sm.all_valid(service_list=['gpsLocationExternal'])
+    map_data_msg.valid = sm.all_alive(service_list=['liveLocationKalman']) and \
+                         sm.all_valid(service_list=['liveLocationKalman'])
 
     liveMapDataSP = map_data_msg.liveMapDataSP
     liveMapDataSP.lastGpsTimestamp = self.last_gps.unixTimestampMillis
@@ -271,7 +292,7 @@ def mapd_thread(sm=None, pm=None):
 
   # *** setup messaging
   if sm is None:
-    sm = messaging.SubMaster(['gpsLocationExternal', 'carControl'])
+    sm = messaging.SubMaster(['liveLocationKalman', 'carControl'])
   if pm is None:
     pm = messaging.PubMaster(['liveMapDataSP'])
 

selfdrive/ui/qt/onroad.cc

@@ -522,7 +522,6 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
   const auto nav_instruction = sm["navInstruction"].getNavInstruction();
   const auto car_control = sm["carControl"].getCarControl();
   const auto radar_state = sm["radarState"].getRadarState();
-  const auto gpsLocationExternal = sm["gpsLocationExternal"].getGpsLocationExternal();
   const auto ltp = sm["liveTorqueParameters"].getLiveTorqueParameters();
   const auto lateral_plan_sp = sm["lateralPlanSP"].getLateralPlanSP();
 
@@ -572,6 +571,29 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
   splitPanelVisible = s.scene.map_visible || s.scene.onroad_settings_visible;
 
   // ############################## DEV UI START ##############################
+
+  if (sm.updated("liveLocationKalman")) {
+      auto locationd_location = sm["liveLocationKalman"].getLiveLocationKalman();
+      auto locationd_pos = locationd_location.getPositionGeodetic();
+      auto locationd_orientation = locationd_location.getCalibratedOrientationNED();
+      auto locationd_velocity = locationd_location.getVelocityCalibrated();
+
+      // Check std norm
+      auto pos_ecef_std = locationd_location.getPositionECEF().getStd();
+      bool pos_accurate_enough = sqrt(pow(pos_ecef_std[0], 2) + pow(pos_ecef_std[1], 2) + pow(pos_ecef_std[2], 2)) < 100;
+
+      auto locationd_valid = (locationd_pos.getValid() && locationd_orientation.getValid() && locationd_velocity.getValid() && pos_accurate_enough);
+
+      if (locationd_valid) {
+        gpsAccuracy = 1.0; //Hardcoding to 1 because not available in liveLocationKalman
+        altitude = locationd_pos.getValue()[2];
+
+        bearingAccuracyDeg = 1.0; //Hardcoding to 1 because not available in liveLocationKalman
+        bearingDeg = locationd_orientation.getValue()[2];
+
+      }
+    }
+
   lead_d_rel = radar_state.getLeadOne().getDRel();
   lead_v_rel = radar_state.getLeadOne().getVRel();
   lead_status = radar_state.getLeadOne().getStatus();
@@ -583,13 +605,9 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
   memoryUsagePercent = sm["deviceState"].getDeviceState().getMemoryUsagePercent();
   devUiEnabled = s.scene.dev_ui_enabled;
   devUiInfo = s.scene.dev_ui_info;
-  gpsAccuracy = gpsLocationExternal.getAccuracy();
-  altitude = gpsLocationExternal.getAltitude();
   vEgo = car_state.getVEgo();
   aEgo = car_state.getAEgo();
   steeringTorqueEps = car_state.getSteeringTorqueEps();
-  bearingAccuracyDeg = gpsLocationExternal.getBearingAccuracyDeg();
-  bearingDeg = gpsLocationExternal.getBearingDeg();
   torquedUseParams = (ltp.getUseParams() || s.scene.live_torque_toggle) && !s.scene.torqued_override;
   latAccelFactorFiltered = ltp.getLatAccelFactorFiltered();
   frictionCoefficientFiltered = ltp.getFrictionCoefficientFiltered();