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>
old-commit-hash: bcf31aea07
taco
Kurt Nistelberger 3 years ago committed by GitHub
parent c437f51224
commit 29d15e1f3b
  1. 18
      selfdrive/sensord/sensors/bmx055_accel.cc
  2. 5
      selfdrive/sensord/sensors/bmx055_accel.h
  3. 18
      selfdrive/sensord/sensors/bmx055_gyro.cc
  4. 5
      selfdrive/sensord/sensors/bmx055_gyro.h
  5. 10
      selfdrive/sensord/sensors/bmx055_magn.cc
  6. 2
      selfdrive/sensord/sensors/bmx055_magn.h
  7. 17
      selfdrive/sensord/sensors/lsm6ds3_accel.cc
  8. 17
      selfdrive/sensord/sensors/lsm6ds3_gyro.cc
  9. 28
      selfdrive/sensord/sensors/mmc5603nj_magn.cc
  10. 2
      selfdrive/sensord/sensors/mmc5603nj_magn.h
  11. 17
      selfdrive/sensord/sensors_qcom2.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];

@ -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();
};

@ -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];

@ -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();
};

@ -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;

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

@ -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;
}

@ -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;
}

@ -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();

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

@ -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;

Loading…
Cancel
Save