From d58396aa122983e754ed427e8f40017ceb78c181 Mon Sep 17 00:00:00 2001 From: Joe Finney Date: Fri, 6 Jul 2018 03:08:06 +0100 Subject: [PATCH] Updates to Compass Calibration and Compass Heading Calculation - Add mutator to allow accelerometer to be added to a compass after construction - Use floating point values for Sample3D:dSquared() operations to resmove possible overflow - Change compass calibration hill climb algorithm to use 100 unit steps to converge faster. - Update autodetect functions to register accelerometer instances with compass instances upon detection - Clean and fix axis alignments for tilt compensation algorithm --- inc/drivers/MicroBitCompass.h | 8 +++ inc/drivers/MicroBitCompassCalibrator.h | 2 +- inc/types/CoordinateSystem.h | 8 ++- source/drivers/MicroBitAccelerometer.cpp | 3 + source/drivers/MicroBitCompass.cpp | 60 ++++++++++++-------- source/drivers/MicroBitCompassCalibrator.cpp | 16 +++--- source/types/CoordinateSystem.cpp | 7 ++- 7 files changed, 68 insertions(+), 36 deletions(-) diff --git a/inc/drivers/MicroBitCompass.h b/inc/drivers/MicroBitCompass.h index 3b211db..2567943 100644 --- a/inc/drivers/MicroBitCompass.h +++ b/inc/drivers/MicroBitCompass.h @@ -198,6 +198,14 @@ class MicroBitCompass : public MicroBitComponent */ int configure(); + /** + * + * Defines the accelerometer to be used for tilt compensation. + * + * @param acceleromter Reference to the accelerometer to use. + */ + void setAccelerometer(MicroBitAccelerometer &accelerometer); + /** * Attempts to set the sample rate of the compass to the specified period value (in ms). * diff --git a/inc/drivers/MicroBitCompassCalibrator.h b/inc/drivers/MicroBitCompassCalibrator.h index e563539..9b410f7 100644 --- a/inc/drivers/MicroBitCompassCalibrator.h +++ b/inc/drivers/MicroBitCompassCalibrator.h @@ -106,7 +106,7 @@ class MicroBitCompassCalibrator * * @return The deviation between the closest and further point in the data array from the point given. */ - static int measureScore(Sample3D &c, Sample3D *data, int samples); + static float measureScore(Sample3D &c, Sample3D *data, int samples); /* * Performs an interative approximation (hill descent) algorithm to determine an diff --git a/inc/types/CoordinateSystem.h b/inc/types/CoordinateSystem.h index 5d42ef2..8c45562 100644 --- a/inc/types/CoordinateSystem.h +++ b/inc/types/CoordinateSystem.h @@ -122,9 +122,13 @@ struct Sample3D return !(x == other.x && y == other.y && z == other.z); } - int dSquared(Sample3D &s) + float dSquared(Sample3D &s) { - return (x - s.x)*(x - s.x) + (y - s.y)*(y - s.y) + (z - s.z)*(z - s.z); + float dx = x - s.x; + float dy = y - s.y; + float dz = z - s.z; + + return (dx*dx) + (dy*dy) + (dz*dz); } }; diff --git a/source/drivers/MicroBitAccelerometer.cpp b/source/drivers/MicroBitAccelerometer.cpp index c2f0c01..097a057 100644 --- a/source/drivers/MicroBitAccelerometer.cpp +++ b/source/drivers/MicroBitAccelerometer.cpp @@ -116,6 +116,9 @@ MicroBitAccelerometer& MicroBitAccelerometer::autoDetect(MicroBitI2C &i2c) } } + if (MicroBitCompass::detectedCompass) + MicroBitCompass::detectedCompass->setAccelerometer(*MicroBitAccelerometer::detectedAccelerometer); + return *MicroBitAccelerometer::detectedAccelerometer; } diff --git a/source/drivers/MicroBitCompass.cpp b/source/drivers/MicroBitCompass.cpp index ec33551..16c08e5 100644 --- a/source/drivers/MicroBitCompass.cpp +++ b/source/drivers/MicroBitCompass.cpp @@ -138,6 +138,10 @@ MicroBitCompass& MicroBitCompass::autoDetect(MicroBitI2C &i2c) } } + // If an accelerometer has been discovered, enable tilt compensation on the e-compass. + if (MicroBitAccelerometer::detectedAccelerometer) + MicroBitCompass::detectedCompass->setAccelerometer(*MicroBitAccelerometer::detectedAccelerometer); + return *MicroBitCompass::detectedCompass; } @@ -296,6 +300,17 @@ int MicroBitCompass::configure() return MICROBIT_NOT_SUPPORTED; } +/** + * + * Defines the accelerometer to be used for tilt compensation. + * + * @param acceleromter Reference to the accelerometer to use. + */ +void MicroBitCompass::setAccelerometer(MicroBitAccelerometer &accelerometer) +{ + this->accelerometer = &accelerometer; +} + /** * Attempts to set the sample rate of the compass to the specified period value (in ms). * @@ -359,10 +374,12 @@ int MicroBitCompass::requestUpdate() */ int MicroBitCompass::update() { - // Store the new data, after performing any necessary coordinate transformations. + // Store the raw data, and apply any calibration data we have. sampleENU.x = CALIBRATED_SAMPLE(sampleENU, x); sampleENU.y = CALIBRATED_SAMPLE(sampleENU, y); sampleENU.z = CALIBRATED_SAMPLE(sampleENU, z); + + // Store the user accessible data, in the requested coordinate space, and taking into account component placement of the sensor. sample = coordinateSpace.transform(sampleENU); // Indicate that a new sample is available @@ -434,36 +451,28 @@ int MicroBitCompass::getZ() */ int MicroBitCompass::tiltCompensatedBearing() { - Sample3D cs = this->getSample(NORTH_EAST_DOWN); - Sample3D as = accelerometer->getSample(NORTH_EAST_DOWN); + // Precompute the tilt compensation parameters to improve readability. + float phi = accelerometer->getRollRadians(); + float theta = accelerometer->getPitchRadians(); // Convert to floating point to reduce rounding errors + Sample3D cs = this->getSample(SIMPLE_CARTESIAN); float x = (float) cs.x; float y = (float) cs.y; float z = (float) cs.z; - float ax = (float) as.x; - float ay = (float) as.y; - float az = (float) as.z; + // Precompute cos and sin of pitch and roll angles to make the calculation a little more efficient. + float sinPhi = sin(phi); + float cosPhi = cos(phi); + float sinTheta = sin(theta); + float cosTheta = cos(theta); - // normalize the readings - float amag = sqrt(ax*ax + ay*ay + az*az); - ax = ax/amag; - ay = ay/amag; - az = az/amag; - - float ax2 = ax*ax; - float ay2 = ay*ay; - - float resultx = x*(1.0f - ax2) - y*ax*ay - z*ax*sqrt(1.0f-ax2-ay2); - float resulty = y*sqrt(1.0f-ax2-ay2) - z*ay; - - float bearing = (360*atan2(resulty,resultx)) / (2*PI); + float bearing = (360*atan2(x*cosTheta + y*sinTheta*sinPhi + z*sinTheta*cosPhi, z*sinPhi - y*cosPhi)) / (2*PI); if (bearing < 0) - bearing += 360.0; + bearing += 360.0f; - return (int) (360.0 - bearing); + return (int) (bearing); } /** @@ -471,12 +480,17 @@ int MicroBitCompass::tiltCompensatedBearing() */ int MicroBitCompass::basicBearing() { - float bearing = (atan2((double)getY(),(double)getX()))*180/PI; + // Convert to floating point to reduce rounding errors + Sample3D cs = this->getSample(SIMPLE_CARTESIAN); + float x = (float) cs.x; + float y = (float) cs.y; + + float bearing = (atan2(x,y))*180/PI; if (bearing < 0) bearing += 360.0; - return (int)(360.0 - bearing); + return (int)bearing; } /** diff --git a/source/drivers/MicroBitCompassCalibrator.cpp b/source/drivers/MicroBitCompassCalibrator.cpp index e738ee2..ee31527 100644 --- a/source/drivers/MicroBitCompassCalibrator.cpp +++ b/source/drivers/MicroBitCompassCalibrator.cpp @@ -28,7 +28,7 @@ DEALINGS IN THE SOFTWARE. #include "EventModel.h" #include "Matrix4.h" -#define CALIBRATION_INCREMENT 10 +#define CALIBRATION_INCREMENT 200 /** * Constructor. @@ -62,15 +62,15 @@ MicroBitCompassCalibrator::MicroBitCompassCalibrator(MicroBitCompass& _compass, * * @return The deviation between the closest and further point in the data array from the point given. */ -int MicroBitCompassCalibrator::measureScore(Sample3D &c, Sample3D *data, int samples) +float MicroBitCompassCalibrator::measureScore(Sample3D &c, Sample3D *data, int samples) { - int minD; - int maxD; + float minD; + float maxD; minD = maxD = c.dSquared(data[0]); for (int i = 1; i < samples; i++) { - int d = c.dSquared(data[i]); + float d = c.dSquared(data[i]); if (d < minD) minD = d; @@ -194,7 +194,7 @@ Sample3D MicroBitCompassCalibrator::approximateCentre(Sample3D *data, int sample Sample3D centre = { 0,0,0 }; Sample3D best = { 0,0,0 }; - int score; + float score; for (int i = 0; i < samples; i++) { @@ -228,7 +228,7 @@ Sample3D MicroBitCompassCalibrator::approximateCentre(Sample3D *data, int sample t.y += y; t.z += z; - int s = measureScore(t, data, samples); + float s = measureScore(t, data, samples); if (s < score) { score = s; @@ -356,7 +356,7 @@ void MicroBitCompassCalibrator::calibrateUX(MicroBitEvent) if (cursor.x == perimeter[i].x && cursor.y == perimeter[i].y && !(visited[i] == 1)) { // Record the sample data for later processing... - data[samples] = compass.getSample(); + data[samples] = compass.getSample(RAW); // Record that this pixel has been visited. visited[i] = 1; diff --git a/source/types/CoordinateSystem.cpp b/source/types/CoordinateSystem.cpp index 45a0a86..72a298b 100644 --- a/source/types/CoordinateSystem.cpp +++ b/source/types/CoordinateSystem.cpp @@ -65,6 +65,10 @@ Sample3D CoordinateSpace::transform(Sample3D s, CoordinateSystem system) Sample3D result = s; int temp; + // If we've been asked to supply raw data, simply return it. + if (system == RAW) + return result; + // Firstly, handle any inversions. // As we know the input is in ENU format, this means we flip the polarity of the X and Z axes. if(upsidedown) @@ -109,8 +113,7 @@ Sample3D CoordinateSpace::transform(Sample3D s, CoordinateSystem system) result.z = -result.z; break; - case EAST_NORTH_UP: - case RAW: + default: // EAST_NORTH_UP break; }