mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-07-08 22:42:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensord: lsm self test (#25855)

Browse Source 
* 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>
This commit is contained in:
Kurt Nistelberger
2022-11-05 04:36:44 +01:00
committed by GitHub
parent e4342b5e16
commit e4cd8a15db
5 changed files with 281 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/sensord/sensors/lsm6ds3_accel.cc
+133
View File
@@ -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
+17 -1
View File
@@ -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
+114 -1
View File
@@ -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
+12
View File
@@ -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
+5
View File
@@ -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()