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>

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'])

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
 

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 = [

selfdrive/common/i2c.cc

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

selfdrive/common/i2c.h

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

selfdrive/sensord/SConscript

@@ -1,5 +1,14 @@
-Import('env', 'common', 'cereal', 'messaging')
+Import('env', 'arch', 'common', 'cereal', 'messaging')
-env.Program('_sensord', 'sensors.cc', LIBS=['hardware', common, cereal, messaging, 'capnp', 'zmq', 'kj'])
+if arch == "aarch64":
-lenv = env.Clone()
+  env.Program('_sensord', 'sensors_qcom.cc', LIBS=['hardware', common, cereal, messaging, 'capnp', 'zmq', 'kj'])
-lenv['LIBPATH'] += ['/system/vendor/lib64']
+  lenv = env.Clone()
-lenv.Program('_gpsd', ['gpsd.cc'], LIBS=['hardware', common, 'diag', 'time_genoff', cereal, messaging, 'capnp', 'zmq', 'kj'])
+  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'])

selfdrive/sensord/sensors/bmx055_accel.cc

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

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);
+};

selfdrive/sensord/sensors/bmx055_gyro.cc

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

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);
+};

selfdrive/sensord/sensors/bmx055_magn.cc

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

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){};
+};

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

selfdrive/sensord/sensors/i2c_sensor.cc

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

selfdrive/sensord/sensors/i2c_sensor.hpp

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

selfdrive/sensord/sensors_qcom2.cc

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