mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-07-13 17:02:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

quectel: move vertical accuracy sanity check (#27819)

Browse Source 
* Update rawgpsd.py

* remove from locationd

* Update system/sensord/rawgps/rawgpsd.py

* flip
This commit is contained in:
Shane Smiskol
2023-04-10 20:46:12 -07:00
committed by GitHub
parent aefd84a3ee
commit c9804f8ec2
2 changed files with 3 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/locationd.cc
+1 -7
View File
@@ -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;
system/sensord/rawgps/rawgpsd.py
+2 -1
View File
@@ -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)