Martin Woolley / microbit-dal

Solution for Bluetooth SIG hands-on training course

Dependencies:   BLE_API mbed-dev-bin nRF51822-bluetooth-mdw

Dependents:   microbit

Fork of microbit-dal-bluetooth-mdw_starter by Martin Woolley

source/drivers/MicroBitCompassCalibrator.cpp@22:23d7b9a4b082, 2016-07-13 (annotated)

Committer:
LancasterUniversity
Date:
Wed Jul 13 12:17:54 2016 +0100
Revision:
22:23d7b9a4b082
Parent:
1:8aa5cdb4ab67 
Synchronized with git rev 7cf98c22

Author: James Devine

microbit-dal: patch for fiber_wake_on_event



fiber_wake_on_event used to crash after forking a FOB fiber.



It would attempt to obtain a new fiber context, and would place it on the wait queue.

Then when that fiber was paged in, the context of that fiber would not have been

initialised, as the function presumed schedule would be called immediately after

fiber initialisation.



This patch catches that edge case.

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 }