diff --git a/system/sensord/SConscript b/system/sensord/SConscript index e2dfb522c6..1222f0bfcd 100644 --- a/system/sensord/SConscript +++ b/system/sensord/SConscript @@ -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', diff --git a/system/sensord/sensors/bmx055_accel.cc b/system/sensord/sensors/bmx055_accel.cc deleted file mode 100644 index bcc31e1d6c..0000000000 --- a/system/sensord/sensors/bmx055_accel.cc +++ /dev/null @@ -1,85 +0,0 @@ -#include "system/sensord/sensors/bmx055_accel.h" - -#include - -#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; -} diff --git a/system/sensord/sensors/bmx055_accel.h b/system/sensord/sensors/bmx055_accel.h deleted file mode 100644 index 2cc316e992..0000000000 --- a/system/sensord/sensors/bmx055_accel.h +++ /dev/null @@ -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(); -}; diff --git a/system/sensord/sensors/bmx055_gyro.cc b/system/sensord/sensors/bmx055_gyro.cc deleted file mode 100644 index 0cc405f654..0000000000 --- a/system/sensord/sensors/bmx055_gyro.cc +++ /dev/null @@ -1,92 +0,0 @@ -#include "system/sensord/sensors/bmx055_gyro.h" - -#include -#include - -#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; -} diff --git a/system/sensord/sensors/bmx055_gyro.h b/system/sensord/sensors/bmx055_gyro.h deleted file mode 100644 index 7be3e56563..0000000000 --- a/system/sensord/sensors/bmx055_gyro.h +++ /dev/null @@ -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(); -}; diff --git a/system/sensord/sensors/bmx055_magn.cc b/system/sensord/sensors/bmx055_magn.cc deleted file mode 100644 index b498c5fe3d..0000000000 --- a/system/sensord/sensors/bmx055_magn.cc +++ /dev/null @@ -1,258 +0,0 @@ -#include "system/sensord/sensors/bmx055_magn.h" - -#include - -#include -#include -#include - -#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; -} diff --git a/system/sensord/sensors/bmx055_magn.h b/system/sensord/sensors/bmx055_magn.h deleted file mode 100644 index 15c4e734b9..0000000000 --- a/system/sensord/sensors/bmx055_magn.h +++ /dev/null @@ -1,64 +0,0 @@ -#pragma once -#include - -#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(); -}; diff --git a/system/sensord/sensors/bmx055_temp.cc b/system/sensord/sensors/bmx055_temp.cc deleted file mode 100644 index da7b86476c..0000000000 --- a/system/sensord/sensors/bmx055_temp.cc +++ /dev/null @@ -1,31 +0,0 @@ -#include "system/sensord/sensors/bmx055_temp.h" - -#include - -#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; -} diff --git a/system/sensord/sensors/bmx055_temp.h b/system/sensord/sensors/bmx055_temp.h deleted file mode 100644 index a2eabae395..0000000000 --- a/system/sensord/sensors/bmx055_temp.h +++ /dev/null @@ -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; } -}; diff --git a/system/sensord/sensors_qcom2.cc b/system/sensord/sensors_qcom2.cc index f9f51539c9..8ff9331b39 100644 --- a/system/sensord/sensors_qcom2.cc +++ b/system/sensord/sensors_qcom2.cc @@ -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> 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"}, diff --git a/system/sensord/tests/test_sensord.py b/system/sensord/tests/test_sensord.py index 15f1f2dc35..1dab652386 100644 --- a/system/sensord/tests/test_sensord.py +++ b/system/sensord/tests/test_sensord.py @@ -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), } diff --git a/system/sensord/tests/ttff_test.py b/system/sensord/tests/ttff_test.py deleted file mode 100755 index a9cc16d707..0000000000 --- a/system/sensord/tests/ttff_test.py +++ /dev/null @@ -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")