From daea946ad25acc1451f9daf828db71b284e2f01c Mon Sep 17 00:00:00 2001
From: Adeeb Shihadeh <adeebshihadeh@gmail.com>
Date: Wed, 2 Sep 2020 14:04:55 -0700
Subject: [PATCH] sensord cleanup (#2111)

* sensors_qcom cleanup

* include vector and map

* abstract sensor init

* use a set

* add to relase files

* fix this up

* no ui changes

* cleanup

Co-authored-by: Comma Device <device@comma.ai>
---
 selfdrive/sensord/sensors_qcom.cc | 83 ++++++++++++-------------------
 1 file changed, 32 insertions(+), 51 deletions(-)

diff --git a/selfdrive/sensord/sensors_qcom.cc b/selfdrive/sensord/sensors_qcom.cc
index b42482d8..0236a8ed 100644
--- a/selfdrive/sensord/sensors_qcom.cc
+++ b/selfdrive/sensord/sensors_qcom.cc
@@ -10,7 +10,8 @@
 #include <sys/types.h>
 #include <sys/resource.h>
 
-#include <pthread.h>
+#include <map>
+#include <set>
 
 #include <cutils/log.h>
 #include <hardware/sensors.h>
@@ -20,15 +21,14 @@
 #include "common/timing.h"
 #include "common/swaglog.h"
 
+// ACCELEROMETER_UNCALIBRATED is only in Android O
+// https://developer.android.com/reference/android/hardware/Sensor.html#STRING_TYPE_ACCELEROMETER_UNCALIBRATED
+
 #define SENSOR_ACCELEROMETER 1
 #define SENSOR_MAGNETOMETER 2
 #define SENSOR_GYRO 4
-
-// ACCELEROMETER_UNCALIBRATED is only in Android O
-// https://developer.android.com/reference/android/hardware/Sensor.html#STRING_TYPE_ACCELEROMETER_UNCALIBRATED
 #define SENSOR_MAGNETOMETER_UNCALIBRATED 3
 #define SENSOR_GYRO_UNCALIBRATED 5
-
 #define SENSOR_PROXIMITY 6
 #define SENSOR_LIGHT 7
 
@@ -70,30 +70,33 @@ void sensor_loop() {
       LOGD("sensor %4d: %4d %60s  %d-%ld us", i, list[i].handle, list[i].name, list[i].minDelay, list[i].maxDelay);
     }
 
-    device->activate(device, SENSOR_MAGNETOMETER_UNCALIBRATED, 0);
-    device->activate(device, SENSOR_GYRO_UNCALIBRATED, 0);
-    device->activate(device, SENSOR_ACCELEROMETER, 0);
-    device->activate(device, SENSOR_MAGNETOMETER, 0);
-    device->activate(device, SENSOR_GYRO, 0);
-    device->activate(device, SENSOR_PROXIMITY, 0);
-    device->activate(device, SENSOR_LIGHT, 0);
+    std::set<int> sensor_types = {
+      SENSOR_TYPE_ACCELEROMETER,
+      SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED,
+      SENSOR_TYPE_MAGNETIC_FIELD,
+      SENSOR_TYPE_GYROSCOPE_UNCALIBRATED,
+      SENSOR_TYPE_GYROSCOPE,
+      SENSOR_TYPE_PROXIMITY,
+      SENSOR_TYPE_LIGHT,
+    };
 
-    device->activate(device, SENSOR_MAGNETOMETER_UNCALIBRATED, 1);
-    device->activate(device, SENSOR_GYRO_UNCALIBRATED, 1);
-    device->activate(device, SENSOR_ACCELEROMETER, 1);
-    device->activate(device, SENSOR_MAGNETOMETER, 1);
-    device->activate(device, SENSOR_GYRO, 1);
-    device->activate(device, SENSOR_PROXIMITY, 1);
-    device->activate(device, SENSOR_LIGHT, 1);
+    std::map<int, int64_t> sensors = {
+      {SENSOR_GYRO_UNCALIBRATED, ms2ns(10)},
+      {SENSOR_MAGNETOMETER_UNCALIBRATED, ms2ns(100)},
+      {SENSOR_ACCELEROMETER, ms2ns(10)},
+      {SENSOR_GYRO, ms2ns(10)},
+      {SENSOR_MAGNETOMETER, ms2ns(100)},
+      {SENSOR_PROXIMITY, ms2ns(100)},
+      {SENSOR_LIGHT, ms2ns(100)}
+    };
 
-    device->setDelay(device, SENSOR_GYRO_UNCALIBRATED, ms2ns(10));
-    device->setDelay(device, SENSOR_MAGNETOMETER_UNCALIBRATED, ms2ns(100));
-    device->setDelay(device, SENSOR_ACCELEROMETER, ms2ns(10));
-    device->setDelay(device, SENSOR_GYRO, ms2ns(10));
-    device->setDelay(device, SENSOR_MAGNETOMETER, ms2ns(100));
-    device->setDelay(device, SENSOR_PROXIMITY, ms2ns(100));
-    device->setDelay(device, SENSOR_LIGHT, ms2ns(100));
+    for (auto &s : sensors) {
+      device->activate(device, s.first, 0);
+      device->activate(device, s.first, 1);
+      device->setDelay(device, s.first, s.second);
+    }
 
+    // TODO: why is this 16?
     static const size_t numEvents = 16;
     sensors_event_t buffer[numEvents];
 
@@ -107,26 +110,14 @@ void sensor_loop() {
 
       int log_events = 0;
       for (int i=0; i < n; i++) {
-        switch (buffer[i].type) {
-        case SENSOR_TYPE_ACCELEROMETER:
-        case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED:
-        case SENSOR_TYPE_MAGNETIC_FIELD:
-        case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
-        case SENSOR_TYPE_GYROSCOPE:
-        case SENSOR_TYPE_PROXIMITY:
-        case SENSOR_TYPE_LIGHT:
+        if (sensor_types.find(buffer[i].type) != sensor_types.end()) {
           log_events++;
-          break;
-        default:
-          continue;
         }
       }
 
-      uint64_t log_time = nanos_since_boot();
-
       capnp::MallocMessageBuilder msg;
       cereal::Event::Builder event = msg.initRoot<cereal::Event>();
-      event.setLogMonoTime(log_time);
+      event.setLogMonoTime(nanos_since_boot());
 
       auto sensor_events = event.initSensorEvents(log_events);
 
@@ -135,21 +126,11 @@ void sensor_loop() {
 
         const sensors_event_t& data = buffer[i];
 
-        switch (data.type) {
-        case SENSOR_TYPE_ACCELEROMETER:
-        case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED:
-        case SENSOR_TYPE_MAGNETIC_FIELD:
-        case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
-        case SENSOR_TYPE_GYROSCOPE:
-        case SENSOR_TYPE_PROXIMITY:
-        case SENSOR_TYPE_LIGHT:
-          break;
-        default:
+        if (sensor_types.find(data.type) == sensor_types.end()) {
           continue;
         }
 
         auto log_event = sensor_events[log_i];
-
         log_event.setSource(cereal::SensorEventData::SensorSource::ANDROID);
         log_event.setVersion(data.version);
         log_event.setSensor(data.sensor);