From 890f7ffd80cceb6ecea0c8e7e9f1813a95699bdb Mon Sep 17 00:00:00 2001 From: Kurt Nistelberger Date: Fri, 4 Nov 2022 07:43:30 +0100 Subject: [PATCH] locationd: add gps sanity check for quectel gps (#26352) * update check * . * . * remove gps kf time check for gps ok * upsi * dont use gps_mode * update refs * Update selfdrive/locationd/locationd.cc Co-authored-by: Shane Smiskol Co-authored-by: Kurt Nistelberger Co-authored-by: Cameron Clough Co-authored-by: Shane Smiskol old-commit-hash: aebb08e10542758c458c3abb219f8b57485e0b61 --- selfdrive/locationd/locationd.cc | 20 ++++++++++++++------ selfdrive/locationd/locationd.h | 3 ++- selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 17 insertions(+), 8 deletions(-) diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index e156af5d64..8d9e247655 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -270,15 +270,22 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R 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) { + // quectel gps verticalAccuracy is clipped to 500 + bool gps_accuracy_insane_quectel = false; + if (!ublox_available) { + gps_accuracy_insane_quectel = log.getVerticalAccuracy() == 500; + } + + if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane || gps_accuracy_insane_quectel) { + this->gps_valid = false; this->determine_gps_mode(current_time); return; } - + double sensor_time = current_time - sensor_time_offset; // Process message - this->last_gps_fix = sensor_time; + this->gps_valid = true; this->gps_mode = true; Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() }; this->converter = std::make_unique(geodetic); @@ -476,9 +483,8 @@ kj::ArrayPtr Localizer::get_message_bytes(MessageBuilder& msg_build return msg_builder.toBytes(); } - bool Localizer::isGpsOK() { - return this->kf->get_filter_time() - this->last_gps_fix < 1.0; + return this->gps_valid; } void Localizer::determine_gps_mode(double current_time) { @@ -498,8 +504,10 @@ void Localizer::determine_gps_mode(double current_time) { } int Localizer::locationd_thread() { + ublox_available = Params().getBool("UbloxAvailable", true); + const char* gps_location_socket; - if (Params().getBool("UbloxAvailable", true)) { + if (ublox_available) { gps_location_socket = "gpsLocationExternal"; } else { gps_location_socket = "gpsLocation"; diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index 280296b06c..d6bb5347c5 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -68,8 +68,9 @@ private: std::unique_ptr converter; int64_t unix_timestamp_millis = 0; - double last_gps_fix = 0; double reset_tracker = 0.0; bool device_fell = false; bool gps_mode = false; + bool gps_valid = false; + bool ublox_available = true; }; diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 68bd35c38c..8eae3ce8b4 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -01b24beff6855e8c4d2fb0efeeefafb46343e013 +6abe3ec1ee19710bdd89ce2882b9503d4aff8e7f