From aa1474467ec19af66d0d241881c607f4ee0f4b9c Mon Sep 17 00:00:00 2001
From: Dean Lee <deanlee3@gmail.com>
Date: Tue, 30 Mar 2021 18:09:06 +0800
Subject: [PATCH] Added new util class FirstOrderFilter (#20303)

* new class FirstOrderFilter

* flick face icon

* revert ui_draw_vision_face

* new function reset()

* reset after update

* use filter to update brightness
old-commit-hash: 6db31b9a33e6caab06645be898957e2414a6dedc
---
 selfdrive/common/util.h    | 16 ++++++++++++++++
 selfdrive/modeld/modeld.cc | 13 +++++++------
 selfdrive/ui/qt/home.cc    |  9 ++-------
 selfdrive/ui/qt/home.hpp   |  3 ++-
 4 files changed, 27 insertions(+), 14 deletions(-)

diff --git a/selfdrive/common/util.h b/selfdrive/common/util.h
index b75c2f4343..98406e0f4b 100644
--- a/selfdrive/common/util.h
+++ b/selfdrive/common/util.h
@@ -161,3 +161,19 @@ struct unique_fd {
   operator int() const { return fd_; }
   int fd_;
 };
+
+class FirstOrderFilter {
+public:
+  FirstOrderFilter(float x0, float ts, float dt) {
+    k_ = (dt / ts) / (1.0 + dt / ts);
+    x_ = x0;
+  }
+  inline float update(float x) {
+    x_ = (1. - k_) * x_ + k_ * x;
+    return x_;
+  }
+  inline void reset(float x) { x_ = x; }
+
+private:
+  float x_, k_;
+};
diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc
index 4eb64cfd4b..e967d5a754 100644
--- a/selfdrive/modeld/modeld.cc
+++ b/selfdrive/modeld/modeld.cc
@@ -72,10 +72,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client) {
   SubMaster sm({"lateralPlan", "roadCameraState"});
 
   // setup filter to track dropped frames
-  const float dt = 1. / MODEL_FREQ;
-  const float ts = 10.0;  // filter time constant (s)
-  const float frame_filter_k = (dt / ts) / (1. + dt / ts);
-  float frames_dropped = 0;
+  FirstOrderFilter frame_dropped_filter(0., 10., 1. / MODEL_FREQ);
 
   uint32_t frame_id = 0, last_vipc_frame_id = 0;
   double last = 0;
@@ -114,8 +111,12 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client) {
 
       // tracked dropped frames
       uint32_t vipc_dropped_frames = extra.frame_id - last_vipc_frame_id - 1;
-      frames_dropped = (1. - frame_filter_k) * frames_dropped + frame_filter_k * (float)std::min(vipc_dropped_frames, 10U);
-      if (run_count < 10) frames_dropped = 0;  // let frame drops warm up
+      float frames_dropped = frame_dropped_filter.update((float)std::min(vipc_dropped_frames, 10U));
+      if (run_count < 10) { // let frame drops warm up
+        frame_dropped_filter.reset(0);
+        frames_dropped = 0.;
+      }
+      
       float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
 
       model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time,
diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc
index 72159facf1..9ce0fd6709 100644
--- a/selfdrive/ui/qt/home.cc
+++ b/selfdrive/ui/qt/home.cc
@@ -223,7 +223,7 @@ static void handle_display_state(UIState* s, bool user_input) {
   }
 }
 
-GLWindow::GLWindow(QWidget* parent) : QOpenGLWidget(parent) {
+GLWindow::GLWindow(QWidget* parent) : brightness_filter(BACKLIGHT_OFFROAD, BACKLIGHT_TS, BACKLIGHT_DT), QOpenGLWidget(parent) {
   timer = new QTimer(this);
   QObject::connect(timer, SIGNAL(timeout()), this, SLOT(timerUpdate()));
 
@@ -236,7 +236,6 @@ GLWindow::GLWindow(QWidget* parent) : QOpenGLWidget(parent) {
     brightness_b = 10.0;
     brightness_m = 0.1;
   }
-  smooth_brightness = BACKLIGHT_OFFROAD;
 }
 
 GLWindow::~GLWindow() {
@@ -265,16 +264,12 @@ void GLWindow::initializeGL() {
 
 void GLWindow::backlightUpdate() {
   // Update brightness
-  float k = (BACKLIGHT_DT / BACKLIGHT_TS) / (1.0f + BACKLIGHT_DT / BACKLIGHT_TS);
-
   float clipped_brightness = std::min(100.0f, (ui_state.scene.light_sensor * brightness_m) + brightness_b);
   if (!ui_state.scene.started) {
     clipped_brightness = BACKLIGHT_OFFROAD;
   }
 
-  smooth_brightness = clipped_brightness * k + smooth_brightness * (1.0f - k);
-
-  int brightness = smooth_brightness;
+  int brightness = brightness_filter.update(clipped_brightness);
   if (!ui_state.awake) {
     brightness = 0;
     emit screen_shutoff();
diff --git a/selfdrive/ui/qt/home.hpp b/selfdrive/ui/qt/home.hpp
index f5cf74cb0b..d19b9d60cb 100644
--- a/selfdrive/ui/qt/home.hpp
+++ b/selfdrive/ui/qt/home.hpp
@@ -11,6 +11,7 @@
 
 #include "sound.hpp"
 #include "ui/ui.hpp"
+#include "common/util.h"
 #include "widgets/offroad_alerts.hpp"
 
 // container window for onroad NVG UI
@@ -46,8 +47,8 @@ private:
   // TODO: make a nice abstraction to handle embedded device stuff
   float brightness_b = 0;
   float brightness_m = 0;
-  float smooth_brightness = 0;
   float last_brightness = 0;
+  FirstOrderFilter brightness_filter;
 
 public slots:
   void timerUpdate();