add hasFix field to gpsLocation (#31778)

* add hasFix field to gpsLocation

* migration

* update refs for ubloxd

* cereal master

cereal

@@ -1 +1 @@
-Subproject commit c5c2a60f1aa796e7de464015349db3c336b79220
+Subproject commit cf7bb3e74974879abef94286fab4d39398fe402b

selfdrive/locationd/locationd.cc

@@ -308,14 +308,12 @@ void Localizer::input_fake_gps_observations(double current_time) {
 }
 
 void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) {
-  // ignore the message if the fix is invalid
-  bool gps_invalid_flag = (log.getFlags() % 2 == 0);
   bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
   bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
   bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK));
   bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
 
-  if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
+  if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
     //this->gps_valid = false;
     this->determine_gps_mode(current_time);
     return;

selfdrive/locationd/test/test_locationd.py

@@ -38,6 +38,7 @@ class TestLocationdProc(unittest.TestCase):
 
     if name == "gpsLocationExternal":
       msg.gpsLocationExternal.flags = 1
+      msg.gpsLocationExternal.hasFix = True
       msg.gpsLocationExternal.verticalAccuracy = 1.0
       msg.gpsLocationExternal.speedAccuracy = 1.0
       msg.gpsLocationExternal.bearingAccuracyDeg = 1.0

selfdrive/locationd/test/test_locationd_scenarios.py

@@ -6,6 +6,7 @@ from collections import defaultdict
 from enum import Enum
 
 from openpilot.tools.lib.logreader import LogReader
+from openpilot.selfdrive.test.process_replay.migration import migrate_all
 from openpilot.selfdrive.test.process_replay.process_replay import replay_process_with_name
 
 TEST_ROUTE = "ff2bd20623fcaeaa|2023-09-05--10-14-54/4"
@@ -107,7 +108,7 @@ class TestLocationdScenarios(unittest.TestCase):
 
   @classmethod
   def setUpClass(cls):
-    cls.logs = list(LogReader(TEST_ROUTE))
+    cls.logs = migrate_all(LogReader(TEST_ROUTE))
 
   def test_base(self):
     """

selfdrive/test/process_replay/migration.py

@@ -7,9 +7,11 @@ from openpilot.selfdrive.manager.process_config import managed_processes
 from panda import Panda
 
 
+# TODO: message migration should happen in-place
 def migrate_all(lr, old_logtime=False, manager_states=False, panda_states=False, camera_states=False):
   msgs = migrate_sensorEvents(lr, old_logtime)
   msgs = migrate_carParams(msgs, old_logtime)
+  msgs = migrate_gpsLocation(msgs)
   if manager_states:
     msgs = migrate_managerState(msgs)
   if panda_states:
@@ -35,6 +37,21 @@ def migrate_managerState(lr):
   return all_msgs
 
 
+def migrate_gpsLocation(lr):
+  all_msgs = []
+  for msg in lr:
+    if msg.which() in ('gpsLocation', 'gpsLocationExternal'):
+      new_msg = msg.as_builder()
+      g = getattr(new_msg, new_msg.which())
+      # hasFix is a newer field
+      if not g.hasFix and g.flags == 1:
+        g.hasFix = True
+      all_msgs.append(new_msg.as_reader())
+    else:
+      all_msgs.append(msg)
+  return all_msgs
+
+
 def migrate_pandaStates(lr):
   all_msgs = []
   # TODO: safety param migration should be handled automatically

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-43efe1cf08cba8c86bc1ae8234b3d3d084a40e5d
+d53d44c21a89d7925d5ad16938e14794907f28b1

system/qcomgpsd/qcomgpsd.py

@@ -352,8 +352,8 @@ def main() -> NoReturn:
       gps.bearingAccuracyDeg = report["q_FltHeadingUncRad"] * 180/math.pi if (report["q_FltHeadingUncRad"] != 0) else 180
       gps.speedAccuracy = math.sqrt(sum([x**2 for x in vNEDsigma]))
       # quectel gps verticalAccuracy is clipped to 500, set invalid if so
-      gps.flags = 1 if gps.verticalAccuracy != 500 else 0
+      gps.hasFix = gps.verticalAccuracy != 500
-      if gps.flags:
+      if gps.hasFix:
         want_assistance = False
         stop_download_event.set()
       pm.send('gpsLocation', msg)

system/ubloxd/ublox_msg.cc

@@ -127,6 +127,7 @@ kj::Array<capnp::word> UbloxMsgParser::gen_nav_pvt(ubx_t::nav_pvt_t *msg) {
   auto gpsLoc = msg_builder.initEvent().initGpsLocationExternal();
   gpsLoc.setSource(cereal::GpsLocationData::SensorSource::UBLOX);
   gpsLoc.setFlags(msg->flags());
+  gpsLoc.setHasFix((msg->flags() % 2) == 1);
   gpsLoc.setLatitude(msg->lat() * 1e-07);
   gpsLoc.setLongitude(msg->lon() * 1e-07);
   gpsLoc.setAltitude(msg->height() * 1e-03);