tici: BMX055 magnetometer& temperature sensor (#2212)

* BMX055 Magnetometer support

* add temp sensor

selfdrive/sensord/SConscript

@@ -12,5 +12,6 @@ else:
     'sensors/bmx055_accel.cc',
     'sensors/bmx055_gyro.cc',
     'sensors/bmx055_magn.cc',
+    'sensors/bmx055_temp.cc',
   ]
   env.Program('_sensord', ['sensors_qcom2.cc'] + sensors, LIBS=[common, cereal, messaging, 'capnp', 'zmq', 'kj'])

selfdrive/sensord/sensors/bmx055_accel.hpp

@@ -7,6 +7,7 @@
 
 // Registers of the chip
 #define BMX055_ACCEL_I2C_REG_ID     0x00
+#define BMX055_ACCEL_I2C_REG_TEMP   0x08
 #define BMX055_ACCEL_I2C_REG_BW     0x10
 #define BMX055_ACCEL_I2C_REG_HBW    0x13
 #define BMX055_ACCEL_I2C_REG_FIFO   0x3F

selfdrive/sensord/sensors/bmx055_magn.cc

@@ -1,13 +1,30 @@
+#include <cassert>
+#include <cstdio>
 #include <unistd.h>
 
 #include "common/swaglog.h"
 
 #include "bmx055_magn.hpp"
 
+int16_t parse_xy(uint8_t lsb, uint8_t msb){
+  // 13 bit
+  uint16_t combined = (uint16_t(msb) << 5) | uint16_t(lsb >> 3);
+  return int16_t(combined << 3) / (1 << 3);
+}
+
+int16_t parse_z(uint8_t lsb, uint8_t msb){
+  // 15 bit
+  uint16_t combined = (uint16_t(msb) << 7) | uint16_t(lsb >> 1);
+  return int16_t(combined << 1) / (1 << 1);
+}
+
+uint16_t parse_rhall(uint8_t lsb, uint8_t msb){
+  // 14 bit
+  return (uint16_t(msb) << 6) | uint16_t(lsb >> 2);
+}
 
 BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {}
 
-
 int BMX055_Magn::init(){
   int ret;
   uint8_t buffer[1];
@@ -16,7 +33,7 @@ int BMX055_Magn::init(){
   ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01);
   if(ret < 0){
     LOGE("Enabling power failed: %d", ret);
-    return ret;
+    goto fail;
   }
   usleep(5 * 1000); // wait until the chip is powered on
 
@@ -24,7 +41,7 @@ int BMX055_Magn::init(){
   ret = read_register(BMX055_MAGN_I2C_REG_ID, buffer, 1);
   if(ret < 0){
     LOGE("Reading chip ID failed: %d", ret);
-    return ret;
+    goto fail;
   }
 
   if(buffer[0] != BMX055_MAGN_CHIP_ID){
@@ -32,12 +49,61 @@ int BMX055_Magn::init(){
     return -1;
   }
 
-  // perform self-test
-
-
-  // sleep -> active (normal, high-precision)
+  // TODO: perform self-test
 
+  // 9 REPXY and 15 REPZ for 100 Hz
+  // 3 REPXY and 3 REPZ for > 300 Hz
+  ret = set_register(BMX055_MAGN_I2C_REG_REPXY, (3 - 1) / 2);
+  if (ret < 0){
+    goto fail;
+  }
 
+  ret = set_register(BMX055_MAGN_I2C_REG_REPZ, 3 - 1);
+  if (ret < 0){
+    goto fail;
+  }
 
   return 0;
+
+ fail:
+  return ret;
+}
+
+
+void BMX055_Magn::get_event(cereal::SensorEventData::Builder &event){
+  uint64_t start_time = nanos_since_boot();
+  uint8_t buffer[8];
+
+  int len = read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
+  assert(len == sizeof(buffer));
+
+  bool ready = buffer[6] & 0x1;
+  if (ready){
+    float x = parse_xy(buffer[0], buffer[1]);
+    float y = parse_xy(buffer[2], buffer[3]);
+    float z = parse_z(buffer[4], buffer[5]);
+    //uint16_t rhall = parse_rhall(buffer[5], buffer[6]);
+
+    // TODO: convert to micro tesla:
+    // https://github.com/BoschSensortec/BMM150-Sensor-API/blob/master/bmm150.c#L1614
+
+    event.setSource(cereal::SensorEventData::SensorSource::BMX055);
+    event.setVersion(1);
+    event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);
+    event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
+    event.setTimestamp(start_time);
+
+    float xyz[] = {x, y, z};
+    kj::ArrayPtr<const float> vs(&xyz[0], 3);
+
+    auto svec = event.initMagneticUncalibrated();
+    svec.setV(vs);
+    svec.setStatus(true);
+  }
+
+  // The BMX055 Magnetometer has no FIFO mode. Self running mode only goes
+  // up to 30 Hz. Therefore we put in forced mode, and request measurements
+  // at a 100 Hz. When reading the registers we have to check the ready bit
+  // To verify the measurement was comleted this cycle.
+  set_register(BMX055_MAGN_I2C_REG_MAG, BMX055_MAGN_FORCED);
 }

selfdrive/sensord/sensors/bmx055_magn.hpp

@@ -6,16 +6,22 @@
 #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
+#define BMX055_MAGN_I2C_REG_ID        0x40
+#define BMX055_MAGN_I2C_REG_PWR_0     0x4B
+#define BMX055_MAGN_I2C_REG_MAG       0x4C
+#define BMX055_MAGN_I2C_REG_DATAX_LSB 0x42
+#define BMX055_MAGN_I2C_REG_RHALL_LSB 0x48
+#define BMX055_MAGN_I2C_REG_REPXY     0x51
+#define BMX055_MAGN_I2C_REG_REPZ      0x52
 
 // Constants
-#define BMX055_MAGN_CHIP_ID         0x32
+#define BMX055_MAGN_CHIP_ID     0x32
+#define BMX055_MAGN_FORCED      (0b01 << 1)
 
 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){};
+  void get_event(cereal::SensorEventData::Builder &event);
 };

selfdrive/sensord/sensors/bmx055_temp.cc

@@ -0,0 +1,44 @@
+#include <cassert>
+#include "common/swaglog.h"
+#include "common/timing.h"
+
+#include "bmx055_temp.hpp"
+#include "bmx055_accel.hpp"
+
+
+BMX055_Temp::BMX055_Temp(I2CBus *bus) : I2CSensor(bus) {}
+
+int BMX055_Temp::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;
+  }
+
+fail:
+  return ret;
+}
+
+void 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));
+  assert(len == sizeof(buffer));
+
+  float temp = 23.0f + int8_t(buffer[0]) / 2.0f;
+
+  event.setSource(cereal::SensorEventData::SensorSource::BMX055);
+  event.setVersion(1);
+  event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
+  event.setTimestamp(start_time);
+  event.setTemperature(temp);
+}

selfdrive/sensord/sensors/bmx055_temp.hpp

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "sensors/i2c_sensor.hpp"
+#include "sensors/bmx055_accel.hpp"
+
+
+class BMX055_Temp : public I2CSensor {
+  uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;}
+public:
+  BMX055_Temp(I2CBus *bus);
+  int init();
+  void get_event(cereal::SensorEventData::Builder &event);
+};

selfdrive/sensord/sensors/constants.hpp

@@ -12,6 +12,7 @@
 #define SENSOR_TYPE_GEOMAGNETIC_FIELD 2
 #define SENSOR_TYPE_GYROSCOPE 4
 #define SENSOR_TYPE_LIGHT 5
+#define SENSOR_TYPE_AMBIENT_TEMPERATURE 13
 #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_qcom2.cc

@@ -14,6 +14,7 @@
 #include "sensors/bmx055_accel.hpp"
 #include "sensors/bmx055_gyro.hpp"
 #include "sensors/bmx055_magn.hpp"
+#include "sensors/bmx055_temp.hpp"
 #include "sensors/light_sensor.hpp"
 
 volatile sig_atomic_t do_exit = 0;
@@ -38,14 +39,16 @@ int sensor_loop() {
   BMX055_Accel accel(i2c_bus_imu);
   BMX055_Gyro gyro(i2c_bus_imu);
   BMX055_Magn magn(i2c_bus_imu);
+  BMX055_Temp temp(i2c_bus_imu);
   LightSensor light("/sys/class/i2c-adapter/i2c-2/2-0038/iio:device1/in_intensity_both_raw");
 
   // Sensor init
   std::vector<Sensor *> sensors;
   sensors.push_back(&accel);
   sensors.push_back(&gyro);
+  sensors.push_back(&magn);
+  sensors.push_back(&temp);
   sensors.push_back(&light);
-  // sensors.push_back(&magn);
 
 
   for (Sensor * sensor : sensors){