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

BMX055 magnetometer fixes and advanced self-test (#20118)

Browse Source 
* bmx055 magnetometer selftest

* try this

* cleanup trim data reading

* parsing fixed now?

* make self test work and increase numreps
old-commit-hash: 17f3683228
commatwo_master
Willem Melching 5 years ago committed by GitHub
parent 3ad3cd5d4b
commit 7f687f5dcd
2 changed files with 123 additions and 63 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. 164
      selfdrive/sensord/sensors/bmx055_magn.cc
  2. 22
      selfdrive/sensord/sensors/bmx055_magn.hpp

164
selfdrive/sensord/sensors/bmx055_magn.cc
Unescape View File

@ -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);

22
selfdrive/sensord/sensors/bmx055_magn.hpp
Unescape View File

@ -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();

Write Preview
Loading…
Cancel
Save