mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-02-19 12:23:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensord: use interrupts to improve LSM6DS3 timing accuracy (#24525)

Browse Source 
* change LSM6DS3TR(-c) gyroscope  and accelerometer to interrupt

* add pthread for linking

* add interrupt collector thread to fetch in parallel to non interrupt based sensors

* change get_event interface to return true on successful read

* fetch sensor interrupts via gpiochip

* avoid sending empty messages (interrupt only, non interupt magn can leave a gap in the orphan block)

* add verifier script to sensor interrupts (sensor_data_to_hist.py)

* add/update sensord testsweet (test_sensord.py)

* add poll timed out check

* unexport interrupt gpio pins

* gpiochip on both edges, but skip falling edge if rising edge is detected, this is handled in the sensor as the status flag is checked if new data is availble

* add test to sensord to verify 100Hz interrupt frequency

* add sensor shutdown/low power mode functionality on sensord exit

* relax test, will be readded in the splitup PR

Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com>
This commit is contained in:
Willem Melching
2022-09-13 07:29:31 +02:00
committed by GitHub
parent 56b05d55eb
commit 84a3c355e5
31 changed files with 711 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
common/gpio.cc
View File

@@ -4,11 +4,11 @@
#include <unistd.h>
#include <cstring>
#include <linux/gpio.h>
#include <sys/ioctl.h>
#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;
}

12
common/gpio.h
View File

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

121
selfdrive/debug/sensor_data_to_hist.py Executable file
View File

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

2
selfdrive/sensord/SConscript
View File

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

3
selfdrive/sensord/sensors/bmx055_accel.cc
View File

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

3
selfdrive/sensord/sensors/bmx055_accel.h
View File

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

4
selfdrive/sensord/sensors/bmx055_gyro.cc
View File

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

3
selfdrive/sensord/sensors/bmx055_gyro.h
View File

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

7
selfdrive/sensord/sensors/bmx055_magn.cc
View File

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

3
selfdrive/sensord/sensors/bmx055_magn.h
View File

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

3
selfdrive/sensord/sensors/bmx055_temp.cc
View File

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

3
selfdrive/sensord/sensors/bmx055_temp.h
View File

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

4
selfdrive/sensord/sensors/file_sensor.cc
View File

@@ -12,3 +12,7 @@ int FileSensor::init() {
FileSensor::~FileSensor() {
file.close();
}
bool FileSensor::has_interrupt_enabled() {
return false;
}

3
selfdrive/sensord/sensors/file_sensor.h
View File

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

23
selfdrive/sensord/sensors/i2c_sensor.cc
View File

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

14
selfdrive/sensord/sensors/i2c_sensor.h
View File

@@ -1,9 +1,13 @@
#pragma once
#include <cstdint>
#include <unistd.h>
#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;
};

4
selfdrive/sensord/sensors/light_sensor.cc
View File

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

3
selfdrive/sensord/sensors/light_sensor.h
View File

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

58
selfdrive/sensord/sensors/lsm6ds3_accel.cc
View File

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

17
selfdrive/sensord/sensors/lsm6ds3_accel.h
View File

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

59
selfdrive/sensord/sensors/lsm6ds3_gyro.cc
View File

@@ -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;
}
void LSM6DS3_Gyro::get_event(cereal::SensorEventData::Builder &event) {
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;
}
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;
}

17
selfdrive/sensord/sensors/lsm6ds3_gyro.h
View File

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

3
selfdrive/sensord/sensors/lsm6ds3_temp.cc
View File

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

3
selfdrive/sensord/sensors/lsm6ds3_temp.h
View File

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

3
selfdrive/sensord/sensors/mmc5603nj_magn.cc
View File

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

3
selfdrive/sensord/sensors/mmc5603nj_magn.h
View File

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

5
selfdrive/sensord/sensors/sensor.h
View File

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

99
selfdrive/sensord/sensors_qcom2.cc
View File

@@ -3,6 +3,8 @@
#include <chrono>
#include <thread>
#include <vector>
#include <poll.h>
#include <linux/gpio.h>
#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<Sensor *>& 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<capnp::Orphan<cereal::SensorEventData>> collected_events;
collected_events.reserve(sensors.size());
for (Sensor *sensor : sensors) {
auto orphan = orphanage.newOrphan<cereal::SensorEventData>();
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<std::mutex> 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<Sensor *> 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<std::mutex> 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;
}

231
selfdrive/sensord/tests/test_sensord.py Executable file → Normal file
View File

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

11
system/hardware/tici/hardware.py
View File

@@ -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', '')

6
system/hardware/tici/pins.py
View File

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