From d51b1205f7e234c88270262d6ddf42b831d8ecd4 Mon Sep 17 00:00:00 2001 From: Joe Finney Date: Thu, 17 Dec 2015 14:08:30 +0000 Subject: [PATCH] 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. --- inc/Matrix4.h | 153 +++++++++++++++++ inc/MicroBit.h | 3 + inc/MicroBitAccelerometer.h | 50 +++++- inc/MicroBitCompass.h | 103 +++++++++--- inc/MicroBitConfig.h | 2 +- inc/MicroBitCoordinateSystem.h | 41 +++++ source/CMakeLists.txt | 1 + source/Matrix4.cpp | 271 +++++++++++++++++++++++++++++++ source/MicroBit.cpp | 135 +++++++++++++++ source/MicroBitAccelerometer.cpp | 119 ++++++++++++-- source/MicroBitCompass.cpp | 253 ++++++++++++++++++++--------- 11 files changed, 1009 insertions(+), 122 deletions(-) create mode 100644 inc/Matrix4.h create mode 100644 inc/MicroBitCoordinateSystem.h create mode 100644 source/Matrix4.cpp diff --git a/inc/Matrix4.h b/inc/Matrix4.h new file mode 100644 index 0000000..2c68f26 --- /dev/null +++ b/inc/Matrix4.h @@ -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 diff --git a/inc/MicroBit.h b/inc/MicroBit.h index 86d2d12..a7d08d7 100644 --- a/inc/MicroBit.h +++ b/inc/MicroBit.h @@ -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: diff --git a/inc/MicroBitAccelerometer.h b/inc/MicroBitAccelerometer.h index a9bc583..9be934b 100644 --- a/inc/MicroBitAccelerometer.h +++ b/inc/MicroBitAccelerometer.h @@ -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 diff --git a/inc/MicroBitCompass.h b/inc/MicroBitCompass.h index c12daee..3659b9d 100644 --- a/inc/MicroBitCompass.h +++ b/inc/MicroBitCompass.h @@ -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. diff --git a/inc/MicroBitConfig.h b/inc/MicroBitConfig.h index 0e347ea..d65b86f 100644 --- a/inc/MicroBitConfig.h +++ b/inc/MicroBitConfig.h @@ -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. diff --git a/inc/MicroBitCoordinateSystem.h b/inc/MicroBitCoordinateSystem.h new file mode 100644 index 0000000..f615812 --- /dev/null +++ b/inc/MicroBitCoordinateSystem.h @@ -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 diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index 47faf72..1620e14 100755 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -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" diff --git a/source/Matrix4.cpp b/source/Matrix4.cpp new file mode 100644 index 0000000..415a15e --- /dev/null +++ b/source/Matrix4.cpp @@ -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; + } +} + diff --git a/source/MicroBit.cpp b/source/MicroBit.cpp index 2550e37..7aa03a0 100644 --- a/source/MicroBit.cpp +++ b/source/MicroBit.cpp @@ -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; iid = 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. diff --git a/source/MicroBitCompass.cpp b/source/MicroBitCompass.cpp index 385784f..483fc16 100644 --- a/source/MicroBitCompass.cpp +++ b/source/MicroBitCompass.cpp @@ -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); - - 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); - } - } + 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)); + // 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) +{ + switch (system) + { + case SIMPLE_CARTESIAN: + case NORTH_EAST_DOWN: + return -(sample.z - average.z); + + case RAW: + default: + return sample.z; + } +} + +/** + * 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 MicroBitCompass::getFieldStrength() { - return sample.z; + double x = getX(); + double y = getY(); + double z = getZ(); + + return (int) sqrt(x*x + y*y + z*z); } /** @@ -364,55 +407,103 @@ int MicroBitCompass::readTemperature() /** * Perform a calibration of the compass. - * This will fire MICROBIT_COMPASS_EVT_CAL_START. + * + * 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 MicroBitCompass::calibrateStart() +int MicroBitCompass::calibrate() { - if(this->isCalibrating()) + // Only perform one calibration process at a time. + if(isCalibrating()) return MICROBIT_CALIBRATION_IN_PROGRESS; + // Delete old calibration data + clearCalibration(); + + // Record that we've started calibrating. status |= MICROBIT_COMPASS_STATUS_CALIBRATING; - // Take a sane snapshot to start with. - minSample = sample; - maxSample = sample; + // Launch any registred calibration alogrithm visialisation + MicroBitEvent(id, MICROBIT_COMPASS_EVT_CALIBRATE); - MicroBitEvent(id, MICROBIT_COMPASS_EVT_CAL_START); + // Record that we've finished calibrating. + status &= ~MICROBIT_COMPASS_STATUS_CALIBRATING; + + // If there are no changes to our sample data, we either have no calibration algorithm, or it couldn't complete succesfully. + if(!(status & MICROBIT_COMPASS_STATUS_CALIBRATED)) + return MICROBIT_CALIBRATION_REQUIRED; return MICROBIT_OK; +} + +/** + * Perform a calibration of the compass. + * This will fire MICROBIT_COMPASS_EVT_CAL_START. + * @return MICROBIT_OK, or MICROBIT_I2C_ERROR if the magnetometer could not be accessed. + * + * @note *** THIS FUNCITON IS NOW DEPRECATED AND WILL BE REMOVED IN THE NEXT MAJOR RELEASE *** + * @note *** PLEASE USE THE calibrate() FUNCTION INSTEAD *** + */ +int MicroBitCompass::calibrateStart() +{ + return calibrate(); } /** * Perform the asynchronous calibration of the compass. * This will fire MICROBIT_COMPASS_EVT_CAL_START and MICROBIT_COMPASS_EVT_CAL_END when finished. - * @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 MicroBitCompass::calibrateAsync() { - eventStartTime = ticks; - calibrateStart(); + calibrate(); } /** * 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 MicroBitCompass::calibrateEnd() { - average.x = (maxSample.x + minSample.x) / 2; - average.y = (maxSample.y + minSample.y) / 2; - average.z = (maxSample.z + minSample.z) / 2; - - status &= ~MICROBIT_COMPASS_STATUS_CALIBRATING; - status |= MICROBIT_COMPASS_STATUS_CALIBRATED; - - MicroBitEvent(id, MICROBIT_COMPASS_EVT_CAL_END); } + +/** + * 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 MicroBitCompass::setCalibration(CompassSample calibration) +{ + average = calibration; + status |= MICROBIT_COMPASS_STATUS_CALIBRATED; +} + +/** + * 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 MicroBitCompass::getCalibration() +{ + return average; +} + /** * Returns 0 or 1. 1 indicates that the compass is calibrated, zero means the compass requires calibration. */