bmm150: minor changes to match reference driver

src/drivers/magnetometer/bosch/bmm150/BMM150.cpp

@@ -108,15 +108,18 @@ float BMM150::compensate_x(int16_t mag_data_x, uint16_t data_rhall)
 {
 	float retval = 0;
 
-	// Processing compensation equations
-	//  not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1624-L1633 as of 2020-09-25
-	float process_comp_x0 = (((float)_trim_data.dig_xyz1) * 16384.f / data_rhall);
-	retval = (process_comp_x0 - 16384.f);
-	float process_comp_x1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.f);
-	float process_comp_x2 = process_comp_x1 + retval * ((float)_trim_data.dig_xy1) / 16384.f;
-	float process_comp_x3 = ((float)_trim_data.dig_x2) + 160.f;
-	float process_comp_x4 = mag_data_x * ((process_comp_x2 + 256.f) * process_comp_x3);
-	retval = ((process_comp_x4 / 8192.f) + (((float)_trim_data.dig_x1) * 8.f)) / 16.f;
+	// Overflow condition check
+	if ((mag_data_x != OVERFLOW_XYAXES) && (data_rhall != 0) && (_trim_data.dig_xyz1 != 0)) {
+		// Processing compensation equations
+		//  not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1624-L1633 as of 2020-09-25
+		float process_comp_x0 = (((float)_trim_data.dig_xyz1) * 16384.0f / data_rhall);
+		retval = (process_comp_x0 - 16384.0f);
+		float process_comp_x1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.0f);
+		float process_comp_x2 = process_comp_x1 + retval * ((float)_trim_data.dig_xy1) / 16384.0f;
+		float process_comp_x3 = ((float)_trim_data.dig_x2) + 160.0f;
+		float process_comp_x4 = mag_data_x * ((process_comp_x2 + 256.0f) * process_comp_x3);
+		retval = ((process_comp_x4 / 8192.0f) + (((float)_trim_data.dig_x1) * 8.0f)) / 16.0f;
+	}
 
 	return retval;
 }
@@ -125,15 +128,17 @@ float BMM150::compensate_y(int16_t mag_data_y, uint16_t data_rhall)
 {
 	float retval = 0;
 
-	// Processing compensation equations
-	//  not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1660-L1667 as of 2020-09-25
-	float process_comp_y0 = ((float)_trim_data.dig_xyz1) * 16384.f / data_rhall;
-	retval = process_comp_y0 - 16384.f;
-	float process_comp_y1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.f);
-	float process_comp_y2 = process_comp_y1 + retval * ((float)_trim_data.dig_xy1) / 16384.f;
-	float process_comp_y3 = ((float)_trim_data.dig_y2) + 160.0f;
-	float process_comp_y4 = mag_data_y * (((process_comp_y2) + 256.f) * process_comp_y3);
-	retval = ((process_comp_y4 / 8192.f) + (((float)_trim_data.dig_y1) * 8.f)) / 16.f;
+	// Overflow condition check
+	if ((mag_data_y != OVERFLOW_XYAXES) && (data_rhall != 0) && (_trim_data.dig_xyz1 != 0)) {
+		// Processing compensation equations
+		float process_comp_y0 = ((float)_trim_data.dig_xyz1) * 16384.0f / data_rhall;
+		retval = process_comp_y0 - 16384.0f;
+		float process_comp_y1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.0f);
+		float process_comp_y2 = process_comp_y1 + retval * ((float)_trim_data.dig_xy1) / 16384.0f;
+		float process_comp_y3 = ((float)_trim_data.dig_y2) + 160.0f;
+		float process_comp_y4 = mag_data_y * (((process_comp_y2) + 256.0f) * process_comp_y3);
+		retval = ((process_comp_y4 / 8192.0f) + (((float)_trim_data.dig_y1) * 8.0f)) / 16.0f;
+	}
 
 	return retval;
 }
@@ -142,35 +147,52 @@ float BMM150::compensate_z(int16_t mag_data_z, uint16_t data_rhall)
 {
 	float retval = 0;
 
-	// Processing compensation equations
-	//  not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1696-L1703 as of 2020-09-25
-	float process_comp_z0 = ((float)mag_data_z) - ((float)_trim_data.dig_z4);
-	float process_comp_z1 = ((float)data_rhall) - ((float)_trim_data.dig_xyz1);
-	float process_comp_z2 = (((float)_trim_data.dig_z3) * process_comp_z1);
-	float process_comp_z3 = ((float)_trim_data.dig_z1) * ((float)data_rhall) / 32768.f;
-	float process_comp_z4 = ((float)_trim_data.dig_z2) + process_comp_z3;
-	float process_comp_z5 = (process_comp_z0 * 131072.f) - process_comp_z2;
-	retval = (process_comp_z5 / ((process_comp_z4) * 4.f)) / 16.f;
+	// Overflow condition check
+	if ((mag_data_z != OVERFLOW_ZAXIS)
+	    && (_trim_data.dig_z2 != 0) && (_trim_data.dig_z1 != 0) && (_trim_data.dig_xyz1 != 0)
+	    && (data_rhall != 0)) {
+		// Processing compensation equations
+		//  not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1696-L1703 as of 2020-09-25
+		float process_comp_z0 = ((float)mag_data_z) - ((float)_trim_data.dig_z4);
+		float process_comp_z1 = ((float)data_rhall) - ((float)_trim_data.dig_xyz1);
+		float process_comp_z2 = (((float)_trim_data.dig_z3) * process_comp_z1);
+		float process_comp_z3 = ((float)_trim_data.dig_z1) * ((float)data_rhall) / 32768.0f;
+		float process_comp_z4 = ((float)_trim_data.dig_z2) + process_comp_z3;
+		float process_comp_z5 = (process_comp_z0 * 131072.0f) - process_comp_z2;
+		retval = (process_comp_z5 / ((process_comp_z4) * 4.0f)) / 16.0f;
+	}
 
 	return retval;
 }
 
 static constexpr int16_t combine_xy_int13(const uint8_t msb, const uint8_t lsb)
 {
-	int16_t x = ((msb << 8) | lsb);
-	return x / 8; // arithmetic shift by 3 (13 bit signed integer)
+	// msb: 8-bit MSB part [12:5] of the 13 bit output data
+	// lsb: 5-bit LSB part [4:0] of the 13 bit output data
+	int16_t msb_data = ((int16_t)((int8_t)msb)) << 5;
+	int16_t lsb_data = ((lsb & 0xF8) >> 3);
+
+	return (int16_t)(msb_data | lsb_data);
 }
 
 static constexpr int16_t combine_z_int15(const uint8_t msb, const uint8_t lsb)
 {
-	int16_t z = ((msb << 8) | lsb);
-	return z / 2; // arithmetic shift by 1 (15 bit signed integer)
+	// msb: 8-bit MSB part [12:5] of the 13 bit output data
+	// lsb: 7-bit LSB part [6:0] of the 15 bit output data
+	int16_t msb_data = ((int16_t)((int8_t)msb)) << 7;
+	int16_t lsb_data = ((lsb & 0xFE) >> 1);
+
+	return (int16_t)(msb_data | lsb_data);
 }
 
 static constexpr uint16_t combine_rhall_uint14(const uint8_t msb, const uint8_t lsb)
 {
-	uint16_t rhall = ((msb << 8) | lsb);
-	return (rhall >> 2) & 0x3FFF; // 14 bit unsigned integer
+	// msb: 8-bit MSB part [13:6] of the 14 bit output data
+	// lsb: 6-bit LSB part [5:0] of the 14 bit output data
+	uint16_t msb_data = ((uint16_t)((uint16_t)msb)) << 6;
+	uint16_t lsb_data = ((lsb & 0xFC) >> 2);
+
+	return (uint16_t)(msb_data | lsb_data);
 }
 
 void BMM150::RunImpl()

src/drivers/magnetometer/bosch/bmm150/BMM150.hpp

@@ -128,7 +128,7 @@ private:
 	register_config_t _register_cfg[size_register_cfg] {
 		// Register                | Set bits, Clear bits
 		{ Register::POWER_CONTROL, POWER_CONTROL_BIT::PowerControl, POWER_CONTROL_BIT::SoftReset },
-		{ Register::OP_MODE,       OP_MODE_BIT::ODR_20Hz, OP_MODE_BIT::Opmode_Sleep | OP_MODE_BIT::Self_Test },
+		{ Register::OP_MODE,       OP_MODE_BIT::ODR_20HZ_SET, OP_MODE_BIT::ODR_20HZ_CLEAR | OP_MODE_BIT::Opmode_Sleep | OP_MODE_BIT::Self_Test },
 		{ Register::REPXY,         REPXY_BIT::XY_HA_SET, REPXY_BIT::XY_HA_CLEAR },
 		{ Register::REPZ,          REPZ_BIT::Z_HA_SET, REPZ_BIT::Z_HA_CLEAR },
 	};

src/drivers/magnetometer/bosch/bmm150/Bosch_BMM150_registers.hpp

@@ -96,11 +96,12 @@ enum POWER_CONTROL_BIT : uint8_t {
 // OP_MODE
 enum OP_MODE_BIT : uint8_t {
 	// 5:3 Data rate control
-	ODR_20Hz     = Bit5 | Bit3, // ODR 20 Hz
+	ODR_20HZ_SET   = Bit5 | Bit3,
+	ODR_20HZ_CLEAR = Bit4,
 
 	// 2:1 Operation mode control
-	Opmode_Sleep = Bit2 | Bit1, // Sleep mode
-	Self_Test    = Bit0,
+	Opmode_Sleep   = Bit2 | Bit1, // Sleep mode
+	Self_Test      = Bit0,
 };
 
 // STATUS