LSM6DS3TR-C support (commaai#21563)

* add chip id * fix temp scale * cleanup source
qbrak · Jul 12, 2021 · fd47239 · fd47239
1 parent b0e3c35
commit fd47239
Show file tree

Hide file tree

Showing 7 changed files with 28 additions and 8 deletions.
diff --git a/cereal b/cereal
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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();