diff --git a/inc/core/MicroBitConfig.h b/inc/core/MicroBitConfig.h index e6df68c..a41a982 100644 --- a/inc/core/MicroBitConfig.h +++ b/inc/core/MicroBitConfig.h @@ -309,6 +309,18 @@ extern uint32_t __etext; #define USE_ACCEL_LSB 0 #endif +// +// Enable a 0..360 degree range on the accelerometer getPitch() +// calculation. Set to '1' to enable. +// +// A value of '0' provides consistency with the (buggy) microbit-dal 2.0 +// and earlier versions, which inadvertently provided only an ambiguous +// 0..180 degree range +// +#ifndef MICROBIT_FULL_RANGE_PITCH_CALCULATION +#define MICROBIT_FULL_RANGE_PITCH_CALCULATION 1 +#endif + // // Display options // diff --git a/source/drivers/MicroBitAccelerometer.cpp b/source/drivers/MicroBitAccelerometer.cpp index da71ccb..11bee2c 100644 --- a/source/drivers/MicroBitAccelerometer.cpp +++ b/source/drivers/MicroBitAccelerometer.cpp @@ -574,8 +574,19 @@ void MicroBitAccelerometer::recalculatePitchRoll() double y = (double) sample.y; double z = (double) sample.z; - roll = atan2(y, z); - pitch = atan(-x / (y*sin(roll) + z*cos(roll))); + roll = atan2(x, -z); + pitch = atan2(y, (x*sin(roll) - z*cos(roll))); + +#if CONFIG_ENABLED(MICROBIT_FULL_RANGE_PITCH_CALCULATION) + + // Handle to the two "negative quadrants", such that we get an output in the +/- 18- degree range. + // This ensures that the pitch values are consistent with the roll values. + if (z > 0.0) + { + double reference = pitch > 0.0 ? (PI / 2.0) : (-PI / 2.0); + pitch = reference + (reference - pitch); + } +#endif status |= MICROBIT_ACCELEROMETER_IMU_DATA_VALID; } diff --git a/source/drivers/MicroBitCompass.cpp b/source/drivers/MicroBitCompass.cpp index 8fee178..bbeba22 100644 --- a/source/drivers/MicroBitCompass.cpp +++ b/source/drivers/MicroBitCompass.cpp @@ -456,7 +456,7 @@ int MicroBitCompass::tiltCompensatedBearing() float theta = accelerometer->getPitchRadians(); // Convert to floating point to reduce rounding errors - Sample3D cs = this->getSample(SIMPLE_CARTESIAN); + Sample3D cs = this->getSample(NORTH_EAST_DOWN); float x = (float) cs.x; float y = (float) cs.y; float z = (float) cs.z; @@ -467,8 +467,13 @@ int MicroBitCompass::tiltCompensatedBearing() float sinTheta = sin(theta); float cosTheta = cos(theta); + // Calculate the tilt compensated bearing, and convert to degrees. float bearing = (360*atan2(x*cosTheta + y*sinTheta*sinPhi + z*sinTheta*cosPhi, z*sinPhi - y*cosPhi)) / (2*PI); + // Handle the 90 degree offset caused by the NORTH_EAST_DOWN based calculation. + bearing = bearing - 90; + + // Ensure the calculated bearing is in the 0..359 degree range. if (bearing < 0) bearing += 360.0f;