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

src/drivers/magnetometer/bmm150/bmm150.cpp

@@ -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")),
+	_duplicates(perf_alloc(PC_COUNT, MODULE_NAME": duplicates"))
-	_got_duplicate(false)
 {
 	_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)) {
+		if ((resistance != 0) && (_dig_xyz1 != 0)) {
-			x = ((dig_xyz1 * 16384.0 / resistance) - 16384.0);
+			/* this is not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/master/bmm150.c */
-			x = (((x_raw * ((((dig_xy2 * ((float)(x * x / (float)268435456.0)) + x * dig_xy1 /
+			x = ((_dig_xyz1 * 16384.0f / resistance) - 16384.0f);
-					   (float)16384.0)) + (float)256.0) * (dig_x2 + (float)160.0))) / (float)8192.0) + (dig_x1 * (float)8.0)) / (float)16.0;
+			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 = ((((float)_dig_xyz1) * 16384.0f / resistance) - 16384.0f);
-			y = (((y_raw * ((((dig_xy2 * (y * y / (float)268435456.0) + y * dig_xy1 / (float)16384.0)) +
+			y = (((y_raw * ((((_dig_xy2 * (y * y / 268435456.0f) + y * _dig_xy1 / 16384.0f)) +
-					 (float)256.0) * (dig_y2 + (float)160.0))) / (float)8192.0) + (dig_y1 * (float)8.0)) / (float)16.0;
+					 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)) {
+		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))) / ((
+			z = ((((z_raw - _dig_z4) * 131072.0f) - (_dig_z3 * (resistance - _dig_xyz1))) / ((
-						dig_z2 + dig_z1 * resistance / (float)32768.0) * (float)4.0)) / (float)16.0;
+						_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_x1 = read_reg(BMM150_DIG_X1);
-	dig_y1 = read_reg(BMM150_DIG_Y1);
+	_dig_y1 = read_reg(BMM150_DIG_Y1);
-	dig_x2 = read_reg(BMM150_DIG_X2);
+	_dig_x2 = read_reg(BMM150_DIG_X2);
-	dig_y2 = read_reg(BMM150_DIG_Y2);
+	_dig_y2 = read_reg(BMM150_DIG_Y2);
-	dig_xy1 = read_reg(BMM150_DIG_XY1);
+	_dig_xy1 = read_reg(BMM150_DIG_XY1);
-	dig_xy2 = read_reg(BMM150_DIG_XY2);
+	_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("Chip Id: %02x:%02x\n", (unsigned)reg, (unsigned)v);
-	printf("\n");
 
 	reg = BMM150_INT_SETT_CTRL_REG;
 	v = read_reg(reg);
-	printf("Int sett Ctrl reg: %02x:%02x ", (unsigned)reg, (unsigned)v);
+	printf("Int sett Ctrl reg: %02x:%02x\n", (unsigned)reg, (unsigned)v);
-	printf("\n");
 
 	reg = BMM150_AXES_EN_CTRL_REG;
 	v = read_reg(reg);
-	printf("Axes En Ctrl reg: %02x:%02x ", (unsigned)reg, (unsigned)v);
+	printf("Axes En Ctrl reg: %02x:%02x\n", (unsigned)reg, (unsigned)v);
-	printf("\n");
 
+	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()

src/drivers/magnetometer/bmm150/bmm150.hpp

@@ -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     _power{BMM150_DEFAULT_POWER_MODE};
-	uint8_t     _output_data_rate;
+	uint8_t     _output_data_rate{BMM150_DEFAULT_POWER_MODE};
 
-	int8_t dig_x1;/**< trim x1 data */
+	int8_t _dig_x1{0};/**< trim x1 data */
-	int8_t dig_y1;/**< trim y1 data */
+	int8_t _dig_y1{0};/**< trim y1 data */
 
-	int8_t dig_x2;/**< trim x2 data */
+	int8_t _dig_x2{0};/**< trim x2 data */
-	int8_t dig_y2;/**< trim y2 data */
+	int8_t _dig_y2{0};/**< trim y2 data */
 
-	uint16_t dig_z1;/**< trim z1 data */
+	uint16_t _dig_z1{0};/**< trim z1 data */
-	int16_t dig_z2;/**< trim z2 data */
+	int16_t _dig_z2{0};/**< trim z2 data */
-	int16_t dig_z3;/**< trim z3 data */
+	int16_t _dig_z3{0};/**< trim z3 data */
-	int16_t dig_z4;/**< trim z4 data */
+	int16_t _dig_z4{0};/**< trim z4 data */
 
-	uint8_t dig_xy1;/**< trim xy1 data */
+	uint8_t _dig_xy1{0};/**< trim xy1 data */
-	int8_t dig_xy2;/**< trim xy2 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();
 
 	/**