From bcf31aea07c26099aa861ab93c5586e645a25928 Mon Sep 17 00:00:00 2001 From: Kurt Nistelberger Date: Mon, 19 Sep 2022 21:01:32 -0700 Subject: [PATCH] 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 --- selfdrive/sensord/sensors/bmx055_accel.cc | 18 +++++++++++++ selfdrive/sensord/sensors/bmx055_accel.h | 5 +++- selfdrive/sensord/sensors/bmx055_gyro.cc | 18 +++++++++++++ selfdrive/sensord/sensors/bmx055_gyro.h | 5 +++- selfdrive/sensord/sensors/bmx055_magn.cc | 10 ++++++++ selfdrive/sensord/sensors/bmx055_magn.h | 2 +- selfdrive/sensord/sensors/lsm6ds3_accel.cc | 17 +++++++++++-- selfdrive/sensord/sensors/lsm6ds3_gyro.cc | 17 +++++++++++-- selfdrive/sensord/sensors/mmc5603nj_magn.cc | 28 +++++++++++++++++++++ selfdrive/sensord/sensors/mmc5603nj_magn.h | 2 +- selfdrive/sensord/sensors_qcom2.cc | 17 +++++++++++++ 11 files changed, 131 insertions(+), 8 deletions(-) diff --git a/selfdrive/sensord/sensors/bmx055_accel.cc b/selfdrive/sensord/sensors/bmx055_accel.cc index e191d0d72b..c6dcdbd7aa 100644 --- a/selfdrive/sensord/sensors/bmx055_accel.cc +++ b/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]; diff --git a/selfdrive/sensord/sensors/bmx055_accel.h b/selfdrive/sensord/sensors/bmx055_accel.h index 3b6dd536a7..6a0f9f1ada 100644 --- a/selfdrive/sensord/sensors/bmx055_accel.h +++ b/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(); }; diff --git a/selfdrive/sensord/sensors/bmx055_gyro.cc b/selfdrive/sensord/sensors/bmx055_gyro.cc index a7ed8debad..4deb15ec6d 100644 --- a/selfdrive/sensord/sensors/bmx055_gyro.cc +++ b/selfdrive/sensord/sensors/bmx055_gyro.cc @@ -4,6 +4,7 @@ #include #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]; diff --git a/selfdrive/sensord/sensors/bmx055_gyro.h b/selfdrive/sensord/sensors/bmx055_gyro.h index fea6c3e192..ac5dacc4a6 100644 --- a/selfdrive/sensord/sensors/bmx055_gyro.h +++ b/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(); }; diff --git a/selfdrive/sensord/sensors/bmx055_magn.cc b/selfdrive/sensord/sensors/bmx055_magn.cc index 9ba6cebd50..74e18b7c82 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.cc +++ b/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; diff --git a/selfdrive/sensord/sensors/bmx055_magn.h b/selfdrive/sensord/sensors/bmx055_magn.h index c762f2c3b9..0549e163f6 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.h +++ b/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(); }; diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.cc b/selfdrive/sensord/sensors/lsm6ds3_accel.cc index 8cc89e457c..513125fd59 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_accel.cc +++ b/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; } diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc index a7321e8fa8..fd0436a5f0 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc +++ b/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; } diff --git a/selfdrive/sensord/sensors/mmc5603nj_magn.cc b/selfdrive/sensord/sensors/mmc5603nj_magn.cc index 2bfd887a74..8af4956edf 100644 --- a/selfdrive/sensord/sensors/mmc5603nj_magn.cc +++ b/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(); diff --git a/selfdrive/sensord/sensors/mmc5603nj_magn.h b/selfdrive/sensord/sensors/mmc5603nj_magn.h index 857bd10a51..2c06cab96f 100644 --- a/selfdrive/sensord/sensors/mmc5603nj_magn.h +++ b/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(); }; diff --git a/selfdrive/sensord/sensors_qcom2.cc b/selfdrive/sensord/sensors_qcom2.cc index aaf79592c6..ded4b5c0b1 100644 --- a/selfdrive/sensord/sensors_qcom2.cc +++ b/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& 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& sensors, PubMaster& pm) { events.adoptWithCaveats(i, kj::mv(collected_events[i])); } + if (ts - init_ts < init_delay) { + continue; + } + std::lock_guard 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 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 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;