diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.cc b/selfdrive/sensord/sensors/lsm6ds3_accel.cc index 27cd4d0c70..c19e3de7ed 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_accel.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_accel.cc @@ -1,17 +1,132 @@ #include "lsm6ds3_accel.h" #include +#include +#include #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) { diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.h b/selfdrive/sensord/sensors/lsm6ds3_accel.h index 84084fc916..c3f66f5803 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_accel.h +++ b/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(); diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc index 014a72bb73..f306be0fe8 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc @@ -2,19 +2,120 @@ #include #include +#include #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) { diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.h b/selfdrive/sensord/sensors/lsm6ds3_gyro.h index 6c61ffcef2..220e6b0cec 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_gyro.h +++ b/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(); diff --git a/selfdrive/sensord/tests/test_sensord.py b/selfdrive/sensord/tests/test_sensord.py index 82bd28446b..c6fe33129a 100755 --- a/selfdrive/sensord/tests/test_sensord.py +++ b/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()