From 17f3683228bdea2d6f57bc108135e8cda30ce453 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Thu, 25 Feb 2021 16:14:54 +0100 Subject: [PATCH] BMX055 magnetometer fixes and advanced self-test (#20118) * bmx055 magnetometer selftest * try this * cleanup trim data reading * parsing fixed now? * make self test work and increase numreps --- selfdrive/sensord/sensors/bmx055_magn.cc | 164 ++++++++++++++-------- selfdrive/sensord/sensors/bmx055_magn.hpp | 22 ++- 2 files changed, 123 insertions(+), 63 deletions(-) diff --git a/selfdrive/sensord/sensors/bmx055_magn.cc b/selfdrive/sensord/sensors/bmx055_magn.cc index f055703131..41b41467b7 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.cc +++ b/selfdrive/sensord/sensors/bmx055_magn.cc @@ -62,31 +62,19 @@ static int16_t compensate_z(trim_data_t trim_data, int16_t mag_data_z, uint16_t return (int16_t)retval; } -static int16_t parse_xy(uint8_t lsb, uint8_t msb){ - // 13 bit - uint16_t combined = (uint16_t(msb) << 5) | uint16_t(lsb >> 3); - return int16_t(combined << 3) / (1 << 3); -} - -static int16_t parse_z(uint8_t lsb, uint8_t msb){ - // 15 bit - uint16_t combined = (uint16_t(msb) << 7) | uint16_t(lsb >> 1); - return int16_t(combined << 1) / (1 << 1); -} - -static uint16_t parse_rhall(uint8_t lsb, uint8_t msb){ - // 14 bit - return (uint16_t(msb) << 6) | uint16_t(lsb >> 2); -} - BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {} int BMX055_Magn::init(){ int ret; uint8_t buffer[1]; uint8_t trim_x1y1[2] = {0}; - uint8_t trim_xyz_data[4] = {0}; - uint8_t trim_xy1xy2[10] = {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 ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01); @@ -110,83 +98,139 @@ int BMX055_Magn::init(){ // 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_Z4, trim_xyz_data, 4); - if(ret < 0){ - goto fail; - } - ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z2, trim_xy1xy2, 10); - if(ret < 0){ - goto fail; - } + 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 = (int8_t)trim_x1y1[0]; - trim_data.dig_y1 = (int8_t)trim_x1y1[1]; + trim_data.dig_x1 = trim_x1y1[0]; + trim_data.dig_y1 = trim_x1y1[1]; - trim_data.dig_x2 = (int8_t)trim_xyz_data[2]; - trim_data.dig_y2 = (int8_t)trim_xyz_data[3]; + trim_data.dig_x2 = trim_x2y2[0]; + trim_data.dig_y2 = trim_x2y2[1]; - trim_data.dig_z1 = read_16_bit(trim_xy1xy2[2], trim_xy1xy2[3]); - trim_data.dig_z2 = read_16_bit(trim_xy1xy2[0], trim_xy1xy2[1]); - trim_data.dig_z3 = read_16_bit(trim_xy1xy2[6], trim_xy1xy2[7]); - trim_data.dig_z4 = read_16_bit(trim_xyz_data[0], trim_xyz_data[1]); + trim_data.dig_xy1 = trim_xy1xy2[1]; // NB: MSB/LSB swapped + trim_data.dig_xy2 = trim_xy1xy2[0]; - trim_data.dig_xy1 = trim_xy1xy2[9]; - trim_data.dig_xy2 = (int8_t)trim_xy1xy2[8]; + 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_xy1xy2[4], trim_xy1xy2[5] & 0x7f); + trim_data.dig_xyz1 = read_16_bit(trim_xyz1[0], trim_xyz1[1] & 0x7f); + assert(trim_data.dig_xyz1 != 0); - // TODO: perform self-test + perform_self_test(); - // 9 REPXY and 15 REPZ for 100 Hz - // 3 REPXY and 3 REPZ for > 300 Hz - ret = set_register(BMX055_MAGN_I2C_REG_REPXY, (3 - 1) / 2); + // 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, 3 - 1); + ret = set_register(BMX055_MAGN_I2C_REG_REPZ, 12 - 1); if (ret < 0){ goto fail; } + return 0; fail: return ret; } - -void BMX055_Magn::get_event(cereal::SensorEventData::Builder &event){ - uint64_t start_time = nanos_since_boot(); +bool BMX055_Magn::perform_self_test(){ uint8_t buffer[8]; + int16_t x, y; + int16_t neg_z, pos_z; - int len = read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer)); - assert(len == sizeof(buffer)); + // 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 x = parse_xy(buffer[0], buffer[1]); - int16_t y = parse_xy(buffer[2], buffer[3]); - int16_t z = parse_z(buffer[4], buffer[5]); - int16_t rhall = parse_rhall(buffer[5], buffer[6]); + 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; +} - x = compensate_x(trim_data, x, rhall); - y = compensate_y(trim_data, y, rhall); - z = compensate_z(trim_data, z, rhall); - // TODO: convert to micro tesla: - // https://github.com/BoschSensortec/BMM150-Sensor-API/blob/master/bmm150.c#L1614 +void BMX055_Magn::get_event(cereal::SensorEventData::Builder &event){ + uint64_t start_time = nanos_since_boot(); + uint8_t buffer[8]; + int16_t x, y, z; + + int len = read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + if (parse_xyz(buffer, &x, &y, &z)){ event.setSource(cereal::SensorEventData::SensorSource::BMX055); event.setVersion(1); event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED); event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED); event.setTimestamp(start_time); + // Axis convention + x = -x; + y = -y; + float xyz[] = {(float)x, (float)y, (float)z}; auto svec = event.initMagneticUncalibrated(); svec.setV(xyz); diff --git a/selfdrive/sensord/sensors/bmx055_magn.hpp b/selfdrive/sensord/sensors/bmx055_magn.hpp index 51fdd72789..0e11934e6c 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.hpp +++ b/selfdrive/sensord/sensors/bmx055_magn.hpp @@ -1,4 +1,5 @@ #pragma once +#include #include "sensors/i2c_sensor.hpp" @@ -14,9 +15,22 @@ #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_Z4 0x62 -#define BMX055_MAGN_I2C_REG_DIG_Z2 0x68 +#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 @@ -40,6 +54,8 @@ struct trim_data_t { 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();