From 9b093c4a3383956a9631ad06869c2da75d35fc71 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 14 Jun 2017 11:00:56 +1000 Subject: [PATCH] AP_Compass: fixed a numerical issue in BMM150 driver we were getting the following: ret=-32768 z=5 rhall=6267 z1=23743 z2=712 z3=-1035 z4=0 xyz1=6264 dividend=163840 dividend2=-1073577207 divisor=5253 the signed/unsigned casts for rhall and xyz1 did not produce the desired result --- libraries/AP_Compass/AP_Compass_BMM150.cpp | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_BMM150.cpp b/libraries/AP_Compass/AP_Compass_BMM150.cpp index eb2fa4e251..5e875d36ac 100644 --- a/libraries/AP_Compass/AP_Compass_BMM150.cpp +++ b/libraries/AP_Compass/AP_Compass_BMM150.cpp @@ -22,6 +22,7 @@ #include #include +#include #define CHIP_ID_REG 0x40 #define CHIP_ID_VAL 0x32 @@ -232,15 +233,23 @@ int16_t AP_Compass_BMM150::_compensate_xy(int16_t xy, uint32_t rhall, int32_t tx int16_t AP_Compass_BMM150::_compensate_z(int16_t z, uint32_t rhall) { - int32_t dividend = ((int32_t)(z - _dig.z4)) << 15; - dividend -= (_dig.z3 * (rhall - _dig.xyz1)) >> 2; + int32_t dividend = int32_t(z - _dig.z4) << 15; + int32_t dividend2 = dividend - ((_dig.z3 * (int32_t(rhall) - int32_t(_dig.xyz1))) >> 2); - int32_t divisor = ((int32_t)_dig.z1) * (rhall << 1); + int32_t divisor = int32_t(_dig.z1) * (rhall << 1); divisor += 0x8000; divisor >>= 16; divisor += _dig.z2; - return constrain_int32(dividend / divisor, -0x8000, 0x8000); + int16_t ret = constrain_int32(dividend2 / divisor, -0x8000, 0x8000); +#if 0 + static uint8_t counter; + if (counter++ == 0) { + printf("ret=%d z=%d rhall=%u z1=%d z2=%d z3=%d z4=%d xyz1=%d dividend=%d dividend2=%d divisor=%d\n", + ret, z, rhall, _dig.z1, _dig.z2, _dig.z3, _dig.z4, _dig.xyz1, dividend, dividend2, divisor); + } +#endif + return ret; } void AP_Compass_BMM150::_update() @@ -259,6 +268,7 @@ void AP_Compass_BMM150::_update() _last_read_ms = now; _dev->write_register(POWER_AND_OPERATIONS_REG, SOFT_RESET); _dev->write_register(POWER_AND_OPERATIONS_REG, POWER_CONTROL_VAL, true); + hal.util->perf_count(_perf_err); } return; }