From ec6bcbdbce0b5b466bea225ca44bfeb23463c6eb Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 12 Jul 2021 19:37:30 +0200 Subject: [PATCH] LSM6DS3TR-C support (#21563) * add chip id * fix temp scale * cleanup source old-commit-hash: fd472392745c91854032fe648a683e25b5271358 --- cereal | 2 +- selfdrive/sensord/sensors/lsm6ds3_accel.cc | 8 ++++++-- selfdrive/sensord/sensors/lsm6ds3_accel.h | 2 ++ selfdrive/sensord/sensors/lsm6ds3_gyro.cc | 8 ++++++-- selfdrive/sensord/sensors/lsm6ds3_gyro.h | 2 ++ selfdrive/sensord/sensors/lsm6ds3_temp.cc | 11 ++++++++--- selfdrive/sensord/sensors/lsm6ds3_temp.h | 3 +++ 7 files changed, 28 insertions(+), 8 deletions(-) diff --git a/cereal b/cereal index e68da2f7d6..d125791566 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit e68da2f7d67e05f3fa157a7df659c5277103ca6b +Subproject commit d12579156691e8f63df1a761d3ccec0709bc3444 diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.cc b/selfdrive/sensord/sensors/lsm6ds3_accel.cc index 17ec1f900f..83222858e7 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_accel.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_accel.cc @@ -17,12 +17,16 @@ int LSM6DS3_Accel::init() { goto fail; } - if(buffer[0] != LSM6DS3_ACCEL_CHIP_ID) { + if(buffer[0] != LSM6DS3_ACCEL_CHIP_ID && buffer[0] != LSM6DS3TRC_ACCEL_CHIP_ID) { LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_ACCEL_CHIP_ID); ret = -1; goto fail; } + if (buffer[0] == LSM6DS3TRC_ACCEL_CHIP_ID) { + source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; + } + // 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) { @@ -46,7 +50,7 @@ void LSM6DS3_Accel::get_event(cereal::SensorEventData::Builder &event) { 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.setSource(source); event.setVersion(1); event.setSensor(SENSOR_ACCELEROMETER); event.setType(SENSOR_TYPE_ACCELEROMETER); diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.h b/selfdrive/sensord/sensors/lsm6ds3_accel.h index e5a949d4f5..4a6b687445 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_accel.h +++ b/selfdrive/sensord/sensors/lsm6ds3_accel.h @@ -12,11 +12,13 @@ // Constants #define LSM6DS3_ACCEL_CHIP_ID 0x69 +#define LSM6DS3TRC_ACCEL_CHIP_ID 0x6A #define LSM6DS3_ACCEL_ODR_104HZ (0b0100 << 4) class LSM6DS3_Accel : public I2CSensor { uint8_t get_device_address() {return LSM6DS3_ACCEL_I2C_ADDR;} + cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; public: LSM6DS3_Accel(I2CBus *bus); int init(); diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc index eae91e4425..6732ead0f1 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc @@ -21,12 +21,16 @@ int LSM6DS3_Gyro::init() { goto fail; } - if(buffer[0] != LSM6DS3_GYRO_CHIP_ID) { + if(buffer[0] != LSM6DS3_GYRO_CHIP_ID && buffer[0] != LSM6DS3TRC_GYRO_CHIP_ID) { LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_GYRO_CHIP_ID); ret = -1; goto fail; } + if (buffer[0] == LSM6DS3TRC_GYRO_CHIP_ID) { + source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; + } + // TODO: set scale. Default is +- 250 deg/s ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_104HZ); if (ret < 0) { @@ -50,7 +54,7 @@ void LSM6DS3_Gyro::get_event(cereal::SensorEventData::Builder &event) { 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.setSource(source); event.setVersion(2); event.setSensor(SENSOR_GYRO_UNCALIBRATED); event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED); diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.h b/selfdrive/sensord/sensors/lsm6ds3_gyro.h index cb8595e760..d7e8f0025a 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_gyro.h +++ b/selfdrive/sensord/sensors/lsm6ds3_gyro.h @@ -12,11 +12,13 @@ // Constants #define LSM6DS3_GYRO_CHIP_ID 0x69 +#define LSM6DS3TRC_GYRO_CHIP_ID 0x6A #define LSM6DS3_GYRO_ODR_104HZ (0b0100 << 4) class LSM6DS3_Gyro : public I2CSensor { uint8_t get_device_address() {return LSM6DS3_GYRO_I2C_ADDR;} + cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; public: LSM6DS3_Gyro(I2CBus *bus); int init(); diff --git a/selfdrive/sensord/sensors/lsm6ds3_temp.cc b/selfdrive/sensord/sensors/lsm6ds3_temp.cc index d6972f929a..6b093fce05 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_temp.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_temp.cc @@ -17,12 +17,16 @@ int LSM6DS3_Temp::init() { goto fail; } - if(buffer[0] != LSM6DS3_TEMP_CHIP_ID) { + if(buffer[0] != LSM6DS3_TEMP_CHIP_ID && buffer[0] != LSM6DS3TRC_TEMP_CHIP_ID) { LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_TEMP_CHIP_ID); ret = -1; goto fail; } + if (buffer[0] == LSM6DS3TRC_TEMP_CHIP_ID) { + source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; + } + fail: return ret; } @@ -34,9 +38,10 @@ void LSM6DS3_Temp::get_event(cereal::SensorEventData::Builder &event) { 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; + float scale = (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) ? 256.0f : 16.0f; + float temp = 25.0f + read_16_bit(buffer[0], buffer[1]) / scale; - event.setSource(cereal::SensorEventData::SensorSource::LSM6DS3); + event.setSource(source); event.setVersion(1); event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE); event.setTimestamp(start_time); diff --git a/selfdrive/sensord/sensors/lsm6ds3_temp.h b/selfdrive/sensord/sensors/lsm6ds3_temp.h index c556631def..8188f46700 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_temp.h +++ b/selfdrive/sensord/sensors/lsm6ds3_temp.h @@ -11,10 +11,13 @@ // Constants #define LSM6DS3_TEMP_CHIP_ID 0x69 +#define LSM6DS3TRC_TEMP_CHIP_ID 0x6A class LSM6DS3_Temp : public I2CSensor { uint8_t get_device_address() {return LSM6DS3_TEMP_I2C_ADDR;} + cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; + public: LSM6DS3_Temp(I2CBus *bus); int init();