From 622e42d504458ebfbb23e945beb1a60bbfd0e63b Mon Sep 17 00:00:00 2001 From: robbederks Date: Wed, 26 Aug 2020 11:30:30 +0200 Subject: [PATCH] Tici sensord (#2072) * tici sensord: WIP * add sensor constants * rename files * base class * init sensors * publish something * dont leak memory * try reading from accel * Accel works * add time and sensor source * Update release files * If we want low BW we want 125hz * this should run gyro as well * add filter on gyro * also filter accel data * Add i2c files * cast makes macos unhappy * Same outputs as android sensord Co-authored-by: Tici Co-authored-by: Willem Melching --- SConstruct | 3 +- release/files_common | 8 +- selfdrive/common/SConscript | 4 +- selfdrive/common/i2c.cc | 80 +++++++++++++++++ selfdrive/common/i2c.h | 16 ++++ selfdrive/sensord/SConscript | 19 ++-- selfdrive/sensord/sensors/bmx055_accel.cc | 71 +++++++++++++++ selfdrive/sensord/sensors/bmx055_accel.hpp | 35 ++++++++ selfdrive/sensord/sensors/bmx055_gyro.cc | 81 +++++++++++++++++ selfdrive/sensord/sensors/bmx055_gyro.hpp | 36 ++++++++ selfdrive/sensord/sensors/bmx055_magn.cc | 43 ++++++++++ selfdrive/sensord/sensors/bmx055_magn.hpp | 21 +++++ selfdrive/sensord/sensors/constants.hpp | 17 ++++ selfdrive/sensord/sensors/i2c_sensor.cc | 24 ++++++ selfdrive/sensord/sensors/i2c_sensor.hpp | 23 +++++ .../sensord/{sensors.cc => sensors_qcom.cc} | 0 selfdrive/sensord/sensors_qcom2.cc | 86 +++++++++++++++++++ 17 files changed, 559 insertions(+), 8 deletions(-) create mode 100644 selfdrive/common/i2c.cc create mode 100644 selfdrive/common/i2c.h create mode 100644 selfdrive/sensord/sensors/bmx055_accel.cc create mode 100644 selfdrive/sensord/sensors/bmx055_accel.hpp create mode 100644 selfdrive/sensord/sensors/bmx055_gyro.cc create mode 100644 selfdrive/sensord/sensors/bmx055_gyro.hpp create mode 100644 selfdrive/sensord/sensors/bmx055_magn.cc create mode 100644 selfdrive/sensord/sensors/bmx055_magn.hpp create mode 100644 selfdrive/sensord/sensors/constants.hpp create mode 100644 selfdrive/sensord/sensors/i2c_sensor.cc create mode 100644 selfdrive/sensord/sensors/i2c_sensor.hpp rename selfdrive/sensord/{sensors.cc => sensors_qcom.cc} (100%) create mode 100644 selfdrive/sensord/sensors_qcom2.cc diff --git a/SConstruct b/SConstruct index 72a4acfc..a12fa929 100644 --- a/SConstruct +++ b/SConstruct @@ -156,6 +156,7 @@ env = Environment( "#selfdrive/camerad/include", "#selfdrive/loggerd/include", "#selfdrive/modeld", + "#selfdrive/sensord", "#selfdrive/ui", "#cereal/messaging", "#cereal", @@ -308,10 +309,10 @@ SConscript(['selfdrive/loggerd/SConscript']) SConscript(['selfdrive/locationd/SConscript']) SConscript(['selfdrive/locationd/models/SConscript']) +SConscript(['selfdrive/sensord/SConscript']) if arch == "aarch64": SConscript(['selfdrive/logcatd/SConscript']) - SConscript(['selfdrive/sensord/SConscript']) if arch != "larch64": SConscript(['selfdrive/ui/SConscript']) diff --git a/release/files_common b/release/files_common index 2df94301..bf350b7e 100644 --- a/release/files_common +++ b/release/files_common @@ -215,8 +215,11 @@ selfdrive/common/visionimg.cc selfdrive/common/visionimg.h selfdrive/common/spinner.c selfdrive/common/spinner.h + selfdrive/common/gpio.cc selfdrive/common/gpio.h +selfdrive/common/i2c.cc +selfdrive/common/i2c.h selfdrive/controls/__init__.py @@ -313,7 +316,10 @@ selfdrive/loggerd/deleter.py selfdrive/sensord/SConscript selfdrive/sensord/gpsd.cc selfdrive/sensord/libdiag.h -selfdrive/sensord/sensors.cc +selfdrive/sensord/sensors_qcom.cc +selfdrive/sensord/sensors_qcom2.cc +selfdrive/sensord/sensors/*.cc +selfdrive/sensord/sensors/*.hpp selfdrive/sensord/sensord selfdrive/sensord/gpsd diff --git a/selfdrive/common/SConscript b/selfdrive/common/SConscript index 0ba5b1ab..f70abf58 100644 --- a/selfdrive/common/SConscript +++ b/selfdrive/common/SConscript @@ -5,7 +5,9 @@ if SHARED: else: fxn = env.Library -_common = fxn('common', ['params.cc', 'swaglog.cc', 'util.c', 'cqueue.c', 'gpio.cc'], LIBS="json11") +common_libs = ['params.cc', 'swaglog.cc', 'util.c', 'cqueue.c', 'gpio.cc', 'i2c.cc'] + +_common = fxn('common', common_libs, LIBS="json11") _visionipc = fxn('visionipc', ['visionipc.c', 'ipc.c']) files = [ diff --git a/selfdrive/common/i2c.cc b/selfdrive/common/i2c.cc new file mode 100644 index 00000000..a37b144d --- /dev/null +++ b/selfdrive/common/i2c.cc @@ -0,0 +1,80 @@ +#include "i2c.h" + +#include +#include +#include +#include +#include +#include +#include "common/swaglog.h" + +#define UNUSED(x) (void)(x) + +#ifdef QCOM2 +// TODO: decide if we want to isntall libi2c-dev everywhere +#include + +I2CBus::I2CBus(uint8_t bus_id){ + char bus_name[20]; + snprintf(bus_name, 20, "/dev/i2c-%d", bus_id); + + i2c_fd = open(bus_name, O_RDWR); + if(i2c_fd < 0){ + throw std::runtime_error("Failed to open I2C bus"); + } +} + +I2CBus::~I2CBus(){ + if(i2c_fd >= 0){ close(i2c_fd); } +} + +int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len){ + int ret = 0; + + ret = ioctl(i2c_fd, I2C_SLAVE, device_address); + if(ret < 0) { goto fail; } + + ret = i2c_smbus_read_i2c_block_data(i2c_fd, register_address, len, buffer); + if((ret < 0) || (ret != len)) { goto fail; } + +fail: + return ret; +} + +int I2CBus::set_register(uint8_t device_address, uint register_address, uint8_t data){ + int ret = 0; + + ret = ioctl(i2c_fd, I2C_SLAVE, device_address); + if(ret < 0) { goto fail; } + + ret = i2c_smbus_write_byte_data(i2c_fd, register_address, data); + if(ret < 0) { goto fail; } + +fail: + return ret; +} + +#else + +I2CBus::I2CBus(uint8_t bus_id){ + UNUSED(bus_id); + i2c_fd = -1; +} + +I2CBus::~I2CBus(){} + +int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len){ + UNUSED(device_address); + UNUSED(register_address); + UNUSED(buffer); + UNUSED(len); + return -1; +} + +int I2CBus::set_register(uint8_t device_address, uint register_address, uint8_t data){ + UNUSED(device_address); + UNUSED(register_address); + UNUSED(data); + return -1; +} +#endif diff --git a/selfdrive/common/i2c.h b/selfdrive/common/i2c.h new file mode 100644 index 00000000..d5788510 --- /dev/null +++ b/selfdrive/common/i2c.h @@ -0,0 +1,16 @@ +#pragma once + +#include +#include + +class I2CBus { + private: + int i2c_fd; + + public: + I2CBus(uint8_t bus_id); + ~I2CBus(); + + int read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len); + int set_register(uint8_t device_address, uint register_address, uint8_t data); +}; diff --git a/selfdrive/sensord/SConscript b/selfdrive/sensord/SConscript index b578ac95..dbfcebe7 100644 --- a/selfdrive/sensord/SConscript +++ b/selfdrive/sensord/SConscript @@ -1,5 +1,14 @@ -Import('env', 'common', 'cereal', 'messaging') -env.Program('_sensord', 'sensors.cc', LIBS=['hardware', common, cereal, messaging, 'capnp', 'zmq', 'kj']) -lenv = env.Clone() -lenv['LIBPATH'] += ['/system/vendor/lib64'] -lenv.Program('_gpsd', ['gpsd.cc'], LIBS=['hardware', common, 'diag', 'time_genoff', cereal, messaging, 'capnp', 'zmq', 'kj']) +Import('env', 'arch', 'common', 'cereal', 'messaging') +if arch == "aarch64": + env.Program('_sensord', 'sensors_qcom.cc', LIBS=['hardware', common, cereal, messaging, 'capnp', 'zmq', 'kj']) + lenv = env.Clone() + lenv['LIBPATH'] += ['/system/vendor/lib64'] + lenv.Program('_gpsd', ['gpsd.cc'], LIBS=['hardware', common, 'diag', 'time_genoff', cereal, messaging, 'capnp', 'zmq', 'kj']) +else: + sensors = [ + 'sensors/i2c_sensor.cc', + 'sensors/bmx055_accel.cc', + 'sensors/bmx055_gyro.cc', + 'sensors/bmx055_magn.cc', + ] + env.Program('_sensord', ['sensors_qcom2.cc'] + sensors, LIBS=[common, cereal, messaging, 'capnp', 'zmq', 'kj']) diff --git a/selfdrive/sensord/sensors/bmx055_accel.cc b/selfdrive/sensord/sensors/bmx055_accel.cc new file mode 100644 index 00000000..5eea0bcd --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_accel.cc @@ -0,0 +1,71 @@ +#include +#include "common/swaglog.h" +#include "common/timing.h" + +#include "bmx055_accel.hpp" + + +BMX055_Accel::BMX055_Accel(I2CBus *bus) : I2CSensor(bus) {} + +int BMX055_Accel::init(){ + int ret = 0; + uint8_t buffer[1]; + + ret = read_register(BMX055_ACCEL_I2C_REG_ID, buffer, 1); + if(ret < 0){ + LOGE("Reading chip ID failed: %d", ret); + goto fail; + } + + if(buffer[0] != BMX055_ACCEL_CHIP_ID){ + LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_ACCEL_CHIP_ID); + ret = -1; + goto fail; + } + + // High bandwidth + // ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_ENABLE); + // if (ret < 0){ + // goto fail; + // } + + // Low bandwidth + ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_DISABLE); + if (ret < 0){ + goto fail; + } + ret = set_register(BMX055_ACCEL_I2C_REG_BW, BMX055_ACCEL_BW_125HZ); + if (ret < 0){ + goto fail; + } + +fail: + return ret; +} + +void 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_FIFO, buffer, sizeof(buffer)); + assert(len == 6); + + // 12 bit = +-2g + float scale = 9.81 * 2.0f / (1 << 11); + float x = -read_12_bit(buffer[0], buffer[1]) * scale; + float y = -read_12_bit(buffer[2], buffer[3]) * scale; + float z = read_12_bit(buffer[4], buffer[5]) * scale; + + event.setSource(cereal::SensorEventData::SensorSource::BMX055); + event.setVersion(1); + event.setSensor(SENSOR_ACCELEROMETER); + event.setType(SENSOR_TYPE_ACCELEROMETER); + event.setTimestamp(start_time); + + float xyz[] = {x, y, z}; + kj::ArrayPtr vs(&xyz[0], 3); + + auto svec = event.initAcceleration(); + svec.setV(vs); + svec.setStatus(true); + +} diff --git a/selfdrive/sensord/sensors/bmx055_accel.hpp b/selfdrive/sensord/sensors/bmx055_accel.hpp new file mode 100644 index 00000000..7bfb97e7 --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_accel.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include "sensors/i2c_sensor.hpp" + +// Address of the chip on the bus +#define BMX055_ACCEL_I2C_ADDR 0x18 + +// Registers of the chip +#define BMX055_ACCEL_I2C_REG_ID 0x00 +#define BMX055_ACCEL_I2C_REG_BW 0x10 +#define BMX055_ACCEL_I2C_REG_HBW 0x13 +#define BMX055_ACCEL_I2C_REG_FIFO 0x3F + +// Constants +#define BMX055_ACCEL_CHIP_ID 0xFA + +#define BMX055_ACCEL_HBW_ENABLE 0b10000000 +#define BMX055_ACCEL_HBW_DISABLE 0b00000000 + +#define BMX055_ACCEL_BW_7_81HZ 0b01000 +#define BMX055_ACCEL_BW_15_63HZ 0b01001 +#define BMX055_ACCEL_BW_31_25HZ 0b01010 +#define BMX055_ACCEL_BW_62_5HZ 0b01011 +#define BMX055_ACCEL_BW_125HZ 0b01100 +#define BMX055_ACCEL_BW_250HZ 0b01101 +#define BMX055_ACCEL_BW_500HZ 0b01110 +#define BMX055_ACCEL_BW_1000HZ 0b01111 + +class BMX055_Accel : public I2CSensor { + uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;} +public: + BMX055_Accel(I2CBus *bus); + int init(); + void get_event(cereal::SensorEventData::Builder &event); +}; diff --git a/selfdrive/sensord/sensors/bmx055_gyro.cc b/selfdrive/sensord/sensors/bmx055_gyro.cc new file mode 100644 index 00000000..61e3d9e4 --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_gyro.cc @@ -0,0 +1,81 @@ +#include +#include +#include "common/swaglog.h" + +#include "bmx055_gyro.hpp" + +#define DEG2RAD(x) ((x) * M_PI / 180.0) + + +BMX055_Gyro::BMX055_Gyro(I2CBus *bus) : I2CSensor(bus) {} + +int BMX055_Gyro::init(){ + int ret = 0; + uint8_t buffer[1]; + + ret =read_register(BMX055_GYRO_I2C_REG_ID, buffer, 1); + if(ret < 0){ + LOGE("Reading chip ID failed: %d", ret); + goto fail; + } + + if(buffer[0] != BMX055_GYRO_CHIP_ID){ + LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_GYRO_CHIP_ID); + ret = -1; + goto fail; + } + + // High bandwidth + // ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_ENABLE); + // if (ret < 0){ + // goto fail; + // } + + // Low bandwidth + ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_DISABLE); + if (ret < 0){ + goto fail; + } + + // 116 Hz filter + ret = set_register(BMX055_GYRO_I2C_REG_BW, BMX055_GYRO_BW_116HZ); + if (ret < 0){ + goto fail; + } + + // +- 125 deg/s range + ret = set_register(BMX055_GYRO_I2C_REG_RANGE, BMX055_GYRO_RANGE_125); + if (ret < 0){ + goto fail; + } + +fail: + return ret; +} + +void 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_FIFO, buffer, sizeof(buffer)); + assert(len == 6); + + // 16 bit = +- 125 deg/s + float scale = 125.0f / (1 << 15); + float x = -DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale); + float y = -DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale); + float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale); + + event.setSource(cereal::SensorEventData::SensorSource::BMX055); + event.setVersion(1); + event.setSensor(SENSOR_GYRO_UNCALIBRATED); + event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED); + event.setTimestamp(start_time); + + float xyz[] = {x, y, z}; + kj::ArrayPtr vs(&xyz[0], 3); + + auto svec = event.initGyroUncalibrated(); + svec.setV(vs); + svec.setStatus(true); + +} diff --git a/selfdrive/sensord/sensors/bmx055_gyro.hpp b/selfdrive/sensord/sensors/bmx055_gyro.hpp new file mode 100644 index 00000000..407ee160 --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_gyro.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include "sensors/i2c_sensor.hpp" + +// Address of the chip on the bus +#define BMX055_GYRO_I2C_ADDR 0x68 + +// Registers of the chip +#define BMX055_GYRO_I2C_REG_ID 0x00 +#define BMX055_GYRO_I2C_REG_RANGE 0x0F +#define BMX055_GYRO_I2C_REG_BW 0x10 +#define BMX055_GYRO_I2C_REG_HBW 0x13 +#define BMX055_GYRO_I2C_REG_FIFO 0x3F + +// Constants +#define BMX055_GYRO_CHIP_ID 0x0F + +#define BMX055_GYRO_HBW_ENABLE 0b10000000 +#define BMX055_GYRO_HBW_DISABLE 0b00000000 + +#define BMX055_GYRO_RANGE_2000 0b000 +#define BMX055_GYRO_RANGE_1000 0b001 +#define BMX055_GYRO_RANGE_500 0b010 +#define BMX055_GYRO_RANGE_250 0b011 +#define BMX055_GYRO_RANGE_125 0b100 + +#define BMX055_GYRO_BW_116HZ 0b0010 + + +class BMX055_Gyro : public I2CSensor { + uint8_t get_device_address() {return BMX055_GYRO_I2C_ADDR;} +public: + BMX055_Gyro(I2CBus *bus); + int init(); + void get_event(cereal::SensorEventData::Builder &event); +}; diff --git a/selfdrive/sensord/sensors/bmx055_magn.cc b/selfdrive/sensord/sensors/bmx055_magn.cc new file mode 100644 index 00000000..858d190f --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_magn.cc @@ -0,0 +1,43 @@ +#include + +#include "common/swaglog.h" + +#include "bmx055_magn.hpp" + + +BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {} + + +int BMX055_Magn::init(){ + int ret; + uint8_t buffer[1]; + + // suspend -> sleep + ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01); + if(ret < 0){ + LOGE("Enabling power failed: %d", ret); + return ret; + } + usleep(5 * 1000); // wait until the chip is powered on + + // read chip ID + ret = read_register(BMX055_MAGN_I2C_REG_ID, buffer, 1); + if(ret < 0){ + LOGE("Reading chip ID failed: %d", ret); + return ret; + } + + if(buffer[0] != BMX055_MAGN_CHIP_ID){ + LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_MAGN_CHIP_ID); + return -1; + } + + // perform self-test + + + // sleep -> active (normal, high-precision) + + + + return 0; +} diff --git a/selfdrive/sensord/sensors/bmx055_magn.hpp b/selfdrive/sensord/sensors/bmx055_magn.hpp new file mode 100644 index 00000000..6be15a17 --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_magn.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include "sensors/i2c_sensor.hpp" + +// Address of the chip on the bus +#define BMX055_MAGN_I2C_ADDR 0x10 + +// Registers of the chip +#define BMX055_MAGN_I2C_REG_ID 0x40 +#define BMX055_MAGN_I2C_REG_PWR_0 0x4B + +// Constants +#define BMX055_MAGN_CHIP_ID 0x32 + +class BMX055_Magn : public I2CSensor{ + uint8_t get_device_address() {return BMX055_MAGN_I2C_ADDR;} +public: + BMX055_Magn(I2CBus *bus); + int init(); + void get_event(cereal::SensorEventData::Builder &event){}; +}; diff --git a/selfdrive/sensord/sensors/constants.hpp b/selfdrive/sensord/sensors/constants.hpp new file mode 100644 index 00000000..69db84ed --- /dev/null +++ b/selfdrive/sensord/sensors/constants.hpp @@ -0,0 +1,17 @@ +#pragma once + + +#define SENSOR_ACCELEROMETER 1 +#define SENSOR_MAGNETOMETER 2 +#define SENSOR_MAGNETOMETER_UNCALIBRATED 3 +#define SENSOR_GYRO 4 +#define SENSOR_GYRO_UNCALIBRATED 5 +#define SENSOR_LIGHT 7 + +#define SENSOR_TYPE_ACCELEROMETER 1 +#define SENSOR_TYPE_GEOMAGNETIC_FIELD 2 +#define SENSOR_TYPE_GYROSCOPE 4 +#define SENSOR_TYPE_LIGHT 5 +#define SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED 14 +#define SENSOR_TYPE_MAGNETIC_FIELD SENSOR_TYPE_GEOMAGNETIC_FIELD +#define SENSOR_TYPE_GYROSCOPE_UNCALIBRATED 16 diff --git a/selfdrive/sensord/sensors/i2c_sensor.cc b/selfdrive/sensord/sensors/i2c_sensor.cc new file mode 100644 index 00000000..e3000c8b --- /dev/null +++ b/selfdrive/sensord/sensors/i2c_sensor.cc @@ -0,0 +1,24 @@ +#include +#include "i2c_sensor.hpp" + +int16_t read_12_bit(uint8_t lsb, uint8_t msb){ + uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb & 0xF0); + return int16_t(combined) / (1 << 4); +} + +int16_t read_16_bit(uint8_t lsb, uint8_t msb){ + uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb); + return int16_t(combined); +} + + +I2CSensor::I2CSensor(I2CBus *bus) : bus(bus){ +} + +int I2CSensor::read_register(uint register_address, uint8_t *buffer, uint8_t len){ + return bus->read_register(get_device_address(), register_address, buffer, len); +} + +int I2CSensor::set_register(uint register_address, uint8_t data){ + return bus->set_register(get_device_address(), register_address, data); +} diff --git a/selfdrive/sensord/sensors/i2c_sensor.hpp b/selfdrive/sensord/sensors/i2c_sensor.hpp new file mode 100644 index 00000000..37d75687 --- /dev/null +++ b/selfdrive/sensord/sensors/i2c_sensor.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include +#include "cereal/gen/cpp/log.capnp.h" +#include "common/i2c.h" +#include "sensors/constants.hpp" + +int16_t read_12_bit(uint8_t lsb, uint8_t msb); +int16_t read_16_bit(uint8_t lsb, uint8_t msb); + + +class I2CSensor { +private: + I2CBus *bus; + virtual uint8_t get_device_address() = 0; + +public: + I2CSensor(I2CBus *bus); + int read_register(uint register_address, uint8_t *buffer, uint8_t len); + int set_register(uint register_address, uint8_t data); + virtual int init() = 0; + virtual void get_event(cereal::SensorEventData::Builder &event) = 0; +}; diff --git a/selfdrive/sensord/sensors.cc b/selfdrive/sensord/sensors_qcom.cc similarity index 100% rename from selfdrive/sensord/sensors.cc rename to selfdrive/sensord/sensors_qcom.cc diff --git a/selfdrive/sensord/sensors_qcom2.cc b/selfdrive/sensord/sensors_qcom2.cc new file mode 100644 index 00000000..c03c5197 --- /dev/null +++ b/selfdrive/sensord/sensors_qcom2.cc @@ -0,0 +1,86 @@ +#include +#include +#include +#include +#include + +#include "messaging.hpp" +#include "common/i2c.h" +#include "common/timing.h" +#include "common/swaglog.h" + +#include "sensors/constants.hpp" +#include "sensors/bmx055_accel.hpp" +#include "sensors/bmx055_gyro.hpp" +#include "sensors/bmx055_magn.hpp" + +volatile sig_atomic_t do_exit = 0; + +#define I2C_BUS_IMU 1 + + +void set_do_exit(int sig) { + do_exit = 1; +} + +int sensor_loop() { + I2CBus *i2c_bus_imu; + + try { + i2c_bus_imu = new I2CBus(I2C_BUS_IMU); + } catch (std::exception &e) { + LOGE("I2CBus init failed"); + return -1; + } + + BMX055_Accel accel(i2c_bus_imu); + BMX055_Gyro gyro(i2c_bus_imu); + BMX055_Magn magn(i2c_bus_imu); + + // Sensor init + std::vector sensors; + sensors.push_back(&accel); + sensors.push_back(&gyro); + // sensors.push_back(&magn); + + + for (I2CSensor * sensor : sensors){ + int err = sensor->init(); + if (err < 0){ + LOGE("Error initializing sensors"); + return -1; + } + } + + PubMaster pm({"sensorEvents"}); + + while (!do_exit){ + uint64_t log_time = nanos_since_boot(); + + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(log_time); + + int num_events = sensors.size(); + auto sensor_events = event.initSensorEvents(num_events); + + for (size_t i = 0; i < num_events; i++){ + auto event = sensor_events[i]; + sensors[i]->get_event(event); + } + + pm.send("sensorEvents", msg); + + // TODO actually run at 100Hz + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + return 0; +} + +int main(int argc, char *argv[]) { + setpriority(PRIO_PROCESS, 0, -13); + signal(SIGINT, set_do_exit); + signal(SIGTERM, set_do_exit); + + return sensor_loop(); +}