Jan Cumps / microbit-dal

microbit-dal

Dependencies:   BLE_API mbed-dev-bin nRF51822

Fork of microbit-dal by Lancaster University

source/drivers/MicroBitCompassCalibrator.cpp@75:23164d324459, 2018-11-07 (annotated)

Committer:
jancumps
Date:
Wed Nov 07 19:56:26 2018 +0000
Revision:
75:23164d324459
Parent:
1:8aa5cdb4ab67 
unchanged

Who changed what in which revision?

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