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.
This commit is contained in:
Joe Finney
parent
de28387ff3
commit
d51b1205f7
|
|
@ -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
|
||||
|
|
@ -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:
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -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();
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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.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()
|
||||
{
|
||||
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.
|
||||
*/
|
||||
|
|
|
|||
Loading…
Reference in New Issue