LSM6DS3 (#2268)

* add initial lsm6ds3 setup

* accel works

* ignore lsm6ds3 in locationd

* add gyro

* add temp

Co-authored-by: Comma Device <device@comma.ai>

selfdrive/locationd/locationd.py

@@ -2,6 +2,7 @@
 import numpy as np
 import sympy as sp
 import cereal.messaging as messaging
+from cereal import log
 import common.transformations.coordinates as coord
 from common.transformations.orientation import ecef_euler_from_ned, \
                                                euler_from_quat, \
@@ -19,6 +20,8 @@ from selfdrive.swaglog import cloudlog
 from sympy.utilities.lambdify import lambdify
 from rednose.helpers.sympy_helpers import euler_rotate
 
+SensorSource = log.SensorEventData.SensorSource
+
 
 VISION_DECIMATION = 2
 SENSOR_DECIMATION = 10
@@ -235,6 +238,10 @@ class Localizer():
   def handle_sensors(self, current_time, log):
     # TODO does not yet account for double sensor readings in the log
     for sensor_reading in log:
+      # TODO: handle messages from two IMUs at the same time
+      if sensor_reading.source == SensorSource.lsm6ds3:
+        continue
+
       # Gyro Uncalibrated
       if sensor_reading.sensor == 5 and sensor_reading.type == 16:
         self.gyro_counter += 1

selfdrive/sensord/SConscript

@@ -13,5 +13,8 @@ else:
     'sensors/bmx055_gyro.cc',
     'sensors/bmx055_magn.cc',
     'sensors/bmx055_temp.cc',
+    'sensors/lsm6ds3_accel.cc',
+    'sensors/lsm6ds3_gyro.cc',
+    'sensors/lsm6ds3_temp.cc',
   ]
   env.Program('_sensord', ['sensors_qcom2.cc'] + sensors, LIBS=[common, cereal, messaging, 'capnp', 'zmq', 'kj'])

selfdrive/sensord/sensors/bmx055_magn.cc

@@ -6,22 +6,24 @@
 
 #include "bmx055_magn.hpp"
 
-int16_t parse_xy(uint8_t lsb, uint8_t msb){
+static 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){
+static 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){
+/*
+static 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) {}
 

selfdrive/sensord/sensors/lsm6ds3_accel.cc

@@ -0,0 +1,62 @@
+#include <cassert>
+#include "common/swaglog.h"
+#include "common/timing.h"
+
+#include "lsm6ds3_accel.hpp"
+
+
+LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus) : I2CSensor(bus) {}
+
+int LSM6DS3_Accel::init(){
+  int ret = 0;
+  uint8_t buffer[1];
+
+  ret = read_register(LSM6DS3_ACCEL_I2C_REG_ID, buffer, 1);
+  if(ret < 0){
+    LOGE("Reading chip ID failed: %d", ret);
+    goto fail;
+  }
+
+  if(buffer[0] != LSM6DS3_ACCEL_CHIP_ID){
+    LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_ACCEL_CHIP_ID);
+    ret = -1;
+    goto fail;
+  }
+
+  // TODO: set scale and bandwith. Default is +- 2G, 50 Hz
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, LSM6DS3_ACCEL_ODR_104HZ);
+  if (ret < 0){
+    goto fail;
+  }
+
+
+fail:
+  return ret;
+}
+
+void LSM6DS3_Accel::get_event(cereal::SensorEventData::Builder &event){
+
+  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));
+
+  float scale = 9.81 * 2.0f / (1 << 15);
+  float x = read_16_bit(buffer[0], buffer[1]) * scale;
+  float y = read_16_bit(buffer[2], buffer[3]) * scale;
+  float z = read_16_bit(buffer[4], buffer[5]) * scale;
+
+  event.setSource(cereal::SensorEventData::SensorSource::LSM6DS3);
+  event.setVersion(1);
+  event.setSensor(SENSOR_ACCELEROMETER);
+  event.setType(SENSOR_TYPE_ACCELEROMETER);
+  event.setTimestamp(start_time);
+
+  float xyz[] = {y, -x, z};
+  kj::ArrayPtr<const float> vs(&xyz[0], 3);
+
+  auto svec = event.initAcceleration();
+  svec.setV(vs);
+  svec.setStatus(true);
+
+}

selfdrive/sensord/sensors/lsm6ds3_accel.hpp

@@ -0,0 +1,24 @@
+#pragma once
+
+#include "sensors/i2c_sensor.hpp"
+
+// Address of the chip on the bus
+#define LSM6DS3_ACCEL_I2C_ADDR       0x6A
+
+// Registers of the chip
+#define LSM6DS3_ACCEL_I2C_REG_ID        0x0F
+#define LSM6DS3_ACCEL_I2C_REG_CTRL1_XL  0x10
+#define LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL 0x28
+
+// Constants
+#define LSM6DS3_ACCEL_CHIP_ID        0x69
+#define LSM6DS3_ACCEL_ODR_104HZ      (0b0100 << 4)
+
+
+class LSM6DS3_Accel : public I2CSensor {
+  uint8_t get_device_address() {return LSM6DS3_ACCEL_I2C_ADDR;}
+public:
+  LSM6DS3_Accel(I2CBus *bus);
+  int init();
+  void get_event(cereal::SensorEventData::Builder &event);
+};

selfdrive/sensord/sensors/lsm6ds3_gyro.cc

@@ -0,0 +1,65 @@
+#include <cassert>
+#include <cmath>
+#include "common/swaglog.h"
+#include "common/timing.h"
+
+#include "lsm6ds3_gyro.hpp"
+
+#define DEG2RAD(x) ((x) * M_PI / 180.0)
+
+
+LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus) : I2CSensor(bus) {}
+
+int LSM6DS3_Gyro::init(){
+  int ret = 0;
+  uint8_t buffer[1];
+
+  ret = read_register(LSM6DS3_GYRO_I2C_REG_ID, buffer, 1);
+  if(ret < 0){
+    LOGE("Reading chip ID failed: %d", ret);
+    goto fail;
+  }
+
+  if(buffer[0] != LSM6DS3_GYRO_CHIP_ID){
+    LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_GYRO_CHIP_ID);
+    ret = -1;
+    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;
+  }
+
+
+fail:
+  return ret;
+}
+
+void LSM6DS3_Gyro::get_event(cereal::SensorEventData::Builder &event){
+
+  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));
+
+  float scale = 250.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::LSM6DS3);
+  event.setVersion(1);
+  event.setSensor(SENSOR_GYRO_UNCALIBRATED);
+  event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED);
+  event.setTimestamp(start_time);
+
+  float xyz[] = {y, -x, z};
+  kj::ArrayPtr<const float> vs(&xyz[0], 3);
+
+  auto svec = event.initAcceleration();
+  svec.setV(vs);
+  svec.setStatus(true);
+
+}

selfdrive/sensord/sensors/lsm6ds3_gyro.hpp

@@ -0,0 +1,24 @@
+#pragma once
+
+#include "sensors/i2c_sensor.hpp"
+
+// Address of the chip on the bus
+#define LSM6DS3_GYRO_I2C_ADDR       0x6A
+
+// Registers of the chip
+#define LSM6DS3_GYRO_I2C_REG_ID        0x0F
+#define LSM6DS3_GYRO_I2C_REG_CTRL2_G   0x11
+#define LSM6DS3_GYRO_I2C_REG_OUTX_L_G  0x22
+
+// Constants
+#define LSM6DS3_GYRO_CHIP_ID        0x69
+#define LSM6DS3_GYRO_ODR_104HZ      (0b0100 << 4)
+
+
+class LSM6DS3_Gyro : public I2CSensor {
+  uint8_t get_device_address() {return LSM6DS3_GYRO_I2C_ADDR;}
+public:
+  LSM6DS3_Gyro(I2CBus *bus);
+  int init();
+  void get_event(cereal::SensorEventData::Builder &event);
+};

selfdrive/sensord/sensors/lsm6ds3_temp.cc

@@ -0,0 +1,45 @@
+#include <cassert>
+#include "common/swaglog.h"
+#include "common/timing.h"
+
+#include "lsm6ds3_temp.hpp"
+
+
+LSM6DS3_Temp::LSM6DS3_Temp(I2CBus *bus) : I2CSensor(bus) {}
+
+int LSM6DS3_Temp::init(){
+  int ret = 0;
+  uint8_t buffer[1];
+
+  ret = read_register(LSM6DS3_TEMP_I2C_REG_ID, buffer, 1);
+  if(ret < 0){
+    LOGE("Reading chip ID failed: %d", ret);
+    goto fail;
+  }
+
+  if(buffer[0] != LSM6DS3_TEMP_CHIP_ID){
+    LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_TEMP_CHIP_ID);
+    ret = -1;
+    goto fail;
+  }
+
+fail:
+  return ret;
+}
+
+void LSM6DS3_Temp::get_event(cereal::SensorEventData::Builder &event){
+
+  uint64_t start_time = nanos_since_boot();
+  uint8_t buffer[2];
+  int len = read_register(LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L, buffer, sizeof(buffer));
+  assert(len == sizeof(buffer));
+
+  float temp = 25.0f + read_16_bit(buffer[0], buffer[1]) / 16.0f;
+
+  event.setSource(cereal::SensorEventData::SensorSource::LSM6DS3);
+  event.setVersion(1);
+  event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
+  event.setTimestamp(start_time);
+  event.setTemperature(temp);
+
+}

selfdrive/sensord/sensors/lsm6ds3_temp.hpp

@@ -0,0 +1,22 @@
+#pragma once
+
+#include "sensors/i2c_sensor.hpp"
+
+// Address of the chip on the bus
+#define LSM6DS3_TEMP_I2C_ADDR       0x6A
+
+// Registers of the chip
+#define LSM6DS3_TEMP_I2C_REG_ID           0x0F
+#define LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L   0x20
+
+// Constants
+#define LSM6DS3_TEMP_CHIP_ID        0x69
+
+
+class LSM6DS3_Temp : public I2CSensor {
+  uint8_t get_device_address() {return LSM6DS3_TEMP_I2C_ADDR;}
+public:
+  LSM6DS3_Temp(I2CBus *bus);
+  int init();
+  void get_event(cereal::SensorEventData::Builder &event);
+};

selfdrive/sensord/sensors_qcom2.cc

@@ -11,10 +11,16 @@
 
 #include "sensors/sensor.hpp"
 #include "sensors/constants.hpp"
+
 #include "sensors/bmx055_accel.hpp"
 #include "sensors/bmx055_gyro.hpp"
 #include "sensors/bmx055_magn.hpp"
 #include "sensors/bmx055_temp.hpp"
+
+#include "sensors/lsm6ds3_accel.hpp"
+#include "sensors/lsm6ds3_gyro.hpp"
+#include "sensors/lsm6ds3_temp.hpp"
+
 #include "sensors/light_sensor.hpp"
 
 volatile sig_atomic_t do_exit = 0;
@@ -36,18 +42,28 @@ int sensor_loop() {
     return -1;
   }
 
-  BMX055_Accel accel(i2c_bus_imu);
-  BMX055_Gyro gyro(i2c_bus_imu);
-  BMX055_Magn magn(i2c_bus_imu);
-  BMX055_Temp temp(i2c_bus_imu);
+  BMX055_Accel bmx055_accel(i2c_bus_imu);
+  BMX055_Gyro bmx055_gyro(i2c_bus_imu);
+  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_Temp lsm6ds3_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(&bmx055_accel);
+  sensors.push_back(&bmx055_gyro);
+  sensors.push_back(&bmx055_magn);
+  sensors.push_back(&bmx055_temp);
+
+  sensors.push_back(&lsm6ds3_accel);
+  sensors.push_back(&lsm6ds3_gyro);
+  sensors.push_back(&lsm6ds3_temp);
+
   sensors.push_back(&light);