mbed.org local branch of microbit-dal. The real version lives in git at https://github.com/lancaster-university/microbit-dal

Dependencies:   BLE_API nRF51822 mbed-dev-bin

Dependents:   microbit Microbit IoTChallenge1 microbit ... more

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers MicroBitCompassCalibrator.cpp Source File

MicroBitCompassCalibrator.cpp

00001 /*
00002 The MIT License (MIT)
00003 
00004 Copyright (c) 2016 British Broadcasting Corporation.
00005 This software is provided by Lancaster University by arrangement with the BBC.
00006 
00007 Permission is hereby granted, free of charge, to any person obtaining a
00008 copy of this software and associated documentation files (the "Software"),
00009 to deal in the Software without restriction, including without limitation
00010 the rights to use, copy, modify, merge, publish, distribute, sublicense,
00011 and/or sell copies of the Software, and to permit persons to whom the
00012 Software is furnished to do so, subject to the following conditions:
00013 
00014 The above copyright notice and this permission notice shall be included in
00015 all copies or substantial portions of the Software.
00016 
00017 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00018 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00019 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
00020 THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00021 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
00022 FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
00023 DEALINGS IN THE SOFTWARE.
00024 */
00025 
00026 #include "MicroBitConfig.h"
00027 #include "MicroBitCompassCalibrator.h"
00028 #include "EventModel.h"
00029 #include "Matrix4.h"
00030 
00031 /**
00032   * Constructor.
00033   *
00034   * Create an object capable of calibrating the compass.
00035   *
00036   * The algorithm uses an accelerometer to ensure that a broad range of sample data has been gathered
00037   * from the compass module, then performs a least mean squares optimisation of the
00038   * results to determine the calibration data for the compass.
00039   *
00040   * The LED matrix display is used to provide feedback to the user on the gestures required.
00041   *
00042   * @param compass The compass instance to calibrate.
00043   *
00044   * @param accelerometer The accelerometer to gather contextual data from.
00045   *
00046   * @param display The LED matrix to display user feedback on.
00047   */
00048 MicroBitCompassCalibrator::MicroBitCompassCalibrator(MicroBitCompass& _compass, MicroBitAccelerometer& _accelerometer, MicroBitDisplay& _display) : compass(_compass), accelerometer(_accelerometer), display(_display)
00049 {
00050     if (EventModel::defaultEventBus)
00051         EventModel::defaultEventBus->listen(MICROBIT_ID_COMPASS, MICROBIT_COMPASS_EVT_CALIBRATE, this, &MicroBitCompassCalibrator::calibrate, MESSAGE_BUS_LISTENER_IMMEDIATE);
00052 }
00053 
00054 /**
00055   * Performs a simple game that in parallel, calibrates the compass.
00056   *
00057   * This function is executed automatically when the user requests a compass bearing, and compass calibration is required.
00058   *
00059   * This function is, by design, synchronous and only returns once calibration is complete.
00060   */
00061 void MicroBitCompassCalibrator::calibrate(MicroBitEvent)
00062 {
00063     struct Point
00064     {
00065         uint8_t x;
00066         uint8_t y;
00067         uint8_t on;
00068     };
00069 
00070     const int PERIMETER_POINTS = 12;
00071     const int PIXEL1_THRESHOLD = 200;
00072     const int PIXEL2_THRESHOLD = 800;
00073 
00074     wait_ms(100);
00075 
00076     Matrix4 X(PERIMETER_POINTS, 4);
00077     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}};
00078     Point cursor = {2,2,0};
00079 
00080     MicroBitImage img(5,5);
00081     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");
00082     int samples = 0;
00083 
00084     // Firstly, we need to take over the display. Ensure all active animations are paused.
00085     display.stopAnimation();
00086     display.scrollAsync("DRAW A CIRCLE");
00087 
00088     for (int i=0; i<110; i++)
00089         wait_ms(100);
00090 
00091     display.stopAnimation();
00092     display.clear();
00093 
00094     while(samples < PERIMETER_POINTS)
00095     {
00096         // update our model of the flash status of the user controlled pixel.
00097         cursor.on = (cursor.on + 1) % 4;
00098 
00099         // take a snapshot of the current accelerometer data.
00100         int x = accelerometer.getX();
00101         int y = accelerometer.getY();
00102 
00103         // Wait a little whie for the button state to stabilise (one scheduler tick).
00104         wait_ms(10);
00105 
00106         // Deterine the position of the user controlled pixel on the screen.
00107         if (x < -PIXEL2_THRESHOLD)
00108             cursor.x = 0;
00109         else if (x < -PIXEL1_THRESHOLD)
00110             cursor.x = 1;
00111         else if (x > PIXEL2_THRESHOLD)
00112             cursor.x = 4;
00113         else if (x > PIXEL1_THRESHOLD)
00114             cursor.x = 3;
00115         else
00116             cursor.x = 2;
00117 
00118         if (y < -PIXEL2_THRESHOLD)
00119             cursor.y = 0;
00120         else if (y < -PIXEL1_THRESHOLD)
00121             cursor.y = 1;
00122         else if (y > PIXEL2_THRESHOLD)
00123             cursor.y = 4;
00124         else if (y > PIXEL1_THRESHOLD)
00125             cursor.y = 3;
00126         else
00127             cursor.y = 2;
00128 
00129         img.clear();
00130 
00131         // Turn on any pixels that have been visited.
00132         for (int i=0; i<PERIMETER_POINTS; i++)
00133             if (perimeter[i].on)
00134                 img.setPixelValue(perimeter[i].x, perimeter[i].y, 255);
00135 
00136         // Update the pixel at the users position.
00137         img.setPixelValue(cursor.x, cursor.y, 255);
00138 
00139         // Update the buffer to the screen.
00140         display.image.paste(img,0,0,0);
00141 
00142         // test if we need to update the state at the users position.
00143         for (int i=0; i<PERIMETER_POINTS; i++)
00144         {
00145             if (cursor.x == perimeter[i].x && cursor.y == perimeter[i].y && !perimeter[i].on)
00146             {
00147                 // Record the sample data for later processing...
00148                 X.set(samples, 0, compass.getX(RAW));
00149                 X.set(samples, 1, compass.getY(RAW));
00150                 X.set(samples, 2, compass.getZ(RAW));
00151                 X.set(samples, 3, 1);
00152 
00153                 // Record that this pixel has been visited.
00154                 perimeter[i].on = 1;
00155                 samples++;
00156             }
00157         }
00158 
00159         wait_ms(100);
00160     }
00161 
00162     // We have enough sample data to make a fairly accurate calibration.
00163     // We use a Least Mean Squares approximation, as detailed in Freescale application note AN2426.
00164 
00165     // Firstly, calculate the square of each sample.
00166     Matrix4 Y(X.height(), 1);
00167     for (int i = 0; i < X.height(); i++)
00168     {
00169         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);
00170         Y.set(i, 0, v);
00171     }
00172 
00173     // Now perform a Least Squares Approximation.
00174     Matrix4 Alpha = X.multiplyT(X).invert();
00175     Matrix4 Gamma = X.multiplyT(Y);
00176     Matrix4 Beta = Alpha.multiply(Gamma);
00177 
00178     // The result contains the approximate zero point of each axis, but doubled.
00179     // Halve each sample, and record this as the compass calibration data.
00180     CompassSample cal ((int)(Beta.get(0,0) / 2), (int)(Beta.get(1,0) / 2), (int)(Beta.get(2,0) / 2));
00181     compass.setCalibration(cal);
00182 
00183     // Show a smiley to indicate that we're done, and continue on with the user program.
00184     display.clear();
00185     display.printAsync(smiley, 0, 0, 0, 1500);
00186     wait_ms(1000);
00187     display.clear();
00188 }