dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

bmm150: minor changes to match reference driver

This commit is contained in:
Daniel Agar 2022-02-04 14:36:08 -05:00
parent 8b2016b4ed
commit c028b964e2
3 changed files with 60 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
src/drivers/magnetometer/bosch/bmm150/BMM150.cpp
View File

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

2
src/drivers/magnetometer/bosch/bmm150/BMM150.hpp
View File

@ -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 },
};

7
src/drivers/magnetometer/bosch/bmm150/Bosch_BMM150_registers.hpp
View File

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