brianhansonxyz/oscarpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

openpilot v0.9.6 release

Browse Source 
date: 2024-01-12T10:13:37
master commit: ba792d576a49a0899b88a753fa1c52956bedf9e6
This commit is contained in:
FrogAi
2024-01-12 22:39:28 -07:00
commit 08e9fb1edc
1881 changed files with 653708 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
system/sensord/sensors/bmx055_accel.cc Normal file
View File

@@ -0,0 +1,78 @@
#include "system/sensord/sensors/bmx055_accel.h"
#include <cassert>
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
BMX055_Accel::BMX055_Accel(I2CBus *bus) : I2CSensor(bus) {}
int BMX055_Accel::init() {
int ret = verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID});
if (ret == -1) return -1;
ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_NORMAL_MODE);
if (ret < 0) {
goto fail;
}
// bmx055 accel has a 1.3ms wakeup time from deep suspend mode
util::sleep_for(10);
// High bandwidth
// ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_ENABLE);
// if (ret < 0) {
// goto fail;
// }
// Low bandwidth
ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_DISABLE);
if (ret < 0) {
goto fail;
}
ret = set_register(BMX055_ACCEL_I2C_REG_BW, BMX055_ACCEL_BW_125HZ);
if (ret < 0) {
goto fail;
}
fail:
return ret;
}
int BMX055_Accel::shutdown() {
// enter deep suspend mode (lowest power mode)
int ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_DEEP_SUSPEND);
if (ret < 0) {
LOGE("Could not move BMX055 ACCEL in deep suspend mode!");
}
return ret;
}
bool BMX055_Accel::get_event(MessageBuilder &msg, uint64_t ts) {
uint64_t start_time = nanos_since_boot();
uint8_t buffer[6];
int len = read_register(BMX055_ACCEL_I2C_REG_X_LSB, buffer, sizeof(buffer));
assert(len == 6);
// 12 bit = +-2g
float scale = 9.81 * 2.0f / (1 << 11);
float x = -read_12_bit(buffer[0], buffer[1]) * scale;
float y = -read_12_bit(buffer[2], buffer[3]) * scale;
float z = read_12_bit(buffer[4], buffer[5]) * scale;
auto event = msg.initEvent().initAccelerometer2();
event.setSource(cereal::SensorEventData::SensorSource::BMX055);
event.setVersion(1);
event.setSensor(SENSOR_ACCELEROMETER);
event.setType(SENSOR_TYPE_ACCELEROMETER);
event.setTimestamp(start_time);
float xyz[] = {x, y, z};
auto svec = event.initAcceleration();
svec.setV(xyz);
svec.setStatus(true);
return true;
}

41
system/sensord/sensors/bmx055_accel.h Normal file
View File

@@ -0,0 +1,41 @@
#pragma once
#include "system/sensord/sensors/i2c_sensor.h"
// Address of the chip on the bus
#define BMX055_ACCEL_I2C_ADDR 0x18
// Registers of the chip
#define BMX055_ACCEL_I2C_REG_ID 0x00
#define BMX055_ACCEL_I2C_REG_X_LSB 0x02
#define BMX055_ACCEL_I2C_REG_TEMP 0x08
#define BMX055_ACCEL_I2C_REG_BW 0x10
#define BMX055_ACCEL_I2C_REG_PMU 0x11
#define BMX055_ACCEL_I2C_REG_HBW 0x13
#define BMX055_ACCEL_I2C_REG_FIFO 0x3F
// Constants
#define BMX055_ACCEL_CHIP_ID 0xFA
#define BMX055_ACCEL_HBW_ENABLE 0b10000000
#define BMX055_ACCEL_HBW_DISABLE 0b00000000
#define BMX055_ACCEL_DEEP_SUSPEND 0b00100000
#define BMX055_ACCEL_NORMAL_MODE 0b00000000
#define BMX055_ACCEL_BW_7_81HZ 0b01000
#define BMX055_ACCEL_BW_15_63HZ 0b01001
#define BMX055_ACCEL_BW_31_25HZ 0b01010
#define BMX055_ACCEL_BW_62_5HZ 0b01011
#define BMX055_ACCEL_BW_125HZ 0b01100
#define BMX055_ACCEL_BW_250HZ 0b01101
#define BMX055_ACCEL_BW_500HZ 0b01110
#define BMX055_ACCEL_BW_1000HZ 0b01111
class BMX055_Accel : public I2CSensor {
uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;}
public:
BMX055_Accel(I2CBus *bus);
int init();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown();
};

88
system/sensord/sensors/bmx055_gyro.cc Normal file
View File

@@ -0,0 +1,88 @@
#include "system/sensord/sensors/bmx055_gyro.h"
#include <cassert>
#include <cmath>
#include "common/swaglog.h"
#include "common/util.h"
#define DEG2RAD(x) ((x) * M_PI / 180.0)
BMX055_Gyro::BMX055_Gyro(I2CBus *bus) : I2CSensor(bus) {}
int BMX055_Gyro::init() {
int ret = verify_chip_id(BMX055_GYRO_I2C_REG_ID, {BMX055_GYRO_CHIP_ID});
if (ret == -1) return -1;
ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_NORMAL_MODE);
if (ret < 0) {
goto fail;
}
// bmx055 gyro has a 30ms wakeup time from deep suspend mode
util::sleep_for(50);
// High bandwidth
// ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_ENABLE);
// if (ret < 0) {
// goto fail;
// }
// Low bandwidth
ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_DISABLE);
if (ret < 0) {
goto fail;
}
// 116 Hz filter
ret = set_register(BMX055_GYRO_I2C_REG_BW, BMX055_GYRO_BW_116HZ);
if (ret < 0) {
goto fail;
}
// +- 125 deg/s range
ret = set_register(BMX055_GYRO_I2C_REG_RANGE, BMX055_GYRO_RANGE_125);
if (ret < 0) {
goto fail;
}
fail:
return ret;
}
int BMX055_Gyro::shutdown() {
// enter deep suspend mode (lowest power mode)
int ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_DEEP_SUSPEND);
if (ret < 0) {
LOGE("Could not move BMX055 GYRO in deep suspend mode!");
}
return ret;
}
bool BMX055_Gyro::get_event(MessageBuilder &msg, uint64_t ts) {
uint64_t start_time = nanos_since_boot();
uint8_t buffer[6];
int len = read_register(BMX055_GYRO_I2C_REG_RATE_X_LSB, buffer, sizeof(buffer));
assert(len == 6);
// 16 bit = +- 125 deg/s
float scale = 125.0f / (1 << 15);
float x = -DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale);
float y = -DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale);
float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale);
auto event = msg.initEvent().initGyroscope2();
event.setSource(cereal::SensorEventData::SensorSource::BMX055);
event.setVersion(1);
event.setSensor(SENSOR_GYRO_UNCALIBRATED);
event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED);
event.setTimestamp(start_time);
float xyz[] = {x, y, z};
auto svec = event.initGyroUncalibrated();
svec.setV(xyz);
svec.setStatus(true);
return true;
}

41
system/sensord/sensors/bmx055_gyro.h Normal file
View File

@@ -0,0 +1,41 @@
#pragma once
#include "system/sensord/sensors/i2c_sensor.h"
// Address of the chip on the bus
#define BMX055_GYRO_I2C_ADDR 0x68
// Registers of the chip
#define BMX055_GYRO_I2C_REG_ID 0x00
#define BMX055_GYRO_I2C_REG_RATE_X_LSB 0x02
#define BMX055_GYRO_I2C_REG_RANGE 0x0F
#define BMX055_GYRO_I2C_REG_BW 0x10
#define BMX055_GYRO_I2C_REG_LPM1 0x11
#define BMX055_GYRO_I2C_REG_HBW 0x13
#define BMX055_GYRO_I2C_REG_FIFO 0x3F
// Constants
#define BMX055_GYRO_CHIP_ID 0x0F
#define BMX055_GYRO_HBW_ENABLE 0b10000000
#define BMX055_GYRO_HBW_DISABLE 0b00000000
#define BMX055_GYRO_DEEP_SUSPEND 0b00100000
#define BMX055_GYRO_NORMAL_MODE 0b00000000
#define BMX055_GYRO_RANGE_2000 0b000
#define BMX055_GYRO_RANGE_1000 0b001
#define BMX055_GYRO_RANGE_500 0b010
#define BMX055_GYRO_RANGE_250 0b011
#define BMX055_GYRO_RANGE_125 0b100
#define BMX055_GYRO_BW_116HZ 0b0010
class BMX055_Gyro : public I2CSensor {
uint8_t get_device_address() {return BMX055_GYRO_I2C_ADDR;}
public:
BMX055_Gyro(I2CBus *bus);
int init();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown();
};

256
system/sensord/sensors/bmx055_magn.cc Normal file
View File

@@ -0,0 +1,256 @@
#include "system/sensord/sensors/bmx055_magn.h"
#include <unistd.h>
#include <algorithm>
#include <cassert>
#include <cstdio>
#include "common/swaglog.h"
#include "common/util.h"
static int16_t compensate_x(trim_data_t trim_data, int16_t mag_data_x, uint16_t data_rhall) {
uint16_t process_comp_x0 = data_rhall;
int32_t process_comp_x1 = ((int32_t)trim_data.dig_xyz1) * 16384;
uint16_t process_comp_x2 = ((uint16_t)(process_comp_x1 / process_comp_x0)) - ((uint16_t)0x4000);
int16_t retval = ((int16_t)process_comp_x2);
int32_t process_comp_x3 = (((int32_t)retval) * ((int32_t)retval));
int32_t process_comp_x4 = (((int32_t)trim_data.dig_xy2) * (process_comp_x3 / 128));
int32_t process_comp_x5 = (int32_t)(((int16_t)trim_data.dig_xy1) * 128);
int32_t process_comp_x6 = ((int32_t)retval) * process_comp_x5;
int32_t process_comp_x7 = (((process_comp_x4 + process_comp_x6) / 512) + ((int32_t)0x100000));
int32_t process_comp_x8 = ((int32_t)(((int16_t)trim_data.dig_x2) + ((int16_t)0xA0)));
int32_t process_comp_x9 = ((process_comp_x7 * process_comp_x8) / 4096);
int32_t process_comp_x10 = ((int32_t)mag_data_x) * process_comp_x9;
retval = ((int16_t)(process_comp_x10 / 8192));
retval = (retval + (((int16_t)trim_data.dig_x1) * 8)) / 16;
return retval;
}
static int16_t compensate_y(trim_data_t trim_data, int16_t mag_data_y, uint16_t data_rhall) {
uint16_t process_comp_y0 = trim_data.dig_xyz1;
int32_t process_comp_y1 = (((int32_t)trim_data.dig_xyz1) * 16384) / process_comp_y0;
uint16_t process_comp_y2 = ((uint16_t)process_comp_y1) - ((uint16_t)0x4000);
int16_t retval = ((int16_t)process_comp_y2);
int32_t process_comp_y3 = ((int32_t) retval) * ((int32_t)retval);
int32_t process_comp_y4 = ((int32_t)trim_data.dig_xy2) * (process_comp_y3 / 128);
int32_t process_comp_y5 = ((int32_t)(((int16_t)trim_data.dig_xy1) * 128));
int32_t process_comp_y6 = ((process_comp_y4 + (((int32_t)retval) * process_comp_y5)) / 512);
int32_t process_comp_y7 = ((int32_t)(((int16_t)trim_data.dig_y2) + ((int16_t)0xA0)));
int32_t process_comp_y8 = (((process_comp_y6 + ((int32_t)0x100000)) * process_comp_y7) / 4096);
int32_t process_comp_y9 = (((int32_t)mag_data_y) * process_comp_y8);
retval = (int16_t)(process_comp_y9 / 8192);
retval = (retval + (((int16_t)trim_data.dig_y1) * 8)) / 16;
return retval;
}
static int16_t compensate_z(trim_data_t trim_data, int16_t mag_data_z, uint16_t data_rhall) {
int16_t process_comp_z0 = ((int16_t)data_rhall) - ((int16_t) trim_data.dig_xyz1);
int32_t process_comp_z1 = (((int32_t)trim_data.dig_z3) * ((int32_t)(process_comp_z0))) / 4;
int32_t process_comp_z2 = (((int32_t)(mag_data_z - trim_data.dig_z4)) * 32768);
int32_t process_comp_z3 = ((int32_t)trim_data.dig_z1) * (((int16_t)data_rhall) * 2);
int16_t process_comp_z4 = (int16_t)((process_comp_z3 + (32768)) / 65536);
int32_t retval = ((process_comp_z2 - process_comp_z1) / (trim_data.dig_z2 + process_comp_z4));
/* saturate result to +/- 2 micro-tesla */
retval = std::clamp(retval, -32767, 32767);
/* Conversion of LSB to micro-tesla*/
retval = retval / 16;
return (int16_t)retval;
}
BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {}
int BMX055_Magn::init() {
uint8_t trim_x1y1[2] = {0};
uint8_t trim_x2y2[2] = {0};
uint8_t trim_xy1xy2[2] = {0};
uint8_t trim_z1[2] = {0};
uint8_t trim_z2[2] = {0};
uint8_t trim_z3[2] = {0};
uint8_t trim_z4[2] = {0};
uint8_t trim_xyz1[2] = {0};
// suspend -> sleep
int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01);
if (ret < 0) {
LOGE("Enabling power failed: %d", ret);
goto fail;
}
util::sleep_for(5); // wait until the chip is powered on
ret = verify_chip_id(BMX055_MAGN_I2C_REG_ID, {BMX055_MAGN_CHIP_ID});
if (ret == -1) {
goto fail;
}
// Load magnetometer trim
ret = read_register(BMX055_MAGN_I2C_REG_DIG_X1, trim_x1y1, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_X2, trim_x2y2, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_XY2, trim_xy1xy2, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z1_LSB, trim_z1, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z2_LSB, trim_z2, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z3_LSB, trim_z3, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z4_LSB, trim_z4, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB, trim_xyz1, 2);
if (ret < 0) goto fail;
// Read trim data
trim_data.dig_x1 = trim_x1y1[0];
trim_data.dig_y1 = trim_x1y1[1];
trim_data.dig_x2 = trim_x2y2[0];
trim_data.dig_y2 = trim_x2y2[1];
trim_data.dig_xy1 = trim_xy1xy2[1]; // NB: MSB/LSB swapped
trim_data.dig_xy2 = trim_xy1xy2[0];
trim_data.dig_z1 = read_16_bit(trim_z1[0], trim_z1[1]);
trim_data.dig_z2 = read_16_bit(trim_z2[0], trim_z2[1]);
trim_data.dig_z3 = read_16_bit(trim_z3[0], trim_z3[1]);
trim_data.dig_z4 = read_16_bit(trim_z4[0], trim_z4[1]);
trim_data.dig_xyz1 = read_16_bit(trim_xyz1[0], trim_xyz1[1] & 0x7f);
assert(trim_data.dig_xyz1 != 0);
perform_self_test();
// f_max = 1 / (145us * nXY + 500us * NZ + 980us)
// Chose NXY = 7, NZ = 12, which gives 125 Hz,
// and has the same ratio as the high accuracy preset
ret = set_register(BMX055_MAGN_I2C_REG_REPXY, (7 - 1) / 2);
if (ret < 0) {
goto fail;
}
ret = set_register(BMX055_MAGN_I2C_REG_REPZ, 12 - 1);
if (ret < 0) {
goto fail;
}
return 0;
fail:
return ret;
}
int BMX055_Magn::shutdown() {
// move to suspend mode
int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0);
if (ret < 0) {
LOGE("Could not move BMX055 MAGN in suspend mode!");
}
return ret;
}
bool BMX055_Magn::perform_self_test() {
uint8_t buffer[8];
int16_t x, y;
int16_t neg_z, pos_z;
// Increase z reps for less false positives (~30 Hz ODR)
set_register(BMX055_MAGN_I2C_REG_REPXY, 1);
set_register(BMX055_MAGN_I2C_REG_REPZ, 64 - 1);
// Clean existing measurement
read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
uint8_t forced = BMX055_MAGN_FORCED;
// Negative current
set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b10) << 6));
util::sleep_for(100);
read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
parse_xyz(buffer, &x, &y, &neg_z);
// Positive current
set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b11) << 6));
util::sleep_for(100);
read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
parse_xyz(buffer, &x, &y, &pos_z);
// Put back in normal mode
set_register(BMX055_MAGN_I2C_REG_MAG, 0);
int16_t diff = pos_z - neg_z;
bool passed = (diff > 180) && (diff < 240);
if (!passed) {
LOGE("self test failed: neg %d pos %d diff %d", neg_z, pos_z, diff);
}
return passed;
}
bool BMX055_Magn::parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z) {
bool ready = buffer[6] & 0x1;
if (ready) {
int16_t mdata_x = (int16_t) (((int16_t)buffer[1] << 8) | buffer[0]) >> 3;
int16_t mdata_y = (int16_t) (((int16_t)buffer[3] << 8) | buffer[2]) >> 3;
int16_t mdata_z = (int16_t) (((int16_t)buffer[5] << 8) | buffer[4]) >> 1;
uint16_t data_r = (uint16_t) (((uint16_t)buffer[7] << 8) | buffer[6]) >> 2;
assert(data_r != 0);
*x = compensate_x(trim_data, mdata_x, data_r);
*y = compensate_y(trim_data, mdata_y, data_r);
*z = compensate_z(trim_data, mdata_z, data_r);
}
return ready;
}
bool BMX055_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
uint64_t start_time = nanos_since_boot();
uint8_t buffer[8];
int16_t _x, _y, x, y, z;
int len = read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
bool parsed = parse_xyz(buffer, &_x, &_y, &z);
if (parsed) {
auto event = msg.initEvent().initMagnetometer();
event.setSource(cereal::SensorEventData::SensorSource::BMX055);
event.setVersion(2);
event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);
event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
event.setTimestamp(start_time);
// Move magnetometer into same reference frame as accel/gryo
x = -_y;
y = _x;
// Axis convention
x = -x;
y = -y;
float xyz[] = {(float)x, (float)y, (float)z};
auto svec = event.initMagneticUncalibrated();
svec.setV(xyz);
svec.setStatus(true);
}
// The BMX055 Magnetometer has no FIFO mode. Self running mode only goes
// up to 30 Hz. Therefore we put in forced mode, and request measurements
// at a 100 Hz. When reading the registers we have to check the ready bit
// To verify the measurement was completed this cycle.
set_register(BMX055_MAGN_I2C_REG_MAG, BMX055_MAGN_FORCED);
return parsed;
}

64
system/sensord/sensors/bmx055_magn.h Normal file
View File

@@ -0,0 +1,64 @@
#pragma once
#include <tuple>
#include "system/sensord/sensors/i2c_sensor.h"
// Address of the chip on the bus
#define BMX055_MAGN_I2C_ADDR 0x10
// Registers of the chip
#define BMX055_MAGN_I2C_REG_ID 0x40
#define BMX055_MAGN_I2C_REG_PWR_0 0x4B
#define BMX055_MAGN_I2C_REG_MAG 0x4C
#define BMX055_MAGN_I2C_REG_DATAX_LSB 0x42
#define BMX055_MAGN_I2C_REG_RHALL_LSB 0x48
#define BMX055_MAGN_I2C_REG_REPXY 0x51
#define BMX055_MAGN_I2C_REG_REPZ 0x52
#define BMX055_MAGN_I2C_REG_DIG_X1 0x5D
#define BMX055_MAGN_I2C_REG_DIG_Y1 0x5E
#define BMX055_MAGN_I2C_REG_DIG_Z4_LSB 0x62
#define BMX055_MAGN_I2C_REG_DIG_Z4_MSB 0x63
#define BMX055_MAGN_I2C_REG_DIG_X2 0x64
#define BMX055_MAGN_I2C_REG_DIG_Y2 0x65
#define BMX055_MAGN_I2C_REG_DIG_Z2_LSB 0x68
#define BMX055_MAGN_I2C_REG_DIG_Z2_MSB 0x69
#define BMX055_MAGN_I2C_REG_DIG_Z1_LSB 0x6A
#define BMX055_MAGN_I2C_REG_DIG_Z1_MSB 0x6B
#define BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB 0x6C
#define BMX055_MAGN_I2C_REG_DIG_XYZ1_MSB 0x6D
#define BMX055_MAGN_I2C_REG_DIG_Z3_LSB 0x6E
#define BMX055_MAGN_I2C_REG_DIG_Z3_MSB 0x6F
#define BMX055_MAGN_I2C_REG_DIG_XY2 0x70
#define BMX055_MAGN_I2C_REG_DIG_XY1 0x71
// Constants
#define BMX055_MAGN_CHIP_ID 0x32
#define BMX055_MAGN_FORCED (0b01 << 1)
struct trim_data_t {
int8_t dig_x1;
int8_t dig_y1;
int8_t dig_x2;
int8_t dig_y2;
uint16_t dig_z1;
int16_t dig_z2;
int16_t dig_z3;
int16_t dig_z4;
uint8_t dig_xy1;
int8_t dig_xy2;
uint16_t dig_xyz1;
};
class BMX055_Magn : public I2CSensor{
uint8_t get_device_address() {return BMX055_MAGN_I2C_ADDR;}
trim_data_t trim_data = {0};
bool perform_self_test();
bool parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z);
public:
BMX055_Magn(I2CBus *bus);
int init();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown();
};

31
system/sensord/sensors/bmx055_temp.cc Normal file
View File

@@ -0,0 +1,31 @@
#include "system/sensord/sensors/bmx055_temp.h"
#include <cassert>
#include "system/sensord/sensors/bmx055_accel.h"
#include "common/swaglog.h"
#include "common/timing.h"
BMX055_Temp::BMX055_Temp(I2CBus *bus) : I2CSensor(bus) {}
int BMX055_Temp::init() {
return verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID}) == -1 ? -1 : 0;
}
bool BMX055_Temp::get_event(MessageBuilder &msg, uint64_t ts) {
uint64_t start_time = nanos_since_boot();
uint8_t buffer[1];
int len = read_register(BMX055_ACCEL_I2C_REG_TEMP, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
float temp = 23.0f + int8_t(buffer[0]) / 2.0f;
auto event = msg.initEvent().initTemperatureSensor();
event.setSource(cereal::SensorEventData::SensorSource::BMX055);
event.setVersion(1);
event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
event.setTimestamp(start_time);
event.setTemperature(temp);
return true;
}

13
system/sensord/sensors/bmx055_temp.h Normal file
View File

@@ -0,0 +1,13 @@
#pragma once
#include "system/sensord/sensors/bmx055_accel.h"
#include "system/sensord/sensors/i2c_sensor.h"
class BMX055_Temp : public I2CSensor {
uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;}
public:
BMX055_Temp(I2CBus *bus);
int init();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown() { return 0; }
};

18
system/sensord/sensors/constants.h Normal file
View File

@@ -0,0 +1,18 @@
#pragma once
#define SENSOR_ACCELEROMETER 1
#define SENSOR_MAGNETOMETER 2
#define SENSOR_MAGNETOMETER_UNCALIBRATED 3
#define SENSOR_GYRO 4
#define SENSOR_GYRO_UNCALIBRATED 5
#define SENSOR_LIGHT 7
#define SENSOR_TYPE_ACCELEROMETER 1
#define SENSOR_TYPE_GEOMAGNETIC_FIELD 2
#define SENSOR_TYPE_GYROSCOPE 4
#define SENSOR_TYPE_LIGHT 5
#define SENSOR_TYPE_AMBIENT_TEMPERATURE 13
#define SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED 14
#define SENSOR_TYPE_MAGNETIC_FIELD SENSOR_TYPE_GEOMAGNETIC_FIELD
#define SENSOR_TYPE_GYROSCOPE_UNCALIBRATED 16

50
system/sensord/sensors/i2c_sensor.cc Normal file
View File

@@ -0,0 +1,50 @@
#include "system/sensord/sensors/i2c_sensor.h"
int16_t read_12_bit(uint8_t lsb, uint8_t msb) {
uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb & 0xF0);
return int16_t(combined) / (1 << 4);
}
int16_t read_16_bit(uint8_t lsb, uint8_t msb) {
uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb);
return int16_t(combined);
}
int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0) {
uint32_t combined = (uint32_t(b0) << 16) | (uint32_t(b1) << 8) | uint32_t(b2);
return int32_t(combined) / (1 << 4);
}
I2CSensor::I2CSensor(I2CBus *bus, int gpio_nr, bool shared_gpio) :
bus(bus), gpio_nr(gpio_nr), shared_gpio(shared_gpio) {}
I2CSensor::~I2CSensor() {
if (gpio_fd != -1) {
close(gpio_fd);
}
}
int I2CSensor::read_register(uint register_address, uint8_t *buffer, uint8_t len) {
return bus->read_register(get_device_address(), register_address, buffer, len);
}
int I2CSensor::set_register(uint register_address, uint8_t data) {
return bus->set_register(get_device_address(), register_address, data);
}
int I2CSensor::init_gpio() {
if (shared_gpio || gpio_nr == 0) {
return 0;
}
gpio_fd = gpiochip_get_ro_value_fd("sensord", GPIOCHIP_INT, gpio_nr);
if (gpio_fd < 0) {
return -1;
}
return 0;
}
bool I2CSensor::has_interrupt_enabled() {
return gpio_nr != 0;
}

51
system/sensord/sensors/i2c_sensor.h Normal file
View File

@@ -0,0 +1,51 @@
#pragma once
#include <cstdint>
#include <unistd.h>
#include <vector>
#include "cereal/gen/cpp/log.capnp.h"
#include "common/i2c.h"
#include "common/gpio.h"
#include "common/swaglog.h"
#include "system/sensord/sensors/constants.h"
#include "system/sensord/sensors/sensor.h"
int16_t read_12_bit(uint8_t lsb, uint8_t msb);
int16_t read_16_bit(uint8_t lsb, uint8_t msb);
int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0);
class I2CSensor : public Sensor {
private:
I2CBus *bus;
int gpio_nr;
bool shared_gpio;
virtual uint8_t get_device_address() = 0;
public:
I2CSensor(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
~I2CSensor();
int read_register(uint register_address, uint8_t *buffer, uint8_t len);
int set_register(uint register_address, uint8_t data);
int init_gpio();
bool has_interrupt_enabled();
virtual int init() = 0;
virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
virtual int shutdown() = 0;
int verify_chip_id(uint8_t address, const std::vector<uint8_t> &expected_ids) {
uint8_t chip_id = 0;
int ret = read_register(address, &chip_id, 1);
if (ret < 0) {
LOGE("Reading chip ID failed: %d", ret);
return -1;
}
for (int i = 0; i < expected_ids.size(); ++i) {
if (chip_id == expected_ids[i]) return chip_id;
}
LOGE("Chip ID wrong. Got: %d, Expected %d", chip_id, expected_ids[0]);
return -1;
}
};

250
system/sensord/sensors/lsm6ds3_accel.cc Normal file
View File

@@ -0,0 +1,250 @@
#include "system/sensord/sensors/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() {
uint8_t value = 0;
bool do_self_test = false;
const char* env_lsm_selftest = std::getenv("LSM_SELF_TEST");
if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) {
do_self_test = true;
}
int ret = verify_chip_id(LSM6DS3_ACCEL_I2C_REG_ID, {LSM6DS3_ACCEL_CHIP_ID, LSM6DS3TRC_ACCEL_CHIP_ID});
if (ret == -1) return -1;
if (ret == LSM6DS3TRC_ACCEL_CHIP_ID) {
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) {
goto fail;
}
ret = set_register(LSM6DS3_ACCEL_I2C_REG_DRDY_CFG, LSM6DS3_ACCEL_DRDY_PULSE_MODE);
if (ret < 0) {
goto fail;
}
// enable data ready interrupt for accel on INT1
// (without resetting existing interrupts)
ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1);
if (ret < 0) {
goto fail;
}
value |= LSM6DS3_ACCEL_INT1_DRDY_XL;
ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value);
fail:
return ret;
}
int LSM6DS3_Accel::shutdown() {
int ret = 0;
// disable data ready interrupt for accel on INT1
uint8_t value = 0;
ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1);
if (ret < 0) {
goto fail;
}
value &= ~(LSM6DS3_ACCEL_INT1_DRDY_XL);
ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value);
if (ret < 0) {
LOGE("Could not disable lsm6ds3 acceleration interrupt!");
goto fail;
}
// enable power-down mode
value = 0;
ret = read_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, &value, 1);
if (ret < 0) {
goto fail;
}
value &= 0x0F;
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, value);
if (ret < 0) {
LOGE("Could not power-down lsm6ds3 accelerometer!");
goto fail;
}
fail:
return ret;
}
bool LSM6DS3_Accel::get_event(MessageBuilder &msg, uint64_t ts) {
// INT1 shared with gyro, check STATUS_REG who triggered
uint8_t status_reg = 0;
read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg));
if ((status_reg & LSM6DS3_ACCEL_DRDY_XLDA) == 0) {
return false;
}
uint8_t buffer[6];
int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
float scale = 9.81 * 2.0f / (1 << 15);
float x = read_16_bit(buffer[0], buffer[1]) * scale;
float y = read_16_bit(buffer[2], buffer[3]) * scale;
float z = read_16_bit(buffer[4], buffer[5]) * scale;
auto event = msg.initEvent().initAccelerometer();
event.setSource(source);
event.setVersion(1);
event.setSensor(SENSOR_ACCELEROMETER);
event.setType(SENSOR_TYPE_ACCELEROMETER);
event.setTimestamp(ts);
float xyz[] = {y, -x, z};
auto svec = event.initAcceleration();
svec.setV(xyz);
svec.setStatus(true);
return true;
}

49
system/sensord/sensors/lsm6ds3_accel.h Normal file
View File

@@ -0,0 +1,49 @@
#pragma once
#include "system/sensord/sensors/i2c_sensor.h"
// Address of the chip on the bus
#define LSM6DS3_ACCEL_I2C_ADDR 0x6A
// Registers of the chip
#define LSM6DS3_ACCEL_I2C_REG_DRDY_CFG 0x0B
#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();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown();
};

233
system/sensord/sensors/lsm6ds3_gyro.cc Normal file
View File

@@ -0,0 +1,233 @@
#include "system/sensord/sensors/lsm6ds3_gyro.h"
#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() {
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;
}
int ret = verify_chip_id(LSM6DS3_GYRO_I2C_REG_ID, {LSM6DS3_GYRO_CHIP_ID, LSM6DS3TRC_GYRO_CHIP_ID});
if (ret == -1) return -1;
if (ret == LSM6DS3TRC_GYRO_CHIP_ID) {
source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
}
ret = init_gpio();
if (ret < 0) {
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) {
goto fail;
}
ret = set_register(LSM6DS3_GYRO_I2C_REG_DRDY_CFG, LSM6DS3_GYRO_DRDY_PULSE_MODE);
if (ret < 0) {
goto fail;
}
// enable data ready interrupt for gyro on INT1
// (without resetting existing interrupts)
ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1);
if (ret < 0) {
goto fail;
}
value |= LSM6DS3_GYRO_INT1_DRDY_G;
ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value);
fail:
return ret;
}
int LSM6DS3_Gyro::shutdown() {
int ret = 0;
// 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) {
goto fail;
}
value &= ~(LSM6DS3_GYRO_INT1_DRDY_G);
ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value);
if (ret < 0) {
LOGE("Could not disable lsm6ds3 gyroscope interrupt!");
goto fail;
}
// enable power-down mode
value = 0;
ret = read_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, &value, 1);
if (ret < 0) {
goto fail;
}
value &= 0x0F;
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, value);
if (ret < 0) {
LOGE("Could not power-down lsm6ds3 gyroscope!");
goto fail;
}
fail:
return ret;
}
bool LSM6DS3_Gyro::get_event(MessageBuilder &msg, uint64_t ts) {
// INT1 shared with accel, check STATUS_REG who triggered
uint8_t status_reg = 0;
read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg));
if ((status_reg & LSM6DS3_GYRO_DRDY_GDA) == 0) {
return false;
}
uint8_t buffer[6];
int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
float scale = 8.75 / 1000.0;
float x = DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale);
float y = DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale);
float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale);
auto event = msg.initEvent().initGyroscope();
event.setSource(source);
event.setVersion(2);
event.setSensor(SENSOR_GYRO_UNCALIBRATED);
event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED);
event.setTimestamp(ts);
float xyz[] = {y, -x, z};
auto svec = event.initGyroUncalibrated();
svec.setV(xyz);
svec.setStatus(true);
return true;
}

45
system/sensord/sensors/lsm6ds3_gyro.h Normal file
View File

@@ -0,0 +1,45 @@
#pragma once
#include "system/sensord/sensors/i2c_sensor.h"
// Address of the chip on the bus
#define LSM6DS3_GYRO_I2C_ADDR 0x6A
// Registers of the chip
#define LSM6DS3_GYRO_I2C_REG_DRDY_CFG 0x0B
#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();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown();
};

37
system/sensord/sensors/lsm6ds3_temp.cc Normal file
View File

@@ -0,0 +1,37 @@
#include "system/sensord/sensors/lsm6ds3_temp.h"
#include <cassert>
#include "common/swaglog.h"
#include "common/timing.h"
LSM6DS3_Temp::LSM6DS3_Temp(I2CBus *bus) : I2CSensor(bus) {}
int LSM6DS3_Temp::init() {
int ret = verify_chip_id(LSM6DS3_TEMP_I2C_REG_ID, {LSM6DS3_TEMP_CHIP_ID, LSM6DS3TRC_TEMP_CHIP_ID});
if (ret == -1) return -1;
if (ret == LSM6DS3TRC_TEMP_CHIP_ID) {
source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
}
return 0;
}
bool LSM6DS3_Temp::get_event(MessageBuilder &msg, uint64_t ts) {
uint64_t start_time = nanos_since_boot();
uint8_t buffer[2];
int len = read_register(LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
float scale = (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) ? 256.0f : 16.0f;
float temp = 25.0f + read_16_bit(buffer[0], buffer[1]) / scale;
auto event = msg.initEvent().initTemperatureSensor();
event.setSource(source);
event.setVersion(1);
event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
event.setTimestamp(start_time);
event.setTemperature(temp);
return true;
}

26
system/sensord/sensors/lsm6ds3_temp.h Normal file
View File

@@ -0,0 +1,26 @@
#pragma once
#include "system/sensord/sensors/i2c_sensor.h"
// Address of the chip on the bus
#define LSM6DS3_TEMP_I2C_ADDR 0x6A
// Registers of the chip
#define LSM6DS3_TEMP_I2C_REG_ID 0x0F
#define LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L 0x20
// 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();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown() { return 0; }
};

108
system/sensord/sensors/mmc5603nj_magn.cc Normal file
View File

@@ -0,0 +1,108 @@
#include "system/sensord/sensors/mmc5603nj_magn.h"
#include <algorithm>
#include <cassert>
#include <vector>
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
MMC5603NJ_Magn::MMC5603NJ_Magn(I2CBus *bus) : I2CSensor(bus) {}
int MMC5603NJ_Magn::init() {
int ret = verify_chip_id(MMC5603NJ_I2C_REG_ID, {MMC5603NJ_CHIP_ID});
if (ret == -1) return -1;
// Set ODR to 0
ret = set_register(MMC5603NJ_I2C_REG_ODR, 0);
if (ret < 0) {
goto fail;
}
// Set BW to 0b01 for 1-150 Hz operation
ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_1, 0b01);
if (ret < 0) {
goto fail;
}
fail:
return ret;
}
int MMC5603NJ_Magn::shutdown() {
int ret = 0;
// disable auto reset of measurements
uint8_t value = 0;
ret = read_register(MMC5603NJ_I2C_REG_INTERNAL_0, &value, 1);
if (ret < 0) {
goto fail;
}
value &= ~(MMC5603NJ_CMM_FREQ_EN | MMC5603NJ_AUTO_SR_EN);
ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_0, value);
if (ret < 0) {
goto fail;
}
// set ODR to 0 to leave continuous mode
ret = set_register(MMC5603NJ_I2C_REG_ODR, 0);
if (ret < 0) {
goto fail;
}
return ret;
fail:
LOGE("Could not disable mmc5603nj auto set reset");
return ret;
}
void MMC5603NJ_Magn::start_measurement() {
set_register(MMC5603NJ_I2C_REG_INTERNAL_0, 0b01);
util::sleep_for(5);
}
std::vector<float> MMC5603NJ_Magn::read_measurement() {
int len;
uint8_t buffer[9];
len = read_register(MMC5603NJ_I2C_REG_XOUT0, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
float scale = 1.0 / 16384.0;
float x = (read_20_bit(buffer[6], buffer[1], buffer[0]) * scale) - 32.0;
float y = (read_20_bit(buffer[7], buffer[3], buffer[2]) * scale) - 32.0;
float z = (read_20_bit(buffer[8], buffer[5], buffer[4]) * scale) - 32.0;
std::vector<float> xyz = {x, y, z};
return xyz;
}
bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
uint64_t start_time = nanos_since_boot();
// SET - RESET cycle
set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_SET);
util::sleep_for(5);
MMC5603NJ_Magn::start_measurement();
std::vector<float> xyz = MMC5603NJ_Magn::read_measurement();
set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_RESET);
util::sleep_for(5);
MMC5603NJ_Magn::start_measurement();
std::vector<float> reset_xyz = MMC5603NJ_Magn::read_measurement();
auto event = msg.initEvent().initMagnetometer();
event.setSource(cereal::SensorEventData::SensorSource::MMC5603NJ);
event.setVersion(1);
event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);
event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
event.setTimestamp(start_time);
float vals[] = {xyz[0], xyz[1], xyz[2], reset_xyz[0], reset_xyz[1], reset_xyz[2]};
bool valid = true;
if (std::any_of(std::begin(vals), std::end(vals), [](float val) { return val == -32.0; })) {
valid = false;
}
auto svec = event.initMagneticUncalibrated();
svec.setV(vals);
svec.setStatus(valid);
return true;
}

37
system/sensord/sensors/mmc5603nj_magn.h Normal file
View File

@@ -0,0 +1,37 @@
#pragma once
#include <vector>
#include "system/sensord/sensors/i2c_sensor.h"
// Address of the chip on the bus
#define MMC5603NJ_I2C_ADDR 0x30
// Registers of the chip
#define MMC5603NJ_I2C_REG_XOUT0 0x00
#define MMC5603NJ_I2C_REG_ODR 0x1A
#define MMC5603NJ_I2C_REG_INTERNAL_0 0x1B
#define MMC5603NJ_I2C_REG_INTERNAL_1 0x1C
#define MMC5603NJ_I2C_REG_INTERNAL_2 0x1D
#define MMC5603NJ_I2C_REG_ID 0x39
// Constants
#define MMC5603NJ_CHIP_ID 0x10
#define MMC5603NJ_CMM_FREQ_EN (1 << 7)
#define MMC5603NJ_AUTO_SR_EN (1 << 5)
#define MMC5603NJ_CMM_EN (1 << 4)
#define MMC5603NJ_EN_PRD_SET (1 << 3)
#define MMC5603NJ_SET (1 << 3)
#define MMC5603NJ_RESET (1 << 4)
class MMC5603NJ_Magn : public I2CSensor {
private:
uint8_t get_device_address() {return MMC5603NJ_I2C_ADDR;}
void start_measurement();
std::vector<float> read_measurement();
public:
MMC5603NJ_Magn(I2CBus *bus);
int init();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown();
};

22
system/sensord/sensors/sensor.h Normal file
View File

@@ -0,0 +1,22 @@
#pragma once
#include "cereal/messaging/messaging.h"
class Sensor {
public:
int gpio_fd = -1;
uint64_t start_ts = 0;
uint64_t init_delay = 500e6; // default dealy 500ms
virtual ~Sensor() {}
virtual int init() = 0;
virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
virtual bool has_interrupt_enabled() = 0;
virtual int shutdown() = 0;
virtual bool is_data_valid(uint64_t current_ts) {
if (start_ts == 0) {
start_ts = current_ts;
}
return (current_ts - start_ts) > init_delay;
}
};