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 <robbe@comma.ai> Co-authored-by: Willem Melching <willem.melching@gmail.com>albatross
robbederks
GitHub
parent
521b6688ea
commit
622e42d504
|
|
@ -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'])
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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 = [
|
||||
|
|
|
|||
|
|
@ -0,0 +1,80 @@
|
|||
#include "i2c.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <unistd.h>
|
||||
#include <stdexcept>
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include "common/swaglog.h"
|
||||
|
||||
#define UNUSED(x) (void)(x)
|
||||
|
||||
#ifdef QCOM2
|
||||
// TODO: decide if we want to isntall libi2c-dev everywhere
|
||||
#include <linux/i2c-dev.h>
|
||||
|
||||
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
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <sys/types.h>
|
||||
|
||||
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);
|
||||
};
|
||||
|
|
@ -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'])
|
||||
|
|
|
|||
|
|
@ -0,0 +1,71 @@
|
|||
#include <cassert>
|
||||
#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<const float> vs(&xyz[0], 3);
|
||||
|
||||
auto svec = event.initAcceleration();
|
||||
svec.setV(vs);
|
||||
svec.setStatus(true);
|
||||
|
||||
}
|
||||
|
|
@ -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);
|
||||
};
|
||||
|
|
@ -0,0 +1,81 @@
|
|||
#include <cassert>
|
||||
#include <cmath>
|
||||
#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<const float> vs(&xyz[0], 3);
|
||||
|
||||
auto svec = event.initGyroUncalibrated();
|
||||
svec.setV(vs);
|
||||
svec.setStatus(true);
|
||||
|
||||
}
|
||||
|
|
@ -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);
|
||||
};
|
||||
|
|
@ -0,0 +1,43 @@
|
|||
#include <unistd.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
|
@ -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){};
|
||||
};
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
#include <iostream>
|
||||
#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);
|
||||
}
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#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;
|
||||
};
|
||||
|
|
@ -0,0 +1,86 @@
|
|||
#include <vector>
|
||||
#include <csignal>
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
#include <sys/resource.h>
|
||||
|
||||
#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<I2CSensor *> 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<cereal::Event>();
|
||||
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();
|
||||
}
|
||||
Loading…
Reference in New Issue