sensord: lsm self test (#25855)

* lsm self test v1

* add lsm6ds3 accel self test

* add self test for lsm6ds3 gyro

* add c variant self test

* code CleanUp

* address PR comments

Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com>
old-commit-hash: e4cd8a15db

selfdrive/sensord/sensors/lsm6ds3_accel.cc

@@ -1,17 +1,132 @@
 #include "lsm6ds3_accel.h"
 
 #include <cassert>
+#include <cmath>
+#include <cstring>
 
 #include "common/swaglog.h"
 #include "common/timing.h"
+#include "common/util.h"
 
 LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus, int gpio_nr, bool shared_gpio) :
   I2CSensor(bus, gpio_nr, shared_gpio) {}
 
+void LSM6DS3_Accel::wait_for_data_ready() {
+  uint8_t drdy = 0;
+  uint8_t buffer[6];
+
+  do {
+    read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
+    drdy &= LSM6DS3_ACCEL_DRDY_XLDA;
+  } while (drdy == 0);
+
+  read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer));
+}
+
+void LSM6DS3_Accel::read_and_avg_data(float* out_buf) {
+  uint8_t drdy = 0;
+  uint8_t buffer[6];
+
+  float scaling = 0.061f;
+  if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) {
+    scaling = 0.122f;
+  }
+
+  for (int i = 0; i < 5; i++) {
+    do {
+      read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
+      drdy &= LSM6DS3_ACCEL_DRDY_XLDA;
+    } while (drdy == 0);
+
+    int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer));
+    assert(len == sizeof(buffer));
+
+    for (int j = 0; j < 3; j++) {
+      out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * scaling;
+    }
+  }
+
+  for (int i = 0; i < 3; i++) {
+    out_buf[i] /= 5.0f;
+  }
+}
+
+int LSM6DS3_Accel::self_test(int test_type) {
+  float val_st_off[3] = {0};
+  float val_st_on[3] = {0};
+  float test_val[3] = {0};
+  uint8_t ODR_FS_MO = LSM6DS3_ACCEL_ODR_52HZ; // full scale: +-2g, ODR: 52Hz
+
+  // prepare sensor for self-test
+
+  // enable block data update and automatic increment
+  int ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC_BDU);
+  if (ret < 0) {
+    return ret;
+  }
+
+  if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) {
+    ODR_FS_MO = LSM6DS3_ACCEL_FS_4G | LSM6DS3_ACCEL_ODR_52HZ;
+  }
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, ODR_FS_MO);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // wait for stable output, and discard first values
+  util::sleep_for(100);
+  wait_for_data_ready();
+  read_and_avg_data(val_st_off);
+
+  // enable Self Test positive (or negative)
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, test_type);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // wait for stable output, and discard first values
+  util::sleep_for(100);
+  wait_for_data_ready();
+  read_and_avg_data(val_st_on);
+
+  // disable sensor
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 0);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // disable self test
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, 0);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // calculate the mg values for self test
+  for (int i = 0; i < 3; i++) {
+    test_val[i] = fabs(val_st_on[i] - val_st_off[i]);
+  }
+
+  // verify test result
+  for (int i = 0; i < 3; i++) {
+    if ((LSM6DS3_ACCEL_MIN_ST_LIMIT_mg > test_val[i]) ||
+        (test_val[i] > LSM6DS3_ACCEL_MAX_ST_LIMIT_mg)) {
+      return -1;
+    }
+  }
+
+  return ret;
+}
+
 int LSM6DS3_Accel::init() {
   int ret = 0;
   uint8_t buffer[1];
   uint8_t value = 0;
+  bool do_self_test = false;
+
+  const char* env_lsm_selftest =env_lsm_selftest = std::getenv("LSM_SELF_TEST");
+  if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) {
+    do_self_test = true;
+  }
 
   ret = read_register(LSM6DS3_ACCEL_I2C_REG_ID, buffer, 1);
   if(ret < 0) {
@@ -29,11 +144,29 @@ int LSM6DS3_Accel::init() {
     source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
   }
 
+  ret = self_test(LSM6DS3_ACCEL_POSITIVE_TEST);
+  if (ret < 0) {
+    LOGE("LSM6DS3 accel positive self-test failed!");
+    if (do_self_test) goto fail;
+  }
+
+  ret = self_test(LSM6DS3_ACCEL_NEGATIVE_TEST);
+  if (ret < 0) {
+    LOGE("LSM6DS3 accel negative self-test failed!");
+    if (do_self_test) goto fail;
+  }
+
   ret = init_gpio();
   if (ret < 0) {
     goto fail;
   }
 
+  // enable continuous update, and automatic increase
+  ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC);
+  if (ret < 0) {
+    goto fail;
+  }
+
   // TODO: set scale and bandwidth. Default is +- 2G, 50 Hz
   ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, LSM6DS3_ACCEL_ODR_104HZ);
   if (ret < 0) {

selfdrive/sensord/sensors/lsm6ds3_accel.h

@@ -10,21 +10,37 @@
 #define LSM6DS3_ACCEL_I2C_REG_ID        0x0F
 #define LSM6DS3_ACCEL_I2C_REG_INT1_CTRL 0x0D
 #define LSM6DS3_ACCEL_I2C_REG_CTRL1_XL  0x10
+#define LSM6DS3_ACCEL_I2C_REG_CTRL3_C   0x12
+#define LSM6DS3_ACCEL_I2C_REG_CTRL5_C   0x14
+#define LSM6DS3_ACCEL_I2C_REG_CTR9_XL   0x18
 #define LSM6DS3_ACCEL_I2C_REG_STAT_REG  0x1E
 #define LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL 0x28
 
 // Constants
 #define LSM6DS3_ACCEL_CHIP_ID         0x69
 #define LSM6DS3TRC_ACCEL_CHIP_ID      0x6A
+#define LSM6DS3_ACCEL_FS_4G           (0b10 << 2)
+#define LSM6DS3_ACCEL_ODR_52HZ        (0b0011 << 4)
 #define LSM6DS3_ACCEL_ODR_104HZ       (0b0100 << 4)
 #define LSM6DS3_ACCEL_INT1_DRDY_XL    0b1
 #define LSM6DS3_ACCEL_DRDY_XLDA       0b1
 #define LSM6DS3_ACCEL_DRDY_PULSE_MODE (1 << 7)
-
+#define LSM6DS3_ACCEL_IF_INC          0b00000100
+#define LSM6DS3_ACCEL_IF_INC_BDU      0b01000100
+#define LSM6DS3_ACCEL_XYZ_DEN         0b11100000
+#define LSM6DS3_ACCEL_POSITIVE_TEST   0b01
+#define LSM6DS3_ACCEL_NEGATIVE_TEST   0b10
+#define LSM6DS3_ACCEL_MIN_ST_LIMIT_mg 90.0f
+#define LSM6DS3_ACCEL_MAX_ST_LIMIT_mg 1700.0f
 
 class LSM6DS3_Accel : public I2CSensor {
   uint8_t get_device_address() {return LSM6DS3_ACCEL_I2C_ADDR;}
   cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3;
+
+  // self test functions
+  int self_test(int test_type);
+  void wait_for_data_ready();
+  void read_and_avg_data(float* val_st_off);
 public:
   LSM6DS3_Accel(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
   int init();

selfdrive/sensord/sensors/lsm6ds3_gyro.cc

@@ -2,19 +2,120 @@
 
 #include <cassert>
 #include <cmath>
+#include <cstring>
 
 #include "common/swaglog.h"
 #include "common/timing.h"
+#include "common/util.h"
 
 #define DEG2RAD(x) ((x) * M_PI / 180.0)
 
 LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus, int gpio_nr, bool shared_gpio) :
   I2CSensor(bus, gpio_nr, shared_gpio) {}
 
+void LSM6DS3_Gyro::wait_for_data_ready() {
+  uint8_t drdy = 0;
+  uint8_t buffer[6];
+
+  do {
+    read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
+    drdy &= LSM6DS3_GYRO_DRDY_GDA;
+  } while (drdy == 0);
+
+  read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
+}
+
+void LSM6DS3_Gyro::read_and_avg_data(float* out_buf) {
+  uint8_t drdy = 0;
+  uint8_t buffer[6];
+
+  for (int i = 0; i < 5; i++) {
+    do {
+      read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
+      drdy &= LSM6DS3_GYRO_DRDY_GDA;
+    } while (drdy == 0);
+
+    int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
+    assert(len == sizeof(buffer));
+
+    for (int j = 0; j < 3; j++) {
+      out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * 70.0f;
+    }
+  }
+
+  // calculate the mg average values
+  for (int i = 0; i < 3; i++) {
+    out_buf[i] /= 5.0f;
+  }
+}
+
+int LSM6DS3_Gyro::self_test(int test_type) {
+  float val_st_off[3] = {0};
+  float val_st_on[3] = {0};
+  float test_val[3] = {0};
+
+  // prepare sensor for self-test
+
+  // full scale: 2000dps, ODR: 208Hz
+  int ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_208HZ | LSM6DS3_GYRO_FS_2000dps);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // wait for stable output, and discard first values
+  util::sleep_for(150);
+  wait_for_data_ready();
+  read_and_avg_data(val_st_off);
+
+  // enable Self Test positive (or negative)
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, test_type);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // wait for stable output, and discard first values
+  util::sleep_for(50);
+  wait_for_data_ready();
+  read_and_avg_data(val_st_on);
+
+  // disable sensor
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, 0);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // disable self test
+  ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, 0);
+  if (ret < 0) {
+    return ret;
+  }
+
+  // calculate the mg values for self test
+  for (int i = 0; i < 3; i++) {
+    test_val[i] = fabs(val_st_on[i] - val_st_off[i]);
+  }
+
+  // verify test result
+  for (int i = 0; i < 3; i++) {
+    if ((LSM6DS3_GYRO_MIN_ST_LIMIT_mdps > test_val[i]) ||
+        (test_val[i] > LSM6DS3_GYRO_MAX_ST_LIMIT_mdps)) {
+      return -1;
+    }
+  }
+
+  return ret;
+}
+
 int LSM6DS3_Gyro::init() {
   int ret = 0;
   uint8_t buffer[1];
   uint8_t value = 0;
+  bool do_self_test = false;
+
+  const char* env_lsm_selftest =env_lsm_selftest = std::getenv("LSM_SELF_TEST");
+  if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) {
+    do_self_test = true;
+  }
 
   ret = read_register(LSM6DS3_GYRO_I2C_REG_ID, buffer, 1);
   if(ret < 0) {
@@ -37,6 +138,18 @@ int LSM6DS3_Gyro::init() {
     goto fail;
   }
 
+  ret = self_test(LSM6DS3_GYRO_POSITIVE_TEST);
+  if (ret < 0 ) {
+    LOGE("LSM6DS3 gyro positive self-test failed!");
+    if (do_self_test) goto fail;
+  }
+
+  ret = self_test(LSM6DS3_GYRO_NEGATIVE_TEST);
+  if (ret < 0) {
+    LOGE("LSM6DS3 gyro negative self-test failed!");
+    if (do_self_test) 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) {
@@ -65,7 +178,7 @@ fail:
 int LSM6DS3_Gyro::shutdown() {
   int ret = 0;
 
-  // disable data ready interrupt for accel on INT1
+  // disable data ready interrupt for gyro on INT1
   uint8_t value = 0;
   ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1);
   if (ret < 0) {

selfdrive/sensord/sensors/lsm6ds3_gyro.h

@@ -10,21 +10,33 @@
 #define LSM6DS3_GYRO_I2C_REG_ID        0x0F
 #define LSM6DS3_GYRO_I2C_REG_INT1_CTRL 0x0D
 #define LSM6DS3_GYRO_I2C_REG_CTRL2_G   0x11
+#define LSM6DS3_GYRO_I2C_REG_CTRL5_C   0x14
 #define LSM6DS3_GYRO_I2C_REG_STAT_REG  0x1E
 #define LSM6DS3_GYRO_I2C_REG_OUTX_L_G  0x22
+#define LSM6DS3_GYRO_POSITIVE_TEST   (0b01 << 2)
+#define LSM6DS3_GYRO_NEGATIVE_TEST   (0b11 << 2)
 
 // Constants
 #define LSM6DS3_GYRO_CHIP_ID         0x69
 #define LSM6DS3TRC_GYRO_CHIP_ID      0x6A
+#define LSM6DS3_GYRO_FS_2000dps      (0b11 << 2)
 #define LSM6DS3_GYRO_ODR_104HZ       (0b0100 << 4)
+#define LSM6DS3_GYRO_ODR_208HZ       (0b0101 << 4)
 #define LSM6DS3_GYRO_INT1_DRDY_G     0b10
 #define LSM6DS3_GYRO_DRDY_GDA        0b10
 #define LSM6DS3_GYRO_DRDY_PULSE_MODE (1 << 7)
+#define LSM6DS3_GYRO_MIN_ST_LIMIT_mdps 150000.0f
+#define LSM6DS3_GYRO_MAX_ST_LIMIT_mdps 700000.0f
 
 
 class LSM6DS3_Gyro : public I2CSensor {
   uint8_t get_device_address() {return LSM6DS3_GYRO_I2C_ADDR;}
   cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3;
+
+  // self test functions
+  int self_test(int test_type);
+  void wait_for_data_ready();
+  void read_and_avg_data(float* val_st_off);
 public:
   LSM6DS3_Gyro(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
   int init();

selfdrive/sensord/tests/test_sensord.py

@@ -104,6 +104,9 @@ class TestSensord(unittest.TestCase):
     # make sure gpiochip0 is readable
     HARDWARE.initialize_hardware()
 
+    # enable LSM self test
+    os.environ["LSM_SELF_TEST"] = "1"
+
     # read initial sensor values every test case can use
     os.system("pkill -f ./_sensord")
     try:
@@ -118,6 +121,8 @@ class TestSensord(unittest.TestCase):
   @classmethod
   def tearDownClass(cls):
     managed_processes["sensord"].stop()
+    if "LSM_SELF_TEST" in os.environ:
+      del os.environ['LSM_SELF_TEST']
 
   def tearDown(self):
     managed_processes["sensord"].stop()