sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

sensord: cleanup, prep for rewrite (#35352)

Browse Source 
* rm bmx

* thanks claude

* fix

* fix mypy
pull/35354/head
Adeeb Shihadeh 3 months ago committed by GitHub
parent 742e30495a
commit 50aaa69137
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
12 changed files with 5 additions and 711 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      system/sensord/SConscript
  2. 85
      system/sensord/sensors/bmx055_accel.cc
  3. 41
      system/sensord/sensors/bmx055_accel.h
  4. 92
      system/sensord/sensors/bmx055_gyro.cc
  5. 41
      system/sensord/sensors/bmx055_gyro.h
  6. 258
      system/sensord/sensors/bmx055_magn.cc
  7. 64
      system/sensord/sensors/bmx055_magn.h
  8. 31
      system/sensord/sensors/bmx055_temp.cc
  9. 13
      system/sensord/sensors/bmx055_temp.h
  10. 9
      system/sensord/sensors_qcom2.cc
  11. 30
      system/sensord/tests/test_sensord.py
  12. 48
      system/sensord/tests/ttff_test.py

4
system/sensord/SConscript
Unescape View File

@ -2,10 +2,6 @@ Import('env', 'arch', 'common', 'messaging')
sensors = [
'sensors/i2c_sensor.cc',
'sensors/bmx055_accel.cc',
'sensors/bmx055_gyro.cc',
'sensors/bmx055_magn.cc',
'sensors/bmx055_temp.cc',
'sensors/lsm6ds3_accel.cc',
'sensors/lsm6ds3_gyro.cc',
'sensors/lsm6ds3_temp.cc',

85
system/sensord/sensors/bmx055_accel.cc
Unescape View File

@ -1,85 +0,0 @@
#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) {
goto fail;
}
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;
}
enabled = true;
fail:
return ret;
}
int BMX055_Accel::shutdown() {
if (!enabled) return 0;
// 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
Unescape View File

@ -1,41 +0,0 @@
#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();
};

92
system/sensord/sensors/bmx055_gyro.cc
Unescape View File

@ -1,92 +0,0 @@
#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;
}
enabled = true;
fail:
return ret;
}
int BMX055_Gyro::shutdown() {
if (!enabled) return 0;
// 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
Unescape View File

@ -1,41 +0,0 @@
#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();
};

258
system/sensord/sensors/bmx055_magn.cc
Unescape View File

@ -1,258 +0,0 @@
#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) {
LOGD("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;
}
enabled = true;
return 0;
fail:
return ret;
}
int BMX055_Magn::shutdown() {
if (!enabled) return 0;
// 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
Unescape View File

@ -1,64 +0,0 @@
#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
Unescape View File

@ -1,31 +0,0 @@
#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
Unescape View File

@ -1,13 +0,0 @@
#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; }
};

9
system/sensord/sensors_qcom2.cc
Unescape View File

@ -14,10 +14,6 @@
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
#include "system/sensord/sensors/bmx055_accel.h"
#include "system/sensord/sensors/bmx055_gyro.h"
#include "system/sensord/sensors/bmx055_magn.h"
#include "system/sensord/sensors/bmx055_temp.h"
#include "system/sensord/sensors/constants.h"
#include "system/sensord/sensors/lsm6ds3_accel.h"
#include "system/sensord/sensors/lsm6ds3_gyro.h"
@ -117,11 +113,6 @@ void polling_loop(Sensor *sensor, std::string msg_name) {
int sensor_loop(I2CBus *i2c_bus_imu) {
// Sensor init
std::vector<std::tuple<Sensor *, std::string>> sensors_init = {
{new BMX055_Accel(i2c_bus_imu), "accelerometer2"},
{new BMX055_Gyro(i2c_bus_imu), "gyroscope2"},
{new BMX055_Magn(i2c_bus_imu), "magnetometer"},
{new BMX055_Temp(i2c_bus_imu), "temperatureSensor2"},
{new LSM6DS3_Accel(i2c_bus_imu, GPIO_LSM_INT), "accelerometer"},
{new LSM6DS3_Gyro(i2c_bus_imu, GPIO_LSM_INT, true), "gyroscope"},
{new LSM6DS3_Temp(i2c_bus_imu), "temperatureSensor"},

30
system/sensord/tests/test_sensord.py
Unescape View File

@ -12,13 +12,6 @@ from openpilot.common.timeout import Timeout
from openpilot.system.hardware import HARDWARE
from openpilot.system.manager.process_config import managed_processes
BMX = {
('bmx055', 'acceleration'),
('bmx055', 'gyroUncalibrated'),
('bmx055', 'magneticUncalibrated'),
('bmx055', 'temperature'),
}
LSM = {
('lsm6ds3', 'acceleration'),
('lsm6ds3', 'gyroUncalibrated'),
@ -30,17 +23,11 @@ MMC = {
('mmc5603nj', 'magneticUncalibrated'),
}
SENSOR_CONFIGURATIONS: list[set] = [
BMX | LSM,
MMC | LSM,
BMX | LSM_C,
MMC| LSM_C,
]
if HARDWARE.get_device_type() == "mici":
SENSOR_CONFIGURATIONS = [
LSM,
LSM_C,
]
SENSOR_CONFIGURATIONS: list[set] = {
"mici": [LSM, LSM_C],
"tizi": [MMC | LSM, MMC | LSM_C],
"tici": [LSM, LSM_C, MMC | LSM, MMC | LSM_C],
}.get(HARDWARE.get_device_type(), [])
Sensor = log.SensorEventData.SensorSource
SensorConfig = namedtuple('SensorConfig', ['type', 'sanity_min', 'sanity_max'])
@ -57,13 +44,6 @@ ALL_SENSORS = {
SensorConfig("temperature", 0, 60),
},
Sensor.bmx055: {
SensorConfig("acceleration", 5, 15),
SensorConfig("gyroUncalibrated", 0, .2),
SensorConfig("magneticUncalibrated", 0, 300),
SensorConfig("temperature", 0, 60),
},
Sensor.mmc5603nj: {
SensorConfig("magneticUncalibrated", 0, 300),
}

48
system/sensord/tests/ttff_test.py
Unescape View File

@ -1,48 +0,0 @@
#!/usr/bin/env python3
import time
import atexit
from cereal import messaging
from openpilot.system.manager.process_config import managed_processes
TIMEOUT = 10*60
def kill():
for proc in ['ubloxd', 'pigeond']:
managed_processes[proc].stop(retry=True, block=True)
if __name__ == "__main__":
# start ubloxd
managed_processes['ubloxd'].start()
atexit.register(kill)
sm = messaging.SubMaster(['ubloxGnss'])
times = []
for i in range(20):
# start pigeond
st = time.monotonic()
managed_processes['pigeond'].start()
# wait for a >4 satellite fix
while True:
sm.update(0)
msg = sm['ubloxGnss']
if msg.which() == 'measurementReport' and sm.updated["ubloxGnss"]:
report = msg.measurementReport
if report.numMeas > 4:
times.append(time.monotonic() - st)
print(f"\033[94m{i}: Got a fix in {round(times[-1], 2)} seconds\033[0m")
break
if time.monotonic() - st > TIMEOUT:
raise TimeoutError("\033[91mFailed to get a fix in {TIMEOUT} seconds!\033[0m")
time.sleep(0.1)
# stop pigeond
managed_processes['pigeond'].stop(retry=True, block=True)
time.sleep(20)
print(f"\033[92mAverage TTFF: {round(sum(times) / len(times), 2)}s\033[0m")
Write Preview
Loading…
Cancel
Save