From c9804f8ec20fd60a42c17d61d07da83406c5ff31 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 10 Apr 2023 20:46:12 -0700 Subject: [PATCH] quectel: move vertical accuracy sanity check (#27819) * Update rawgpsd.py * remove from locationd * Update system/sensord/rawgps/rawgpsd.py * flip --- selfdrive/locationd/locationd.cc | 8 +------- system/sensord/rawgps/rawgpsd.py | 3 ++- 2 files changed, 3 insertions(+), 8 deletions(-) diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 87c5f644c5..de2373c1da 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -303,13 +303,7 @@ 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); - // 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) { + if (gps_invalid_flag || 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/system/sensord/rawgps/rawgpsd.py b/system/sensord/rawgps/rawgpsd.py index f75ceee7ed..8f243af9a5 100755 --- a/system/sensord/rawgps/rawgpsd.py +++ b/system/sensord/rawgps/rawgpsd.py @@ -270,7 +270,6 @@ def main() -> NoReturn: msg = messaging.new_message('gpsLocation') gps = msg.gpsLocation - gps.flags = 1 gps.latitude = report["t_DblFinalPosLatLon[0]"] * 180/math.pi gps.longitude = report["t_DblFinalPosLatLon[1]"] * 180/math.pi gps.altitude = report["q_FltFinalPosAlt"] @@ -283,6 +282,8 @@ def main() -> NoReturn: gps.verticalAccuracy = report["q_FltVdop"] gps.bearingAccuracyDeg = report["q_FltHeadingUncRad"] * 180/math.pi 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 pm.send('gpsLocation', msg)