nav: various cleanup (#21521)

* nav: various cleanup

* eta resize instant

* show
old-commit-hash: 9ea913bcc8

selfdrive/common/util.h

@@ -127,6 +127,7 @@ public:
     return x_;
   }
   inline void reset(float x) { x_ = x; }
+  inline float x(){ return x_; }
 
 private:
   float x_, k_;

selfdrive/ui/qt/maps/map.cc

@@ -4,7 +4,6 @@
 
 #include <QDebug>
 
-#include "selfdrive/common/util.h"
 #include "selfdrive/common/swaglog.h"
 #include "selfdrive/ui/ui.h"
 #include "selfdrive/ui/qt/util.h"
@@ -13,7 +12,6 @@
 
 
 const int PAN_TIMEOUT = 100;
-const bool DRAW_MODEL_PATH = false;
 const qreal REROUTE_DISTANCE = 25;
 const float MANEUVER_TRANSITION_THRESHOLD = 10;
 
@@ -24,19 +22,17 @@ const float MIN_PITCH = 0;
 const float MAP_SCALE = 2;
 
 
-MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings) {
-  if (DRAW_MODEL_PATH) {
-    sm = new SubMaster({"liveLocationKalman", "modelV2"});
-  } else {
+MapWindow::MapWindow(const QMapboxGLSettings &settings) :
+  m_settings(settings), velocity_filter(0, 10, 0.1) {
   sm = new SubMaster({"liveLocationKalman"});
-  }
 
   timer = new QTimer(this);
   QObject::connect(timer, SIGNAL(timeout()), this, SLOT(timerUpdate()));
+  timer->start(100);
 
   recompute_timer = new QTimer(this);
-  recompute_timer->start(1000);
   QObject::connect(recompute_timer, SIGNAL(timeout()), this, SLOT(recomputeRoute()));
+  recompute_timer->start(1000);
 
   // Instructions
   map_instructions = new MapInstructions(this);
@@ -124,20 +120,12 @@ void MapWindow::timerUpdate() {
     update();
   }
 
-  loaded_once = loaded_once || m_map->isFullyLoaded();
-  if (!loaded_once) {
-    map_instructions->showError("Map loading");
-    return;
-  }
-
-  initLayers();
-
   sm->update(0);
   if (sm->updated("liveLocationKalman")) {
     auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman();
     gps_ok = location.getGpsOK();
 
-    bool localizer_valid = location.getStatus() == cereal::LiveLocationKalman::Status::VALID;
+    localizer_valid = location.getStatus() == cereal::LiveLocationKalman::Status::VALID;
 
     if (localizer_valid) {
       auto pos = location.getPositionGeodetic();
@@ -149,42 +137,48 @@ void MapWindow::timerUpdate() {
 
       last_position = coordinate;
       last_bearing = bearing;
+      velocity_filter.update(velocity);
+    } else {
+      map_instructions->showError("Waiting for GPS");
+    }
+  }
+
+  if (m_map.isNull()) return;
+
+  // Update map once it's loaded
+  loaded_once = loaded_once || m_map->isFullyLoaded();
+  if (!loaded_once) {
+    map_instructions->showError("Map loading");
+    return;
+  }
+
+  initLayers();
 
   if (pan_counter == 0) {
-        m_map->setCoordinate(coordinate);
-        m_map->setBearing(bearing);
+    if (last_position) m_map->setCoordinate(*last_position);
+    if (last_bearing) m_map->setBearing(*last_bearing);
   } else {
     pan_counter--;
   }
 
   if (zoom_counter == 0) {
-        static FirstOrderFilter velocity_filter(velocity, 10, 0.1);
-        m_map->setZoom(util::map_val<float>(velocity_filter.update(velocity), 0, 30, MAX_ZOOM, MIN_ZOOM));
+    m_map->setZoom(util::map_val<float>(velocity_filter.x(), 0, 30, MAX_ZOOM, MIN_ZOOM));
   } else {
     zoom_counter--;
   }
 
   // Update current location marker
-      auto point = coordinate_to_collection(coordinate);
+  if (localizer_valid) {
+    auto point = coordinate_to_collection(*last_position);
     QMapbox::Feature feature1(QMapbox::Feature::PointType, point, {}, {});
     QVariantMap carPosSource;
     carPosSource["type"] = "geojson";
     carPosSource["data"] = QVariant::fromValue<QMapbox::Feature>(feature1);
     m_map->updateSource("carPosSource", carPosSource);
-
-      // Update model path
-      if (DRAW_MODEL_PATH) {
-        auto model = (*sm)["modelV2"].getModelV2();
-        auto path_points = model_to_collection(location.getCalibratedOrientationECEF(), location.getPositionECEF(), model.getPosition());
-        QMapbox::Feature feature2(QMapbox::Feature::LineStringType, path_points, {}, {});
-        QVariantMap modelPathSource;
-        modelPathSource["type"] = "geojson";
-        modelPathSource["data"] = QVariant::fromValue<QMapbox::Feature>(feature2);
-        m_map->updateSource("modelPathSource", modelPathSource);
   }
 
-      if (segment.isValid()) {
   // Show route instructions
+  if (segment.isValid()) {
     auto cur_maneuver = segment.maneuver();
     auto attrs = cur_maneuver.extendedAttributes();
     if (cur_maneuver.isValid() && attrs.contains("mapbox.banner_instructions")) {
@@ -222,10 +216,6 @@ void MapWindow::timerUpdate() {
       }
     }
   }
-    } else {
-      map_instructions->showError("Waiting for GPS");
-    }
-  }
 }
 
 void MapWindow::resizeGL(int w, int h) {
@@ -252,7 +242,6 @@ void MapWindow::initializeGL() {
       loaded_once = true;
     }
   });
-  timer->start(100);
 }
 
 void MapWindow::paintGL() {
@@ -268,23 +257,23 @@ static float get_time_typical(const QGeoRouteSegment &segment) {
 
 
 void MapWindow::recomputeRoute() {
-  // Last position is valid if read from param or from GPS
   if (!last_position) {
     return;
   }
 
-  bool should_recompute = shouldRecompute();
   auto new_destination = coordinate_from_param("NavDestination");
-
   if (!new_destination) {
     clearRoute();
     return;
   }
 
+  bool should_recompute = shouldRecompute();
   if (*new_destination != nav_destination) {
     qWarning() << "Got new destination from NavDestination param" << *new_destination;
+
     setVisible(true); // Show map on destination set/change
     // TODO: close sidebar
+
     should_recompute = true;
   }
 
@@ -292,8 +281,8 @@ void MapWindow::recomputeRoute() {
 
   if (!gps_ok && segment.isValid()) return; // Don't recompute when gps drifts in tunnels
 
-  // Only do API request when map is loaded
-  if (!m_map.isNull()) {
+  // Only do API request when map is fully loaded
+  if (loaded_once) {
     if (recompute_countdown == 0 && should_recompute) {
       recompute_countdown = std::pow(2, recompute_backoff);
       recompute_backoff = std::min(7, recompute_backoff + 1);
@@ -363,7 +352,6 @@ void MapWindow::routeCalculated(QGeoRouteReply *reply) {
     }
   } else {
     qWarning() << "Got error in route reply" << reply->errorString();
-  
   }
 
   reply->deleteLater();
@@ -726,8 +714,6 @@ MapETA::MapETA(QWidget * parent) : QWidget(parent) {
 
 
 void MapETA::updateETA(float s, float s_typical, float d) {
-  setVisible(true);
-
   // ETA
   auto eta_time = QDateTime::currentDateTime().addSecs(s).time();
   if (params.getBool("NavSettingTime24h")) {
@@ -775,6 +761,9 @@ void MapETA::updateETA(float s, float s_typical, float d) {
   distance_str.setNum(num, 'f', num < 100 ? 1 : 0);
   distance->setText(distance_str);
 
+  show();
+  adjustSize();
+  repaint();
   adjustSize();
 
   // Rounded corners

selfdrive/ui/qt/maps/map.h

@@ -24,6 +24,7 @@
 #include <QPixmap>
 
 #include "selfdrive/common/params.h"
+#include "selfdrive/common/util.h"
 #include "cereal/messaging/messaging.h"
 
 class MapInstructions : public QWidget {
@@ -105,6 +106,8 @@ private:
   // Position
   std::optional<QMapbox::Coordinate> last_position;
   std::optional<float> last_bearing;
+  FirstOrderFilter velocity_filter;
+  bool localizer_valid = false;
 
   // Route
   bool gps_ok = false;