fsdfds
Dependencies: BLE_API mbed-dev-bin nRF51822
Fork of microbit-dal by
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 }
Generated on Wed Jul 13 2022 00:58:03 by 1.7.2