Introduce Updates to Compass Calibration Algorithm and UX
- Updated compass calibration algorithm, based on iterative approximation - Updated calibration UX
This commit is contained in:
parent
e79284d797
commit
bbf56210ed
|
@ -47,6 +47,18 @@ DEALINGS IN THE SOFTWARE.
|
|||
#define MICROBIT_COMPASS_EVT_CALIBRATE 3
|
||||
#define MICROBIT_COMPASS_EVT_CALIBRATION_NEEDED 4
|
||||
|
||||
struct CompassCalibration
|
||||
{
|
||||
Sample3D centre; // Zero offset of the compass.
|
||||
Sample3D scale; // Scale factor to apply in each axis to accomodate 1st order directional fields.
|
||||
int radius; // Indication of field strength - the "distance" from the centre to outmost sample.
|
||||
|
||||
CompassCalibration() : centre(), scale(1024, 1024, 1024)
|
||||
{
|
||||
radius = 0;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Class definition for a general e-compass.
|
||||
*/
|
||||
|
@ -55,7 +67,7 @@ class MicroBitCompass : public MicroBitComponent
|
|||
protected:
|
||||
|
||||
uint16_t samplePeriod; // The time between samples, in milliseconds.
|
||||
Sample3D average; // The zero offset of this compass (generated by calibration)
|
||||
CompassCalibration calibration; // The calibration data of this compass
|
||||
Sample3D sample; // The last sample read, in the coordinate system specified by the coordinateSpace variable.
|
||||
Sample3D sampleENU; // The last sample read, in raw ENU format (stored in case requests are made for data in other coordinate spaces)
|
||||
CoordinateSpace &coordinateSpace; // The coordinate space transform (if any) to apply to the raw data from the hardware.
|
||||
|
@ -149,7 +161,7 @@ class MicroBitCompass : public MicroBitComponent
|
|||
*
|
||||
* @param calibration A Sample3D containing the offsets for the x, y and z axis.
|
||||
*/
|
||||
void setCalibration(Sample3D calibration);
|
||||
void setCalibration(CompassCalibration calibration);
|
||||
|
||||
/**
|
||||
* Provides the calibration data currently in use by the compass.
|
||||
|
@ -158,7 +170,7 @@ class MicroBitCompass : public MicroBitComponent
|
|||
*
|
||||
* @return A Sample3D containing the offsets for the x, y and z axis.
|
||||
*/
|
||||
Sample3D getCalibration();
|
||||
CompassCalibration getCalibration();
|
||||
|
||||
/**
|
||||
* Returns 0 or 1. 1 indicates that the compass is calibrated, zero means the compass requires calibration.
|
||||
|
|
|
@ -78,7 +78,59 @@ class MicroBitCompassCalibrator
|
|||
*
|
||||
* This function is, by design, synchronous and only returns once calibration is complete.
|
||||
*/
|
||||
void calibrate(MicroBitEvent);
|
||||
void calibrateUX(MicroBitEvent);
|
||||
/**
|
||||
* Calculates an independent X, Y, Z scale factor and centre for a given set of data points,
|
||||
* assumed to be on a bounding sphere
|
||||
*
|
||||
* @param data An array of all data points
|
||||
* @param samples The number of samples in the 'data' array.
|
||||
*
|
||||
* This algorithm should be called with no fewer than 12 points, but testing has indicated >21
|
||||
* points provides a more robust calculation.
|
||||
*
|
||||
* @return A calibration structure containing the a calculated centre point, the radius of the
|
||||
* minimum enclosing spere of points and a scaling factor for each axis that places those
|
||||
* points as close as possible to the surface of the containing sphere.
|
||||
*/
|
||||
static CompassCalibration calibrate(Sample3D *data, int samples);
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
* Scoring function for a hill climb algorithm.
|
||||
*
|
||||
* @param c An approximated centre point
|
||||
* @param data a collection of data points
|
||||
* @param samples the number of samples in the 'data' array
|
||||
*
|
||||
* @return The deviation between the closest and further point in the data array from the point given.
|
||||
*/
|
||||
static int measureScore(Sample3D &c, Sample3D *data, int samples);
|
||||
|
||||
/*
|
||||
* Performs an interative approximation (hill descent) algorithm to determine an
|
||||
* estimated centre point of a sphere upon which the given data points reside.
|
||||
*
|
||||
* @param data an array containing sample points
|
||||
* @param samples the number of sample points in the 'data' array.
|
||||
*
|
||||
* @return the approximated centre point of the points in the 'data' array.
|
||||
*/
|
||||
static Sample3D approximateCentre(Sample3D *data, int samples);
|
||||
|
||||
/**
|
||||
* Calculates an independent scale factor for X,Y and Z axes that places the given data points on a bounding sphere
|
||||
*
|
||||
* @param centre A proviously calculated centre point of all data.
|
||||
* @param data An array of all data points
|
||||
* @param samples The number of samples in the 'data' array.
|
||||
*
|
||||
* @return A calibration structure containing the centre point provided, the radius of the minimum
|
||||
* enclosing spere of points and a scaling factor for each axis that places those points as close as possible
|
||||
* to the surface of the containing sphere.
|
||||
*/
|
||||
static CompassCalibration spherify(Sample3D centre, Sample3D *data, int samples);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
@ -121,6 +121,12 @@ struct Sample3D
|
|||
{
|
||||
return !(x == other.x && y == other.y && z == other.z);
|
||||
}
|
||||
|
||||
int dSquared(Sample3D &s)
|
||||
{
|
||||
return (x - s.x)*(x - s.x) + (y - s.y)*(y - s.y) + (z - s.z)*(z - s.z);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
|
|
@ -33,6 +33,10 @@ DEALINGS IN THE SOFTWARE.
|
|||
#include "LSM303Magnetometer.h"
|
||||
#include "FXOS8700.h"
|
||||
|
||||
//
|
||||
// Internal convenience macro to apply calibration to a given sample.
|
||||
//
|
||||
#define CALIBRATED_SAMPLE(sample, axis) (((sample.axis - calibration.centre.axis) * calibration.scale.axis) >> 10)
|
||||
|
||||
/**
|
||||
* Constructor.
|
||||
|
@ -42,7 +46,7 @@ DEALINGS IN THE SOFTWARE.
|
|||
* @param coordinateSpace the orientation of the sensor. Defaults to: SIMPLE_CARTESIAN
|
||||
*
|
||||
*/
|
||||
MicroBitCompass::MicroBitCompass(CoordinateSpace &cspace, uint16_t id) : sample(), sampleENU(), coordinateSpace(cspace)
|
||||
MicroBitCompass::MicroBitCompass(CoordinateSpace &cspace, uint16_t id) : calibration(), sample(), sampleENU(), coordinateSpace(cspace)
|
||||
{
|
||||
accelerometer = NULL;
|
||||
init(id);
|
||||
|
@ -57,7 +61,7 @@ MicroBitCompass::MicroBitCompass(CoordinateSpace &cspace, uint16_t id) : sample(
|
|||
* @param coordinateSpace the orientation of the sensor. Defaults to: SIMPLE_CARTESIAN
|
||||
*
|
||||
*/
|
||||
MicroBitCompass::MicroBitCompass(MicroBitAccelerometer &accel, CoordinateSpace &cspace, uint16_t id) : sample(), sampleENU(), coordinateSpace(cspace)
|
||||
MicroBitCompass::MicroBitCompass(MicroBitAccelerometer &accel, CoordinateSpace &cspace, uint16_t id) : calibration(), sample(), sampleENU(), coordinateSpace(cspace)
|
||||
{
|
||||
accelerometer = &accel;
|
||||
init(id);
|
||||
|
@ -236,9 +240,9 @@ int MicroBitCompass::calibrate()
|
|||
*
|
||||
* @param calibration A Sample3D containing the offsets for the x, y and z axis.
|
||||
*/
|
||||
void MicroBitCompass::setCalibration(Sample3D calibration)
|
||||
void MicroBitCompass::setCalibration(CompassCalibration calibration)
|
||||
{
|
||||
average = calibration;
|
||||
this->calibration = calibration;
|
||||
status |= MICROBIT_COMPASS_STATUS_CALIBRATED;
|
||||
}
|
||||
|
||||
|
@ -249,9 +253,9 @@ void MicroBitCompass::setCalibration(Sample3D calibration)
|
|||
*
|
||||
* @return A Sample3D containing the offsets for the x, y and z axis.
|
||||
*/
|
||||
Sample3D MicroBitCompass::getCalibration()
|
||||
CompassCalibration MicroBitCompass::getCalibration()
|
||||
{
|
||||
return average;
|
||||
return calibration;
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -275,7 +279,7 @@ int MicroBitCompass::isCalibrating()
|
|||
*/
|
||||
void MicroBitCompass::clearCalibration()
|
||||
{
|
||||
average = Sample3D();
|
||||
calibration = CompassCalibration();
|
||||
status &= ~MICROBIT_COMPASS_STATUS_CALIBRATED;
|
||||
}
|
||||
|
||||
|
@ -356,7 +360,10 @@ int MicroBitCompass::requestUpdate()
|
|||
int MicroBitCompass::update()
|
||||
{
|
||||
// Store the new data, after performing any necessary coordinate transformations.
|
||||
sample = coordinateSpace.transform(sampleENU - average);
|
||||
sampleENU.x = CALIBRATED_SAMPLE(sampleENU, x);
|
||||
sampleENU.y = CALIBRATED_SAMPLE(sampleENU, y);
|
||||
sampleENU.z = CALIBRATED_SAMPLE(sampleENU, z);
|
||||
sample = coordinateSpace.transform(sampleENU);
|
||||
|
||||
// Indicate that a new sample is available
|
||||
MicroBitEvent e(id, MICROBIT_COMPASS_EVT_DATA_UPDATE);
|
||||
|
@ -373,7 +380,7 @@ int MicroBitCompass::update()
|
|||
Sample3D MicroBitCompass::getSample(CoordinateSystem coordinateSystem)
|
||||
{
|
||||
requestUpdate();
|
||||
return coordinateSpace.transform(sampleENU - average, coordinateSystem);
|
||||
return coordinateSpace.transform(sampleENU, coordinateSystem);
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -427,28 +434,36 @@ int MicroBitCompass::getZ()
|
|||
*/
|
||||
int MicroBitCompass::tiltCompensatedBearing()
|
||||
{
|
||||
// Precompute the tilt compensation parameters to improve readability.
|
||||
float phi = accelerometer->getRollRadians();
|
||||
float theta = accelerometer->getPitchRadians();
|
||||
Sample3D cs = this->getSample(NORTH_EAST_DOWN);
|
||||
Sample3D as = accelerometer->getSample(NORTH_EAST_DOWN);
|
||||
|
||||
Sample3D s = getSample(NORTH_EAST_DOWN);
|
||||
// Convert to floating point to reduce rounding errors
|
||||
float x = (float) cs.x;
|
||||
float y = (float) cs.y;
|
||||
float z = (float) cs.z;
|
||||
|
||||
float x = (float) s.x;
|
||||
float y = (float) s.y;
|
||||
float z = (float) s.z;
|
||||
float ax = (float) as.x;
|
||||
float ay = (float) as.y;
|
||||
float az = (float) as.z;
|
||||
|
||||
// Precompute cos and sin of pitch and roll angles to make the calculation a little more efficient.
|
||||
float sinPhi = sin(phi);
|
||||
float cosPhi = cos(phi);
|
||||
float sinTheta = sin(theta);
|
||||
float cosTheta = cos(theta);
|
||||
// normalize the readings
|
||||
float amag = sqrt(ax*ax + ay*ay + az*az);
|
||||
ax = ax/amag;
|
||||
ay = ay/amag;
|
||||
az = az/amag;
|
||||
|
||||
float bearing = (360*atan2(z*sinPhi - y*cosPhi, x*cosTheta + y*sinTheta*sinPhi + z*sinTheta*cosPhi)) / (2*PI);
|
||||
float ax2 = ax*ax;
|
||||
float ay2 = ay*ay;
|
||||
|
||||
float resultx = x*(1.0f - ax2) - y*ax*ay - z*ax*sqrt(1.0f-ax2-ay2);
|
||||
float resulty = y*sqrt(1.0f-ax2-ay2) - z*ay;
|
||||
|
||||
float bearing = (360*atan2(resulty,resultx)) / (2*PI);
|
||||
|
||||
if (bearing < 0)
|
||||
bearing += 360.0;
|
||||
|
||||
return (int) bearing;
|
||||
return (int) (360.0 - bearing);
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -456,7 +471,7 @@ int MicroBitCompass::tiltCompensatedBearing()
|
|||
*/
|
||||
int MicroBitCompass::basicBearing()
|
||||
{
|
||||
float bearing = (atan2((double)(sample.y - average.y),(double)(sample.x - average.x)))*180/PI;
|
||||
float bearing = (atan2((double)getY(),(double)getX()))*180/PI;
|
||||
|
||||
if (bearing < 0)
|
||||
bearing += 360.0;
|
||||
|
|
|
@ -28,6 +28,8 @@ DEALINGS IN THE SOFTWARE.
|
|||
#include "EventModel.h"
|
||||
#include "Matrix4.h"
|
||||
|
||||
#define CALIBRATION_INCREMENT 10
|
||||
|
||||
/**
|
||||
* Constructor.
|
||||
*
|
||||
|
@ -48,61 +50,269 @@ DEALINGS IN THE SOFTWARE.
|
|||
MicroBitCompassCalibrator::MicroBitCompassCalibrator(MicroBitCompass& _compass, MicroBitAccelerometer& _accelerometer, MicroBitDisplay& _display) : compass(_compass), accelerometer(_accelerometer), display(_display)
|
||||
{
|
||||
if (EventModel::defaultEventBus)
|
||||
EventModel::defaultEventBus->listen(MICROBIT_ID_COMPASS, MICROBIT_COMPASS_EVT_CALIBRATE, this, &MicroBitCompassCalibrator::calibrate, MESSAGE_BUS_LISTENER_IMMEDIATE);
|
||||
EventModel::defaultEventBus->listen(MICROBIT_ID_COMPASS, MICROBIT_COMPASS_EVT_CALIBRATE, this, &MicroBitCompassCalibrator::calibrateUX, MESSAGE_BUS_LISTENER_IMMEDIATE);
|
||||
}
|
||||
|
||||
/**
|
||||
* Performs a simple game that in parallel, calibrates the compass.
|
||||
*
|
||||
* This function is executed automatically when the user requests a compass bearing, and compass calibration is required.
|
||||
*
|
||||
* This function is, by design, synchronous and only returns once calibration is complete.
|
||||
*/
|
||||
void MicroBitCompassCalibrator::calibrate(MicroBitEvent)
|
||||
* Scoring function for a hill climb algorithm.
|
||||
*
|
||||
* @param c An approximated centre point
|
||||
* @param data a collection of data points
|
||||
* @param samples the number of samples in the 'data' array
|
||||
*
|
||||
* @return The deviation between the closest and further point in the data array from the point given.
|
||||
*/
|
||||
int MicroBitCompassCalibrator::measureScore(Sample3D &c, Sample3D *data, int samples)
|
||||
{
|
||||
int minD;
|
||||
int maxD;
|
||||
|
||||
minD = maxD = c.dSquared(data[0]);
|
||||
for (int i = 1; i < samples; i++)
|
||||
{
|
||||
int d = c.dSquared(data[i]);
|
||||
|
||||
if (d < minD)
|
||||
minD = d;
|
||||
|
||||
if (d > maxD)
|
||||
maxD = d;
|
||||
}
|
||||
|
||||
return (maxD - minD);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates an independent X, Y, Z scale factor and centre for a given set of data points, assumed to be on
|
||||
* a bounding sphere
|
||||
*
|
||||
* @param data An array of all data points
|
||||
* @param samples The number of samples in the 'data' array.
|
||||
*
|
||||
* This algorithm should be called with no fewer than 12 points, but testing has indicated >21 points provides
|
||||
* a more robust calculation.
|
||||
*
|
||||
* @return A calibration structure containing the a calculated centre point, the radius of the minimum
|
||||
* enclosing spere of points and a scaling factor for each axis that places those points as close as possible
|
||||
* to the surface of the containing sphere.
|
||||
*/
|
||||
CompassCalibration MicroBitCompassCalibrator::calibrate(Sample3D *data, int samples)
|
||||
{
|
||||
Sample3D centre = approximateCentre(data, samples);
|
||||
return spherify(centre, data, samples);
|
||||
}
|
||||
/**
|
||||
* Calculates an independent scale factor for X,Y and Z axes that places the given data points on a bounding sphere
|
||||
*
|
||||
* @param centre A proviously calculated centre point of all data.
|
||||
* @param data An array of all data points
|
||||
* @param samples The number of samples in the 'data' array.
|
||||
*
|
||||
* @return A calibration structure containing the centre point provided, the radius of the minimum
|
||||
* enclosing spere of points and a scaling factor for each axis that places those points as close as possible
|
||||
* to the surface of the containing sphere.
|
||||
*/
|
||||
CompassCalibration MicroBitCompassCalibrator::spherify(Sample3D centre, Sample3D *data, int samples)
|
||||
{
|
||||
// First, determine the radius of the enclosing sphere from the given centre.
|
||||
// n.b. this will likely be different to the radius from the centre of mass previously calculated.
|
||||
// We use the same algorithm though.
|
||||
CompassCalibration result;
|
||||
|
||||
float radius = 0;
|
||||
float scaleX = 0.0;
|
||||
float scaleY = 0.0;
|
||||
float scaleZ = 0.0;
|
||||
|
||||
float scale = 0.0;
|
||||
float weightX = 0.0;
|
||||
float weightY = 0.0;
|
||||
float weightZ = 0.0;
|
||||
|
||||
for (int i = 0; i < samples; i++)
|
||||
{
|
||||
int d = sqrt((float)centre.dSquared(data[i]));
|
||||
|
||||
if (d > radius)
|
||||
radius = d;
|
||||
}
|
||||
|
||||
// Now, for each data point, determine a scalar multiplier for the vector between the centre and that point that
|
||||
// takes the point onto the surface of the enclosing sphere.
|
||||
for (int i = 0; i < samples; i++)
|
||||
{
|
||||
// Calculate the distance from this point to the centre of the sphere
|
||||
float d = sqrt(centre.dSquared(data[i]));
|
||||
|
||||
// Now determine a scalar multiplier that, when applied to the vector to the centre,
|
||||
// will place this point on the surface of the sphere.
|
||||
float s = (radius / d) - 1;
|
||||
|
||||
scale = max(scale, s);
|
||||
|
||||
// next, determine the scale effect this has on each of our components.
|
||||
float dx = (data[i].x - centre.x);
|
||||
float dy = (data[i].y - centre.y);
|
||||
float dz = (data[i].z - centre.z);
|
||||
|
||||
weightX += s * fabsf(dx / d);
|
||||
weightY += s * fabsf(dy / d);
|
||||
weightZ += s * fabsf(dz / d);
|
||||
}
|
||||
|
||||
float wmag = sqrt((weightX * weightX) + (weightY * weightY) + (weightZ * weightZ));
|
||||
|
||||
scaleX = 1.0 + scale * (weightX / wmag);
|
||||
scaleY = 1.0 + scale * (weightY / wmag);
|
||||
scaleZ = 1.0 + scale * (weightZ / wmag);
|
||||
|
||||
result.scale.x = (int)(1024 * scaleX);
|
||||
result.scale.y = (int)(1024 * scaleY);
|
||||
result.scale.z = (int)(1024 * scaleZ);
|
||||
|
||||
result.centre.x = centre.x;
|
||||
result.centre.y = centre.y;
|
||||
result.centre.z = centre.z;
|
||||
|
||||
result.radius = radius;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/*
|
||||
* Performs an interative approximation (hill descent) algorithm to determine an
|
||||
* estimated centre point of a sphere upon which the given data points reside.
|
||||
*
|
||||
* @param data an array containing sample points
|
||||
* @param samples the number of sample points in the 'data' array.
|
||||
*
|
||||
* @return the approximated centre point of the points in the 'data' array.
|
||||
*/
|
||||
Sample3D MicroBitCompassCalibrator::approximateCentre(Sample3D *data, int samples)
|
||||
{
|
||||
Sample3D c,t;
|
||||
Sample3D centre = { 0,0,0 };
|
||||
Sample3D best = { 0,0,0 };
|
||||
|
||||
int score;
|
||||
|
||||
for (int i = 0; i < samples; i++)
|
||||
{
|
||||
centre.x += data[i].x;
|
||||
centre.y += data[i].y;
|
||||
centre.z += data[i].z;
|
||||
}
|
||||
|
||||
// Calclulate a centre of mass for our input samples. We only use this for validation purposes.
|
||||
centre.x = centre.x / samples;
|
||||
centre.y = centre.y / samples;
|
||||
centre.z = centre.z / samples;
|
||||
|
||||
// Start hill climb in the centre of mass.
|
||||
c = centre;
|
||||
|
||||
// calculate the nearest and furthest point to us.
|
||||
score = measureScore(c, data, samples);
|
||||
|
||||
// iteratively attempt to improve position...
|
||||
while (1)
|
||||
{
|
||||
for (int x = -CALIBRATION_INCREMENT; x <= CALIBRATION_INCREMENT; x=x+CALIBRATION_INCREMENT)
|
||||
{
|
||||
for (int y = -CALIBRATION_INCREMENT; y <= CALIBRATION_INCREMENT; y=y+CALIBRATION_INCREMENT)
|
||||
{
|
||||
for (int z = -CALIBRATION_INCREMENT; z <= CALIBRATION_INCREMENT; z=z+CALIBRATION_INCREMENT)
|
||||
{
|
||||
t = c;
|
||||
t.x += x;
|
||||
t.y += y;
|
||||
t.z += z;
|
||||
|
||||
int s = measureScore(t, data, samples);
|
||||
if (s < score)
|
||||
{
|
||||
score = s;
|
||||
best = t;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (best.x == c.x && best.y == c.y && best.z == c.z)
|
||||
break;
|
||||
|
||||
c = best;
|
||||
}
|
||||
|
||||
return c;
|
||||
}
|
||||
|
||||
/**
|
||||
* Performs a simple game that in parallel, calibrates the compass.
|
||||
*
|
||||
* This function is executed automatically when the user requests a compass bearing, and compass calibration is required.
|
||||
*
|
||||
* This function is, by design, synchronous and only returns once calibration is complete.
|
||||
*/
|
||||
void MicroBitCompassCalibrator::calibrateUX(MicroBitEvent)
|
||||
{
|
||||
struct Point
|
||||
{
|
||||
uint8_t x;
|
||||
uint8_t y;
|
||||
uint8_t on;
|
||||
};
|
||||
|
||||
const int PERIMETER_POINTS = 12;
|
||||
const int PERIMETER_POINTS = 25;
|
||||
|
||||
const int PIXEL1_THRESHOLD = 200;
|
||||
const int PIXEL2_THRESHOLD = 800;
|
||||
const int PIXEL2_THRESHOLD = 680;
|
||||
const int REDISPLAY_MSG_TIMEOUT_MS = 30000;
|
||||
const int SAMPLES_END_MSG_COUNT = 15;
|
||||
const int TIME_STEP = 100;
|
||||
const int MSG_TIME = 155 * TIME_STEP; //We require MSG_TIME % TIME_STEP == 0
|
||||
|
||||
wait_ms(100);
|
||||
|
||||
Matrix4 X(PERIMETER_POINTS, 4);
|
||||
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}};
|
||||
Point cursor = {2,2,0};
|
||||
static const Point perimeter[PERIMETER_POINTS] = {{0,0}, {1,0}, {2,0}, {3,0}, {4,0}, {0,1}, {1,1}, {2,1}, {3,1}, {4,1}, {0,2}, {1,2}, {2,2}, {3,2}, {4,2}, {0,3}, {1,3}, {2,3}, {3,3}, {4,3}, {0,4}, {1,4}, {2,4}, {3,4}, {4,4}};
|
||||
Point cursor = {2,2};
|
||||
|
||||
MicroBitImage img(5,5);
|
||||
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");
|
||||
int samples = 0;
|
||||
|
||||
Sample3D data[PERIMETER_POINTS];
|
||||
uint8_t visited[PERIMETER_POINTS] = { 0 };
|
||||
uint8_t cursor_on = 0;
|
||||
uint8_t samples = 0;
|
||||
uint8_t samples_this_period = 0;
|
||||
int16_t remaining_scroll_time = MSG_TIME; // 32s maximum in uint16_t
|
||||
|
||||
// Firstly, we need to take over the display. Ensure all active animations are paused.
|
||||
display.stopAnimation();
|
||||
display.scrollAsync("DRAW A CIRCLE");
|
||||
|
||||
for (int i=0; i<110; i++)
|
||||
wait_ms(100);
|
||||
|
||||
display.stopAnimation();
|
||||
display.clear();
|
||||
|
||||
while(samples < PERIMETER_POINTS)
|
||||
{
|
||||
// Scroll a message the first time we enter this loop and every REDISPLAY_MSG_TIMEOUT_MS
|
||||
if (remaining_scroll_time == MSG_TIME || remaining_scroll_time <= -REDISPLAY_MSG_TIMEOUT_MS) {
|
||||
display.clear();
|
||||
display.scrollAsync("TILT TO FILL SCREEN "); // Takes about 14s
|
||||
|
||||
remaining_scroll_time = MSG_TIME;
|
||||
samples_this_period = 0;
|
||||
}
|
||||
else if (remaining_scroll_time == 0 || samples_this_period == SAMPLES_END_MSG_COUNT)
|
||||
{
|
||||
// This stops the scrolling at the end of the message.
|
||||
// ...and it is the source of the ((MSG_TIME % TIME_STEP) == 0) requirement
|
||||
display.stopAnimation();
|
||||
}
|
||||
|
||||
// update our model of the flash status of the user controlled pixel.
|
||||
cursor.on = (cursor.on + 1) % 4;
|
||||
cursor_on = (cursor_on + 1) % 4;
|
||||
|
||||
// take a snapshot of the current accelerometer data.
|
||||
int x = accelerometer.getX();
|
||||
int y = accelerometer.getY();
|
||||
|
||||
// Wait a little whie for the button state to stabilise (one scheduler tick).
|
||||
wait_ms(10);
|
||||
|
||||
// Deterine the position of the user controlled pixel on the screen.
|
||||
if (x < -PIXEL2_THRESHOLD)
|
||||
cursor.x = 0;
|
||||
|
@ -130,60 +340,35 @@ void MicroBitCompassCalibrator::calibrate(MicroBitEvent)
|
|||
|
||||
// Turn on any pixels that have been visited.
|
||||
for (int i=0; i<PERIMETER_POINTS; i++)
|
||||
if (perimeter[i].on)
|
||||
if (visited[i] == 1)
|
||||
img.setPixelValue(perimeter[i].x, perimeter[i].y, 255);
|
||||
|
||||
// Update the pixel at the users position.
|
||||
img.setPixelValue(cursor.x, cursor.y, 255);
|
||||
img.setPixelValue(cursor.x, cursor.y, cursor_on);
|
||||
|
||||
// Update the buffer to the screen.
|
||||
display.image.paste(img,0,0,0);
|
||||
// Update the buffer to the screen ONLY if we've finished scrolling the message
|
||||
if (remaining_scroll_time < 0 || samples_this_period > SAMPLES_END_MSG_COUNT)
|
||||
display.image.paste(img,0,0,0);
|
||||
|
||||
// test if we need to update the state at the users position.
|
||||
for (int i=0; i<PERIMETER_POINTS; i++)
|
||||
{
|
||||
if (cursor.x == perimeter[i].x && cursor.y == perimeter[i].y && !perimeter[i].on)
|
||||
if (cursor.x == perimeter[i].x && cursor.y == perimeter[i].y && !(visited[i] == 1))
|
||||
{
|
||||
// Record the sample data for later processing...
|
||||
Sample3D s = compass.getSample(RAW);
|
||||
X.set(samples, 0, s.x);
|
||||
X.set(samples, 1, s.y);
|
||||
X.set(samples, 2, s.z);
|
||||
X.set(samples, 3, 1);
|
||||
|
||||
//X.set(samples, 0, compass.getX(RAW));
|
||||
//X.set(samples, 1, compass.getY(RAW));
|
||||
//X.set(samples, 2, compass.getZ(RAW));
|
||||
data[samples] = compass.getSample();
|
||||
|
||||
// Record that this pixel has been visited.
|
||||
perimeter[i].on = 1;
|
||||
visited[i] = 1;
|
||||
samples++;
|
||||
samples_this_period++;
|
||||
}
|
||||
}
|
||||
|
||||
wait_ms(100);
|
||||
wait_ms(TIME_STEP);
|
||||
remaining_scroll_time-=TIME_STEP;
|
||||
}
|
||||
|
||||
// We have enough sample data to make a fairly accurate calibration.
|
||||
// We use a Least Mean Squares approximation, as detailed in Freescale application note AN2426.
|
||||
|
||||
// Firstly, calculate the square of each sample.
|
||||
Matrix4 Y(X.height(), 1);
|
||||
for (int i = 0; i < X.height(); i++)
|
||||
{
|
||||
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);
|
||||
Y.set(i, 0, v);
|
||||
}
|
||||
|
||||
// Now perform a Least Squares Approximation.
|
||||
Matrix4 Alpha = X.multiplyT(X).invert();
|
||||
Matrix4 Gamma = X.multiplyT(Y);
|
||||
Matrix4 Beta = Alpha.multiply(Gamma);
|
||||
|
||||
// The result contains the approximate zero point of each axis, but doubled.
|
||||
// Halve each sample, and record this as the compass calibration data.
|
||||
Sample3D cal ((int)(Beta.get(0,0) / 2), (int)(Beta.get(1,0) / 2), (int)(Beta.get(2,0) / 2));
|
||||
compass.setCalibration(cal);
|
||||
compass.setCalibration(calibrate(data, samples));
|
||||
|
||||
// Show a smiley to indicate that we're done, and continue on with the user program.
|
||||
display.clear();
|
||||
|
|
Loading…
Reference in New Issue