From 6bf5c6215528894aa4423b0a8f058b4cb1b07f7c Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 8 Mar 2024 11:32:38 -0800 Subject: [PATCH] add hasFix field to gpsLocation (#31778) * add hasFix field to gpsLocation * migration * update refs for ubloxd * cereal master old-commit-hash: a0389d7120624b35fb169d680889ae56e1adb10a --- cereal | 2 +- selfdrive/locationd/locationd.cc | 4 +--- selfdrive/locationd/test/test_locationd.py | 1 + .../locationd/test/test_locationd_scenarios.py | 3 ++- selfdrive/test/process_replay/migration.py | 17 +++++++++++++++++ selfdrive/test/process_replay/ref_commit | 2 +- system/qcomgpsd/qcomgpsd.py | 4 ++-- system/ubloxd/ublox_msg.cc | 1 + 8 files changed, 26 insertions(+), 8 deletions(-) diff --git a/cereal b/cereal index c5c2a60f1a..cf7bb3e749 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit c5c2a60f1aa796e7de464015349db3c336b79220 +Subproject commit cf7bb3e74974879abef94286fab4d39398fe402b diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 26999cd684..2ac392a778 100644 --- a/selfdrive/locationd/locationd.cc +++ b/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; diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index 78de9216dc..cd032dbaf0 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/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 diff --git a/selfdrive/locationd/test/test_locationd_scenarios.py b/selfdrive/locationd/test/test_locationd_scenarios.py index f48c83ce46..3fdd47275f 100755 --- a/selfdrive/locationd/test/test_locationd_scenarios.py +++ b/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): """ diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py index ef74314172..d480309169 100644 --- a/selfdrive/test/process_replay/migration.py +++ b/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 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 46d684211c..97646ce064 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -43efe1cf08cba8c86bc1ae8234b3d3d084a40e5d +d53d44c21a89d7925d5ad16938e14794907f28b1 \ No newline at end of file diff --git a/system/qcomgpsd/qcomgpsd.py b/system/qcomgpsd/qcomgpsd.py index 25b547cf5e..859e024e68 100755 --- a/system/qcomgpsd/qcomgpsd.py +++ b/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 - if gps.flags: + gps.hasFix = gps.verticalAccuracy != 500 + if gps.hasFix: want_assistance = False stop_download_event.set() pm.send('gpsLocation', msg) diff --git a/system/ubloxd/ublox_msg.cc b/system/ubloxd/ublox_msg.cc index eb1a1e4b19..26b33a1e32 100644 --- a/system/ubloxd/ublox_msg.cc +++ b/system/ubloxd/ublox_msg.cc @@ -127,6 +127,7 @@ kj::Array 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);