Neil Johnson / microbit-ARMBand

Official Sheffield ARMBand micro:bit program

microbit/microbit-dal/source/drivers/MicroBitCompassCalibrator.cpp@0:b9164b348919, 2016-10-17 (annotated)

Committer:
MrBedfordVan
Date:
Mon Oct 17 12:41:20 2016 +0000
Revision:
0:b9164b348919
Official Sheffield ARMBand Micro:bit program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MrBedfordVan 0:b9164b348919 1 /*
MrBedfordVan 0:b9164b348919 2 The MIT License (MIT)
MrBedfordVan 0:b9164b348919 3
MrBedfordVan 0:b9164b348919 4 Copyright (c) 2016 British Broadcasting Corporation.
MrBedfordVan 0:b9164b348919 5 This software is provided by Lancaster University by arrangement with the BBC.
MrBedfordVan 0:b9164b348919 6
MrBedfordVan 0:b9164b348919 7 Permission is hereby granted, free of charge, to any person obtaining a
MrBedfordVan 0:b9164b348919 8 copy of this software and associated documentation files (the "Software"),
MrBedfordVan 0:b9164b348919 9 to deal in the Software without restriction, including without limitation
MrBedfordVan 0:b9164b348919 10 the rights to use, copy, modify, merge, publish, distribute, sublicense,
MrBedfordVan 0:b9164b348919 11 and/or sell copies of the Software, and to permit persons to whom the
MrBedfordVan 0:b9164b348919 12 Software is furnished to do so, subject to the following conditions:
MrBedfordVan 0:b9164b348919 13
MrBedfordVan 0:b9164b348919 14 The above copyright notice and this permission notice shall be included in
MrBedfordVan 0:b9164b348919 15 all copies or substantial portions of the Software.
MrBedfordVan 0:b9164b348919 16
MrBedfordVan 0:b9164b348919 17 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
MrBedfordVan 0:b9164b348919 18 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
MrBedfordVan 0:b9164b348919 19 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
MrBedfordVan 0:b9164b348919 20 THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
MrBedfordVan 0:b9164b348919 21 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
MrBedfordVan 0:b9164b348919 22 FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
MrBedfordVan 0:b9164b348919 23 DEALINGS IN THE SOFTWARE.
MrBedfordVan 0:b9164b348919 24 */
MrBedfordVan 0:b9164b348919 25
MrBedfordVan 0:b9164b348919 26 #include "MicroBitConfig.h"
MrBedfordVan 0:b9164b348919 27 #include "MicroBitCompassCalibrator.h"
MrBedfordVan 0:b9164b348919 28 #include "EventModel.h"
MrBedfordVan 0:b9164b348919 29 #include "Matrix4.h"
MrBedfordVan 0:b9164b348919 30
MrBedfordVan 0:b9164b348919 31 /**
MrBedfordVan 0:b9164b348919 32 * Constructor.
MrBedfordVan 0:b9164b348919 33 *
MrBedfordVan 0:b9164b348919 34 * Create an object capable of calibrating the compass.
MrBedfordVan 0:b9164b348919 35 *
MrBedfordVan 0:b9164b348919 36 * The algorithm uses an accelerometer to ensure that a broad range of sample data has been gathered
MrBedfordVan 0:b9164b348919 37 * from the compass module, then performs a least mean squares optimisation of the
MrBedfordVan 0:b9164b348919 38 * results to determine the calibration data for the compass.
MrBedfordVan 0:b9164b348919 39 *
MrBedfordVan 0:b9164b348919 40 * The LED matrix display is used to provide feedback to the user on the gestures required.
MrBedfordVan 0:b9164b348919 41 *
MrBedfordVan 0:b9164b348919 42 * @param compass The compass instance to calibrate.
MrBedfordVan 0:b9164b348919 43 *
MrBedfordVan 0:b9164b348919 44 * @param accelerometer The accelerometer to gather contextual data from.
MrBedfordVan 0:b9164b348919 45 *
MrBedfordVan 0:b9164b348919 46 * @param display The LED matrix to display user feedback on.
MrBedfordVan 0:b9164b348919 47 */
MrBedfordVan 0:b9164b348919 48 MicroBitCompassCalibrator::MicroBitCompassCalibrator(MicroBitCompass& _compass, MicroBitAccelerometer& _accelerometer, MicroBitDisplay& _display) : compass(_compass), accelerometer(_accelerometer), display(_display)
MrBedfordVan 0:b9164b348919 49 {
MrBedfordVan 0:b9164b348919 50 if (EventModel::defaultEventBus)
MrBedfordVan 0:b9164b348919 51 EventModel::defaultEventBus->listen(MICROBIT_ID_COMPASS, MICROBIT_COMPASS_EVT_CALIBRATE, this, &MicroBitCompassCalibrator::calibrate, MESSAGE_BUS_LISTENER_IMMEDIATE);
MrBedfordVan 0:b9164b348919 52 }
MrBedfordVan 0:b9164b348919 53
MrBedfordVan 0:b9164b348919 54 /**
MrBedfordVan 0:b9164b348919 55 * Performs a simple game that in parallel, calibrates the compass.
MrBedfordVan 0:b9164b348919 56 *
MrBedfordVan 0:b9164b348919 57 * This function is executed automatically when the user requests a compass bearing, and compass calibration is required.
MrBedfordVan 0:b9164b348919 58 *
MrBedfordVan 0:b9164b348919 59 * This function is, by design, synchronous and only returns once calibration is complete.
MrBedfordVan 0:b9164b348919 60 */
MrBedfordVan 0:b9164b348919 61 void MicroBitCompassCalibrator::calibrate(MicroBitEvent)
MrBedfordVan 0:b9164b348919 62 {
MrBedfordVan 0:b9164b348919 63 struct Point
MrBedfordVan 0:b9164b348919 64 {
MrBedfordVan 0:b9164b348919 65 uint8_t x;
MrBedfordVan 0:b9164b348919 66 uint8_t y;
MrBedfordVan 0:b9164b348919 67 uint8_t on;
MrBedfordVan 0:b9164b348919 68 };
MrBedfordVan 0:b9164b348919 69
MrBedfordVan 0:b9164b348919 70 const int PERIMETER_POINTS = 12;
MrBedfordVan 0:b9164b348919 71 const int PIXEL1_THRESHOLD = 200;
MrBedfordVan 0:b9164b348919 72 const int PIXEL2_THRESHOLD = 800;
MrBedfordVan 0:b9164b348919 73
MrBedfordVan 0:b9164b348919 74 wait_ms(100);
MrBedfordVan 0:b9164b348919 75
MrBedfordVan 0:b9164b348919 76 Matrix4 X(PERIMETER_POINTS, 4);
MrBedfordVan 0:b9164b348919 77 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}};
MrBedfordVan 0:b9164b348919 78 Point cursor = {2,2,0};
MrBedfordVan 0:b9164b348919 79
MrBedfordVan 0:b9164b348919 80 MicroBitImage img(5,5);
MrBedfordVan 0:b9164b348919 81 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");
MrBedfordVan 0:b9164b348919 82 int samples = 0;
MrBedfordVan 0:b9164b348919 83
MrBedfordVan 0:b9164b348919 84 // Firstly, we need to take over the display. Ensure all active animations are paused.
MrBedfordVan 0:b9164b348919 85 display.stopAnimation();
MrBedfordVan 0:b9164b348919 86 display.scrollAsync("DRAW A CIRCLE");
MrBedfordVan 0:b9164b348919 87
MrBedfordVan 0:b9164b348919 88 for (int i=0; i<110; i++)
MrBedfordVan 0:b9164b348919 89 wait_ms(100);
MrBedfordVan 0:b9164b348919 90
MrBedfordVan 0:b9164b348919 91 display.stopAnimation();
MrBedfordVan 0:b9164b348919 92 display.clear();
MrBedfordVan 0:b9164b348919 93
MrBedfordVan 0:b9164b348919 94 while(samples < PERIMETER_POINTS)
MrBedfordVan 0:b9164b348919 95 {
MrBedfordVan 0:b9164b348919 96 // update our model of the flash status of the user controlled pixel.
MrBedfordVan 0:b9164b348919 97 cursor.on = (cursor.on + 1) % 4;
MrBedfordVan 0:b9164b348919 98
MrBedfordVan 0:b9164b348919 99 // take a snapshot of the current accelerometer data.
MrBedfordVan 0:b9164b348919 100 int x = accelerometer.getX();
MrBedfordVan 0:b9164b348919 101 int y = accelerometer.getY();
MrBedfordVan 0:b9164b348919 102
MrBedfordVan 0:b9164b348919 103 // Wait a little whie for the button state to stabilise (one scheduler tick).
MrBedfordVan 0:b9164b348919 104 wait_ms(10);
MrBedfordVan 0:b9164b348919 105
MrBedfordVan 0:b9164b348919 106 // Deterine the position of the user controlled pixel on the screen.
MrBedfordVan 0:b9164b348919 107 if (x < -PIXEL2_THRESHOLD)
MrBedfordVan 0:b9164b348919 108 cursor.x = 0;
MrBedfordVan 0:b9164b348919 109 else if (x < -PIXEL1_THRESHOLD)
MrBedfordVan 0:b9164b348919 110 cursor.x = 1;
MrBedfordVan 0:b9164b348919 111 else if (x > PIXEL2_THRESHOLD)
MrBedfordVan 0:b9164b348919 112 cursor.x = 4;
MrBedfordVan 0:b9164b348919 113 else if (x > PIXEL1_THRESHOLD)
MrBedfordVan 0:b9164b348919 114 cursor.x = 3;
MrBedfordVan 0:b9164b348919 115 else
MrBedfordVan 0:b9164b348919 116 cursor.x = 2;
MrBedfordVan 0:b9164b348919 117
MrBedfordVan 0:b9164b348919 118 if (y < -PIXEL2_THRESHOLD)
MrBedfordVan 0:b9164b348919 119 cursor.y = 0;
MrBedfordVan 0:b9164b348919 120 else if (y < -PIXEL1_THRESHOLD)
MrBedfordVan 0:b9164b348919 121 cursor.y = 1;
MrBedfordVan 0:b9164b348919 122 else if (y > PIXEL2_THRESHOLD)
MrBedfordVan 0:b9164b348919 123 cursor.y = 4;
MrBedfordVan 0:b9164b348919 124 else if (y > PIXEL1_THRESHOLD)
MrBedfordVan 0:b9164b348919 125 cursor.y = 3;
MrBedfordVan 0:b9164b348919 126 else
MrBedfordVan 0:b9164b348919 127 cursor.y = 2;
MrBedfordVan 0:b9164b348919 128
MrBedfordVan 0:b9164b348919 129 img.clear();
MrBedfordVan 0:b9164b348919 130
MrBedfordVan 0:b9164b348919 131 // Turn on any pixels that have been visited.
MrBedfordVan 0:b9164b348919 132 for (int i=0; i<PERIMETER_POINTS; i++)
MrBedfordVan 0:b9164b348919 133 if (perimeter[i].on)
MrBedfordVan 0:b9164b348919 134 img.setPixelValue(perimeter[i].x, perimeter[i].y, 255);
MrBedfordVan 0:b9164b348919 135
MrBedfordVan 0:b9164b348919 136 // Update the pixel at the users position.
MrBedfordVan 0:b9164b348919 137 img.setPixelValue(cursor.x, cursor.y, 255);
MrBedfordVan 0:b9164b348919 138
MrBedfordVan 0:b9164b348919 139 // Update the buffer to the screen.
MrBedfordVan 0:b9164b348919 140 display.image.paste(img,0,0,0);
MrBedfordVan 0:b9164b348919 141
MrBedfordVan 0:b9164b348919 142 // test if we need to update the state at the users position.
MrBedfordVan 0:b9164b348919 143 for (int i=0; i<PERIMETER_POINTS; i++)
MrBedfordVan 0:b9164b348919 144 {
MrBedfordVan 0:b9164b348919 145 if (cursor.x == perimeter[i].x && cursor.y == perimeter[i].y && !perimeter[i].on)
MrBedfordVan 0:b9164b348919 146 {
MrBedfordVan 0:b9164b348919 147 // Record the sample data for later processing...
MrBedfordVan 0:b9164b348919 148 X.set(samples, 0, compass.getX(RAW));
MrBedfordVan 0:b9164b348919 149 X.set(samples, 1, compass.getY(RAW));
MrBedfordVan 0:b9164b348919 150 X.set(samples, 2, compass.getZ(RAW));
MrBedfordVan 0:b9164b348919 151 X.set(samples, 3, 1);
MrBedfordVan 0:b9164b348919 152
MrBedfordVan 0:b9164b348919 153 // Record that this pixel has been visited.
MrBedfordVan 0:b9164b348919 154 perimeter[i].on = 1;
MrBedfordVan 0:b9164b348919 155 samples++;
MrBedfordVan 0:b9164b348919 156 }
MrBedfordVan 0:b9164b348919 157 }
MrBedfordVan 0:b9164b348919 158
MrBedfordVan 0:b9164b348919 159 wait_ms(100);
MrBedfordVan 0:b9164b348919 160 }
MrBedfordVan 0:b9164b348919 161
MrBedfordVan 0:b9164b348919 162 // We have enough sample data to make a fairly accurate calibration.
MrBedfordVan 0:b9164b348919 163 // We use a Least Mean Squares approximation, as detailed in Freescale application note AN2426.
MrBedfordVan 0:b9164b348919 164
MrBedfordVan 0:b9164b348919 165 // Firstly, calculate the square of each sample.
MrBedfordVan 0:b9164b348919 166 Matrix4 Y(X.height(), 1);
MrBedfordVan 0:b9164b348919 167 for (int i = 0; i < X.height(); i++)
MrBedfordVan 0:b9164b348919 168 {
MrBedfordVan 0:b9164b348919 169 float v = X.get(i, 0)*X.get(i, 0) + X.get(i, 1)*X.get(i, 1) + X.get(i, 2)*X.get(i, 2);
MrBedfordVan 0:b9164b348919 170 Y.set(i, 0, v);
MrBedfordVan 0:b9164b348919 171 }
MrBedfordVan 0:b9164b348919 172
MrBedfordVan 0:b9164b348919 173 // Now perform a Least Squares Approximation.
MrBedfordVan 0:b9164b348919 174 Matrix4 Alpha = X.multiplyT(X).invert();
MrBedfordVan 0:b9164b348919 175 Matrix4 Gamma = X.multiplyT(Y);
MrBedfordVan 0:b9164b348919 176 Matrix4 Beta = Alpha.multiply(Gamma);
MrBedfordVan 0:b9164b348919 177
MrBedfordVan 0:b9164b348919 178 // The result contains the approximate zero point of each axis, but doubled.
MrBedfordVan 0:b9164b348919 179 // Halve each sample, and record this as the compass calibration data.
MrBedfordVan 0:b9164b348919 180 CompassSample cal ((int)(Beta.get(0,0) / 2), (int)(Beta.get(1,0) / 2), (int)(Beta.get(2,0) / 2));
MrBedfordVan 0:b9164b348919 181 compass.setCalibration(cal);
MrBedfordVan 0:b9164b348919 182
MrBedfordVan 0:b9164b348919 183 // Show a smiley to indicate that we're done, and continue on with the user program.
MrBedfordVan 0:b9164b348919 184 display.clear();
MrBedfordVan 0:b9164b348919 185 display.printAsync(smiley, 0, 0, 0, 1500);
MrBedfordVan 0:b9164b348919 186 wait_ms(1000);
MrBedfordVan 0:b9164b348919 187 display.clear();
MrBedfordVan 0:b9164b348919 188 }