all the changes I want to make

common/util.h

@@ -153,19 +153,26 @@ struct unique_fd {
 
 class FirstOrderFilter {
 public:
-  FirstOrderFilter(float x0, float ts, float dt) {
+  FirstOrderFilter(float x0, float ts, float dt, bool initialized = true) {
     k_ = (dt / ts) / (1.0 + dt / ts);
     x_ = x0;
+    initialized_ = initialized;
   }
   inline float update(float x) {
-    x_ = (1. - k_) * x_ + k_ * x;
+    if (initialized_) {
+      x_ = (1. - k_) * x_ + k_ * x;
+    } else {
+      initialized_ = true;
+      x_ = x;
+    }
     return x_;
   }
-  inline void reset(float x) { x_ = x; }
+  inline void reset(float x, bool initialized = true) { x_ = x; initialized_ = initialized; }
   inline float x(){ return x_; }
 
 private:
   float x_, k_;
+  bool initialized_;
 };
 
 template<typename T>

selfdrive/ui/qt/maps/map.cc

@@ -18,7 +18,7 @@ const float MAX_PITCH = 50;
 const float MIN_PITCH = 0;
 const float MAP_SCALE = 2;
 
-MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings), velocity_filter(0, 10, 0.05) {
+MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings), velocity_filter(0, 10, 0.05, false) {
   QObject::connect(uiState(), &UIState::uiUpdate, this, &MapWindow::updateState);
 
   map_overlay = new QWidget (this);
@@ -151,7 +151,8 @@ void MapWindow::updateState(const UIState &s) {
     if (locationd_valid) {
       last_position = QMapbox::Coordinate(locationd_pos.getValue()[0], locationd_pos.getValue()[1]);
       last_bearing = RAD2DEG(locationd_orientation.getValue()[2]);
-      velocity_filter.update(locationd_velocity.getValue()[0]);
+      vehicle_speed_ = locationd_velocity.getValue()[0];
+      velocity_filter.update(std::max(10.0, locationd_velocity.getValue()[0]));
     }
   }
 
@@ -202,6 +203,7 @@ void MapWindow::updateState(const UIState &s) {
     if (last_position) m_map->setCoordinate(*last_position);
     if (last_bearing) m_map->setBearing(*last_bearing);
     m_map->setZoom(util::map_val<float>(velocity_filter.x(), 0, 30, MAX_ZOOM, MIN_ZOOM));
+    qDebug() << "velocity_filter.x()" << velocity_filter.x();
   } else {
     interaction_counter--;
   }
@@ -304,6 +306,8 @@ void MapWindow::mousePressEvent(QMouseEvent *ev) {
 void MapWindow::mouseDoubleClickEvent(QMouseEvent *ev) {
   if (last_position) m_map->setCoordinate(*last_position);
   if (last_bearing) m_map->setBearing(*last_bearing);
+//  velocity_filter.reset(0, false);
+  velocity_filter.reset(vehicle_speed_);
   m_map->setZoom(util::map_val<float>(velocity_filter.x(), 0, 30, MAX_ZOOM, MIN_ZOOM));
   update();
 

selfdrive/ui/qt/maps/map.h

@@ -54,6 +54,7 @@ private:
   // Panning
   QPointF m_lastPos;
   int interaction_counter = 0;
+  double vehicle_speed_ = 0.0;
 
   // Position
   std::optional<QMapbox::Coordinate> last_valid_nav_dest;