Schoumi
/
microbit-dal
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

microbit: Added support for compass tilt compensation 

An e-compass solution requires knowwlede two pieces of data to provide an
accurate heading:

  - Accurate calibration of the magnetometer hardware so that reliable
    measurements can be taken.
  - Knowledge of the pitch and roll of of device, so that the correct
    components of the X/Y and Z axis sensors of the magnetomer can be used
    to sense the magnetic field in a horizontal plane regardless of the tilt
    of the device.

This commit represent changes to the MicroBitAccelerometer and MicroBitCompass
classes to implemen tthese goals. More specifically, this commit provides:

 - The introduciton of an interactive calibration 'game', that can rapidly
   gather all the data required to calibrate the compass.

 - An improved calibration algorithm based on a Least Mean Squares approach of
   compass samples, as documened in Freescale Application Note AN4248.

 - The inclusion of a simple Matrix4 class to enable efficient Least Mean
   Squares implementation.

 - A change from asynchronous to synchronous calibration of the compass when
   first used. This is in repsonse to a feature request for this from users
   and high level languages using microbit-dal.

 - Support for detemrining tilt and roll angle in MicroBitAccelerometer

 - Support for multiple co-ordinate spaces in MicroBitAccelerometer and
   MicroBitCompass. Data can now be read in either RAW (unaltered) data.
   MICORBIT_SIMPLE_CARTESIAN (as used previously) or NORTH_EAST_DOWN
   (the industry convention in mobile phones, tablets and aviation)

 - Implementation of a tilt compensated algorithm, used when determining
   device heading.
master
Joe Finney 2015-12-17 14:08:30 +00:00
parent de28387ff3
commit d51b1205f7
11 changed files with 1008 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

153
inc/Matrix4.h Normal file
View File

@ -0,0 +1,153 @@
#ifndef MICROBIT_MATRIX4_H
#define MICROBIT_MATRIX4_H
/**
* Class definition for a simple matrix, that is optimised for nx4 or 4xn matrices.
*
* This class is heavily optimised for these commonly used matrices as used in 3D geometry.
* Whilst this class does support basic operations on matrices of any dimension, it is not intended as a
* general purpose matrix class as inversion operations are only provided for 4x4 matrices.
* For programmers needing more flexible Matrix support, the Matrix and MatrixMath classes from
* Ernsesto Palacios provide a good basis:
*
* https://developer.mbed.org/cookbook/MatrixClass
* https://developer.mbed.org/users/Yo_Robot/code/MatrixMath/
*/
class Matrix4
{
double *data; // Linear buffer representing the matrix.
int rows; // The number of rows in the matrix.
int cols; // The number of columns in the matrix.
public:
/**
* Constructor.
* Create a matrix of the given size.
* @param rows the number of rows in the matrix to be created.
* @param cols the number of columns in the matrix to be created.
*
* Example:
* @code
* Matrix4(10, 4); // Creates a Matrix with 10 rows and 4 columns.
* @endcode
*/
Matrix4(int rows, int cols);
/**
* Constructor.
* Create a matrix that is an identical copy of the given matrix.
* @param matrix The matrix to copy.
*
* Example:
* @code
*
* Matrix newMatrix(matrix); .
* @endcode
*/
Matrix4(const Matrix4 &matrix);
/**
* Determines the number of columns in this matrix.
*
* @return The number of columns in the matrix.
*
* Example:
* @code
* int c = matrix.width();
* @endcode
*/
int width();
/**
* Determines the number of rows in this matrix.
*
* @return The number of rows in the matrix.
*
* Example:
* @code
* int r = matrix.height();
* @endcode
*/
int height();
/**
* Reads the matrix element at the given position.
*
* @param row The row of the element to read
* @param col The column of the element to read
* @return The value of the matrix element at the given position. NAN is returned if the given index is out of range.
*
* Example:
* @code
* double v = matrix.get(1,2);
* @endcode
*/
double get(int row, int col);
/**
* Writes the matrix element at the given position.
*
* @param row The row of the element to write
* @param col The column of the element to write
* @param v The new value of the element
*
* Example:
* @code
* matrix.set(1,2,42.0);
* @endcode
*/
void set(int row, int col, double v);
/**
* Transposes this matrix.
* @return the resultant matrix.
*
* Example:
* @code
* matrix.transpose();
* @endcode
*/
Matrix4 transpose();
/**
* Multiplies this matrix with the given matrix (if possible).
* @return the resultant matrix. An empty matrix is returned if the operation canot be completed.
*
* Example:
* @code
* Matrix result = matrixA.multiply(matrixB);
* @endcode
*/
Matrix4 multiply(Matrix4 &matrix);
/**
* Performs an optimisaed inversion of a 4x4 matrix.
* Only 4x4 matrics are supported by this operation.
*
* @return the resultant matrix. An empty matrix is returned if the operation canot be completed.
*
* Example:
* @code
* Matrix result = matrixA.invert();
* @endcode
*/
Matrix4 invert();
/**
* Prints this matrix to the console.
*
* Example:
* @code
* matrix.print();
* @endcode
*/
void print();
/**
* Destructor.
*/
~Matrix4();
};
#endif

3
inc/MicroBit.h
View File

@ -7,6 +7,7 @@
#include "MicroBitHeapAllocator.h"
#include "MicroBitPanic.h"
#include "ErrorNo.h"
#include "Matrix4.h"
#include "MicroBitCompat.h"
#include "MicroBitComponent.h"
#include "ManagedType.h"
@ -90,7 +91,9 @@ class MicroBit
private:
void seedRandom();
void compassCalibrator(MicroBitEvent e);
uint32_t randomValue;
public:

50
inc/MicroBitAccelerometer.h
View File

@ -3,12 +3,18 @@
#include "mbed.h"
#include "MicroBitComponent.h"
#include "MicroBitCoordinateSystem.h"
/**
* Relevant pin assignments
*/
#define MICROBIT_PIN_ACCEL_DATA_READY P0_28
/**
* Status flags
*/
#define MICROBIT_ACCEL_PITCH_ROLL_VALID 0x01
/*
* I2C constants
*/
@ -60,6 +66,7 @@ struct MMA8653SampleRangeConfig
uint8_t xyz_data_cfg;
};
extern const MMA8653SampleRangeConfig MMA8653SampleRange[];
extern const MMA8653SampleRateConfig MMA8653SampleRate[];
@ -81,6 +88,8 @@ class MicroBitAccelerometer : public MicroBitComponent
uint8_t sampleRange; // The sample range of the accelerometer in g.
MMA8653Sample sample; // The last sample read.
DigitalIn int1; // Data ready interrupt.
float pitch; // Pitch of the device, in radians.
float roll; // Roll of the device, in radians.
public:
@ -158,7 +167,7 @@ class MicroBitAccelerometer : public MicroBitComponent
/**
* Reads the X axis value of the latest update from the accelerometer.
* Currently limited to +/- 2g
* @param system The coordinate system to use. By default, a simple cartesian system is provided.
* @return The force measured in the X axis, in milli-g.
*
* Example:
@ -166,11 +175,10 @@ class MicroBitAccelerometer : public MicroBitComponent
* uBit.accelerometer.getX();
* @endcode
*/
int getX();
int getX(MicroBitCoordinateSystem system = SIMPLE_CARTESIAN);
/**
* Reads the Y axis value of the latest update from the accelerometer.
* Currently limited to +/- 2g
* @return The force measured in the Y axis, in milli-g.
*
* Example:
@ -178,11 +186,10 @@ class MicroBitAccelerometer : public MicroBitComponent
* uBit.accelerometer.getY();
* @endcode
*/
int getY();
int getY(MicroBitCoordinateSystem system = SIMPLE_CARTESIAN);
/**
* Reads the Z axis value of the latest update from the accelerometer.
* Currently limited to +/- 2g
* @return The force measured in the Z axis, in milli-g.
*
* Example:
@ -190,7 +197,31 @@ class MicroBitAccelerometer : public MicroBitComponent
* uBit.accelerometer.getZ();
* @endcode
*/
int getZ();
int getZ(MicroBitCoordinateSystem system = SIMPLE_CARTESIAN);
/**
* Provides a rotation compensated pitch of the device, based on the latest update from the accelerometer.
* @return The pitch of the device, in degrees.
*
* Example:
* @code
* uBit.accelerometer.getPitch();
* @endcode
*/
int getPitch();
float getPitchRadians();
/**
* Provides a rotation compensated roll of the device, based on the latest update from the accelerometer.
* @return The roll of the device, in degrees.
*
* Example:
* @code
* uBit.accelerometer.getRoll();
* @endcode
*/
int getRoll();
float getRollRadians();
/**
* periodic callback from MicroBit idle thread.
@ -224,6 +255,13 @@ class MicroBitAccelerometer : public MicroBitComponent
* @return MICROBIT_OK on success, MICROBIT_INVALID_PARAMETER or MICROBIT_I2C_ERROR if the the read request failed.
*/
int readCommand(uint8_t reg, uint8_t* buffer, int length);
/**
* Recalculate roll and pitch values for the current sample.
* We only do this at most once per sample, as the necessary trigonemteric functions are rather
* heavyweight for a CPU without a floating point unit...
*/
void recalculatePitchRoll();
};
#endif

103
inc/MicroBitCompass.h
View File

@ -3,6 +3,7 @@
#include "mbed.h"
#include "MicroBitComponent.h"
#include "MicroBitCoordinateSystem.h"
/**
* Relevant pin assignments
@ -52,11 +53,13 @@ extern const MAG3110SampleRateConfig MAG3110SampleRate[];
/*
* Compass events
*/
#define MICROBIT_COMPASS_EVT_CAL_REQUIRED 1
#define MICROBIT_COMPASS_EVT_CAL_START 2
#define MICROBIT_COMPASS_EVT_CAL_END 3
#define MICROBIT_COMPASS_EVT_CAL_REQUIRED 1 // DEPRECATED
#define MICROBIT_COMPASS_EVT_CAL_START 2 // DEPRECATED
#define MICROBIT_COMPASS_EVT_CAL_END 3 // DEPRECATED
#define MICROBIT_COMPASS_EVT_DATA_UPDATE 4
#define MICROBIT_COMPASS_EVT_CONFIG_NEEDED 5
#define MICROBIT_COMPASS_EVT_CALIBRATE 6
/*
* Status Bits
@ -64,8 +67,10 @@ extern const MAG3110SampleRateConfig MAG3110SampleRate[];
#define MICROBIT_COMPASS_STATUS_CALIBRATED 1
#define MICROBIT_COMPASS_STATUS_CALIBRATING 2
#define MICROBIT_COMPASS_CALIBRATE_PERIOD 10000
/*
* Term to convert sample data into SI units
*/
#define MAG3110_NORMALIZE_SAMPLE(x) (100*x)
/*
* MAG3110 MAGIC ID value
@ -75,9 +80,9 @@ extern const MAG3110SampleRateConfig MAG3110SampleRate[];
struct CompassSample
{
int16_t x;
int16_t y;
int16_t z;
int x;
int y;
int z;
CompassSample()
{
@ -85,6 +90,13 @@ struct CompassSample
this->y = 0;
this->z = 0;
}
CompassSample(int x, int y, int z)
{
this->x = x;
this->y = y;
this->z = z;
}
};
/**
@ -102,13 +114,10 @@ class MicroBitCompass : public MicroBitComponent
uint16_t address; // I2C address of the magnetmometer.
uint16_t samplePeriod; // The time between samples, in millseconds.
unsigned long eventStartTime; // used to store the current system clock when async calibration has started
CompassSample minSample; // Calibration sample.
CompassSample maxSample; // Calibration sample.
CompassSample average; // Centre point of sample data.
CompassSample sample; // The latest sample data recorded.
DigitalIn int1; // Data ready interrupt.
CompassSample average; // Centre point of sample data.
CompassSample sample; // The latest sample data recorded.
DigitalIn int1; // Data ready interrupt.
public:
@ -158,8 +167,12 @@ class MicroBitCompass : public MicroBitComponent
/**
* Gets the current heading of the device, relative to magnetic north.
* If he compass is not calibrated, it will raise the MICROBIT_COMPASS_EVT_CALIBRATE event.
* Users wishing to implekent their own calibration algorithms should listen for this event,
* and implement their evnet handler using MESSAGE_BUS_LISTENER_IMMEDIATE model. This ensure that
* calibration is complete before the user program continues.
*
* @return the current heading, in degrees. Or MICROBIT_COMPASS_IS_CALIBRATING if the compass is calibrating.
* Or MICROBIT_COMPASS_CALIBRATE_REQUIRED if the compass requires calibration.
*
* Example:
* @code
@ -188,7 +201,7 @@ class MicroBitCompass : public MicroBitComponent
* uBit.compass.getX();
* @endcode
*/
int getX();
int getX(MicroBitCoordinateSystem system = SIMPLE_CARTESIAN);
/**
* Reads the Y axis value of the latest update from the compass.
@ -199,7 +212,7 @@ class MicroBitCompass : public MicroBitComponent
* uBit.compass.getY();
* @endcode
*/
int getY();
int getY(MicroBitCoordinateSystem system = SIMPLE_CARTESIAN);
/**
* Reads the Z axis value of the latest update from the compass.
@ -210,7 +223,18 @@ class MicroBitCompass : public MicroBitComponent
* uBit.compass.getZ();
* @endcode
*/
int getZ();
int getZ(MicroBitCoordinateSystem system = SIMPLE_CARTESIAN);
/**
* Determines the overall magnetic field strength based on the latest update from the compass.
* @return The magnetic force measured across all axes, in nano teslas.
*
* Example:
* @code
* uBit.compass.getFieldStrength();
* @endcode
*/
int getFieldStrength();
/**
* Reads the currently die temperature of the compass.
@ -218,28 +242,65 @@ class MicroBitCompass : public MicroBitComponent
*/
int readTemperature();
/**
* Perform a calibration of the compass.
*
* This method will be clled automatically if a user attmepts to read a compass value when
* the compass is uncalibrated. It can also be called at any time by the user.
*
* Any old calibration data is deleted.
* The method will only return once the compass has been calibrated.
*
* @return MICROBIT_OK, or MICROBIT_I2C_ERROR if the magnetometer could not be accessed.
* @note THIS MUST BE CALLED TO GAIN RELIABLE VALUES FROM THE COMPASS
*/
int calibrate();
/**
* Perform the asynchronous calibration of the compass.
* This will fire MICROBIT_COMPASS_EVT_CAL_START and MICROBIT_COMPASS_EVT_CAL_END when finished.
* @return MICROBIT_OK, or MICROBIT_I2C_ERROR if the magnetometer could not be accessed.
* @note THIS MUST BE CALLED TO GAIN RELIABLE VALUES FROM THE COMPASS
*
* @note *** THIS FUNCITON IS NOW DEPRECATED AND WILL BE REMOVED IN THE NEXT MAJOR RELEASE ***
* @note *** PLEASE USE THE calibrate() FUNCTION INSTEAD ***
*/
void calibrateAsync();
/**
* Perform a calibration of the compass.
* This will fire MICROBIT_COMPASS_EVT_CAL_START.
* @note THIS MUST BE CALLED TO GAIN RELIABLE VALUES FROM THE COMPASS
*
* @note *** THIS FUNCITON IS NOW DEPRECATED AND WILL BE REMOVED IN THE NEXT MAJOR RELEASE ***
* @note *** PLEASE USE THE calibrate() FUNCTION INSTEAD ***
*/
int calibrateStart();
/**
* Complete the calibration of the compass.
* This will fire MICROBIT_COMPASS_EVT_CAL_END.
* @note THIS MUST BE CALLED TO GAIN RELIABLE VALUES FROM THE COMPASS
*
* @note *** THIS FUNCITON IS NOW DEPRECATED AND WILL BE REMOVED IN THE NEXT MAJOR RELEASE ***
*/
void calibrateEnd();
/**
* Configure the compass to use the given calibration data.
* Claibration data is essenutially the perceived zero offset of each axis of the compass.
* After calibration should now take into account trimming errors in the deivce, hard iron offsets on the device
* and local magnetic effects present at the time of claibration.
*
* @param The x, y and z xero offsets to use as calibration data
*/
void setCalibration(CompassSample calibration);
/**
* Provides the calibration data currently in use by the compass.
* More specifically, the x, y and z zero offsets of the compass.
*
* @return The x, y and z xero offsets of the compass.
*/
CompassSample getCalibration();
/**
* Periodic callback from MicroBit idle thread.
* Check if any data is ready for reading by checking the interrupt.

2
inc/MicroBitConfig.h
View File

@ -39,7 +39,7 @@
// The proportion of SRAM available on the mbed heap to reserve for the micro:bit heap.
#ifndef MICROBIT_HEAP_SIZE
#define MICROBIT_HEAP_SIZE 0.95
#define MICROBIT_HEAP_SIZE 0.25
#endif
// if defined, reuse the 8K of SRAM reserved for SoftDevice (Nordic's memory resident BLE stack) as heap memory.

41
inc/MicroBitCoordinateSystem.h Normal file
View File

@ -0,0 +1,41 @@
#ifndef MICROBIT_COORDINATE_SYSTEM_H
#define MICROBIT_COORDINATE_SYSTEM_H
/**
* Co-ordinate systems that can be used.
* RAW: Unaltered data. Data will be returned directly from the accelerometer.
*
* SIMPLE_CARTESIAN: Data will be returned based on an easy to understand alignment, consistent with the cartesian system taught in schools.
* When held upright, facing the user:
*
* /
* +--------------------+ z
* | |
* | ..... |
* | * ..... * |
* ^ | ..... |
* | | |
* y +--------------------+ x-->
*
*
* NORTH_EAST_DOWN: Data will be returned based on the industry convention of the North East Down (NED) system.
* When held upright, facing the user:
*
* z
* +--------------------+ /
* | |
* | ..... |
* | * ..... * |
* ^ | ..... |
* | | |
* x +--------------------+ y-->
*
*/
enum MicroBitCoordinateSystem
{
RAW,
SIMPLE_CARTESIAN,
NORTH_EAST_DOWN
};
#endif

1
source/CMakeLists.txt
View File

@ -17,6 +17,7 @@ set(YOTTA_AUTO_MICROBIT-DAL_CPP_FILES
"MicroBitEvent.cpp"
"MicroBitFiber.cpp"
"ManagedString.cpp"
"Matrix4.cpp"
"MicroBitAccelerometer.cpp"
"MicroBitThermometer.cpp"
"MicroBitIO.cpp"

271
source/Matrix4.cpp Normal file
View File

@ -0,0 +1,271 @@
#include "MicroBit.h"
/**
* Class definition for a simple matrix, optimised for n x 4 or 4 x n matrices.
*
* This class is heavily optimised for these commonly used matrices as used in 3D geometry,
* and is not intended as a general purpose matrix class. For programmers needing more flexible
* Matrix support, the mbed Matrix and MatrixMath classes from Ernsesto Palacios provide a good basis:
*
* https://developer.mbed.org/cookbook/MatrixClass
* https://developer.mbed.org/users/Yo_Robot/code/MatrixMath/
*/
/**
* Constructor.
* Create a matrix of the given size.
* @param rows the number of rows in the matrix to be created.
* @param cols the number of columns in the matrix to be created.
*
* Example:
* @code
* Matrix4(10, 4); // Creates a Matrix with 10 rows and 4 columns.
* @endcode
*/
Matrix4::Matrix4(int rows, int cols)
{
this->rows = rows;
this->cols = cols;
int size = rows * cols;
if (size > 0)
data = new double[size];
else
data = NULL;
}
/**
* Constructor.
* Create a matrix that is an identicval copy of the given matrix.
* @param matrix The matrix to copy.
*
* Example:
* @code
*
* Matrix newMatrix(matrix); .
* @endcode
*/
Matrix4::Matrix4(const Matrix4 &matrix)
{
this->rows = matrix.rows;
this->cols = matrix.cols;
int size = rows * cols;
if (size > 0)
{
data = new double[size];
for (int i = 0; i < size; i++)
data[i] = matrix.data[i];
}
else
{
data = NULL;
}
}
/**
* Determines the number of columns in this matrix.
*
* @return The number of columns in the matrix.
*
* Example:
* @code
* int c = matrix.width();
* @endcode
*/
int Matrix4::width()
{
return cols;
}
/**
* Determines the number of rows in this matrix.
*
* @return The number of rows in the matrix.
*
* Example:
* @code
* int r = matrix.height();
* @endcode
*/
int Matrix4::height()
{
return rows;
}
/**
* Reads the matrix element at the given position.
*
* @param row The row of the element to read
* @param col The column of the element to read
* @return The value of the matrix element at the given position. NAN is returned if the given index is out of range.
*
* Example:
* @code
* double v = matrix.get(1,2);
* @endcode
*/
double Matrix4::get(int row, int col)
{
if (row < 0 || col < 0 || row >= rows || col >= cols)
return 0;
return data[width() * row + col];
}
/**
* Writes the matrix element at the given position.
*
* @param row The row of the element to write
* @param col The column of the element to write
* @param v The new value of the element
*
* Example:
* @code
* matrix.set(1,2,42.0);
* @endcode
*/
void Matrix4::set(int row, int col, double v)
{
if (row < 0 || col < 0 || row >= rows || col >= cols)
return;
data[width() * row + col] = v;
}
/**
* Transposes this matrix.
* @return the resultant matrix.
*
* Example:
* @code
* matrix.transpose();
* @endcode
*/
Matrix4 Matrix4::transpose()
{
Matrix4 result = Matrix4(cols, rows);
for (int i = 0; i < width(); i++)
for (int j = 0; j < height(); j++)
result.set(i, j, get(j, i));
return result;
}
/**
* Multiplies this matrix with the given matrix (if possible).
* @return the resultant matrix. An empty matrix is returned if the operation canot be completed.
*
* Example:
* @code
* Matrix result = matrixA.multiply(matrixB);
* @endcode
*/
Matrix4 Matrix4::multiply(Matrix4 &matrix)
{
if (width() != matrix.height())
return Matrix4(0, 0);
Matrix4 result(height(), matrix.width());
for (int r = 0; r < result.height(); r++)
{
for (int c = 0; c < result.width(); c++)
{
double v = 0.0;
for (int i = 0; i < width(); i++)
v += get(r, i) * matrix.get(i, c);
result.set(r, c, v);
}
}
return result;
}
/**
* Performs an optimised inversion of a 4x4 matrix.
* Only 4x4 matrices are supported by this operation.
*
* @return the resultant matrix. An empty matrix is returned if the operation canot be completed.
*
* Example:
* @code
* Matrix result = matrixA.invert();
* @endcode
*/
Matrix4 Matrix4::invert()
{
// We only support square matrices of size 4...
if (width() != height() || width() != 4)
return Matrix4(0, 0);
Matrix4 result(width(), height());
result.data[0] = data[5] * data[10] * data[15] - data[5] * data[11] * data[14] - data[9] * data[6] * data[15] + data[9] * data[7] * data[14] + data[13] * data[6] * data[11] - data[13] * data[7] * data[10];
result.data[1] = -data[1] * data[10] * data[15] + data[1] * data[11] * data[14] + data[9] * data[2] * data[15] - data[9] * data[3] * data[14] - data[13] * data[2] * data[11] + data[13] * data[3] * data[10];
result.data[2] = data[1] * data[6] * data[15] - data[1] * data[7] * data[14] - data[5] * data[2] * data[15] + data[5] * data[3] * data[14] + data[13] * data[2] * data[7] - data[13] * data[3] * data[6];
result.data[3] = -data[1] * data[6] * data[11] + data[1] * data[7] * data[10] + data[5] * data[2] * data[11] - data[5] * data[3] * data[10] - data[9] * data[2] * data[7] + data[9] * data[3] * data[6];
result.data[4] = -data[4] * data[10] * data[15] + data[4] * data[11] * data[14] + data[8] * data[6] * data[15] - data[8] * data[7] * data[14] - data[12] * data[6] * data[11] + data[12] * data[7] * data[10];
result.data[5] = data[0] * data[10] * data[15] - data[0] * data[11] * data[14] - data[8] * data[2] * data[15] + data[8] * data[3] * data[14] + data[12] * data[2] * data[11] - data[12] * data[3] * data[10];
result.data[6] = -data[0] * data[6] * data[15] + data[0] * data[7] * data[14] + data[4] * data[2] * data[15] - data[4] * data[3] * data[14] - data[12] * data[2] * data[7] + data[12] * data[3] * data[6];
result.data[7] = data[0] * data[6] * data[11] - data[0] * data[7] * data[10] - data[4] * data[2] * data[11] + data[4] * data[3] * data[10] + data[8] * data[2] * data[7] - data[8] * data[3] * data[6];
result.data[8] = data[4] * data[9] * data[15] - data[4] * data[11] * data[13] - data[8] * data[5] * data[15] + data[8] * data[7] * data[13] + data[12] * data[5] * data[11] - data[12] * data[7] * data[9];
result.data[9] = -data[0] * data[9] * data[15] + data[0] * data[11] * data[13] + data[8] * data[1] * data[15] - data[8] * data[3] * data[13] - data[12] * data[1] * data[11] + data[12] * data[3] * data[9];
result.data[10] = data[0] * data[5] * data[15] - data[0] * data[7] * data[13] - data[4] * data[1] * data[15] + data[4] * data[3] * data[13] + data[12] * data[1] * data[7] - data[12] * data[3] * data[5];
result.data[11] = -data[0] * data[5] * data[11] + data[0] * data[7] * data[9] + data[4] * data[1] * data[11] - data[4] * data[3] * data[9] - data[8] * data[1] * data[7] + data[8] * data[3] * data[5];
result.data[12] = -data[4] * data[9] * data[14] + data[4] * data[10] * data[13] + data[8] * data[5] * data[14] - data[8] * data[6] * data[13] - data[12] * data[5] * data[10] + data[12] * data[6] * data[9];
result.data[13] = data[0] * data[9] * data[14] - data[0] * data[10] * data[13] - data[8] * data[1] * data[14] + data[8] * data[2] * data[13] + data[12] * data[1] * data[10] - data[12] * data[2] * data[9];
result.data[14] = -data[0] * data[5] * data[14] + data[0] * data[6] * data[13] + data[4] * data[1] * data[14] - data[4] * data[2] * data[13] - data[12] * data[1] * data[6] + data[12] * data[2] * data[5];
result.data[15] = data[0] * data[5] * data[10] - data[0] * data[6] * data[9] - data[4] * data[1] * data[10] + data[4] * data[2] * data[9] + data[8] * data[1] * data[6] - data[8] * data[2] * data[5];
double det = data[0] * result.data[0] + data[1] * result.data[4] + data[2] * result.data[8] + data[3] * result.data[12];
if (det == 0)
return Matrix4(0, 0);
det = 1.0f / det;
for (int i = 0; i < 16; i++)
result.data[i] *= det;
return result;
}
/**
* Prints this matrix to the console.
*
* Example:
* @code
* matrix.print();
* @endcode
*/
void Matrix4::print()
{
for (int r = 0; r < height(); r++)
{
for (int c = 0; c < width(); c++)
{
uBit.serial.printf("%d\t", (int)get(r, c));
}
uBit.serial.printf("\n");
}
}
/**
* Destructor.
*/
Matrix4::~Matrix4()
{
if (data != NULL)
{
delete data;
data = NULL;
}
}

135
source/MicroBit.cpp
View File

@ -172,6 +172,141 @@ void MicroBit::init()
// Start refreshing the Matrix Display
systemTicker.attach(this, &MicroBit::systemTick, MICROBIT_DISPLAY_REFRESH_PERIOD);
// Register our compass calibration algorithm.
MessageBus.listen(MICROBIT_ID_COMPASS, MICROBIT_COMPASS_EVT_CALIBRATE, this, &MicroBit::compassCalibrator, 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 MicroBit::compassCalibrator(MicroBitEvent)
{
struct Point
{
uint8_t x;
uint8_t y;
uint8_t on;
};
const int PERIMETER_POINTS = 12;
const int PIXEL1_THRESHOLD = 200;
const int PIXEL2_THRESHOLD = 800;
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};
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;
// 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++)
{
if (buttonA.isPressed() || buttonB.isPressed())
{
break;
}
sleep(100);
}
display.stopAnimation();
display.clear();
while(samples < PERIMETER_POINTS)
{
// update our model of the flash status of the user controlled pixel.
cursor.on = (cursor.on + 1) % 4;
// take a snapshot of the current accelerometer data.
int x = uBit.accelerometer.getX();
int y = uBit.accelerometer.getY();
// Deterine the position of the user controlled pixel on the screen.
if (x < -PIXEL2_THRESHOLD)
cursor.x = 0;
else if (x < -PIXEL1_THRESHOLD)
cursor.x = 1;
else if (x > PIXEL2_THRESHOLD)
cursor.x = 4;
else if (x > PIXEL1_THRESHOLD)
cursor.x = 3;
else
cursor.x = 2;
if (y < -PIXEL2_THRESHOLD)
cursor.y = 0;
else if (y < -PIXEL1_THRESHOLD)
cursor.y = 1;
else if (y > PIXEL2_THRESHOLD)
cursor.y = 4;
else if (y > PIXEL1_THRESHOLD)
cursor.y = 3;
else
cursor.y = 2;
img.clear();
// Turn on any pixels that have been visited.
for (int i=0; i<PERIMETER_POINTS; i++)
if (perimeter[i].on)
img.setPixelValue(perimeter[i].x, perimeter[i].y, 255);
// Update the flashing pixel at the users position, if
img.setPixelValue(cursor.x, cursor.y, 255);
// Update the buffer to the screen.
uBit.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)
{
// Record the sample data for later processing...
X.set(samples, 0, compass.getX(RAW));
X.set(samples, 1, compass.getY(RAW));
X.set(samples, 2, compass.getZ(RAW));
X.set(samples, 3, 1);
// Record that this pixel has been visited.
perimeter[i].on = 1;
samples++;
}
}
uBit.sleep(100);
}
// 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++)
{
double 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 XT = X.transpose();
Matrix4 Beta = XT.multiply(X).invert().multiply(XT).multiply(Y);
// The result contains the approximate zero point of each axis, but doubled.
// Halve each sample, and record this as the compass calibration data.
CompassSample cal = {(int)(Beta.get(0,0) / 2), (int)(Beta.get(1,0) / 2), (int)(Beta.get(2,0) / 2)};
compass.setCalibration(cal);
// Show a smiley to indicate that we're done, and continue on with the user program.
display.clear();
display.print(smiley, 0, 0, 0, 1500);
display.clear();
}
/**

119
source/MicroBitAccelerometer.cpp
View File

@ -138,6 +138,7 @@ MicroBitAccelerometer::MicroBitAccelerometer(uint16_t id, uint16_t address) : sa
{
// Store our identifiers.
this->id = id;
this->status = 0;
this->address = address;
// Update our internal state for 50Hz at +/- 2g (50Hz has a period af 20ms).
@ -195,10 +196,6 @@ int MicroBitAccelerometer::update()
sample.y *= 8;
sample.z *= 8;
// Invert the x and y axes, so that the reference frame aligns with micro:bit expectations
sample.x = -sample.x;
sample.y = -sample.y;
#if CONFIG_ENABLED(USE_ACCEL_LSB)
// Add in LSB values.
sample.x += (data[1] / 64);
@ -211,6 +208,9 @@ int MicroBitAccelerometer::update()
sample.y *= this->sampleRange;
sample.z *= this->sampleRange;
// Indicat that pitch and roll data is now stale, and needs to be recalculated if needed.
status &= ~MICROBIT_ACCEL_PITCH_ROLL_VALID;
// Indicate that a new sample is available
MicroBitEvent e(id, MICROBIT_ACCELEROMETER_EVT_DATA_UPDATE);
@ -263,49 +263,142 @@ int MicroBitAccelerometer::getRange()
/**
* Reads the X axis value of the latest update from the accelerometer.
* Currently limited to +/- 2g
* @param system The coordinate system to use. By default, a simple cartesian system is provided.
* @return The force measured in the X axis, in milli-g.
*
* Example:
* @code
* uBit.accelerometer.getX();
* uBit.accelerometer.getX(RAW);
* @endcode
*/
int MicroBitAccelerometer::getX()
int MicroBitAccelerometer::getX(MicroBitCoordinateSystem system)
{
return sample.x;
switch (system)
{
case SIMPLE_CARTESIAN:
return -sample.x;
case NORTH_EAST_DOWN:
return sample.y;
case RAW:
default:
return sample.x;
}
}
/**
* Reads the Y axis value of the latest update from the accelerometer.
* Currently limited to +/- 2g
* @param system The coordinate system to use. By default, a simple cartesian system is provided.
* @return The force measured in the Y axis, in milli-g.
*
* Example:
* @code
* uBit.accelerometer.getY();
* uBit.accelerometer.getY(RAW);
* @endcode
*/
int MicroBitAccelerometer::getY()
int MicroBitAccelerometer::getY(MicroBitCoordinateSystem system)
{
return sample.y;
switch (system)
{
case SIMPLE_CARTESIAN:
return -sample.y;
case NORTH_EAST_DOWN:
return -sample.x;
case RAW:
default:
return sample.y;
}
}
/**
* Reads the Z axis value of the latest update from the accelerometer.
* Currently limited to +/- 2g
* @param system The coordinate system to use. By default, a simple cartesian system is provided.
* @return The force measured in the Z axis, in milli-g.
*
* Example:
* @code
* uBit.accelerometer.getZ();
* uBit.accelerometer.getZ(RAW);
* @endcode
*/
int MicroBitAccelerometer::getZ()
int MicroBitAccelerometer::getZ(MicroBitCoordinateSystem system)
{
return sample.z;
switch (system)
{
case NORTH_EAST_DOWN:
return -sample.z;
case SIMPLE_CARTESIAN:
case RAW:
default:
return sample.z;
}
}
/**
* Provides a rotation compensated pitch of the device, based on the latest update from the accelerometer.
* @return The pitch of the device, in degrees.
*
* Example:
* @code
* uBit.accelerometer.getPitch();
* @endcode
*/
int MicroBitAccelerometer::getPitch()
{
return (int) ((360*getPitchRadians()) / (2*PI));
}
float MicroBitAccelerometer::getPitchRadians()
{
if (!(status & MICROBIT_ACCEL_PITCH_ROLL_VALID))
recalculatePitchRoll();
return pitch;
}
/**
* Provides a rotation compensated roll of the device, based on the latest update from the accelerometer.
* @return The roll of the device, in degrees.
*
* Example:
* @code
* uBit.accelerometer.getRoll();
* @endcode
*/
int MicroBitAccelerometer::getRoll()
{
return (int) ((360*getRollRadians()) / (2*PI));
}
float MicroBitAccelerometer::getRollRadians()
{
if (!(status & MICROBIT_ACCEL_PITCH_ROLL_VALID))
recalculatePitchRoll();
return roll;
}
/**
* Recalculate roll and pitch values for the current sample.
* We only do this at most once per sample, as the necessary trigonemteric functions are rather
* heavyweight for a CPU without a floating point unit...
*/
void MicroBitAccelerometer::recalculatePitchRoll()
{
float x = (float) getX(NORTH_EAST_DOWN);
float y = (float) getY(NORTH_EAST_DOWN);
float z = (float) getZ(NORTH_EAST_DOWN);
roll = atan2(getY(NORTH_EAST_DOWN), getZ(NORTH_EAST_DOWN));
pitch = atan(-x / (y*sin(roll) + z*cos(roll)));
status |= MICROBIT_ACCEL_PITCH_ROLL_VALID;
}
/**
* periodic callback from MicroBit clock.

251
source/MicroBitCompass.cpp
View File

@ -23,16 +23,14 @@ MicroBitCompass::MicroBitCompass(uint16_t id, uint16_t address) : average(), sam
this->id = id;
this->address = address;
//we presume the device calibrated until the average values are read.
// We presume the device calibrated until the average values are read.
this->status = 0x01;
//initialise eventStartTime to 0
this->eventStartTime = 0;
// Select 10Hz update rate, with oversampling, and enable the device.
this->samplePeriod = 100;
this->configure();
// Assume that we have no calibraiton information.
status &= ~MICROBIT_COMPASS_STATUS_CALIBRATED;
// Indicate that we're up and running.
@ -130,115 +128,160 @@ int MicroBitCompass::read8(uint8_t reg)
return data;
}
/**
* Gets the current heading of the device, relative to magnetic north.
* @return the current heading, in degrees. Or MICROBIT_COMPASS_IS_CALIBRATING if the compass is calibrating.
* Or MICROBIT_COMPASS_CALIBRATE_REQUIRED if the compass requires calibration.
*
* Example:
* @code
* uBit.compass.heading();
* @endcode
*/
* Gets the current heading of the device, relative to magnetic north.
* If he compass is not calibrated, it will raise the MICROBIT_COMPASS_EVT_CALIBRATE event.
* Users wishing to implekent their own calibration algorithms should listen for this event,
* and implement their evnet handler using MESSAGE_BUS_LISTENER_IMMEDIATE model. This ensure that
* calibration is complete before the user program continues.
*
* @return the current heading, in degrees. Or MICROBIT_CALIBRATION_IN_PROGRESS if the compass is calibrating.
*
* Example:
* @code
* uBit.compass.heading();
* @endcode
*/
int MicroBitCompass::heading()
{
float bearing;
if(status & MICROBIT_COMPASS_STATUS_CALIBRATING)
return MICROBIT_CALIBRATION_IN_PROGRESS;
else if(!(status & MICROBIT_COMPASS_STATUS_CALIBRATED))
{
MicroBitEvent(id, MICROBIT_COMPASS_EVT_CAL_REQUIRED);
return MICROBIT_CALIBRATION_REQUIRED;
}
if(!(status & MICROBIT_COMPASS_STATUS_CALIBRATED))
calibrate();
// Precompute the tilt compensation parameters to improve readability.
float bearing = (atan2((double)(sample.y - average.y),(double)(sample.x - average.x)))*180/PI;
float phi = uBit.accelerometer.getRollRadians();
float theta = uBit.accelerometer.getPitchRadians();
float x = (float) getX(NORTH_EAST_DOWN);
float y = (float) getY(NORTH_EAST_DOWN);
float z = (float) getZ(NORTH_EAST_DOWN);
// 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);
bearing = (360*atan2(z*sinPhi - y*cosPhi, x*cosTheta + y*sinTheta*sinPhi + z*sinTheta*cosPhi)) / (2*PI);
if (bearing < 0)
bearing += 360.0;
return (int) (360.0 - bearing);
return (int) bearing;
}
/**
* Periodic callback from MicroBit clock.
* Check if any data is ready for reading by checking the interrupt.
*/
void MicroBitCompass::idleTick()
{
// Poll interrupt line from accelerometer.
// Active HI. Interrupt is cleared in data read of MAG_OUT_X_MSB.
// Poll interrupt line from accelerometer (Active HI).
// Interrupt is cleared on data read of MAG_OUT_X_MSB.
if(int1)
{
sample.x = (int16_t) read16(MAG_OUT_X_MSB);
sample.y = (int16_t) read16(MAG_OUT_Y_MSB);
sample.z = (int16_t) read16(MAG_OUT_Z_MSB);
sample.x = MAG3110_NORMALIZE_SAMPLE((int) read16(MAG_OUT_X_MSB));
sample.y = MAG3110_NORMALIZE_SAMPLE((int) read16(MAG_OUT_Y_MSB));
sample.z = MAG3110_NORMALIZE_SAMPLE((int) read16(MAG_OUT_Z_MSB));
if (status & MICROBIT_COMPASS_STATUS_CALIBRATING)
{
minSample.x = min(sample.x, minSample.x);
minSample.y = min(sample.y, minSample.y);
minSample.z = min(sample.z, minSample.z);
maxSample.x = max(sample.x, maxSample.x);
maxSample.y = max(sample.y, maxSample.y);
maxSample.z = max(sample.z, maxSample.z);
if(eventStartTime && ticks > eventStartTime + MICROBIT_COMPASS_CALIBRATE_PERIOD)
{
eventStartTime = 0;
calibrateEnd();
}
}
else
{
// Indicate that a new sample is available
MicroBitEvent e(id, MICROBIT_COMPASS_EVT_DATA_UPDATE);
}
// Indicate that a new sample is available
MicroBitEvent e(id, MICROBIT_COMPASS_EVT_DATA_UPDATE);
}
}
/**
* Reads the X axis value of the latest update from the compass.
* @return The magnetic force measured in the X axis, in no specific units.
* @return The magnetic force measured in the X axis, in nano teslas.
*
* Example:
* @code
* uBit.compass.getX();
* @endcode
*/
int MicroBitCompass::getX()
int MicroBitCompass::getX(MicroBitCoordinateSystem system)
{
return sample.x;
switch (system)
{
case SIMPLE_CARTESIAN:
return sample.x - average.x;
case NORTH_EAST_DOWN:
return -(sample.y - average.y);
case RAW:
default:
return sample.x;
}
}
/**
* Reads the Y axis value of the latest update from the compass.
* @return The magnetic force measured in the Y axis, in no specific units.
* @return The magnetic force measured in the Y axis, in nano teslas.
*
* Example:
* @code
* uBit.compass.getY();
* @endcode
*/
int MicroBitCompass::getY()
int MicroBitCompass::getY(MicroBitCoordinateSystem system)
{
return sample.y;
switch (system)
{
case SIMPLE_CARTESIAN:
return -(sample.y - average.y);
case NORTH_EAST_DOWN:
return (sample.x - average.x);
case RAW:
default:
return sample.y;
}
}
/**
* Reads the Z axis value of the latest update from the compass.
* @return The magnetic force measured in the Z axis, in no specific units.
* @return The magnetic force measured in the Z axis, in nano teslas.
*
* Example:
* @code
* uBit.compass.getZ();
* @endcode
*/
int MicroBitCompass::getZ()
int MicroBitCompass::getZ(MicroBitCoordinateSystem system)
{
return sample.z;
switch (system)
{
case SIMPLE_CARTESIAN:
case NORTH_EAST_DOWN:
return -(sample.z - average