From 53b2a39a90a9f28ff9e955616da209949e99bfa3 Mon Sep 17 00:00:00 2001
From: Adeeb Shihadeh <adeebshihadeh@gmail.com>
Date: Sun, 18 Oct 2020 19:03:41 -0700
Subject: [PATCH] tap to wake with accelerometer (#2267)

old-commit-hash: 9e03c2e9293eadd6853382b1b5c7e853ddc808f1
---
 selfdrive/sensord/sensors_qcom.cc |  1 +
 selfdrive/ui/android/ui.cc        | 64 ++++++++++++-------------------
 selfdrive/ui/ui.cc                |  5 +++
 selfdrive/ui/ui.hpp               |  2 +-
 4 files changed, 32 insertions(+), 40 deletions(-)

diff --git a/selfdrive/sensord/sensors_qcom.cc b/selfdrive/sensord/sensors_qcom.cc
index 389014f2e5..58295679e9 100644
--- a/selfdrive/sensord/sensors_qcom.cc
+++ b/selfdrive/sensord/sensors_qcom.cc
@@ -99,6 +99,7 @@ void sensor_loop() {
     std::set<int> offroad_sensors = {
       SENSOR_LIGHT,
       SENSOR_ACCELEROMETER,
+      SENSOR_GYRO_UNCALIBRATED,
     };
 
     // init all the sensors
diff --git a/selfdrive/ui/android/ui.cc b/selfdrive/ui/android/ui.cc
index 4ec5687f81..24a0db2a2b 100644
--- a/selfdrive/ui/android/ui.cc
+++ b/selfdrive/ui/android/ui.cc
@@ -29,47 +29,36 @@ static void ui_set_brightness(UIState *s, int brightness) {
   }
 }
 
-int event_processing_enabled = -1;
-static void enable_event_processing(bool yes) {
-  if (event_processing_enabled != 1 && yes) {
-    system("service call window 18 i32 1");  // enable event processing
-    event_processing_enabled = 1;
-  } else if (event_processing_enabled != 0 && !yes) {
-    system("service call window 18 i32 0");  // disable event processing
-    event_processing_enabled = 0;
-  }
-}
-
-// TODO: implement double tap to wake and actually turn display off
 static void handle_display_state(UIState *s, bool user_input) {
 
-  static int display_mode = HWC_POWER_MODE_OFF;
-  static int display_timeout = 0;
+  static int awake_timeout = 0;
+  awake_timeout = std::max(awake_timeout-1, 0);
+
+  // tap detection while display is off
+  const float accel_samples = 5*UI_FREQ;
+  static float accel_prev, gyro_prev = 0;
+
+  bool accel_trigger = abs(s->accel_sensor - accel_prev) > 0.2;
+  bool gyro_trigger = abs(s->gyro_sensor - gyro_prev) > 0.15;
+  user_input = user_input || (accel_trigger && gyro_trigger);
+  gyro_prev = s->gyro_sensor;
+  accel_prev = (accel_prev*(accel_samples - 1) + s->accel_sensor) / accel_samples;
 
   // determine desired state
-  int desired_mode = display_mode;
+  bool should_wake = s->awake;
   if (user_input || s->ignition || s->started) {
-    desired_mode = HWC_POWER_MODE_NORMAL;
-    display_timeout = 30*UI_FREQ;
-  } else {
-    display_timeout = std::max(display_timeout-1, 0);
-    if (display_timeout == 0) {
-      desired_mode = HWC_POWER_MODE_DOZE;
-    }
+    should_wake = true;
+    awake_timeout = 30*UI_FREQ;
+  } else if (awake_timeout == 0){
+    should_wake = false;
   }
 
   // handle state transition
-  if (display_mode != desired_mode) {
-    LOGW("setting display mode %d", desired_mode);
-
-    display_mode = desired_mode;
-    s->awake = display_mode == HWC_POWER_MODE_NORMAL;
+  if (s->awake != should_wake) {
+    s->awake = should_wake;
+    int display_mode = s->awake ? HWC_POWER_MODE_NORMAL : HWC_POWER_MODE_OFF;
+    LOGW("setting display mode %d", display_mode);
     framebuffer_set_power(s->fb, display_mode);
-    enable_event_processing(s->awake);
-
-    if (!s->awake) {
-      ui_set_brightness(s, 0);
-    }
   }
 }
 
@@ -129,14 +118,12 @@ int main(int argc, char* argv[]) {
   ui_init(s);
   s->sound = &sound;
 
+  TouchState touch = {0};
+  touch_init(&touch);
   handle_display_state(s, true);
-  enable_event_processing(true);
 
   PubMaster *pm = new PubMaster({"offroadLayout"});
 
-  TouchState touch = {0};
-  touch_init(&touch);
-
   // light sensor scaling and volume params
   const bool LEON = util::read_file("/proc/cmdline").find("letv") != std::string::npos;
 
@@ -156,9 +143,8 @@ int main(int argc, char* argv[]) {
   s->sound->setVolume(MIN_VOLUME);
 
   while (!do_exit) {
-    if (!s->started || !s->vision_connected) {
-      // Delay a while to avoid 9% cpu usage while car is not started and user is keeping touching on the screen.
-      usleep(30 * 1000);
+    if (!s->started) {
+      usleep(50 * 1000);
     }
     double u1 = millis_since_boot();
 
diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
index 12728f8ee0..4ebc50f067 100644
--- a/selfdrive/ui/ui.cc
+++ b/selfdrive/ui/ui.cc
@@ -4,6 +4,7 @@
 #include <signal.h>
 #include <unistd.h>
 #include <assert.h>
+#include <math.h>
 #include <poll.h>
 #include <sys/mman.h>
 
@@ -221,6 +222,10 @@ void update_sockets(UIState *s) {
     for (auto sensor : sm["sensorEvents"].getSensorEvents()) {
       if (sensor.which() == cereal::SensorEventData::LIGHT) {
         s->light_sensor = sensor.getLight();
+      } else if (!s->started && sensor.which() == cereal::SensorEventData::ACCELERATION) {
+        s->accel_sensor = sensor.getAcceleration().getV()[2];
+      } else if (!s->started && sensor.which() == cereal::SensorEventData::GYRO_UNCALIBRATED) {
+        s->gyro_sensor = sensor.getGyroUncalibrated().getV()[1];
       }
     }
   }
diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp
index a894b70371..0a66e1dc1a 100644
--- a/selfdrive/ui/ui.hpp
+++ b/selfdrive/ui/ui.hpp
@@ -178,7 +178,7 @@ typedef struct UIState {
 
   // device state
   bool awake;
-  float light_sensor;
+  float light_sensor, accel_sensor, gyro_sensor;
 
   bool started;
   bool ignition;