sensord: move sensors in lowest power mode on exit/kill (#25787)

* add low power modes

* add sleep to lsm gyro init

* bmx055 gyro has a 30ms wakeup time from deep suspend

* Sensord skip init values, first 500ms (#25775)

* remove lsm gyro sleep, handled by general cut

Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com>

selfdrive/sensord/sensors/bmx055_accel.cc

@@ -4,6 +4,7 @@
 
 #include "common/swaglog.h"
 #include "common/timing.h"
+#include "common/util.h"
 
 BMX055_Accel::BMX055_Accel(I2CBus *bus) : I2CSensor(bus) {}
 
@@ -23,6 +24,13 @@ int BMX055_Accel::init() {
     goto fail;
   }
 
+  ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_NORMAL_MODE);
+  if (ret < 0) {
+    goto fail;
+  }
+  // bmx055 accel has a 1.3ms wakeup time from deep suspend mode
+  util::sleep_for(10);
+
   // High bandwidth
   // ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_ENABLE);
   // if (ret < 0) {
@@ -43,6 +51,16 @@ fail:
   return ret;
 }
 
+int BMX055_Accel::shutdown()  {
+  // enter deep suspend mode (lowest power mode)
+  int ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_DEEP_SUSPEND);
+  if (ret < 0) {
+    LOGE("Could not move BMX055 ACCEL in deep suspend mode!")
+  }
+
+  return ret;
+}
+
 bool BMX055_Accel::get_event(cereal::SensorEventData::Builder &event) {
   uint64_t start_time = nanos_since_boot();
   uint8_t buffer[6];

selfdrive/sensord/sensors/bmx055_accel.h

@@ -10,6 +10,7 @@
 #define BMX055_ACCEL_I2C_REG_X_LSB  0x02
 #define BMX055_ACCEL_I2C_REG_TEMP   0x08
 #define BMX055_ACCEL_I2C_REG_BW     0x10
+#define BMX055_ACCEL_I2C_REG_PMU    0x11
 #define BMX055_ACCEL_I2C_REG_HBW    0x13
 #define BMX055_ACCEL_I2C_REG_FIFO   0x3F
 
@@ -18,6 +19,8 @@
 
 #define BMX055_ACCEL_HBW_ENABLE       0b10000000
 #define BMX055_ACCEL_HBW_DISABLE      0b00000000
+#define BMX055_ACCEL_DEEP_SUSPEND     0b00100000
+#define BMX055_ACCEL_NORMAL_MODE      0b00000000
 
 #define BMX055_ACCEL_BW_7_81HZ  0b01000
 #define BMX055_ACCEL_BW_15_63HZ 0b01001
@@ -34,5 +37,5 @@ public:
   BMX055_Accel(I2CBus *bus);
   int init();
   bool get_event(cereal::SensorEventData::Builder &event);
-  int shutdown() { return 0; }
+  int shutdown();
 };

selfdrive/sensord/sensors/bmx055_gyro.cc

@@ -4,6 +4,7 @@
 #include <cmath>
 
 #include "common/swaglog.h"
+#include "common/util.h"
 
 #define DEG2RAD(x) ((x) * M_PI / 180.0)
 
@@ -26,6 +27,13 @@ int BMX055_Gyro::init() {
     goto fail;
   }
 
+  ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_NORMAL_MODE);
+  if (ret < 0) {
+    goto fail;
+  }
+  // bmx055 gyro has a 30ms wakeup time from deep suspend mode
+  util::sleep_for(50);
+
   // High bandwidth
   // ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_ENABLE);
   // if (ret < 0) {
@@ -54,6 +62,16 @@ fail:
   return ret;
 }
 
+int BMX055_Gyro::shutdown()  {
+  // enter deep suspend mode (lowest power mode)
+  int ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_DEEP_SUSPEND);
+  if (ret < 0) {
+    LOGE("Could not move BMX055 GYRO in deep suspend mode!")
+  }
+
+  return ret;
+}
+
 bool BMX055_Gyro::get_event(cereal::SensorEventData::Builder &event) {
   uint64_t start_time = nanos_since_boot();
   uint8_t buffer[6];

selfdrive/sensord/sensors/bmx055_gyro.h

@@ -10,6 +10,7 @@
 #define BMX055_GYRO_I2C_REG_RATE_X_LSB 0x02
 #define BMX055_GYRO_I2C_REG_RANGE      0x0F
 #define BMX055_GYRO_I2C_REG_BW         0x10
+#define BMX055_GYRO_I2C_REG_LPM1       0x11
 #define BMX055_GYRO_I2C_REG_HBW        0x13
 #define BMX055_GYRO_I2C_REG_FIFO       0x3F
 
@@ -18,6 +19,8 @@
 
 #define BMX055_GYRO_HBW_ENABLE       0b10000000
 #define BMX055_GYRO_HBW_DISABLE      0b00000000
+#define BMX055_GYRO_DEEP_SUSPEND     0b00100000
+#define BMX055_GYRO_NORMAL_MODE      0b00000000
 
 #define BMX055_GYRO_RANGE_2000      0b000
 #define BMX055_GYRO_RANGE_1000      0b001
@@ -34,5 +37,5 @@ public:
   BMX055_Gyro(I2CBus *bus);
   int init();
   bool get_event(cereal::SensorEventData::Builder &event);
-  int shutdown() { return 0; }
+  int shutdown();
 };

selfdrive/sensord/sensors/bmx055_magn.cc

@@ -155,6 +155,16 @@ int BMX055_Magn::init() {
   return ret;
 }
 
+int BMX055_Magn::shutdown() {
+  // move to suspend mode
+  int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0);
+  if (ret < 0) {
+    LOGE("Could not move BMX055 MAGN in suspend mode!")
+  }
+
+  return ret;
+}
+
 bool BMX055_Magn::perform_self_test() {
   uint8_t buffer[8];
   int16_t x, y;

selfdrive/sensord/sensors/bmx055_magn.h

@@ -60,5 +60,5 @@ public:
   BMX055_Magn(I2CBus *bus);
   int init();
   bool get_event(cereal::SensorEventData::Builder &event);
-  int shutdown() { return 0; }
+  int shutdown();
 };

selfdrive/sensord/sensors/lsm6ds3_accel.cc

@@ -71,12 +71,25 @@ int LSM6DS3_Accel::shutdown() {
   value &= ~(LSM6DS3_ACCEL_INT1_DRDY_XL);
   ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value);
   if (ret < 0) {
+    LOGE("Could not disable lsm6ds3 acceleration interrupt!")
+    goto fail;
+  }
+
+  // enable power-down mode
+  value = 0;
+  ret = read_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, &value, 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  value &= 0x0F;
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, value);
+  if (ret < 0) {
+    LOGE("Could not power-down lsm6ds3 accelerometer!")
     goto fail;
   }
-  return ret;
 
 fail:
-  LOGE("Could not disable lsm6ds3 acceleration interrupt!")
   return ret;
 }
 

selfdrive/sensord/sensors/lsm6ds3_gyro.cc

@@ -74,12 +74,25 @@ int LSM6DS3_Gyro::shutdown() {
   value &= ~(LSM6DS3_GYRO_INT1_DRDY_G);
   ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value);
   if (ret < 0) {
+    LOGE("Could not disable lsm6ds3 gyroscope interrupt!")
+    goto fail;
+  }
+
+  // enable power-down mode
+  value = 0;
+  ret = read_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, &value, 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  value &= 0x0F;
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, value);
+  if (ret < 0) {
+    LOGE("Could not power-down lsm6ds3 gyroscope!")
     goto fail;
   }
-  return ret;
 
 fail:
-  LOGE("Could not disable lsm6ds3 gyroscope interrupt!")
   return ret;
 }
 

selfdrive/sensord/sensors/mmc5603nj_magn.cc

@@ -51,6 +51,34 @@ fail:
   return ret;
 }
 
+int MMC5603NJ_Magn::shutdown() {
+  int ret = 0;
+
+  // disable auto reset of measurements
+  uint8_t value = 0;
+  ret = read_register(MMC5603NJ_I2C_REG_INTERNAL_0, &value, 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  value &= ~(MMC5603NJ_CMM_FREQ_EN | MMC5603NJ_AUTO_SR_EN);
+  ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_0, value);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // set ODR to 0 to leave continuous mode
+  ret = set_register(MMC5603NJ_I2C_REG_ODR, 0);
+  if (ret < 0) {
+    goto fail;
+  }
+  return ret;
+
+fail:
+  LOGE("Could not disable mmc5603nj auto set reset")
+  return ret;
+}
+
 bool MMC5603NJ_Magn::get_event(cereal::SensorEventData::Builder &event) {
 
   uint64_t start_time = nanos_since_boot();

selfdrive/sensord/sensors/mmc5603nj_magn.h

@@ -26,5 +26,5 @@ public:
   MMC5603NJ_Magn(I2CBus *bus);
   int init();
   bool get_event(cereal::SensorEventData::Builder &event);
-  int shutdown() { return 0; }
+  int shutdown();
 };

selfdrive/sensord/sensors_qcom2.cc

@@ -28,6 +28,10 @@
 ExitHandler do_exit;
 std::mutex pm_mutex;
 
+// filter first values (0.5sec) as those may contain inaccuracies
+uint64_t init_ts = 0;
+constexpr uint64_t init_delay = 500*1e6;
+
 void interrupt_loop(int fd, std::vector<Sensor *>& sensors, PubMaster& pm) {
   struct pollfd fd_list[1] = {0};
   fd_list[0].fd = fd;
@@ -88,6 +92,10 @@ void interrupt_loop(int fd, std::vector<Sensor *>& sensors, PubMaster& pm) {
       events.adoptWithCaveats(i, kj::mv(collected_events[i]));
     }
 
+    if (ts - init_ts < init_delay) {
+      continue;
+    }
+
     std::lock_guard<std::mutex> lock(pm_mutex);
     pm.send("sensorEvents", msg);
   }
@@ -167,6 +175,7 @@ int sensor_loop() {
   }
 
   PubMaster pm({"sensorEvents"});
+  init_ts = nanos_since_boot();
 
   // thread for reading events via interrupts
   std::vector<Sensor *> lsm_interrupt_sensors = {&lsm6ds3_accel, &lsm6ds3_gyro};
@@ -185,6 +194,10 @@ int sensor_loop() {
       sensors[i]->get_event(event);
     }
 
+    if (nanos_since_boot() - init_ts < init_delay) {
+      continue;
+    }
+
     {
       std::lock_guard<std::mutex> lock(pm_mutex);
       pm.send("sensorEvents", msg);
@@ -194,6 +207,10 @@ int sensor_loop() {
     std::this_thread::sleep_for(std::chrono::milliseconds(10) - (end - begin));
   }
 
+  for (Sensor *sensor :  sensors) {
+    sensor->shutdown();
+  }
+
   lsm_interrupt_thread.join();
   delete i2c_bus_imu;
   return 0;