diff --git a/common/gpio.cc b/common/gpio.cc index 73ff1b3f52..9f5c211a4b 100644 --- a/common/gpio.cc +++ b/common/gpio.cc @@ -4,11 +4,11 @@ #include #include +#include +#include #include "common/util.h" - -// We assume that all pins have already been exported on boot, -// and that we have permission to write to them. +#include "common/swaglog.h" int gpio_init(int pin_nr, bool output) { char pin_dir_path[50]; @@ -30,3 +30,36 @@ int gpio_set(int pin_nr, bool high) { } return util::write_file(pin_val_path, (void*)(high ? "1" : "0"), 1); } + +int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int pin_nr) { + + // Assumed that all interrupt pins are unexported and rights are given to + // read from gpiochip0. + std::string gpiochip_path = "/dev/gpiochip" + std::to_string(gpiochiop_id); + int fd = open(gpiochip_path.c_str(), O_RDONLY); + if (fd < 0) { + LOGE("Error opening gpiochip0 fd") + return -1; + } + + // Setup event + struct gpioevent_request rq; + rq.lineoffset = pin_nr; + rq.handleflags = GPIOHANDLE_REQUEST_INPUT; + + /* Requesting both edges as the data ready pulse from the lsm6ds sensor is + very short(75us) and is mostly detected as falling edge instead of rising. + So if it is detected as rising the following falling edge is skipped. */ + rq.eventflags = GPIOEVENT_REQUEST_BOTH_EDGES; + + strncpy(rq.consumer_label, consumer_label, std::size(rq.consumer_label) - 1); + int ret = ioctl(fd, GPIO_GET_LINEEVENT_IOCTL, &rq); + if (ret == -1) { + LOGE("Unable to get line event from ioctl : %s", strerror(errno)); + close(fd); + return -1; + } + + close(fd); + return rq.fd; +} diff --git a/common/gpio.h b/common/gpio.h index e030019875..b2f67f8ba3 100644 --- a/common/gpio.h +++ b/common/gpio.h @@ -8,6 +8,11 @@ #define GPIO_UBLOX_PWR_EN 34 #define GPIO_STM_RST_N 124 #define GPIO_STM_BOOT0 134 + #define GPIO_BMX_ACCEL_INT 21 + #define GPIO_BMX_GYRO_INT 23 + #define GPIO_BMX_MAGN_INT 87 + #define GPIO_LSM_INT 84 + #define GPIOCHIP_INT 0 #else #define GPIO_HUB_RST_N 0 #define GPIO_UBLOX_RST_N 0 @@ -15,7 +20,14 @@ #define GPIO_UBLOX_PWR_EN 0 #define GPIO_STM_RST_N 0 #define GPIO_STM_BOOT0 0 + #define GPIO_BMX_ACCEL_INT 0 + #define GPIO_BMX_GYRO_INT 0 + #define GPIO_BMX_MAGN_INT 0 + #define GPIO_LSM_INT 0 + #define GPIOCHIP_INT 0 #endif int gpio_init(int pin_nr, bool output); int gpio_set(int pin_nr, bool high); + +int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int pin_nr); diff --git a/selfdrive/debug/sensor_data_to_hist.py b/selfdrive/debug/sensor_data_to_hist.py new file mode 100755 index 0000000000..3c30b3c17c --- /dev/null +++ b/selfdrive/debug/sensor_data_to_hist.py @@ -0,0 +1,121 @@ +#!/usr/bin/env python3 +''' +printing the gap between interrupts in a histogram to check if the +frequency is what we expect, the bmx is not interrupt driven for as we +get interrupts in a 2kHz rate. +''' + +import argparse +import sys +from collections import defaultdict + +from tools.lib.logreader import LogReader +from tools.lib.route import Route + +import matplotlib.pyplot as plt + +SRC_BMX = "bmx055" +SRC_LSM = "lsm6ds3" + + +def parseEvents(log_reader): + bmx_data = defaultdict(list) + lsm_data = defaultdict(list) + + for m in log_reader: + # only sensorEvents + if m.which() != 'sensorEvents': + continue + + for se in m.sensorEvents: + # convert data to dictionary + d = se.to_dict() + + if d["timestamp"] == 0: + continue # empty event? + + if d["source"] == SRC_BMX and "acceleration" in d: + bmx_data["accel"].append(d["timestamp"] / 1e9) + + if d["source"] == SRC_BMX and "gyroUncalibrated" in d: + bmx_data["gyro"].append(d["timestamp"] / 1e9) + + if d["source"] == SRC_LSM and "acceleration" in d: + lsm_data["accel"].append(d["timestamp"] / 1e9) + + if d["source"] == SRC_LSM and "gyroUncalibrated" in d: + lsm_data["gyro"].append(d["timestamp"] / 1e9) + + return bmx_data, lsm_data + + +def cleanData(data): + if len(data) == 0: + return [], [] + + data.sort() + prev = data[0] + diffs = [] + for v in data[1:]: + diffs.append(v - prev) + prev = v + return data, diffs + + +def logAvgValues(data, sensor): + if len(data) == 0: + print(f"{sensor}: no data to average") + return + + avg = sum(data) / len(data) + hz = 1 / avg + print(f"{sensor}: data_points: {len(data)} avg [ns]: {avg} avg [Hz]: {hz}") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("route", type=str, help="route name") + parser.add_argument("segment", type=int, help="segment number") + args = parser.parse_args() + + r = Route(args.route) + logs = r.log_paths() + + if len(logs) == 0: + print("NO data routes") + sys.exit(0) + + if args.segment >= len(logs): + print(f"RouteID: {args.segment} out of range, max: {len(logs) -1}") + sys.exit(0) + + lr = LogReader(logs[args.segment]) + bmx_data, lsm_data = parseEvents(lr) + + # sort bmx accel data, and then cal all the diffs, and to a histogram of those + bmx_accel, bmx_accel_diffs = cleanData(bmx_data["accel"]) + bmx_gyro, bmx_gyro_diffs = cleanData(bmx_data["gyro"]) + lsm_accel, lsm_accel_diffs = cleanData(lsm_data["accel"]) + lsm_gyro, lsm_gyro_diffs = cleanData(lsm_data["gyro"]) + + # get out the averages + logAvgValues(bmx_accel_diffs, "bmx accel") + logAvgValues(bmx_gyro_diffs, "bmx gyro ") + logAvgValues(lsm_accel_diffs, "lsm accel") + logAvgValues(lsm_gyro_diffs, "lsm gyro ") + + fig, axs = plt.subplots(1, 2, tight_layout=True) + axs[0].hist(bmx_accel_diffs, bins=50) + axs[0].set_title("bmx_accel") + axs[1].hist(bmx_gyro_diffs, bins=50) + axs[1].set_title("bmx_gyro") + + figl, axsl = plt.subplots(1, 2, tight_layout=True) + axsl[0].hist(lsm_accel_diffs, bins=50) + axsl[0].set_title("lsm_accel") + axsl[1].hist(lsm_gyro_diffs, bins=50) + axsl[1].set_title("lsm_gyro") + + print("check plot...") + plt.show() + diff --git a/selfdrive/sensord/SConscript b/selfdrive/sensord/SConscript index db32887e7f..8f26c00853 100644 --- a/selfdrive/sensord/SConscript +++ b/selfdrive/sensord/SConscript @@ -13,7 +13,7 @@ sensors = [ 'sensors/lsm6ds3_temp.cc', 'sensors/mmc5603nj_magn.cc', ] -libs = [common, cereal, messaging, 'capnp', 'zmq', 'kj'] +libs = [common, cereal, messaging, 'capnp', 'zmq', 'kj', 'pthread'] if arch == "larch64": libs.append('i2c') env.Program('_sensord', ['sensors_qcom2.cc'] + sensors, LIBS=libs) diff --git a/selfdrive/sensord/sensors/bmx055_accel.cc b/selfdrive/sensord/sensors/bmx055_accel.cc index d17e3fe617..e191d0d72b 100644 --- a/selfdrive/sensord/sensors/bmx055_accel.cc +++ b/selfdrive/sensord/sensors/bmx055_accel.cc @@ -43,7 +43,7 @@ fail: return ret; } -void BMX055_Accel::get_event(cereal::SensorEventData::Builder &event) { +bool BMX055_Accel::get_event(cereal::SensorEventData::Builder &event) { uint64_t start_time = nanos_since_boot(); uint8_t buffer[6]; int len = read_register(BMX055_ACCEL_I2C_REG_X_LSB, buffer, sizeof(buffer)); @@ -66,4 +66,5 @@ void BMX055_Accel::get_event(cereal::SensorEventData::Builder &event) { svec.setV(xyz); svec.setStatus(true); + return true; } diff --git a/selfdrive/sensord/sensors/bmx055_accel.h b/selfdrive/sensord/sensors/bmx055_accel.h index 86ec419cde..3b6dd536a7 100644 --- a/selfdrive/sensord/sensors/bmx055_accel.h +++ b/selfdrive/sensord/sensors/bmx055_accel.h @@ -33,5 +33,6 @@ class BMX055_Accel : public I2CSensor { public: BMX055_Accel(I2CBus *bus); int init(); - void get_event(cereal::SensorEventData::Builder &event); + bool get_event(cereal::SensorEventData::Builder &event); + int shutdown() { return 0; } }; diff --git a/selfdrive/sensord/sensors/bmx055_gyro.cc b/selfdrive/sensord/sensors/bmx055_gyro.cc index 74b22d8fef..a7ed8debad 100644 --- a/selfdrive/sensord/sensors/bmx055_gyro.cc +++ b/selfdrive/sensord/sensors/bmx055_gyro.cc @@ -54,7 +54,7 @@ fail: return ret; } -void BMX055_Gyro::get_event(cereal::SensorEventData::Builder &event) { +bool BMX055_Gyro::get_event(cereal::SensorEventData::Builder &event) { uint64_t start_time = nanos_since_boot(); uint8_t buffer[6]; int len = read_register(BMX055_GYRO_I2C_REG_RATE_X_LSB, buffer, sizeof(buffer)); @@ -76,5 +76,5 @@ void BMX055_Gyro::get_event(cereal::SensorEventData::Builder &event) { auto svec = event.initGyroUncalibrated(); svec.setV(xyz); svec.setStatus(true); - + return true; } diff --git a/selfdrive/sensord/sensors/bmx055_gyro.h b/selfdrive/sensord/sensors/bmx055_gyro.h index ed0c16ff05..fea6c3e192 100644 --- a/selfdrive/sensord/sensors/bmx055_gyro.h +++ b/selfdrive/sensord/sensors/bmx055_gyro.h @@ -33,5 +33,6 @@ class BMX055_Gyro : public I2CSensor { public: BMX055_Gyro(I2CBus *bus); int init(); - void get_event(cereal::SensorEventData::Builder &event); + bool get_event(cereal::SensorEventData::Builder &event); + int shutdown() { return 0; } }; diff --git a/selfdrive/sensord/sensors/bmx055_magn.cc b/selfdrive/sensord/sensors/bmx055_magn.cc index a2c793eff6..9ba6cebd50 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.cc +++ b/selfdrive/sensord/sensors/bmx055_magn.cc @@ -213,7 +213,7 @@ bool BMX055_Magn::parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t * } -void BMX055_Magn::get_event(cereal::SensorEventData::Builder &event) { +bool BMX055_Magn::get_event(cereal::SensorEventData::Builder &event) { uint64_t start_time = nanos_since_boot(); uint8_t buffer[8]; int16_t _x, _y, x, y, z; @@ -221,7 +221,8 @@ void BMX055_Magn::get_event(cereal::SensorEventData::Builder &event) { int len = read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer)); assert(len == sizeof(buffer)); - if (parse_xyz(buffer, &_x, &_y, &z)) { + bool parsed = parse_xyz(buffer, &_x, &_y, &z); + if (parsed) { event.setSource(cereal::SensorEventData::SensorSource::BMX055); event.setVersion(2); event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED); @@ -247,4 +248,6 @@ void BMX055_Magn::get_event(cereal::SensorEventData::Builder &event) { // at a 100 Hz. When reading the registers we have to check the ready bit // To verify the measurement was completed this cycle. set_register(BMX055_MAGN_I2C_REG_MAG, BMX055_MAGN_FORCED); + + return parsed; } diff --git a/selfdrive/sensord/sensors/bmx055_magn.h b/selfdrive/sensord/sensors/bmx055_magn.h index d60fd5515c..c762f2c3b9 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.h +++ b/selfdrive/sensord/sensors/bmx055_magn.h @@ -59,5 +59,6 @@ class BMX055_Magn : public I2CSensor{ public: BMX055_Magn(I2CBus *bus); int init(); - void get_event(cereal::SensorEventData::Builder &event); + bool get_event(cereal::SensorEventData::Builder &event); + int shutdown() { return 0; } }; diff --git a/selfdrive/sensord/sensors/bmx055_temp.cc b/selfdrive/sensord/sensors/bmx055_temp.cc index 85bdea9e61..3cee34ef19 100644 --- a/selfdrive/sensord/sensors/bmx055_temp.cc +++ b/selfdrive/sensord/sensors/bmx055_temp.cc @@ -28,7 +28,7 @@ fail: return ret; } -void BMX055_Temp::get_event(cereal::SensorEventData::Builder &event) { +bool BMX055_Temp::get_event(cereal::SensorEventData::Builder &event) { uint64_t start_time = nanos_since_boot(); uint8_t buffer[1]; int len = read_register(BMX055_ACCEL_I2C_REG_TEMP, buffer, sizeof(buffer)); @@ -41,4 +41,5 @@ void BMX055_Temp::get_event(cereal::SensorEventData::Builder &event) { event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE); event.setTimestamp(start_time); event.setTemperature(temp); + return true; } diff --git a/selfdrive/sensord/sensors/bmx055_temp.h b/selfdrive/sensord/sensors/bmx055_temp.h index 5ffaa8fb6b..f5d771a29c 100644 --- a/selfdrive/sensord/sensors/bmx055_temp.h +++ b/selfdrive/sensord/sensors/bmx055_temp.h @@ -8,5 +8,6 @@ class BMX055_Temp : public I2CSensor { public: BMX055_Temp(I2CBus *bus); int init(); - void get_event(cereal::SensorEventData::Builder &event); + bool get_event(cereal::SensorEventData::Builder &event); + int shutdown() { return 0; } }; diff --git a/selfdrive/sensord/sensors/file_sensor.cc b/selfdrive/sensord/sensors/file_sensor.cc index 812a41fa8a..80e5121564 100644 --- a/selfdrive/sensord/sensors/file_sensor.cc +++ b/selfdrive/sensord/sensors/file_sensor.cc @@ -12,3 +12,7 @@ int FileSensor::init() { FileSensor::~FileSensor() { file.close(); } + +bool FileSensor::has_interrupt_enabled() { + return false; +} \ No newline at end of file diff --git a/selfdrive/sensord/sensors/file_sensor.h b/selfdrive/sensord/sensors/file_sensor.h index c5b4643e16..5bcaee66a8 100644 --- a/selfdrive/sensord/sensors/file_sensor.h +++ b/selfdrive/sensord/sensors/file_sensor.h @@ -14,5 +14,6 @@ public: FileSensor(std::string filename); ~FileSensor(); int init(); - virtual void get_event(cereal::SensorEventData::Builder &event) = 0; + bool has_interrupt_enabled(); + virtual bool get_event(cereal::SensorEventData::Builder &event) = 0; }; diff --git a/selfdrive/sensord/sensors/i2c_sensor.cc b/selfdrive/sensord/sensors/i2c_sensor.cc index 40dfa4a736..06a216478b 100644 --- a/selfdrive/sensord/sensors/i2c_sensor.cc +++ b/selfdrive/sensord/sensors/i2c_sensor.cc @@ -15,8 +15,12 @@ int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0) { return int32_t(combined) / (1 << 4); } +I2CSensor::I2CSensor(I2CBus *bus, int gpio_nr, bool shared_gpio) : bus(bus), gpio_nr(gpio_nr), shared_gpio(shared_gpio) {} -I2CSensor::I2CSensor(I2CBus *bus) : bus(bus) { +I2CSensor::~I2CSensor() { + if (gpio_fd != -1) { + close(gpio_fd); + } } int I2CSensor::read_register(uint register_address, uint8_t *buffer, uint8_t len) { @@ -26,3 +30,20 @@ int I2CSensor::read_register(uint register_address, uint8_t *buffer, uint8_t len int I2CSensor::set_register(uint register_address, uint8_t data) { return bus->set_register(get_device_address(), register_address, data); } + +int I2CSensor::init_gpio() { + if (shared_gpio || gpio_nr == 0) { + return 0; + } + + gpio_fd = gpiochip_get_ro_value_fd("sensord", GPIOCHIP_INT, gpio_nr); + if (gpio_fd < 0) { + return -1; + } + + return 0; +} + +bool I2CSensor::has_interrupt_enabled() { + return gpio_nr != 0; +} diff --git a/selfdrive/sensord/sensors/i2c_sensor.h b/selfdrive/sensord/sensors/i2c_sensor.h index 7832475a98..f6820a2471 100644 --- a/selfdrive/sensord/sensors/i2c_sensor.h +++ b/selfdrive/sensord/sensors/i2c_sensor.h @@ -1,9 +1,13 @@ #pragma once #include +#include #include "cereal/gen/cpp/log.capnp.h" + #include "common/i2c.h" +#include "common/gpio.h" + #include "selfdrive/sensord/sensors/constants.h" #include "selfdrive/sensord/sensors/sensor.h" @@ -15,12 +19,18 @@ int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0); class I2CSensor : public Sensor { private: I2CBus *bus; + int gpio_nr; + bool shared_gpio; virtual uint8_t get_device_address() = 0; public: - I2CSensor(I2CBus *bus); + I2CSensor(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false); + ~I2CSensor(); int read_register(uint register_address, uint8_t *buffer, uint8_t len); int set_register(uint register_address, uint8_t data); + int init_gpio(); + bool has_interrupt_enabled(); virtual int init() = 0; - virtual void get_event(cereal::SensorEventData::Builder &event) = 0; + virtual bool get_event(cereal::SensorEventData::Builder &event) = 0; + virtual int shutdown() = 0; }; diff --git a/selfdrive/sensord/sensors/light_sensor.cc b/selfdrive/sensord/sensors/light_sensor.cc index 4497343684..321ac75c7e 100644 --- a/selfdrive/sensord/sensors/light_sensor.cc +++ b/selfdrive/sensord/sensors/light_sensor.cc @@ -5,7 +5,7 @@ #include "common/timing.h" #include "selfdrive/sensord/sensors/constants.h" -void LightSensor::get_event(cereal::SensorEventData::Builder &event) { +bool LightSensor::get_event(cereal::SensorEventData::Builder &event) { uint64_t start_time = nanos_since_boot(); file.clear(); file.seekg(0); @@ -19,4 +19,6 @@ void LightSensor::get_event(cereal::SensorEventData::Builder &event) { event.setType(SENSOR_TYPE_LIGHT); event.setTimestamp(start_time); event.setLight(value); + + return true; } diff --git a/selfdrive/sensord/sensors/light_sensor.h b/selfdrive/sensord/sensors/light_sensor.h index faf901d41c..63bda74755 100644 --- a/selfdrive/sensord/sensors/light_sensor.h +++ b/selfdrive/sensord/sensors/light_sensor.h @@ -4,5 +4,6 @@ class LightSensor : public FileSensor { public: LightSensor(std::string filename) : FileSensor(filename){}; - void get_event(cereal::SensorEventData::Builder &event); + bool get_event(cereal::SensorEventData::Builder &event); + int shutdown() { return 0; } }; diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.cc b/selfdrive/sensord/sensors/lsm6ds3_accel.cc index d923986dbe..8cc89e457c 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_accel.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_accel.cc @@ -5,11 +5,12 @@ #include "common/swaglog.h" #include "common/timing.h" -LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus) : I2CSensor(bus) {} +LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus, int gpio_nr, bool shared_gpio) : I2CSensor(bus, gpio_nr, shared_gpio) {} int LSM6DS3_Accel::init() { int ret = 0; uint8_t buffer[1]; + uint8_t value = 0; ret = read_register(LSM6DS3_ACCEL_I2C_REG_ID, buffer, 1); if(ret < 0) { @@ -27,20 +28,69 @@ int LSM6DS3_Accel::init() { source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; } + ret = init_gpio(); + if (ret < 0) { + goto fail; + } + // TODO: set scale and bandwidth. Default is +- 2G, 50 Hz ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, LSM6DS3_ACCEL_ODR_104HZ); if (ret < 0) { goto fail; } + ret = set_register(LSM6DS3_ACCEL_I2C_REG_DRDY_CFG, LSM6DS3_ACCEL_DRDY_PULSE_MODE); + if (ret < 0) { + goto fail; + } + + // enable data ready interrupt for accel on INT1 + // (without resetting existing interrupts) + ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1); + if (ret < 0) { + goto fail; + } + + value |= LSM6DS3_ACCEL_INT1_DRDY_XL; + ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value); fail: return ret; } -void LSM6DS3_Accel::get_event(cereal::SensorEventData::Builder &event) { +int LSM6DS3_Accel::shutdown() { + int ret = 0; + + // disable data ready interrupt for accel on INT1 + uint8_t value = 0; + ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1); + if (ret < 0) { + goto fail; + } + + value &= ~(LSM6DS3_ACCEL_INT1_DRDY_XL); + ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value); + if (ret < 0) { + goto fail; + } + return ret; + +fail: + LOGE("Could not disable lsm6ds3 acceleration interrupt!") + return ret; +} + +bool LSM6DS3_Accel::get_event(cereal::SensorEventData::Builder &event) { + + if (has_interrupt_enabled()) { + // INT1 shared with gyro, check STATUS_REG who triggered + uint8_t status_reg = 0; + read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg)); + if ((status_reg & LSM6DS3_ACCEL_DRDY_XLDA) == 0) { + return false; + } + } - uint64_t start_time = nanos_since_boot(); uint8_t buffer[6]; int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer)); assert(len == sizeof(buffer)); @@ -54,11 +104,11 @@ void LSM6DS3_Accel::get_event(cereal::SensorEventData::Builder &event) { event.setVersion(1); event.setSensor(SENSOR_ACCELEROMETER); event.setType(SENSOR_TYPE_ACCELEROMETER); - event.setTimestamp(start_time); float xyz[] = {y, -x, z}; auto svec = event.initAcceleration(); svec.setV(xyz); svec.setStatus(true); + return true; } diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.h b/selfdrive/sensord/sensors/lsm6ds3_accel.h index 4a6b687445..6ed94a8f12 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_accel.h +++ b/selfdrive/sensord/sensors/lsm6ds3_accel.h @@ -6,21 +6,28 @@ #define LSM6DS3_ACCEL_I2C_ADDR 0x6A // Registers of the chip +#define LSM6DS3_ACCEL_I2C_REG_DRDY_CFG 0x0B #define LSM6DS3_ACCEL_I2C_REG_ID 0x0F +#define LSM6DS3_ACCEL_I2C_REG_INT1_CTRL 0x0D #define LSM6DS3_ACCEL_I2C_REG_CTRL1_XL 0x10 +#define LSM6DS3_ACCEL_I2C_REG_STAT_REG 0x1E #define LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL 0x28 // Constants -#define LSM6DS3_ACCEL_CHIP_ID 0x69 -#define LSM6DS3TRC_ACCEL_CHIP_ID 0x6A -#define LSM6DS3_ACCEL_ODR_104HZ (0b0100 << 4) +#define LSM6DS3_ACCEL_CHIP_ID 0x69 +#define LSM6DS3TRC_ACCEL_CHIP_ID 0x6A +#define LSM6DS3_ACCEL_ODR_104HZ (0b0100 << 4) +#define LSM6DS3_ACCEL_INT1_DRDY_XL 0b1 +#define LSM6DS3_ACCEL_DRDY_XLDA 0b1 +#define LSM6DS3_ACCEL_DRDY_PULSE_MODE (1 << 7) class LSM6DS3_Accel : public I2CSensor { uint8_t get_device_address() {return LSM6DS3_ACCEL_I2C_ADDR;} cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; public: - LSM6DS3_Accel(I2CBus *bus); + LSM6DS3_Accel(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false); int init(); - void get_event(cereal::SensorEventData::Builder &event); + bool get_event(cereal::SensorEventData::Builder &event); + int shutdown(); }; diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc index c7711d34e3..a7321e8fa8 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc @@ -8,12 +8,12 @@ #define DEG2RAD(x) ((x) * M_PI / 180.0) - -LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus) : I2CSensor(bus) {} +LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus, int gpio_nr, bool shared_gpio) : I2CSensor(bus, gpio_nr, shared_gpio) {} int LSM6DS3_Gyro::init() { int ret = 0; uint8_t buffer[1]; + uint8_t value = 0; ret = read_register(LSM6DS3_GYRO_I2C_REG_ID, buffer, 1); if(ret < 0) { @@ -31,20 +31,69 @@ int LSM6DS3_Gyro::init() { source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; } + ret = init_gpio(); + if (ret < 0) { + goto fail; + } + // TODO: set scale. Default is +- 250 deg/s ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_104HZ); if (ret < 0) { goto fail; } + ret = set_register(LSM6DS3_GYRO_I2C_REG_DRDY_CFG, LSM6DS3_GYRO_DRDY_PULSE_MODE); + if (ret < 0) { + goto fail; + } + + // enable data ready interrupt for gyro on INT1 + // (without resetting existing interrupts) + ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1); + if (ret < 0) { + goto fail; + } + + value |= LSM6DS3_GYRO_INT1_DRDY_G; + ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value); + +fail: + return ret; +} + +int LSM6DS3_Gyro::shutdown() { + int ret = 0; + + // disable data ready interrupt for accel on INT1 + uint8_t value = 0; + ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1); + if (ret < 0) { + goto fail; + } + + value &= ~(LSM6DS3_GYRO_INT1_DRDY_G); + ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value); + if (ret < 0) { + goto fail; + } + return ret; fail: + LOGE("Could not disable lsm6ds3 gyroscope interrupt!") return ret; } -void LSM6DS3_Gyro::get_event(cereal::SensorEventData::Builder &event) { +bool LSM6DS3_Gyro::get_event(cereal::SensorEventData::Builder &event) { + + if (has_interrupt_enabled()) { + // INT1 shared with accel, check STATUS_REG who triggered + uint8_t status_reg = 0; + read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg)); + if ((status_reg & LSM6DS3_GYRO_DRDY_GDA) == 0) { + return false; + } + } - uint64_t start_time = nanos_since_boot(); uint8_t buffer[6]; int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer)); assert(len == sizeof(buffer)); @@ -58,11 +107,11 @@ void LSM6DS3_Gyro::get_event(cereal::SensorEventData::Builder &event) { event.setVersion(2); event.setSensor(SENSOR_GYRO_UNCALIBRATED); event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED); - event.setTimestamp(start_time); float xyz[] = {y, -x, z}; auto svec = event.initGyroUncalibrated(); svec.setV(xyz); svec.setStatus(true); + return true; } diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.h b/selfdrive/sensord/sensors/lsm6ds3_gyro.h index d7e8f0025a..c2ed5ab76e 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_gyro.h +++ b/selfdrive/sensord/sensors/lsm6ds3_gyro.h @@ -6,21 +6,28 @@ #define LSM6DS3_GYRO_I2C_ADDR 0x6A // Registers of the chip +#define LSM6DS3_GYRO_I2C_REG_DRDY_CFG 0x0B #define LSM6DS3_GYRO_I2C_REG_ID 0x0F +#define LSM6DS3_GYRO_I2C_REG_INT1_CTRL 0x0D #define LSM6DS3_GYRO_I2C_REG_CTRL2_G 0x11 +#define LSM6DS3_GYRO_I2C_REG_STAT_REG 0x1E #define LSM6DS3_GYRO_I2C_REG_OUTX_L_G 0x22 // Constants -#define LSM6DS3_GYRO_CHIP_ID 0x69 -#define LSM6DS3TRC_GYRO_CHIP_ID 0x6A -#define LSM6DS3_GYRO_ODR_104HZ (0b0100 << 4) +#define LSM6DS3_GYRO_CHIP_ID 0x69 +#define LSM6DS3TRC_GYRO_CHIP_ID 0x6A +#define LSM6DS3_GYRO_ODR_104HZ (0b0100 << 4) +#define LSM6DS3_GYRO_INT1_DRDY_G 0b10 +#define LSM6DS3_GYRO_DRDY_GDA 0b10 +#define LSM6DS3_GYRO_DRDY_PULSE_MODE (1 << 7) class LSM6DS3_Gyro : public I2CSensor { uint8_t get_device_address() {return LSM6DS3_GYRO_I2C_ADDR;} cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; public: - LSM6DS3_Gyro(I2CBus *bus); + LSM6DS3_Gyro(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false); int init(); - void get_event(cereal::SensorEventData::Builder &event); + bool get_event(cereal::SensorEventData::Builder &event); + int shutdown(); }; diff --git a/selfdrive/sensord/sensors/lsm6ds3_temp.cc b/selfdrive/sensord/sensors/lsm6ds3_temp.cc index 1dd179d69e..082ffee08f 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_temp.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_temp.cc @@ -31,7 +31,7 @@ fail: return ret; } -void LSM6DS3_Temp::get_event(cereal::SensorEventData::Builder &event) { +bool LSM6DS3_Temp::get_event(cereal::SensorEventData::Builder &event) { uint64_t start_time = nanos_since_boot(); uint8_t buffer[2]; @@ -47,4 +47,5 @@ void LSM6DS3_Temp::get_event(cereal::SensorEventData::Builder &event) { event.setTimestamp(start_time); event.setTemperature(temp); + return true; } diff --git a/selfdrive/sensord/sensors/lsm6ds3_temp.h b/selfdrive/sensord/sensors/lsm6ds3_temp.h index 8188f46700..9d95236901 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_temp.h +++ b/selfdrive/sensord/sensors/lsm6ds3_temp.h @@ -21,5 +21,6 @@ class LSM6DS3_Temp : public I2CSensor { public: LSM6DS3_Temp(I2CBus *bus); int init(); - void get_event(cereal::SensorEventData::Builder &event); + bool get_event(cereal::SensorEventData::Builder &event); + int shutdown() { return 0; } }; diff --git a/selfdrive/sensord/sensors/mmc5603nj_magn.cc b/selfdrive/sensord/sensors/mmc5603nj_magn.cc index 7c654ce7a3..2bfd887a74 100644 --- a/selfdrive/sensord/sensors/mmc5603nj_magn.cc +++ b/selfdrive/sensord/sensors/mmc5603nj_magn.cc @@ -51,7 +51,7 @@ fail: return ret; } -void MMC5603NJ_Magn::get_event(cereal::SensorEventData::Builder &event) { +bool MMC5603NJ_Magn::get_event(cereal::SensorEventData::Builder &event) { uint64_t start_time = nanos_since_boot(); uint8_t buffer[9]; @@ -74,4 +74,5 @@ void MMC5603NJ_Magn::get_event(cereal::SensorEventData::Builder &event) { svec.setV(xyz); svec.setStatus(true); + return true; } diff --git a/selfdrive/sensord/sensors/mmc5603nj_magn.h b/selfdrive/sensord/sensors/mmc5603nj_magn.h index 58840bbf27..857bd10a51 100644 --- a/selfdrive/sensord/sensors/mmc5603nj_magn.h +++ b/selfdrive/sensord/sensors/mmc5603nj_magn.h @@ -25,5 +25,6 @@ class MMC5603NJ_Magn : public I2CSensor { public: MMC5603NJ_Magn(I2CBus *bus); int init(); - void get_event(cereal::SensorEventData::Builder &event); + bool get_event(cereal::SensorEventData::Builder &event); + int shutdown() { return 0; } }; diff --git a/selfdrive/sensord/sensors/sensor.h b/selfdrive/sensord/sensors/sensor.h index 3fb58ad2ae..0bdc560275 100644 --- a/selfdrive/sensord/sensors/sensor.h +++ b/selfdrive/sensord/sensors/sensor.h @@ -4,7 +4,10 @@ class Sensor { public: + int gpio_fd = -1; virtual ~Sensor() {}; virtual int init() = 0; - virtual void get_event(cereal::SensorEventData::Builder &event) = 0; + virtual bool get_event(cereal::SensorEventData::Builder &event) = 0; + virtual bool has_interrupt_enabled() = 0; + virtual int shutdown() = 0; }; diff --git a/selfdrive/sensord/sensors_qcom2.cc b/selfdrive/sensord/sensors_qcom2.cc index 65fe43f65f..a9d6e31d3e 100644 --- a/selfdrive/sensord/sensors_qcom2.cc +++ b/selfdrive/sensord/sensors_qcom2.cc @@ -3,6 +3,8 @@ #include #include #include +#include +#include #include "cereal/messaging/messaging.h" #include "common/i2c.h" @@ -24,6 +26,79 @@ #define I2C_BUS_IMU 1 ExitHandler do_exit; +std::mutex pm_mutex; + +void interrupt_loop(int fd, std::vector& sensors, PubMaster& pm) { + struct pollfd fd_list[1] = {0}; + fd_list[0].fd = fd; + fd_list[0].events = POLLIN | POLLPRI; + + uint64_t offset = nanos_since_epoch() - nanos_since_boot(); + + while (!do_exit) { + int err = poll(fd_list, 1, 100); + if (err == -1) { + if (errno == EINTR) { + continue; + } + return; + } else if (err == 0) { + LOGE("poll timed out"); + continue; + } + + if ((fd_list[0].revents & (POLLIN | POLLPRI)) == 0) { + LOGE("no poll events set"); + continue; + } + + // Read all events + struct gpioevent_data evdata[16]; + err = read(fd, evdata, sizeof(evdata)); + if (err < 0 || err % sizeof(*evdata) != 0) { + LOGE("error reading event data %d", err); + continue; + } + + int num_events = err / sizeof(*evdata); + uint64_t ts = evdata[num_events - 1].timestamp - offset; + + MessageBuilder msg; + auto orphanage = msg.getOrphanage(); + std::vector> collected_events; + collected_events.reserve(sensors.size()); + + for (Sensor *sensor : sensors) { + auto orphan = orphanage.newOrphan(); + auto event = orphan.get(); + if (!sensor->get_event(event)) { + continue; + } + + event.setTimestamp(ts); + collected_events.push_back(kj::mv(orphan)); + } + + if (collected_events.size() == 0) { + continue; + } + + auto events = msg.initEvent().initSensorEvents(collected_events.size()); + for (int i = 0; i < collected_events.size(); ++i) { + events.adoptWithCaveats(i, kj::mv(collected_events[i])); + } + + { + std::lock_guard lock(pm_mutex); + pm.send("sensorEvents", msg); + } + } + + // poweroff sensors, disable interrupts + for (Sensor *sensor : sensors) { + sensor->shutdown(); + } +} int sensor_loop() { I2CBus *i2c_bus_imu; @@ -40,8 +115,8 @@ int sensor_loop() { BMX055_Magn bmx055_magn(i2c_bus_imu); BMX055_Temp bmx055_temp(i2c_bus_imu); - LSM6DS3_Accel lsm6ds3_accel(i2c_bus_imu); - LSM6DS3_Gyro lsm6ds3_gyro(i2c_bus_imu); + LSM6DS3_Accel lsm6ds3_accel(i2c_bus_imu, GPIO_LSM_INT); + LSM6DS3_Gyro lsm6ds3_gyro(i2c_bus_imu, GPIO_LSM_INT, true); // GPIO shared with accel LSM6DS3_Temp lsm6ds3_temp(i2c_bus_imu); MMC5603NJ_Magn mmc5603nj_magn(i2c_bus_imu); @@ -73,23 +148,33 @@ int sensor_loop() { // Fail on required sensors if (sensor.second) { LOGE("Error initializing sensors"); + delete i2c_bus_imu; return -1; } } else { if (sensor.first == &bmx055_magn || sensor.first == &mmc5603nj_magn) { has_magnetometer = true; } - sensors.push_back(sensor.first); + + if (!sensor.first->has_interrupt_enabled()) { + sensors.push_back(sensor.first); + } } } if (!has_magnetometer) { LOGE("No magnetometer present"); + delete i2c_bus_imu; return -1; } PubMaster pm({"sensorEvents"}); + // thread for reading events via interrupts + std::vector lsm_interrupt_sensors = {&lsm6ds3_accel, &lsm6ds3_gyro}; + std::thread lsm_interrupt_thread(&interrupt_loop, lsm6ds3_accel.gpio_fd, std::ref(lsm_interrupt_sensors), std::ref(pm)); + + // polling loop for non interrupt handled sensors while (!do_exit) { std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now(); @@ -102,11 +187,17 @@ int sensor_loop() { sensors[i]->get_event(event); } - pm.send("sensorEvents", msg); + { + std::lock_guard lock(pm_mutex); + pm.send("sensorEvents", msg); + } std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); std::this_thread::sleep_for(std::chrono::milliseconds(10) - (end - begin)); } + + lsm_interrupt_thread.join(); + delete i2c_bus_imu; return 0; } diff --git a/selfdrive/sensord/tests/test_sensord.py b/selfdrive/sensord/tests/test_sensord.py old mode 100755 new mode 100644 index 9fd918c971..7abaf9ca35 --- a/selfdrive/sensord/tests/test_sensord.py +++ b/selfdrive/sensord/tests/test_sensord.py @@ -2,12 +2,15 @@ import time import unittest +import numpy as np +from collections import namedtuple +from smbus2 import SMBus import cereal.messaging as messaging +from cereal import log from system.hardware import TICI from selfdrive.test.helpers import with_processes - -TEST_TIMESPAN = 10 +from selfdrive.manager.process_config import managed_processes SENSOR_CONFIGURATIONS = ( { @@ -46,6 +49,63 @@ SENSOR_CONFIGURATIONS = ( }, ) +Sensor = log.SensorEventData.SensorSource +SensorConfig = namedtuple('SensorConfig', ['type', 'min_samples', 'sanity_min', 'sanity_max']) +ALL_SENSORS = { + Sensor.rpr0521: { + SensorConfig("light", 100, 0, 150), + }, + + Sensor.lsm6ds3: { + SensorConfig("acceleration", 100, 5, 15), + SensorConfig("gyroUncalibrated", 100, 0, .2), + SensorConfig("temperature", 100, 0, 60), + }, + + Sensor.lsm6ds3trc: { + SensorConfig("acceleration", 100, 5, 15), + SensorConfig("gyroUncalibrated", 100, 0, .2), + SensorConfig("temperature", 100, 0, 60), + }, + + Sensor.bmx055: { + SensorConfig("acceleration", 100, 5, 15), + SensorConfig("gyroUncalibrated", 100, 0, .2), + SensorConfig("magneticUncalibrated", 100, 0, 300), + SensorConfig("temperature", 100, 0, 60), + }, + + Sensor.mmc5603nj: { + SensorConfig("magneticUncalibrated", 100, 0, 300), + } +} + +SENSOR_BUS = 1 +I2C_ADDR_LSM = 0x6A +LSM_INT_GPIO = 84 + +def read_sensor_events(duration_sec): + sensor_events = messaging.sub_sock("sensorEvents", timeout=0.1) + start_time_sec = time.monotonic() + events = [] + while time.monotonic() - start_time_sec < duration_sec: + events += messaging.drain_sock(sensor_events) + time.sleep(0.01) + + assert len(events) != 0, "No sensor events collected" + return events + +def get_proc_interrupts(int_pin): + + with open("/proc/interrupts") as f: + lines = f.read().split("\n") + + for line in lines: + if f" {int_pin} " in line: + return ''.join(list(filter(lambda e: e != '', line.split(' ')))[1:6]) + + return "" + class TestSensord(unittest.TestCase): @classmethod @@ -55,24 +115,175 @@ class TestSensord(unittest.TestCase): @with_processes(['sensord']) def test_sensors_present(self): - sensor_events = messaging.sub_sock("sensorEvents", timeout=0.1) - - start_time_sec = time.time() - events = [] - while time.time() - start_time_sec < TEST_TIMESPAN: - events += messaging.drain_sock(sensor_events) - time.sleep(0.01) + # verify correct sensors configuration + events = read_sensor_events(10) seen = set() for event in events: for measurement in event.sensorEvents: - # Filter out unset events + # filter unset events (bmx magn) if measurement.version == 0: continue seen.add((str(measurement.source), measurement.which())) self.assertIn(seen, SENSOR_CONFIGURATIONS) + @with_processes(['sensord']) + def test_lsm6ds3_100Hz(self): + # verify measurements are sampled and published at a 100Hz rate + events = read_sensor_events(3) # 3sec (about 300 measurements) + + data_points = set() + for event in events: + for measurement in event.sensorEvents: + + # skip lsm6ds3 temperature measurements + if measurement.which() == 'temperature': + continue + + if str(measurement.source).startswith("lsm6ds3"): + data_points.add(measurement.timestamp) + + assert len(data_points) != 0, "No lsm6ds3 sensor events" + + data_list = list(data_points) + data_list.sort() + tdiffs = np.diff(data_list) + + high_delay_diffs = set(filter(lambda d: d >= 10.1*10**6, tdiffs)) + assert len(high_delay_diffs) < 10, f"Too many high delay packages: {high_delay_diffs}" + + avg_diff = sum(tdiffs)/len(tdiffs) + assert avg_diff > 9.6*10**6, f"avg difference {avg_diff}, below threshold" + + stddev = np.std(tdiffs) + assert stddev < 1.5*10**6, f"Standard-dev to big {stddev}" + + @with_processes(['sensord']) + def test_events_check(self): + # verify if all sensors produce events + events = read_sensor_events(3) + + sensor_events = dict() + for event in events: + for measurement in event.sensorEvents: + + # filter unset events (bmx magn) + if measurement.version == 0: + continue + + if measurement.type in sensor_events: + sensor_events[measurement.type] += 1 + else: + sensor_events[measurement.type] = 1 + + for s in sensor_events: + err_msg = f"Sensor {s}: 200 < {sensor_events[s]}" + assert sensor_events[s] > 200, err_msg + + @with_processes(['sensord']) + def test_logmonottime_timestamp_diff(self): + # ensure diff between the message logMonotime and sample timestamp is small + events = read_sensor_events(3) + + tdiffs = list() + for event in events: + for measurement in event.sensorEvents: + + # filter unset events (bmx magn) + if measurement.version == 0: + continue + + # check if gyro and accel timestamps are before logMonoTime + if str(measurement.source).startswith("lsm6ds3"): + if measurement.which() != 'temperature': + err_msg = f"Timestamp after logMonoTime: {measurement.timestamp} > {event.logMonoTime}" + assert measurement.timestamp < event.logMonoTime, err_msg + + # negative values might occur, as non interrupt packages created + # before the sensor is read + tdiffs.append(abs(event.logMonoTime - measurement.timestamp)) + + high_delay_diffs = set(filter(lambda d: d >= 10*10**6, tdiffs)) + assert len(high_delay_diffs) < 15, f"Too many high delay packages: {high_delay_diffs}" + + avg_diff = round(sum(tdiffs)/len(tdiffs), 4) + assert avg_diff < 4*10**6, f"Avg packet diff: {avg_diff:.1f}ns" + + stddev = np.std(tdiffs) + assert stddev < 2*10**6, f"Timing diffs have to high stddev: {stddev}" + + @with_processes(['sensord']) + def test_sensor_values_sanity_check(self): + + events = read_sensor_events(2) + + sensor_values = dict() + for event in events: + for m in event.sensorEvents: + + # filter unset events (bmx magn) + if m.version == 0: + continue + + key = (m.source.raw, m.which()) + values = getattr(m, m.which()) + if hasattr(values, 'v'): + values = values.v + values = np.atleast_1d(values) + + if key in sensor_values: + sensor_values[key].append(values) + else: + sensor_values[key] = [values] + + # Sanity check sensor values and counts + for sensor, stype in sensor_values: + + for s in ALL_SENSORS[sensor]: + if s.type != stype: + continue + + key = (sensor, s.type) + val_cnt = len(sensor_values[key]) + err_msg = f"Sensor {sensor} {s.type} got {val_cnt} measurements, expected {s.min_samples}" + assert val_cnt > s.min_samples, err_msg + + mean_norm = np.mean(np.linalg.norm(sensor_values[key], axis=1)) + err_msg = f"Sensor '{sensor} {s.type}' failed sanity checks {mean_norm} is not between {s.sanity_min} and {s.sanity_max}" + assert s.sanity_min <= mean_norm <= s.sanity_max, err_msg + + def test_sensor_verify_no_interrupts_after_stop(self): + + managed_processes["sensord"].start() + time.sleep(1) + + # check if the interrupts are enableds + with SMBus(SENSOR_BUS, force=True) as bus: + int1_ctrl_reg = bus.read_byte_data(I2C_ADDR_LSM, 0x0D) + assert int1_ctrl_reg == 3, "Interrupts not enabled!" + + # read /proc/interrupts to verify interrupts are received + state_one = get_proc_interrupts(LSM_INT_GPIO) + time.sleep(1) + state_two = get_proc_interrupts(LSM_INT_GPIO) + + assert state_one != state_two, "no Interrupts received after sensord start!" + + managed_processes["sensord"].stop() + + # check if the interrupts got disabled + with SMBus(SENSOR_BUS, force=True) as bus: + int1_ctrl_reg = bus.read_byte_data(I2C_ADDR_LSM, 0x0D) + assert int1_ctrl_reg == 0, "Interrupts not disabled!" + + # read /proc/interrupts to verify no more interrupts are received + state_one = get_proc_interrupts(LSM_INT_GPIO) + time.sleep(1) + state_two = get_proc_interrupts(LSM_INT_GPIO) + assert state_one == state_two, "Interrupts received after sensord stop!" + if __name__ == "__main__": unittest.main() + diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py index 340093b604..dd6b79e123 100644 --- a/system/hardware/tici/hardware.py +++ b/system/hardware/tici/hardware.py @@ -462,6 +462,17 @@ class Tici(HardwareBase): sudo_write("N", "/sys/kernel/debug/msm_vidc/clock_scaling") sudo_write("Y", "/sys/kernel/debug/msm_vidc/disable_thermal_mitigation") + # *** unexport GPIO for sensors *** + # remove from /userspace/usr/comma/gpio.sh + sudo_write(str(GPIO.BMX055_ACCEL_INT), "/sys/class/gpio/unexport") + sudo_write(str(GPIO.BMX055_GYRO_INT), "/sys/class/gpio/unexport") + sudo_write(str(GPIO.BMX055_MAGN_INT), "/sys/class/gpio/unexport") + sudo_write(str(GPIO.LSM_INT), "/sys/class/gpio/unexport") + + # *** set /dev/gpiochip0 rights to make accessible by sensord + os.system("sudo chmod +r /dev/gpiochip0") + + def configure_modem(self): sim_id = self.get_sim_info().get('sim_id', '') diff --git a/system/hardware/tici/pins.py b/system/hardware/tici/pins.py index 61e528d304..fe31b9311d 100644 --- a/system/hardware/tici/pins.py +++ b/system/hardware/tici/pins.py @@ -19,3 +19,9 @@ class GPIO: CAM0_RSTN = 9 CAM1_RSTN = 7 CAM2_RSTN = 12 + + # Sensor interrupts + BMX055_ACCEL_INT = 21 + BMX055_GYRO_INT = 23 + BMX055_MAGN_INT = 87 + LSM_INT = 84