From bbf56210ed3d1dc10a48cc9ea2fdcb8c0ad5cd30 Mon Sep 17 00:00:00 2001 From: Joe Finney Date: Thu, 5 Jul 2018 23:48:41 +0100 Subject: [PATCH] Introduce Updates to Compass Calibration Algorithm and UX - Updated compass calibration algorithm, based on iterative approximation - Updated calibration UX --- inc/drivers/MicroBitCompass.h | 18 +- inc/drivers/MicroBitCompassCalibrator.h | 54 +++- inc/types/CoordinateSystem.h | 6 + source/drivers/MicroBitCompass.cpp | 63 ++-- source/drivers/MicroBitCompassCalibrator.cpp | 311 +++++++++++++++---- 5 files changed, 361 insertions(+), 91 deletions(-) diff --git a/inc/drivers/MicroBitCompass.h b/inc/drivers/MicroBitCompass.h index 2047218..3b211db 100644 --- a/inc/drivers/MicroBitCompass.h +++ b/inc/drivers/MicroBitCompass.h @@ -47,6 +47,18 @@ DEALINGS IN THE SOFTWARE. #define MICROBIT_COMPASS_EVT_CALIBRATE 3 #define MICROBIT_COMPASS_EVT_CALIBRATION_NEEDED 4 +struct CompassCalibration +{ + Sample3D centre; // Zero offset of the compass. + Sample3D scale; // Scale factor to apply in each axis to accomodate 1st order directional fields. + int radius; // Indication of field strength - the "distance" from the centre to outmost sample. + + CompassCalibration() : centre(), scale(1024, 1024, 1024) + { + radius = 0; + } +}; + /** * Class definition for a general e-compass. */ @@ -55,7 +67,7 @@ class MicroBitCompass : public MicroBitComponent protected: uint16_t samplePeriod; // The time between samples, in milliseconds. - Sample3D average; // The zero offset of this compass (generated by calibration) + CompassCalibration calibration; // The calibration data of this compass Sample3D sample; // The last sample read, in the coordinate system specified by the coordinateSpace variable. Sample3D sampleENU; // The last sample read, in raw ENU format (stored in case requests are made for data in other coordinate spaces) CoordinateSpace &coordinateSpace; // The coordinate space transform (if any) to apply to the raw data from the hardware. @@ -149,7 +161,7 @@ class MicroBitCompass : public MicroBitComponent * * @param calibration A Sample3D containing the offsets for the x, y and z axis. */ - void setCalibration(Sample3D calibration); + void setCalibration(CompassCalibration calibration); /** * Provides the calibration data currently in use by the compass. @@ -158,7 +170,7 @@ class MicroBitCompass : public MicroBitComponent * * @return A Sample3D containing the offsets for the x, y and z axis. */ - Sample3D getCalibration(); + CompassCalibration getCalibration(); /** * Returns 0 or 1. 1 indicates that the compass is calibrated, zero means the compass requires calibration. diff --git a/inc/drivers/MicroBitCompassCalibrator.h b/inc/drivers/MicroBitCompassCalibrator.h index de707e2..e563539 100644 --- a/inc/drivers/MicroBitCompassCalibrator.h +++ b/inc/drivers/MicroBitCompassCalibrator.h @@ -78,7 +78,59 @@ class MicroBitCompassCalibrator * * This function is, by design, synchronous and only returns once calibration is complete. */ - void calibrate(MicroBitEvent); + void calibrateUX(MicroBitEvent); + /** + * Calculates an independent X, Y, Z scale factor and centre for a given set of data points, + * assumed to be on a bounding sphere + * + * @param data An array of all data points + * @param samples The number of samples in the 'data' array. + * + * This algorithm should be called with no fewer than 12 points, but testing has indicated >21 + * points provides a more robust calculation. + * + * @return A calibration structure containing the a calculated centre point, the radius of the + * minimum enclosing spere of points and a scaling factor for each axis that places those + * points as close as possible to the surface of the containing sphere. + */ + static CompassCalibration calibrate(Sample3D *data, int samples); + + private: + + /** + * Scoring function for a hill climb algorithm. + * + * @param c An approximated centre point + * @param data a collection of data points + * @param samples the number of samples in the 'data' array + * + * @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); + + /* + * Performs an interative approximation (hill descent) algorithm to determine an + * estimated centre point of a sphere upon which the given data points reside. + * + * @param data an array containing sample points + * @param samples the number of sample points in the 'data' array. + * + * @return the approximated centre point of the points in the 'data' array. + */ + static Sample3D approximateCentre(Sample3D *data, int samples); + + /** + * Calculates an independent scale factor for X,Y and Z axes that places the given data points on a bounding sphere + * + * @param centre A proviously calculated centre point of all data. + * @param data An array of all data points + * @param samples The number of samples in the 'data' array. + * + * @return A calibration structure containing the centre point provided, the radius of the minimum + * enclosing spere of points and a scaling factor for each axis that places those points as close as possible + * to the surface of the containing sphere. + */ + static CompassCalibration spherify(Sample3D centre, Sample3D *data, int samples); }; #endif diff --git a/inc/types/CoordinateSystem.h b/inc/types/CoordinateSystem.h index ebfca32..5d42ef2 100644 --- a/inc/types/CoordinateSystem.h +++ b/inc/types/CoordinateSystem.h @@ -121,6 +121,12 @@ struct Sample3D { return !(x == other.x && y == other.y && z == other.z); } + + int dSquared(Sample3D &s) + { + return (x - s.x)*(x - s.x) + (y - s.y)*(y - s.y) + (z - s.z)*(z - s.z); + } + }; diff --git a/source/drivers/MicroBitCompass.cpp b/source/drivers/MicroBitCompass.cpp index 627791e..ec33551 100644 --- a/source/drivers/MicroBitCompass.cpp +++ b/source/drivers/MicroBitCompass.cpp @@ -33,6 +33,10 @@ DEALINGS IN THE SOFTWARE. #include "LSM303Magnetometer.h" #include "FXOS8700.h" +// +// Internal convenience macro to apply calibration to a given sample. +// +#define CALIBRATED_SAMPLE(sample, axis) (((sample.axis - calibration.centre.axis) * calibration.scale.axis) >> 10) /** * Constructor. @@ -42,7 +46,7 @@ DEALINGS IN THE SOFTWARE. * @param coordinateSpace the orientation of the sensor. Defaults to: SIMPLE_CARTESIAN * */ -MicroBitCompass::MicroBitCompass(CoordinateSpace &cspace, uint16_t id) : sample(), sampleENU(), coordinateSpace(cspace) +MicroBitCompass::MicroBitCompass(CoordinateSpace &cspace, uint16_t id) : calibration(), sample(), sampleENU(), coordinateSpace(cspace) { accelerometer = NULL; init(id); @@ -57,7 +61,7 @@ MicroBitCompass::MicroBitCompass(CoordinateSpace &cspace, uint16_t id) : sample( * @param coordinateSpace the orientation of the sensor. Defaults to: SIMPLE_CARTESIAN * */ -MicroBitCompass::MicroBitCompass(MicroBitAccelerometer &accel, CoordinateSpace &cspace, uint16_t id) : sample(), sampleENU(), coordinateSpace(cspace) +MicroBitCompass::MicroBitCompass(MicroBitAccelerometer &accel, CoordinateSpace &cspace, uint16_t id) : calibration(), sample(), sampleENU(), coordinateSpace(cspace) { accelerometer = &accel; init(id); @@ -236,9 +240,9 @@ int MicroBitCompass::calibrate() * * @param calibration A Sample3D containing the offsets for the x, y and z axis. */ -void MicroBitCompass::setCalibration(Sample3D calibration) +void MicroBitCompass::setCalibration(CompassCalibration calibration) { - average = calibration; + this->calibration = calibration; status |= MICROBIT_COMPASS_STATUS_CALIBRATED; } @@ -249,9 +253,9 @@ void MicroBitCompass::setCalibration(Sample3D calibration) * * @return A Sample3D containing the offsets for the x, y and z axis. */ -Sample3D MicroBitCompass::getCalibration() +CompassCalibration MicroBitCompass::getCalibration() { - return average; + return calibration; } /** @@ -275,7 +279,7 @@ int MicroBitCompass::isCalibrating() */ void MicroBitCompass::clearCalibration() { - average = Sample3D(); + calibration = CompassCalibration(); status &= ~MICROBIT_COMPASS_STATUS_CALIBRATED; } @@ -356,7 +360,10 @@ int MicroBitCompass::requestUpdate() int MicroBitCompass::update() { // Store the new data, after performing any necessary coordinate transformations. - sample = coordinateSpace.transform(sampleENU - average); + sampleENU.x = CALIBRATED_SAMPLE(sampleENU, x); + sampleENU.y = CALIBRATED_SAMPLE(sampleENU, y); + sampleENU.z = CALIBRATED_SAMPLE(sampleENU, z); + sample = coordinateSpace.transform(sampleENU); // Indicate that a new sample is available MicroBitEvent e(id, MICROBIT_COMPASS_EVT_DATA_UPDATE); @@ -373,7 +380,7 @@ int MicroBitCompass::update() Sample3D MicroBitCompass::getSample(CoordinateSystem coordinateSystem) { requestUpdate(); - return coordinateSpace.transform(sampleENU - average, coordinateSystem); + return coordinateSpace.transform(sampleENU, coordinateSystem); } /** @@ -427,28 +434,36 @@ int MicroBitCompass::getZ() */ int MicroBitCompass::tiltCompensatedBearing() { - // Precompute the tilt compensation parameters to improve readability. - float phi = accelerometer->getRollRadians(); - float theta = accelerometer->getPitchRadians(); + Sample3D cs = this->getSample(NORTH_EAST_DOWN); + Sample3D as = accelerometer->getSample(NORTH_EAST_DOWN); - Sample3D s = getSample(NORTH_EAST_DOWN); + // Convert to floating point to reduce rounding errors + float x = (float) cs.x; + float y = (float) cs.y; + float z = (float) cs.z; - float x = (float) s.x; - float y = (float) s.y; - float z = (float) s.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 bearing = (360*atan2(z*sinPhi - y*cosPhi, x*cosTheta + y*sinTheta*sinPhi + z*sinTheta*cosPhi)) / (2*PI); + 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); if (bearing < 0) bearing += 360.0; - return (int) bearing; + return (int) (360.0 - bearing); } /** @@ -456,7 +471,7 @@ int MicroBitCompass::tiltCompensatedBearing() */ int MicroBitCompass::basicBearing() { - float bearing = (atan2((double)(sample.y - average.y),(double)(sample.x - average.x)))*180/PI; + float bearing = (atan2((double)getY(),(double)getX()))*180/PI; if (bearing < 0) bearing += 360.0; diff --git a/source/drivers/MicroBitCompassCalibrator.cpp b/source/drivers/MicroBitCompassCalibrator.cpp index 75ed0c9..e738ee2 100644 --- a/source/drivers/MicroBitCompassCalibrator.cpp +++ b/source/drivers/MicroBitCompassCalibrator.cpp @@ -28,6 +28,8 @@ DEALINGS IN THE SOFTWARE. #include "EventModel.h" #include "Matrix4.h" +#define CALIBRATION_INCREMENT 10 + /** * Constructor. * @@ -48,61 +50,269 @@ DEALINGS IN THE SOFTWARE. MicroBitCompassCalibrator::MicroBitCompassCalibrator(MicroBitCompass& _compass, MicroBitAccelerometer& _accelerometer, MicroBitDisplay& _display) : compass(_compass), accelerometer(_accelerometer), display(_display) { if (EventModel::defaultEventBus) - EventModel::defaultEventBus->listen(MICROBIT_ID_COMPASS, MICROBIT_COMPASS_EVT_CALIBRATE, this, &MicroBitCompassCalibrator::calibrate, MESSAGE_BUS_LISTENER_IMMEDIATE); + EventModel::defaultEventBus->listen(MICROBIT_ID_COMPASS, MICROBIT_COMPASS_EVT_CALIBRATE, this, &MicroBitCompassCalibrator::calibrateUX, MESSAGE_BUS_LISTENER_IMMEDIATE); } /** - * Performs a simple game that in parallel, calibrates the compass. - * - * This function is executed automatically when the user requests a compass bearing, and compass calibration is required. - * - * This function is, by design, synchronous and only returns once calibration is complete. - */ -void MicroBitCompassCalibrator::calibrate(MicroBitEvent) + * Scoring function for a hill climb algorithm. + * + * @param c An approximated centre point + * @param data a collection of data points + * @param samples the number of samples in the 'data' array + * + * @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) +{ + int minD; + int maxD; + + minD = maxD = c.dSquared(data[0]); + for (int i = 1; i < samples; i++) + { + int d = c.dSquared(data[i]); + + if (d < minD) + minD = d; + + if (d > maxD) + maxD = d; + } + + return (maxD - minD); +} + +/** + * Calculates an independent X, Y, Z scale factor and centre for a given set of data points, assumed to be on + * a bounding sphere + * + * @param data An array of all data points + * @param samples The number of samples in the 'data' array. + * + * This algorithm should be called with no fewer than 12 points, but testing has indicated >21 points provides + * a more robust calculation. + * + * @return A calibration structure containing the a calculated centre point, the radius of the minimum + * enclosing spere of points and a scaling factor for each axis that places those points as close as possible + * to the surface of the containing sphere. + */ +CompassCalibration MicroBitCompassCalibrator::calibrate(Sample3D *data, int samples) +{ + Sample3D centre = approximateCentre(data, samples); + return spherify(centre, data, samples); +} +/** + * Calculates an independent scale factor for X,Y and Z axes that places the given data points on a bounding sphere + * + * @param centre A proviously calculated centre point of all data. + * @param data An array of all data points + * @param samples The number of samples in the 'data' array. + * + * @return A calibration structure containing the centre point provided, the radius of the minimum + * enclosing spere of points and a scaling factor for each axis that places those points as close as possible + * to the surface of the containing sphere. + */ +CompassCalibration MicroBitCompassCalibrator::spherify(Sample3D centre, Sample3D *data, int samples) +{ + // First, determine the radius of the enclosing sphere from the given centre. + // n.b. this will likely be different to the radius from the centre of mass previously calculated. + // We use the same algorithm though. + CompassCalibration result; + + float radius = 0; + float scaleX = 0.0; + float scaleY = 0.0; + float scaleZ = 0.0; + + float scale = 0.0; + float weightX = 0.0; + float weightY = 0.0; + float weightZ = 0.0; + + for (int i = 0; i < samples; i++) + { + int d = sqrt((float)centre.dSquared(data[i])); + + if (d > radius) + radius = d; + } + + // Now, for each data point, determine a scalar multiplier for the vector between the centre and that point that + // takes the point onto the surface of the enclosing sphere. + for (int i = 0; i < samples; i++) + { + // Calculate the distance from this point to the centre of the sphere + float d = sqrt(centre.dSquared(data[i])); + + // Now determine a scalar multiplier that, when applied to the vector to the centre, + // will place this point on the surface of the sphere. + float s = (radius / d) - 1; + + scale = max(scale, s); + + // next, determine the scale effect this has on each of our components. + float dx = (data[i].x - centre.x); + float dy = (data[i].y - centre.y); + float dz = (data[i].z - centre.z); + + weightX += s * fabsf(dx / d); + weightY += s * fabsf(dy / d); + weightZ += s * fabsf(dz / d); + } + + float wmag = sqrt((weightX * weightX) + (weightY * weightY) + (weightZ * weightZ)); + + scaleX = 1.0 + scale * (weightX / wmag); + scaleY = 1.0 + scale * (weightY / wmag); + scaleZ = 1.0 + scale * (weightZ / wmag); + + result.scale.x = (int)(1024 * scaleX); + result.scale.y = (int)(1024 * scaleY); + result.scale.z = (int)(1024 * scaleZ); + + result.centre.x = centre.x; + result.centre.y = centre.y; + result.centre.z = centre.z; + + result.radius = radius; + + return result; +} + +/* + * Performs an interative approximation (hill descent) algorithm to determine an + * estimated centre point of a sphere upon which the given data points reside. + * + * @param data an array containing sample points + * @param samples the number of sample points in the 'data' array. + * + * @return the approximated centre point of the points in the 'data' array. + */ +Sample3D MicroBitCompassCalibrator::approximateCentre(Sample3D *data, int samples) +{ + Sample3D c,t; + Sample3D centre = { 0,0,0 }; + Sample3D best = { 0,0,0 }; + + int score; + + for (int i = 0; i < samples; i++) + { + centre.x += data[i].x; + centre.y += data[i].y; + centre.z += data[i].z; + } + + // Calclulate a centre of mass for our input samples. We only use this for validation purposes. + centre.x = centre.x / samples; + centre.y = centre.y / samples; + centre.z = centre.z / samples; + + // Start hill climb in the centre of mass. + c = centre; + + // calculate the nearest and furthest point to us. + score = measureScore(c, data, samples); + + // iteratively attempt to improve position... + while (1) + { + for (int x = -CALIBRATION_INCREMENT; x <= CALIBRATION_INCREMENT; x=x+CALIBRATION_INCREMENT) + { + for (int y = -CALIBRATION_INCREMENT; y <= CALIBRATION_INCREMENT; y=y+CALIBRATION_INCREMENT) + { + for (int z = -CALIBRATION_INCREMENT; z <= CALIBRATION_INCREMENT; z=z+CALIBRATION_INCREMENT) + { + t = c; + t.x += x; + t.y += y; + t.z += z; + + int s = measureScore(t, data, samples); + if (s < score) + { + score = s; + best = t; + } + } + } + } + + if (best.x == c.x && best.y == c.y && best.z == c.z) + break; + + c = best; + } + + return c; +} + +/** + * Performs a simple game that in parallel, calibrates the compass. + * + * This function is executed automatically when the user requests a compass bearing, and compass calibration is required. + * + * This function is, by design, synchronous and only returns once calibration is complete. + */ +void MicroBitCompassCalibrator::calibrateUX(MicroBitEvent) { struct Point { uint8_t x; uint8_t y; - uint8_t on; }; - const int PERIMETER_POINTS = 12; + const int PERIMETER_POINTS = 25; + const int PIXEL1_THRESHOLD = 200; - const int PIXEL2_THRESHOLD = 800; + const int PIXEL2_THRESHOLD = 680; + const int REDISPLAY_MSG_TIMEOUT_MS = 30000; + const int SAMPLES_END_MSG_COUNT = 15; + const int TIME_STEP = 100; + const int MSG_TIME = 155 * TIME_STEP; //We require MSG_TIME % TIME_STEP == 0 wait_ms(100); - Matrix4 X(PERIMETER_POINTS, 4); - Point perimeter[PERIMETER_POINTS] = {{1,0,0}, {2,0,0}, {3,0,0}, {4,1,0}, {4,2,0}, {4,3,0}, {3,4,0}, {2,4,0}, {1,4,0}, {0,3,0}, {0,2,0}, {0,1,0}}; - Point cursor = {2,2,0}; + static const Point perimeter[PERIMETER_POINTS] = {{0,0}, {1,0}, {2,0}, {3,0}, {4,0}, {0,1}, {1,1}, {2,1}, {3,1}, {4,1}, {0,2}, {1,2}, {2,2}, {3,2}, {4,2}, {0,3}, {1,3}, {2,3}, {3,3}, {4,3}, {0,4}, {1,4}, {2,4}, {3,4}, {4,4}}; + Point cursor = {2,2}; MicroBitImage img(5,5); MicroBitImage smiley("0,255,0,255,0\n0,255,0,255,0\n0,0,0,0,0\n255,0,0,0,255\n0,255,255,255,0\n"); - int samples = 0; + + Sample3D data[PERIMETER_POINTS]; + uint8_t visited[PERIMETER_POINTS] = { 0 }; + uint8_t cursor_on = 0; + uint8_t samples = 0; + uint8_t samples_this_period = 0; + int16_t remaining_scroll_time = MSG_TIME; // 32s maximum in uint16_t // Firstly, we need to take over the display. Ensure all active animations are paused. display.stopAnimation(); - display.scrollAsync("DRAW A CIRCLE"); - - for (int i=0; i<110; i++) - wait_ms(100); - - display.stopAnimation(); - display.clear(); while(samples < PERIMETER_POINTS) { + // Scroll a message the first time we enter this loop and every REDISPLAY_MSG_TIMEOUT_MS + if (remaining_scroll_time == MSG_TIME || remaining_scroll_time <= -REDISPLAY_MSG_TIMEOUT_MS) { + display.clear(); + display.scrollAsync("TILT TO FILL SCREEN "); // Takes about 14s + + remaining_scroll_time = MSG_TIME; + samples_this_period = 0; + } + else if (remaining_scroll_time == 0 || samples_this_period == SAMPLES_END_MSG_COUNT) + { + // This stops the scrolling at the end of the message. + // ...and it is the source of the ((MSG_TIME % TIME_STEP) == 0) requirement + display.stopAnimation(); + } + // update our model of the flash status of the user controlled pixel. - cursor.on = (cursor.on + 1) % 4; + cursor_on = (cursor_on + 1) % 4; // take a snapshot of the current accelerometer data. int x = accelerometer.getX(); int y = accelerometer.getY(); - // Wait a little whie for the button state to stabilise (one scheduler tick). - wait_ms(10); - // Deterine the position of the user controlled pixel on the screen. if (x < -PIXEL2_THRESHOLD) cursor.x = 0; @@ -130,60 +340,35 @@ void MicroBitCompassCalibrator::calibrate(MicroBitEvent) // Turn on any pixels that have been visited. for (int i=0; i SAMPLES_END_MSG_COUNT) + display.image.paste(img,0,0,0); // test if we need to update the state at the users position. for (int i=0; i