From e51da619c15dea3ac6cc77f4f9a387923e2a3ea6 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 5 Oct 2020 16:16:25 +0200 Subject: [PATCH] LSM6DS3 (#2268) * add initial lsm6ds3 setup * accel works * ignore lsm6ds3 in locationd * add gyro * add temp Co-authored-by: Comma Device --- selfdrive/locationd/locationd.py | 7 +++ selfdrive/sensord/SConscript | 3 + selfdrive/sensord/sensors/bmx055_magn.cc | 8 ++- selfdrive/sensord/sensors/lsm6ds3_accel.cc | 62 ++++++++++++++++++++ selfdrive/sensord/sensors/lsm6ds3_accel.hpp | 24 ++++++++ selfdrive/sensord/sensors/lsm6ds3_gyro.cc | 65 +++++++++++++++++++++ selfdrive/sensord/sensors/lsm6ds3_gyro.hpp | 24 ++++++++ selfdrive/sensord/sensors/lsm6ds3_temp.cc | 45 ++++++++++++++ selfdrive/sensord/sensors/lsm6ds3_temp.hpp | 22 +++++++ selfdrive/sensord/sensors_qcom2.cc | 32 +++++++--- 10 files changed, 281 insertions(+), 11 deletions(-) create mode 100644 selfdrive/sensord/sensors/lsm6ds3_accel.cc create mode 100644 selfdrive/sensord/sensors/lsm6ds3_accel.hpp create mode 100644 selfdrive/sensord/sensors/lsm6ds3_gyro.cc create mode 100644 selfdrive/sensord/sensors/lsm6ds3_gyro.hpp create mode 100644 selfdrive/sensord/sensors/lsm6ds3_temp.cc create mode 100644 selfdrive/sensord/sensors/lsm6ds3_temp.hpp diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index be78176844..b6acc12092 100755 --- a/selfdrive/locationd/locationd.py +++ b/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 diff --git a/selfdrive/sensord/SConscript b/selfdrive/sensord/SConscript index b5c8b78f99..facbf6403a 100644 --- a/selfdrive/sensord/SConscript +++ b/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']) diff --git a/selfdrive/sensord/sensors/bmx055_magn.cc b/selfdrive/sensord/sensors/bmx055_magn.cc index 1c23ceb6a6..bc742e435b 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.cc +++ b/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) {} diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.cc b/selfdrive/sensord/sensors/lsm6ds3_accel.cc new file mode 100644 index 0000000000..a7253b0a62 --- /dev/null +++ b/selfdrive/sensord/sensors/lsm6ds3_accel.cc @@ -0,0 +1,62 @@ +#include +#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 vs(&xyz[0], 3); + + auto svec = event.initAcceleration(); + svec.setV(vs); + svec.setStatus(true); + +} diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.hpp b/selfdrive/sensord/sensors/lsm6ds3_accel.hpp new file mode 100644 index 0000000000..314dcf8b52 --- /dev/null +++ b/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); +}; diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc new file mode 100644 index 0000000000..bfa5478643 --- /dev/null +++ b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc @@ -0,0 +1,65 @@ +#include +#include +#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 vs(&xyz[0], 3); + + auto svec = event.initAcceleration(); + svec.setV(vs); + svec.setStatus(true); + +} diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.hpp b/selfdrive/sensord/sensors/lsm6ds3_gyro.hpp new file mode 100644 index 0000000000..13cec204e9 --- /dev/null +++ b/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); +}; diff --git a/selfdrive/sensord/sensors/lsm6ds3_temp.cc b/selfdrive/sensord/sensors/lsm6ds3_temp.cc new file mode 100644 index 0000000000..04b0d3d4fa --- /dev/null +++ b/selfdrive/sensord/sensors/lsm6ds3_temp.cc @@ -0,0 +1,45 @@ +#include +#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); + +} diff --git a/selfdrive/sensord/sensors/lsm6ds3_temp.hpp b/selfdrive/sensord/sensors/lsm6ds3_temp.hpp new file mode 100644 index 0000000000..eea5ff4a6c --- /dev/null +++ b/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); +}; diff --git a/selfdrive/sensord/sensors_qcom2.cc b/selfdrive/sensord/sensors_qcom2.cc index d9da486054..1dd5ed10a6 100644 --- a/selfdrive/sensord/sensors_qcom2.cc +++ b/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 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);