spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

LSM6DS3TR-C support (#21563) 

* add chip id

* fix temp scale

* cleanup source
pull/21567/head
Willem Melching 2021-07-12 19:37:30 +02:00 committed by GitHub
parent b0e3c35edd
commit fd47239274
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 28 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cereal

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

8
selfdrive/sensord/sensors/lsm6ds3_accel.cc
View File

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

2
selfdrive/sensord/sensors/lsm6ds3_accel.h
View File

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

8
selfdrive/sensord/sensors/lsm6ds3_gyro.cc
View File

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

2
selfdrive/sensord/sensors/lsm6ds3_gyro.h
View File

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

11
selfdrive/sensord/sensors/lsm6ds3_temp.cc
View File

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

3
selfdrive/sensord/sensors/lsm6ds3_temp.h
View File

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