LSM6DS3TR-C support (#21563)

* add chip id

* fix temp scale

* cleanup source
old-commit-hash: fd47239274

cereal

@@ -1 +1 @@
-Subproject commit e68da2f7d67e05f3fa157a7df659c5277103ca6b
+Subproject commit d12579156691e8f63df1a761d3ccec0709bc3444

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);

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();

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);

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();

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);

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();