From c028b964e2c7f0b1f363290d01ba0eba5cd7057d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 4 Feb 2022 14:36:08 -0500 Subject: [PATCH] bmm150: minor changes to match reference driver --- .../magnetometer/bosch/bmm150/BMM150.cpp | 88 ++++++++++++------- .../magnetometer/bosch/bmm150/BMM150.hpp | 2 +- .../bosch/bmm150/Bosch_BMM150_registers.hpp | 7 +- 3 files changed, 60 insertions(+), 37 deletions(-) diff --git a/src/drivers/magnetometer/bosch/bmm150/BMM150.cpp b/src/drivers/magnetometer/bosch/bmm150/BMM150.cpp index cfadf3cff2..d761ae513f 100644 --- a/src/drivers/magnetometer/bosch/bmm150/BMM150.cpp +++ b/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() diff --git a/src/drivers/magnetometer/bosch/bmm150/BMM150.hpp b/src/drivers/magnetometer/bosch/bmm150/BMM150.hpp index 2b715e1c09..093a893d9d 100644 --- a/src/drivers/magnetometer/bosch/bmm150/BMM150.hpp +++ b/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 }, }; diff --git a/src/drivers/magnetometer/bosch/bmm150/Bosch_BMM150_registers.hpp b/src/drivers/magnetometer/bosch/bmm150/Bosch_BMM150_registers.hpp index 6248bb4ca3..78c128ff4c 100644 --- a/src/drivers/magnetometer/bosch/bmm150/Bosch_BMM150_registers.hpp +++ b/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