From 85c4b1a400e9ab80536c0bf2ac8f1bacc4476c2c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Tue, 27 Jun 2023 19:09:45 -0700 Subject: [PATCH] Nav: show gps as long as it's 100m accuracy (#28713) * Nav: show gps as long as it's 100m accuracy * Get norm * get std --- selfdrive/ui/qt/maps/map.cc | 11 +++++++---- selfdrive/ui/qt/maps/map.h | 1 - 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index 2ba7ef7af..eb8839b2d 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -136,8 +136,11 @@ void MapWindow::updateState(const UIState &s) { auto locationd_orientation = locationd_location.getCalibratedOrientationNED(); auto locationd_velocity = locationd_location.getVelocityCalibrated(); - locationd_valid = (locationd_location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && - locationd_pos.getValid() && locationd_orientation.getValid() && locationd_velocity.getValid(); + // Check std norm + auto pos_ecef_std = locationd_location.getPositionECEF().getStd(); + bool pos_accurate_enough = sqrt(pow(pos_ecef_std[0], 2) + pow(pos_ecef_std[1], 2) + pow(pos_ecef_std[2], 2)) < 100; + + locationd_valid = (locationd_pos.getValid() && locationd_orientation.getValid() && locationd_velocity.getValid() && pos_accurate_enough); if (locationd_valid) { last_position = QMapbox::Coordinate(locationd_pos.getValue()[0], locationd_pos.getValue()[1]); @@ -168,7 +171,7 @@ void MapWindow::updateState(const UIState &s) { initLayers(); - if (locationd_valid || laikad_valid) { + if (locationd_valid) { map_instructions->noError(); // Update current location marker @@ -201,7 +204,7 @@ void MapWindow::updateState(const UIState &s) { auto i = sm["navInstruction"].getNavInstruction(); emit ETAChanged(i.getTimeRemaining(), i.getTimeRemainingTypical(), i.getDistanceRemaining()); - if (locationd_valid || laikad_valid) { + if (locationd_valid) { m_map->setPitch(MAX_PITCH); // TODO: smooth pitching based on maneuver distance emit distanceChanged(i.getManeuverDistance()); // TODO: combine with instructionsChanged emit instructionsChanged(i); diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h index 658d26ee4..6bf326a3f 100644 --- a/selfdrive/ui/qt/maps/map.h +++ b/selfdrive/ui/qt/maps/map.h @@ -106,7 +106,6 @@ private: std::optional last_position; std::optional last_bearing; FirstOrderFilter velocity_filter; - bool laikad_valid = false; bool locationd_valid = false; MapInstructions* map_instructions;