mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-29 13:54:07 +08:00
bmm150: cleanup, slightly increase data accuracy
- increased REP_XY and REP_Z: improves accuracy a bit, while increasing measurement time (still allows to go to 50Hz, previous max was 100Hz) - avoid extra transfer in measure() - extend regdump output - general code style cleanup
This commit is contained in:
Beat Küng
Daniel Agar
parent
1175c08829
commit
71b942392d
@ -44,32 +44,16 @@ BMM150::BMM150(I2CSPIBusOption bus_option, const int bus, int bus_frequency, enu
|
||||
I2C(DRV_MAG_DEVTYPE_BMM150, MODULE_NAME, bus, BMM150_SLAVE_ADDRESS, bus_frequency),
|
||||
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
|
||||
_px4_mag(get_device_id(), external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation),
|
||||
_call_interval(0),
|
||||
_collect_phase(false),
|
||||
_power(BMM150_DEFAULT_POWER_MODE),
|
||||
_output_data_rate(BMM150_DATA_RATE_30HZ),
|
||||
dig_x1(0),
|
||||
dig_y1(0),
|
||||
dig_x2(0),
|
||||
dig_y2(0),
|
||||
dig_z1(0),
|
||||
dig_z2(0),
|
||||
dig_z3(0),
|
||||
dig_z4(0),
|
||||
dig_xy1(0),
|
||||
dig_xy2(0),
|
||||
dig_xyz1(0),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
|
||||
_bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME": bad transfers")),
|
||||
_good_transfers(perf_alloc(PC_COUNT, MODULE_NAME": good transfers")),
|
||||
_measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")),
|
||||
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors")),
|
||||
_duplicates(perf_alloc(PC_COUNT, MODULE_NAME": duplicates")),
|
||||
_got_duplicate(false)
|
||||
_duplicates(perf_alloc(PC_COUNT, MODULE_NAME": duplicates"))
|
||||
{
|
||||
_px4_mag.set_external(external());
|
||||
|
||||
// default range scale from from uT to gauss
|
||||
// default range scale from uT to gauss
|
||||
_px4_mag.set_scale(0.01f);
|
||||
}
|
||||
|
||||
@ -193,7 +177,7 @@ int BMM150::collect()
|
||||
{
|
||||
_collect_phase = false;
|
||||
|
||||
uint8_t mag_data[8], status;
|
||||
uint8_t mag_data[8];
|
||||
uint16_t resistance, lsb, msb, msblsb;
|
||||
|
||||
int16_t x_raw{0};
|
||||
@ -207,8 +191,6 @@ int BMM150::collect()
|
||||
/* start collecting data */
|
||||
perf_begin(_sample_perf);
|
||||
|
||||
status = read_reg(BMM150_R_LSB);
|
||||
|
||||
/* Read Magnetometer data*/
|
||||
const hrt_abstime timestamp_sample = hrt_absolute_time();
|
||||
|
||||
@ -253,15 +235,12 @@ int BMM150::collect()
|
||||
resistance = (uint16_t)msblsb / 4;
|
||||
|
||||
/* Check whether data is new or old */
|
||||
if (!(status & 0x01)) {
|
||||
if (!(mag_data[6] & 0x01)) {
|
||||
perf_end(_sample_perf);
|
||||
perf_count(_duplicates);
|
||||
_got_duplicate = true;
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
_got_duplicate = false;
|
||||
|
||||
if (x_raw == 0 &&
|
||||
y_raw == 0 &&
|
||||
z_raw == 0 &&
|
||||
@ -279,10 +258,11 @@ int BMM150::collect()
|
||||
/* Compensation for X axis */
|
||||
if (x_raw != BMM150_FLIP_OVERFLOW_ADCVAL) {
|
||||
/* no overflow */
|
||||
if ((resistance != 0) && (dig_xyz1 != 0)) {
|
||||
x = ((dig_xyz1 * 16384.0 / resistance) - 16384.0);
|
||||
x = (((x_raw * ((((dig_xy2 * ((float)(x * x / (float)268435456.0)) + x * dig_xy1 /
|
||||
(float)16384.0)) + (float)256.0) * (dig_x2 + (float)160.0))) / (float)8192.0) + (dig_x1 * (float)8.0)) / (float)16.0;
|
||||
if ((resistance != 0) && (_dig_xyz1 != 0)) {
|
||||
/* this is not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/master/bmm150.c */
|
||||
x = ((_dig_xyz1 * 16384.0f / resistance) - 16384.0f);
|
||||
x = (((x_raw * ((((_dig_xy2 * ((float)(x * x / 268435456.0f)) + x * _dig_xy1 /
|
||||
16384.0f)) + 256.0f) * (_dig_x2 + 160.0f))) / 8192.0f) + (_dig_x1 * 8.0f)) / 16.0f;
|
||||
|
||||
} else {
|
||||
x = BMM150_OVERFLOW_OUTPUT_FLOAT;
|
||||
@ -295,11 +275,11 @@ int BMM150::collect()
|
||||
/* Compensation for Y axis */
|
||||
if (y_raw != BMM150_FLIP_OVERFLOW_ADCVAL) {
|
||||
/* no overflow */
|
||||
if ((resistance != 0) && (dig_xyz1 != 0)) {
|
||||
if ((resistance != 0) && (_dig_xyz1 != 0)) {
|
||||
|
||||
y = ((((float)dig_xyz1) * (float)16384.0 / resistance) - (float)16384.0);
|
||||
y = (((y_raw * ((((dig_xy2 * (y * y / (float)268435456.0) + y * dig_xy1 / (float)16384.0)) +
|
||||
(float)256.0) * (dig_y2 + (float)160.0))) / (float)8192.0) + (dig_y1 * (float)8.0)) / (float)16.0;
|
||||
y = ((((float)_dig_xyz1) * 16384.0f / resistance) - 16384.0f);
|
||||
y = (((y_raw * ((((_dig_xy2 * (y * y / 268435456.0f) + y * _dig_xy1 / 16384.0f)) +
|
||||
256.0f) * (_dig_y2 + 160.0f))) / 8192.0f) + (_dig_y1 * 8.0f)) / 16.0f;
|
||||
|
||||
|
||||
} else {
|
||||
@ -315,9 +295,9 @@ int BMM150::collect()
|
||||
/* Compensation for Z axis */
|
||||
if (z_raw != BMM150_HALL_OVERFLOW_ADCVAL) {
|
||||
/* no overflow */
|
||||
if ((dig_z2 != 0) && (dig_z1 != 0) && (dig_xyz1 != 0) && (resistance != 0)) {
|
||||
z = ((((z_raw - dig_z4) * (float)131072.0) - (dig_z3 * (resistance - dig_xyz1))) / ((
|
||||
dig_z2 + dig_z1 * resistance / (float)32768.0) * (float)4.0)) / (float)16.0;
|
||||
if ((_dig_z2 != 0) && (_dig_z1 != 0) && (_dig_xyz1 != 0) && (resistance != 0)) {
|
||||
z = ((((z_raw - _dig_z4) * 131072.0f) - (_dig_z3 * (resistance - _dig_xyz1))) / ((
|
||||
_dig_z2 + _dig_z1 * resistance / 32768.0f) * 4.0f)) / 16.0f;
|
||||
}
|
||||
|
||||
} else {
|
||||
@ -332,6 +312,11 @@ int BMM150::collect()
|
||||
|
||||
_px4_mag.update(timestamp_sample, x, y, z);
|
||||
|
||||
_last_raw_x = x_raw;
|
||||
_last_raw_y = y_raw;
|
||||
_last_raw_z = z_raw;
|
||||
_last_resistance = resistance;
|
||||
|
||||
perf_end(_sample_perf);
|
||||
return OK;
|
||||
}
|
||||
@ -469,42 +454,42 @@ int BMM150::init_trim_registers()
|
||||
uint8_t data[2] = {0};
|
||||
uint16_t msb, lsb, msblsb;
|
||||
|
||||
dig_x1 = read_reg(BMM150_DIG_X1);
|
||||
dig_y1 = read_reg(BMM150_DIG_Y1);
|
||||
dig_x2 = read_reg(BMM150_DIG_X2);
|
||||
dig_y2 = read_reg(BMM150_DIG_Y2);
|
||||
dig_xy1 = read_reg(BMM150_DIG_XY1);
|
||||
dig_xy2 = read_reg(BMM150_DIG_XY2);
|
||||
_dig_x1 = read_reg(BMM150_DIG_X1);
|
||||
_dig_y1 = read_reg(BMM150_DIG_Y1);
|
||||
_dig_x2 = read_reg(BMM150_DIG_X2);
|
||||
_dig_y2 = read_reg(BMM150_DIG_Y2);
|
||||
_dig_xy1 = read_reg(BMM150_DIG_XY1);
|
||||
_dig_xy2 = read_reg(BMM150_DIG_XY2);
|
||||
|
||||
ret += get_data(BMM150_DIG_Z1_LSB, data, 2);
|
||||
lsb = data[0];
|
||||
msb = (data[1] << 8);
|
||||
msb = ((uint16_t)data[1]) << 8;
|
||||
msblsb = (msb | lsb);
|
||||
dig_z1 = (uint16_t)msblsb;
|
||||
_dig_z1 = (uint16_t)msblsb;
|
||||
|
||||
ret += get_data(BMM150_DIG_Z2_LSB, data, 2);
|
||||
lsb = data[0];
|
||||
msb = ((int8_t)data[1] << 8);
|
||||
msb = ((uint16_t)data[1]) << 8;
|
||||
msblsb = (msb | lsb);
|
||||
dig_z2 = (int16_t)msblsb;
|
||||
_dig_z2 = (int16_t)msblsb;
|
||||
|
||||
ret += get_data(BMM150_DIG_Z3_LSB, data, 2);
|
||||
lsb = data[0];
|
||||
msb = ((int8_t)data[1] << 8);
|
||||
msb = ((uint16_t)data[1]) << 8;
|
||||
msblsb = (msb | lsb);
|
||||
dig_z3 = (int16_t)msblsb;
|
||||
_dig_z3 = (int16_t)msblsb;
|
||||
|
||||
ret += get_data(BMM150_DIG_Z4_LSB, data, 2);
|
||||
lsb = data[0];
|
||||
msb = ((int8_t)data[1] << 8);
|
||||
msb = ((uint16_t)data[1]) << 8;
|
||||
msblsb = (msb | lsb);
|
||||
dig_z4 = (int16_t)msblsb;
|
||||
_dig_z4 = (int16_t)msblsb;
|
||||
|
||||
ret += get_data(BMM150_DIG_XYZ1_LSB, data, 2);
|
||||
lsb = data[0];
|
||||
msb = ((data[1] & 0x7F) << 8);
|
||||
msb = ((uint16_t)(data[1] & 0x7F) << 8);
|
||||
msblsb = (msb | lsb);
|
||||
dig_xyz1 = (uint16_t)msblsb;
|
||||
_dig_xyz1 = (uint16_t)msblsb;
|
||||
|
||||
return ret;
|
||||
}
|
||||
@ -573,19 +558,21 @@ void BMM150::print_registers()
|
||||
|
||||
uint8_t reg = BMM150_CHIP_ID_REG;
|
||||
uint8_t v = read_reg(reg);
|
||||
printf("Chip Id: %02x:%02x ", (unsigned)reg, (unsigned)v);
|
||||
printf("\n");
|
||||
printf("Chip Id: %02x:%02x\n", (unsigned)reg, (unsigned)v);
|
||||
|
||||
reg = BMM150_INT_SETT_CTRL_REG;
|
||||
v = read_reg(reg);
|
||||
printf("Int sett Ctrl reg: %02x:%02x ", (unsigned)reg, (unsigned)v);
|
||||
printf("\n");
|
||||
printf("Int sett Ctrl reg: %02x:%02x\n", (unsigned)reg, (unsigned)v);
|
||||
|
||||
reg = BMM150_AXES_EN_CTRL_REG;
|
||||
v = read_reg(reg);
|
||||
printf("Axes En Ctrl reg: %02x:%02x ", (unsigned)reg, (unsigned)v);
|
||||
printf("\n");
|
||||
printf("Axes En Ctrl reg: %02x:%02x\n", (unsigned)reg, (unsigned)v);
|
||||
|
||||
printf("Trim data: %i %i, %i %i, %i %i %i %i, %i %i, %i\n",
|
||||
_dig_x1, _dig_y1, _dig_x2, _dig_y2, _dig_z1, _dig_z2, _dig_z3, _dig_z4, _dig_xy1, _dig_xy2, _dig_xyz1);
|
||||
|
||||
printf("Latest raw data: x=%i y=%i z=%i resistance=%i\n",
|
||||
_last_raw_x, _last_raw_y, _last_raw_z, _last_resistance);
|
||||
}
|
||||
|
||||
void BMM150::print_usage()
|
||||
|
||||
@ -116,13 +116,13 @@
|
||||
|
||||
/* Preset modes - Repetitions-XY Rates */
|
||||
#define BMM150_LOWPOWER_REPXY 1
|
||||
#define BMM150_REGULAR_REPXY 4
|
||||
#define BMM150_REGULAR_REPXY 10
|
||||
#define BMM150_HIGHACCURACY_REPXY 23
|
||||
#define BMM150_ENHANCED_REPXY 7
|
||||
|
||||
/* Preset modes - Repetitions-Z Rates */
|
||||
#define BMM150_LOWPOWER_REPZ 2
|
||||
#define BMM150_REGULAR_REPZ 14
|
||||
#define BMM150_REGULAR_REPZ 30
|
||||
#define BMM150_HIGHACCURACY_REPZ 82
|
||||
#define BMM150_ENHANCED_REPZ 26
|
||||
|
||||
@ -215,28 +215,33 @@ private:
|
||||
PX4Magnetometer _px4_mag;
|
||||
|
||||
/* altitude conversion calibration */
|
||||
unsigned _call_interval;
|
||||
unsigned _call_interval{0};
|
||||
|
||||
bool _collect_phase;
|
||||
bool _collect_phase{false};
|
||||
|
||||
uint8_t _power;
|
||||
uint8_t _output_data_rate;
|
||||
uint8_t _power{BMM150_DEFAULT_POWER_MODE};
|
||||
uint8_t _output_data_rate{BMM150_DEFAULT_POWER_MODE};
|
||||
|
||||
int8_t dig_x1;/**< trim x1 data */
|
||||
int8_t dig_y1;/**< trim y1 data */
|
||||
int8_t _dig_x1{0};/**< trim x1 data */
|
||||
int8_t _dig_y1{0};/**< trim y1 data */
|
||||
|
||||
int8_t dig_x2;/**< trim x2 data */
|
||||
int8_t dig_y2;/**< trim y2 data */
|
||||
int8_t _dig_x2{0};/**< trim x2 data */
|
||||
int8_t _dig_y2{0};/**< trim y2 data */
|
||||
|
||||
uint16_t dig_z1;/**< trim z1 data */
|
||||
int16_t dig_z2;/**< trim z2 data */
|
||||
int16_t dig_z3;/**< trim z3 data */
|
||||
int16_t dig_z4;/**< trim z4 data */
|
||||
uint16_t _dig_z1{0};/**< trim z1 data */
|
||||
int16_t _dig_z2{0};/**< trim z2 data */
|
||||
int16_t _dig_z3{0};/**< trim z3 data */
|
||||
int16_t _dig_z4{0};/**< trim z4 data */
|
||||
|
||||
uint8_t dig_xy1;/**< trim xy1 data */
|
||||
int8_t dig_xy2;/**< trim xy2 data */
|
||||
uint8_t _dig_xy1{0};/**< trim xy1 data */
|
||||
int8_t _dig_xy2{0};/**< trim xy2 data */
|
||||
|
||||
uint16_t dig_xyz1;/**< trim xyz1 data */
|
||||
uint16_t _dig_xyz1{0};/**< trim xyz1 data */
|
||||
|
||||
int16_t _last_raw_x{0};
|
||||
int16_t _last_raw_y{0};
|
||||
int16_t _last_raw_z{0};
|
||||
uint16_t _last_resistance{0};
|
||||
|
||||
perf_counter_t _sample_perf;
|
||||
perf_counter_t _bad_transfers;
|
||||
@ -245,8 +250,6 @@ private:
|
||||
perf_counter_t _comms_errors;
|
||||
perf_counter_t _duplicates;
|
||||
|
||||
bool _got_duplicate;
|
||||
|
||||
int init_trim_registers();
|
||||
|
||||
/**
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user