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 numrepspull/20159/head
Willem Melching
GitHub
parent
a53c361bd4
commit
17f3683228
|
|
@ -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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
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));
|
||||
|
||||
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]);
|
||||
|
||||
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
|
||||
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -1,4 +1,5 @@
|
|||
#pragma once
|
||||
#include <tuple>
|
||||
|
||||
#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();
|
||||
|
|
|
|||
Loading…
Reference in New Issue