mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-03 12:32:06 +08:00
sensord: cleanup, prep for rewrite (#35352)
* rm bmx * thanks claude * fix * fix mypy
This commit is contained in:
Adeeb Shihadeh
@@ -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',
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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();
|
||||
};
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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();
|
||||
};
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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();
|
||||
};
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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; }
|
||||
};
|
||||
@@ -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"},
|
||||
|
||||
@@ -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),
|
||||
}
|
||||
|
||||
@@ -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")
|
||||
Reference in New Issue
Block a user