From e79284d797abcc22342f43991fe362241f5b8a6f Mon Sep 17 00:00:00 2001 From: Joe Finney Date: Fri, 15 Jun 2018 18:39:56 +0100 Subject: [PATCH 1/9] Introduce accelerometer/magnetometer autodetection - Autodetection logic added to MicroBitAccelerometer/MicroBitCompass - Added single byte I2C read utility function into MicroBitI2C - Added isDetected() method into sensor drivers --- inc/core/ErrorNo.h | 3 ++ inc/drivers/FXOS8700.h | 15 ++---- inc/drivers/LSM303Accelerometer.h | 10 +++- inc/drivers/LSM303Magnetometer.h | 10 +++- inc/drivers/MAG3110.h | 10 +++- inc/drivers/MMA8653.h | 11 ++++- inc/drivers/MicroBitAccelerometer.h | 12 +++++ inc/drivers/MicroBitCompass.h | 14 +++++- inc/drivers/MicroBitI2C.h | 12 +++++ source/drivers/FXOS8700.cpp | 48 ++++++++------------ source/drivers/LSM303Accelerometer.cpp | 11 ++++- source/drivers/LSM303Magnetometer.cpp | 12 ++++- source/drivers/MAG3110.cpp | 12 ++++- source/drivers/MMA8653.cpp | 11 ++++- source/drivers/MicroBitAccelerometer.cpp | 58 ++++++++++++++++++++++++ source/drivers/MicroBitCompass.cpp | 57 ++++++++++++++++++++++- source/drivers/MicroBitI2C.cpp | 23 +++++++++- 17 files changed, 273 insertions(+), 56 deletions(-) diff --git a/inc/core/ErrorNo.h b/inc/core/ErrorNo.h index b8a1190..dadec32 100644 --- a/inc/core/ErrorNo.h +++ b/inc/core/ErrorNo.h @@ -82,5 +82,8 @@ enum PanicCode{ // Dereference of a NULL pointer through the ManagedType class, MICROBIT_NULL_DEREFERENCE = 40, + + // A requested hardware peripheral could not be found, + MICROBIT_HARDWARE_UNAVAILABLE = 50, }; #endif diff --git a/inc/drivers/FXOS8700.h b/inc/drivers/FXOS8700.h index 17977f7..f558197 100644 --- a/inc/drivers/FXOS8700.h +++ b/inc/drivers/FXOS8700.h @@ -176,7 +176,7 @@ DEALINGS IN THE SOFTWARE. class FXOS8700 : public MicroBitAccelerometer, public MicroBitCompass { MicroBitI2C& i2c; // The I2C interface to use. - MicroBitPin &int1; // Data ready interrupt. + MicroBitPin int1; // Data ready interrupt. uint16_t address; // I2C address of this accelerometer. public: @@ -190,7 +190,7 @@ class FXOS8700 : public MicroBitAccelerometer, public MicroBitCompass * @param address the default I2C address of the accelerometer. Defaults to: FXOS8700_DEFAULT_ADDR. * */ - FXOS8700(MicroBitI2C &_i2c, MicroBitPin &_int1, CoordinateSpace &coordinateSpace, uint16_t address = FXOS8700_DEFAULT_ADDR, uint16_t aid = MICROBIT_ID_ACCELEROMETER, uint16_t cid = MICROBIT_ID_COMPASS); + FXOS8700(MicroBitI2C &_i2c, MicroBitPin _int1, CoordinateSpace &coordinateSpace, uint16_t address = FXOS8700_DEFAULT_ADDR, uint16_t aid = MICROBIT_ID_ACCELEROMETER, uint16_t cid = MICROBIT_ID_COMPASS); /** * Configures the accelerometer for G range and sample rate defined @@ -218,16 +218,11 @@ class FXOS8700 : public MicroBitAccelerometer, public MicroBitCompass virtual int requestUpdate(); /** - * Attempts to read the 8 bit ID from the accelerometer, this can be used for - * validation purposes. + * Attempts to read the 8 bit WHO_AM_I value from the accelerometer * - * @return the 8 bit ID returned by the accelerometer, or DEVICE_I2C_ERROR if the request fails. - * - * @code - * accelerometer.whoAmI(); - * @endcode + * @return true if the WHO_AM_I value is succesfully read. false otherwise. */ - int whoAmI(); + static int isDetected(MicroBitI2C &i2c, uint16_t address = FXOS8700_DEFAULT_ADDR); /** * A periodic callback invoked by the fiber scheduler idle thread. diff --git a/inc/drivers/LSM303Accelerometer.h b/inc/drivers/LSM303Accelerometer.h index 11c3761..0ca8048 100644 --- a/inc/drivers/LSM303Accelerometer.h +++ b/inc/drivers/LSM303Accelerometer.h @@ -92,7 +92,7 @@ DEALINGS IN THE SOFTWARE. class LSM303Accelerometer : public MicroBitAccelerometer { MicroBitI2C& i2c; // The I2C interface to use. - MicroBitPin& int1; // Data ready interrupt. + MicroBitPin int1; // Data ready interrupt. uint16_t address; // I2C address of this accelerometer. public: @@ -105,7 +105,7 @@ class LSM303Accelerometer : public MicroBitAccelerometer * @param id The unique EventModel id of this component. Defaults to: MICROBIT_ID_ACCELEROMETER * */ - LSM303Accelerometer(MicroBitI2C& _i2c, MicroBitPin &_int1, CoordinateSpace &coordinateSpace, uint16_t address = LSM303_A_DEFAULT_ADDR, uint16_t id = MICROBIT_ID_ACCELEROMETER); + LSM303Accelerometer(MicroBitI2C& _i2c, MicroBitPin _int1, CoordinateSpace &coordinateSpace, uint16_t address = LSM303_A_DEFAULT_ADDR, uint16_t id = MICROBIT_ID_ACCELEROMETER); /** * Configures the accelerometer for G range and sample rate defined @@ -140,6 +140,12 @@ class LSM303Accelerometer : public MicroBitAccelerometer */ virtual void idleTick(); + /** + * Attempts to read the 8 bit WHO_AM_I value from the accelerometer + * + * @return true if the WHO_AM_I value is succesfully read. false otherwise. + */ + static int isDetected(MicroBitI2C &i2c, uint16_t address = LSM303_A_DEFAULT_ADDR); /** * Destructor. diff --git a/inc/drivers/LSM303Magnetometer.h b/inc/drivers/LSM303Magnetometer.h index 89ea508..ad624ec 100644 --- a/inc/drivers/LSM303Magnetometer.h +++ b/inc/drivers/LSM303Magnetometer.h @@ -84,7 +84,7 @@ DEALINGS IN THE SOFTWARE. class LSM303Magnetometer : public MicroBitCompass { MicroBitI2C& i2c; // The I2C interface to use. - MicroBitPin& int1; // Data ready interrupt. + MicroBitPin int1; // Data ready interrupt. uint16_t address; // I2C address of this compass. public: @@ -97,7 +97,7 @@ class LSM303Magnetometer : public MicroBitCompass * @param id The unique EventModel id of this component. Defaults to: MICROBIT_ID_ACCELEROMETER * */ - LSM303Magnetometer(MicroBitI2C& _i2c, MicroBitPin &_int1, CoordinateSpace &coordinateSpace, uint16_t address = LSM303_M_DEFAULT_ADDR, uint16_t id = MICROBIT_ID_COMPASS); + LSM303Magnetometer(MicroBitI2C& _i2c, MicroBitPin _int1, CoordinateSpace &coordinateSpace, uint16_t address = LSM303_M_DEFAULT_ADDR, uint16_t id = MICROBIT_ID_COMPASS); /** * Configures the compass for the sample rate defined in this object. @@ -126,6 +126,12 @@ class LSM303Magnetometer : public MicroBitCompass */ virtual void idleTick(); + /** + * Attempts to read the 8 bit WHO_AM_I value from the accelerometer + * + * @return true if the WHO_AM_I value is succesfully read. false otherwise. + */ + static int isDetected(MicroBitI2C &i2c, uint16_t address = LSM303_M_DEFAULT_ADDR); /** * Destructor. diff --git a/inc/drivers/MAG3110.h b/inc/drivers/MAG3110.h index 898d548..7749893 100644 --- a/inc/drivers/MAG3110.h +++ b/inc/drivers/MAG3110.h @@ -80,7 +80,7 @@ DEALINGS IN THE SOFTWARE. class MAG3110 : public MicroBitCompass { MicroBitI2C& i2c; // The I2C interface to use. - MicroBitPin& int1; // Data ready interrupt. + MicroBitPin int1; // Data ready interrupt. uint16_t address; // I2C address of this compass. public: @@ -93,7 +93,7 @@ class MAG3110 : public MicroBitCompass * @param id The unique EventModel id of this component. Defaults to: MICROBIT_ID_ACCELEROMETER * */ - MAG3110(MicroBitI2C& _i2c, MicroBitPin &_int1, CoordinateSpace &coordinateSpace, uint16_t address = MAG3110_DEFAULT_ADDR, uint16_t id = MICROBIT_ID_COMPASS); + MAG3110(MicroBitI2C& _i2c, MicroBitPin _int1, CoordinateSpace &coordinateSpace, uint16_t address = MAG3110_DEFAULT_ADDR, uint16_t id = MICROBIT_ID_COMPASS); /** * Configures the compass for the sample rate defined in this object. @@ -122,6 +122,12 @@ class MAG3110 : public MicroBitCompass */ virtual void idleTick(); + /** + * Attempts to read the 8 bit WHO_AM_I value from the accelerometer + * + * @return true if the WHO_AM_I value is succesfully read. false otherwise. + */ + static int isDetected(MicroBitI2C &i2c, uint16_t address = MAG3110_DEFAULT_ADDR); /** * Destructor. diff --git a/inc/drivers/MMA8653.h b/inc/drivers/MMA8653.h index 62fe477..bfe5530 100644 --- a/inc/drivers/MMA8653.h +++ b/inc/drivers/MMA8653.h @@ -68,7 +68,7 @@ DEALINGS IN THE SOFTWARE. class MMA8653 : public MicroBitAccelerometer { MicroBitI2C& i2c; // The I2C interface to use. - MicroBitPin& int1; // Data ready interrupt. + MicroBitPin int1; // Data ready interrupt. uint16_t address; // I2C address of this accelerometer. public: @@ -81,7 +81,7 @@ class MMA8653 : public MicroBitAccelerometer * @param id The unique EventModel id of this component. Defaults to: MICROBIT_ID_ACCELEROMETER * */ - MMA8653(MicroBitI2C& _i2c, MicroBitPin &_int1, CoordinateSpace &coordinateSpace, uint16_t address = MMA8653_DEFAULT_ADDR, uint16_t id = MICROBIT_ID_ACCELEROMETER); + MMA8653(MicroBitI2C& _i2c, MicroBitPin _int1, CoordinateSpace &coordinateSpace, uint16_t address = MMA8653_DEFAULT_ADDR, uint16_t id = MICROBIT_ID_ACCELEROMETER); /** * Configures the accelerometer for G range and sample rate defined @@ -116,6 +116,13 @@ class MMA8653 : public MicroBitAccelerometer */ virtual void idleTick(); + /** + * Attempts to read the 8 bit WHO_AM_I value from the accelerometer + * + * @return true if the WHO_AM_I value is succesfully read. false otherwise. + */ + static int isDetected(MicroBitI2C &i2c, uint16_t address = MMA8653_DEFAULT_ADDR); + /** * Destructor. diff --git a/inc/drivers/MicroBitAccelerometer.h b/inc/drivers/MicroBitAccelerometer.h index 9e1598a..c6c73eb 100644 --- a/inc/drivers/MicroBitAccelerometer.h +++ b/inc/drivers/MicroBitAccelerometer.h @@ -29,6 +29,7 @@ DEALINGS IN THE SOFTWARE. #include "MicroBitComponent.h" #include "MicroBitPin.h" #include "CoordinateSystem.h" +#include "MicroBitI2C.h" /** * Status flags @@ -117,6 +118,8 @@ class MicroBitAccelerometer : public MicroBitComponent public: + static MicroBitAccelerometer *detectedAccelerometer; // The autodetected instance of a MicroBitAcelerometer driver. + /** * Constructor. * Create a software abstraction of an accelerometer. @@ -127,6 +130,15 @@ class MicroBitAccelerometer : public MicroBitComponent */ MicroBitAccelerometer(CoordinateSpace &coordinateSpace, uint16_t id = MICROBIT_ID_ACCELEROMETER); + /** + * Device autodetection. Scans the given I2C bus for supported accelerometer devices. + * if found, constructs an appropriate driver and returns it. + * + * @param i2c the bus to scan. + * + */ + static MicroBitAccelerometer& autoDetect(MicroBitI2C &i2c); + /** * Attempts to set the sample rate of the accelerometer to the specified value (in ms). * diff --git a/inc/drivers/MicroBitCompass.h b/inc/drivers/MicroBitCompass.h index 62e1cdc..2047218 100644 --- a/inc/drivers/MicroBitCompass.h +++ b/inc/drivers/MicroBitCompass.h @@ -59,10 +59,12 @@ class MicroBitCompass : public MicroBitComponent Sample3D sample; // The last sample read, in the coordinate system specified by the coordinateSpace variable. Sample3D sampleENU; // The last sample read, in raw ENU format (stored in case requests are made for data in other coordinate spaces) CoordinateSpace &coordinateSpace; // The coordinate space transform (if any) to apply to the raw data from the hardware. - MicroBitAccelerometer* accelerometer; // The accelerometer to use for tilt compensation. + MicroBitAccelerometer* accelerometer; // The accelerometer to use for tilt compensation. public: + static MicroBitCompass *detectedCompass; // The autodetected instance of a MicroBitAcelerometer driver. + /** * Constructor. * Create a software abstraction of an e-compass. @@ -84,6 +86,16 @@ class MicroBitCompass : public MicroBitComponent */ MicroBitCompass(MicroBitAccelerometer &accel, CoordinateSpace &coordinateSpace, uint16_t id = MICROBIT_ID_COMPASS); + /** + * Device autodetection. Scans the given I2C bus for supported compass devices. + * if found, constructs an appropriate driver and returns it. + * + * @param i2c the bus to scan. + * + */ + static MicroBitCompass& autoDetect(MicroBitI2C &i2c); + + /** * Gets the current heading of the device, relative to magnetic north. * diff --git a/inc/drivers/MicroBitI2C.h b/inc/drivers/MicroBitI2C.h index 563c09b..e626aeb 100644 --- a/inc/drivers/MicroBitI2C.h +++ b/inc/drivers/MicroBitI2C.h @@ -131,6 +131,18 @@ class MicroBitI2C : public I2C */ int readRegister(uint8_t address, uint8_t reg, uint8_t* buffer, int length); + /** + * Issues a single byte read command, and returns the value read, or an error. + * + * Blocks the calling thread until complete. + * + * @param address The address of the I2C device to write to. + * @param reg The address of the register to access. + * + * @return the byte read on success, MICROBIT_INVALID_PARAMETER or MICROBIT_I2C_ERROR if the the read request failed. + */ + int readRegister(uint8_t address, uint8_t reg); + }; #endif diff --git a/source/drivers/FXOS8700.cpp b/source/drivers/FXOS8700.cpp index da32638..095d028 100644 --- a/source/drivers/FXOS8700.cpp +++ b/source/drivers/FXOS8700.cpp @@ -149,7 +149,7 @@ int FXOS8700::configure() * @param address the default I2C address of the accelerometer. Defaults to: FXS8700_DEFAULT_ADDR. * */ -FXOS8700::FXOS8700(MicroBitI2C &_i2c, MicroBitPin &_int1, CoordinateSpace &coordinateSpace, uint16_t address, uint16_t aid, uint16_t cid) : +FXOS8700::FXOS8700(MicroBitI2C &_i2c, MicroBitPin _int1, CoordinateSpace &coordinateSpace, uint16_t address, uint16_t aid, uint16_t cid) : MicroBitAccelerometer(coordinateSpace, aid), MicroBitCompass(coordinateSpace, cid), i2c(_i2c), int1(_int1) @@ -165,38 +165,26 @@ FXOS8700::FXOS8700(MicroBitI2C &_i2c, MicroBitPin &_int1, CoordinateSpace &coord } /** - * Attempts to read the 8 bit ID from the accelerometer, this can be used for - * validation purposes. - * - * @return the 8 bit ID returned by the accelerometer, or MICROBIT_I2C_ERROR if the request fails. - * - * @code - * accelerometer.whoAmI(); - * @endcode - */ -int FXOS8700::whoAmI() + * Attempts to read the 8 bit WHO_AM_I value from the accelerometer + * + * @return true if the WHO_AM_I value is succesfully read. false otherwise. + */ +int FXOS8700::isDetected(MicroBitI2C &i2c, uint16_t address) { - uint8_t data; - int result; - - result = i2c.readRegister(address, FXOS8700_WHO_AM_I, &data, 1); - if (result !=0) - return MICROBIT_I2C_ERROR; - - return (int)data; + return i2c.readRegister(address, FXOS8700_WHO_AM_I) == FXOS8700_WHOAMI_VAL; } - /** - * Poll to see if new data is available from the hardware. If so, update it. - * n.b. it is not necessary to explicitly call this funciton to update data - * (it normally happens in the background when the scheduler is idle), but a check is performed - * if the user explicitly requests up to date data. - * - * @return MICROBIT_OK on success, MICROBIT_I2C_ERROR if the update fails. - * - * @note This method should be overidden by the hardware driver to implement the requested - * changes in hardware. - */ +/** + * Poll to see if new data is available from the hardware. If so, update it. + * n.b. it is not necessary to explicitly call this funciton to update data + * (it normally happens in the background when the scheduler is idle), but a check is performed + * if the user explicitly requests up to date data. + * + * @return MICROBIT_OK on success, MICROBIT_I2C_ERROR if the update fails. + * + * @note This method should be overidden by the hardware driver to implement the requested + * changes in hardware. + */ int FXOS8700::requestUpdate() { // Ensure we're scheduled to update the data periodically diff --git a/source/drivers/LSM303Accelerometer.cpp b/source/drivers/LSM303Accelerometer.cpp index e79d0c1..2f81f12 100644 --- a/source/drivers/LSM303Accelerometer.cpp +++ b/source/drivers/LSM303Accelerometer.cpp @@ -73,7 +73,7 @@ CREATE_KEY_VALUE_TABLE(accelerometerPeriod, accelerometerPeriodData); * @param id The unique EventModel id of this component. Defaults to: MICROBIT_ID_ACCELEROMETER * */ -LSM303Accelerometer::LSM303Accelerometer(MicroBitI2C& _i2c, MicroBitPin &_int1, CoordinateSpace &coordinateSpace, uint16_t address, uint16_t id) : MicroBitAccelerometer(coordinateSpace, id), i2c(_i2c), int1(_int1) +LSM303Accelerometer::LSM303Accelerometer(MicroBitI2C& _i2c, MicroBitPin _int1, CoordinateSpace &coordinateSpace, uint16_t address, uint16_t id) : MicroBitAccelerometer(coordinateSpace, id), i2c(_i2c), int1(_int1) { // Store our identifiers. this->status = 0; @@ -190,6 +190,15 @@ void LSM303Accelerometer::idleTick() requestUpdate(); } +/** + * Attempts to read the 8 bit WHO_AM_I value from the accelerometer + * + * @return true if the WHO_AM_I value is succesfully read. false otherwise. + */ +int LSM303Accelerometer::isDetected(MicroBitI2C &i2c, uint16_t address) +{ + return i2c.readRegister(address, LSM303_WHO_AM_I_A) == LSM303_A_WHOAMI_VAL; +} /** * Destructor. diff --git a/source/drivers/LSM303Magnetometer.cpp b/source/drivers/LSM303Magnetometer.cpp index 8044857..abd0c83 100644 --- a/source/drivers/LSM303Magnetometer.cpp +++ b/source/drivers/LSM303Magnetometer.cpp @@ -84,7 +84,7 @@ int LSM303Magnetometer::configure() * @param address the default I2C address of the magnetometer. Defaults to: FXS8700_DEFAULT_ADDR. * */ -LSM303Magnetometer::LSM303Magnetometer(MicroBitI2C &_i2c, MicroBitPin &_int1, CoordinateSpace &coordinateSpace, uint16_t address, uint16_t id) : MicroBitCompass(coordinateSpace, id), i2c(_i2c), int1(_int1) +LSM303Magnetometer::LSM303Magnetometer(MicroBitI2C &_i2c, MicroBitPin _int1, CoordinateSpace &coordinateSpace, uint16_t address, uint16_t id) : MicroBitCompass(coordinateSpace, id), i2c(_i2c), int1(_int1) { // Store our identifiers. this->address = address; @@ -160,6 +160,16 @@ void LSM303Magnetometer::idleTick() requestUpdate(); } +/** + * Attempts to read the 8 bit WHO_AM_I value from the accelerometer + * + * @return true if the WHO_AM_I value is succesfully read. false otherwise. + */ +int LSM303Magnetometer::isDetected(MicroBitI2C &i2c, uint16_t address) +{ + return i2c.readRegister(address, LSM303_WHO_AM_I_M) == LSM303_M_WHOAMI_VAL; +} + /** * Destructor for FXS8700, where we deregister from the array of fiber components. */ diff --git a/source/drivers/MAG3110.cpp b/source/drivers/MAG3110.cpp index 45f365b..8bc18fd 100644 --- a/source/drivers/MAG3110.cpp +++ b/source/drivers/MAG3110.cpp @@ -115,7 +115,7 @@ int MAG3110::configure() * @param address the default I2C address of the magnetometer. Defaults to: FXS8700_DEFAULT_ADDR. * */ -MAG3110::MAG3110(MicroBitI2C &_i2c, MicroBitPin &_int1, CoordinateSpace &coordinateSpace, uint16_t address, uint16_t id) : MicroBitCompass(coordinateSpace, id), i2c(_i2c), int1(_int1) +MAG3110::MAG3110(MicroBitI2C &_i2c, MicroBitPin _int1, CoordinateSpace &coordinateSpace, uint16_t address, uint16_t id) : MicroBitCompass(coordinateSpace, id), i2c(_i2c), int1(_int1) { // Store our identifiers. this->address = address; @@ -190,6 +190,16 @@ void MAG3110::idleTick() requestUpdate(); } +/** + * Attempts to read the 8 bit WHO_AM_I value from the accelerometer + * + * @return true if the WHO_AM_I value is succesfully read. false otherwise. + */ +int MAG3110::isDetected(MicroBitI2C &i2c, uint16_t address) +{ + return i2c.readRegister(address, MAG_WHOAMI) == MAG3110_WHOAMI_VAL; +} + /** * Destructor for FXS8700, where we deregister from the array of fiber components. */ diff --git a/source/drivers/MMA8653.cpp b/source/drivers/MMA8653.cpp index 0c0e270..8195fc4 100644 --- a/source/drivers/MMA8653.cpp +++ b/source/drivers/MMA8653.cpp @@ -71,7 +71,7 @@ CREATE_KEY_VALUE_TABLE(accelerometerPeriod, accelerometerPeriodData); * @param id The unique EventModel id of this component. Defaults to: MICROBIT_ID_ACCELEROMETER * */ -MMA8653::MMA8653(MicroBitI2C& _i2c, MicroBitPin &_int1, CoordinateSpace &coordinateSpace, uint16_t address, uint16_t id) : MicroBitAccelerometer(coordinateSpace, id), i2c(_i2c), int1(_int1) +MMA8653::MMA8653(MicroBitI2C& _i2c, MicroBitPin _int1, CoordinateSpace &coordinateSpace, uint16_t address, uint16_t id) : MicroBitAccelerometer(coordinateSpace, id), i2c(_i2c), int1(_int1) { // Store our identifiers. this->status = 0; @@ -205,6 +205,15 @@ void MMA8653::idleTick() requestUpdate(); } +/** + * Attempts to read the 8 bit WHO_AM_I value from the accelerometer + * + * @return true if the WHO_AM_I value is succesfully read. false otherwise. + */ +int MMA8653::isDetected(MicroBitI2C &i2c, uint16_t address) +{ + return i2c.readRegister(address, MMA8653_WHOAMI) == MMA8653_WHOAMI_VAL; +} /** * Destructor. diff --git a/source/drivers/MicroBitAccelerometer.cpp b/source/drivers/MicroBitAccelerometer.cpp index 1406da2..c2f0c01 100644 --- a/source/drivers/MicroBitAccelerometer.cpp +++ b/source/drivers/MicroBitAccelerometer.cpp @@ -27,7 +27,12 @@ DEALINGS IN THE SOFTWARE. #include "MicroBitEvent.h" #include "MicroBitCompat.h" #include "MicroBitFiber.h" +#include "MicroBitDevice.h" +#include "MicroBitI2C.h" +#include "MMA8653.h" +#include "FXOS8700.h" +#include "LSM303Accelerometer.h" /** * Constructor. @@ -63,6 +68,58 @@ MicroBitAccelerometer::MicroBitAccelerometer(CoordinateSpace &cspace, uint16_t i this->shake.impulse_8 = 1; } +/** + * Device autodetection. Scans the given I2C bus for supported accelerometer devices. + * if found, constructs an appropriate driver and returns it. + * + * @param i2c the bus to scan. + * @param id the unique EventModel id of this component. Defaults to: MICROBIT_ID_ACCELEROMETER + * + */ +MicroBitAccelerometer& MicroBitAccelerometer::autoDetect(MicroBitI2C &i2c) +{ + if (MicroBitAccelerometer::detectedAccelerometer == NULL) + { + // Configuration of IRQ lines + MicroBitPin int1(MICROBIT_ID_IO_INT1, P0_28, PIN_CAPABILITY_STANDARD); + MicroBitPin int2(MICROBIT_ID_IO_INT2, P0_29, PIN_CAPABILITY_STANDARD); + MicroBitPin int3(MICROBIT_ID_IO_INT3, P0_27, PIN_CAPABILITY_STANDARD); + + // All known accelerometer/magnetometer peripherals have the same alignment + CoordinateSpace &coordinateSpace = *(new CoordinateSpace(SIMPLE_CARTESIAN, true, COORDINATE_SPACE_ROTATED_0)); + + // Now, probe for connected peripherals, if none have already been found. + if (MMA8653::isDetected(i2c)) + MicroBitAccelerometer::detectedAccelerometer = new MMA8653(i2c, int1, coordinateSpace); + + else if (LSM303Accelerometer::isDetected(i2c)) + MicroBitAccelerometer::detectedAccelerometer = new LSM303Accelerometer(i2c, int1, coordinateSpace); + + else if (FXOS8700::isDetected(i2c)) + { + FXOS8700 *fxos = new FXOS8700(i2c, int3, coordinateSpace); + MicroBitAccelerometer::detectedAccelerometer = fxos; + MicroBitCompass::detectedCompass = fxos; + } + + // Insert this case to support FXOS on the microbit1.5-SN + //else if (FXOS8700::isDetected(i2c, 0x3A)) + //{ + // FXOS8700 *fxos = new FXOS8700(i2c, int3, coordinateSpace, 0x3A); + // MicroBitAccelerometer::detectedAccelerometer = fxos; + // MicroBitCompass::detectedCompass = fxos; + //} + + else + { + microbit_panic(MICROBIT_HARDWARE_UNAVAILABLE); + } + } + + return *MicroBitAccelerometer::detectedAccelerometer; +} + + /** * Stores data from the accelerometer sensor in our buffer, and perform gesture tracking. * @@ -546,3 +603,4 @@ MicroBitAccelerometer::~MicroBitAccelerometer() { } +MicroBitAccelerometer* MicroBitAccelerometer::detectedAccelerometer = NULL; diff --git a/source/drivers/MicroBitCompass.cpp b/source/drivers/MicroBitCompass.cpp index 72d7b69..627791e 100644 --- a/source/drivers/MicroBitCompass.cpp +++ b/source/drivers/MicroBitCompass.cpp @@ -27,6 +27,11 @@ DEALINGS IN THE SOFTWARE. #include "MicroBitEvent.h" #include "MicroBitCompat.h" #include "MicroBitFiber.h" +#include "MicroBitDevice.h" + +#include "MAG3110.h" +#include "LSM303Magnetometer.h" +#include "FXOS8700.h" /** @@ -81,6 +86,56 @@ void MicroBitCompass::init(uint16_t id) status |= MICROBIT_COMPONENT_RUNNING; } +/** + * Device autodetection. Scans the given I2C bus for supported accelerometer devices. + * if found, constructs an appropriate driver and returns it. + * + * @param i2c the bus to scan. + * @param id the unique EventModel id of this component. Defaults to: MICROBIT_ID_ACCELEROMETER + * + */ +MicroBitCompass& MicroBitCompass::autoDetect(MicroBitI2C &i2c) +{ + if (MicroBitCompass::detectedCompass == NULL) + { + // Configuration of IRQ lines + MicroBitPin int1(MICROBIT_ID_IO_INT1, P0_28, PIN_CAPABILITY_STANDARD); + MicroBitPin int2(MICROBIT_ID_IO_INT2, P0_29, PIN_CAPABILITY_STANDARD); + MicroBitPin int3(MICROBIT_ID_IO_INT3, P0_27, PIN_CAPABILITY_STANDARD); + + // All known accelerometer/magnetometer peripherals have the same alignment + CoordinateSpace &coordinateSpace = *(new CoordinateSpace(SIMPLE_CARTESIAN, true, COORDINATE_SPACE_ROTATED_0)); + + // Now, probe for connected peripherals, if none have already been found. + if (MAG3110::isDetected(i2c)) + MicroBitCompass::detectedCompass = new MAG3110(i2c, int2, coordinateSpace); + + else if (LSM303Magnetometer::isDetected(i2c)) + MicroBitCompass::detectedCompass = new LSM303Magnetometer(i2c, int2, coordinateSpace); + + else if (FXOS8700::isDetected(i2c)) + { + FXOS8700 *fxos = new FXOS8700(i2c, int3, coordinateSpace); + MicroBitAccelerometer::detectedAccelerometer = fxos; + MicroBitCompass::detectedCompass = fxos; + } + + // Insert this case to support FXOS on the microbit1.5-SN + //else if (FXOS8700::isDetected(i2c, 0x3A)) + //{ + // FXOS8700 *fxos = new FXOS8700(i2c, int3, coordinateSpace, 0x3A); + // MicroBitAccelerometer::detectedAccelerometer = fxos; + // MicroBitCompass::detectedCompass = fxos; + //} + + else + { + microbit_panic(MICROBIT_HARDWARE_UNAVAILABLE); + } + } + + return *MicroBitCompass::detectedCompass; +} /** * Gets the current heading of the device, relative to magnetic north. @@ -416,4 +471,4 @@ MicroBitCompass::~MicroBitCompass() { } - +MicroBitCompass* MicroBitCompass::detectedCompass= NULL; diff --git a/source/drivers/MicroBitI2C.cpp b/source/drivers/MicroBitI2C.cpp index 46a5c89..c8edfd7 100644 --- a/source/drivers/MicroBitI2C.cpp +++ b/source/drivers/MicroBitI2C.cpp @@ -162,10 +162,9 @@ int MicroBitI2C::writeRegister(uint8_t address, uint8_t reg, uint8_t value) * * Blocks the calling thread until complete. * + * @param address The address of the I2C device to write to. * @param reg The address of the register to access. - * * @param buffer Memory area to read the data into. - * * @param length The number of bytes to read. * * @return MICROBIT_OK on success, MICROBIT_INVALID_PARAMETER or MICROBIT_I2C_ERROR if the the read request failed. @@ -187,3 +186,23 @@ int MicroBitI2C::readRegister(uint8_t address, uint8_t reg, uint8_t* buffer, int return MICROBIT_OK; } + +/** + * Issues a single byte read command, and returns the value read, or an error. + * + * Blocks the calling thread until complete. + * + * @param address The address of the I2C device to write to. + * @param reg The address of the register to access. + * + * @return the byte read on success, MICROBIT_INVALID_PARAMETER or MICROBIT_I2C_ERROR if the the read request failed. + */ +int MicroBitI2C::readRegister(uint8_t address, uint8_t reg) +{ + int result; + uint8_t data; + + result = readRegister(address, reg, &data, 1); + + return (result == MICROBIT_OK) ? (int)data : result; +} From bbf56210ed3d1dc10a48cc9ea2fdcb8c0ad5cd30 Mon Sep 17 00:00:00 2001 From: Joe Finney Date: Thu, 5 Jul 2018 23:48:41 +0100 Subject: [PATCH 2/9] Introduce Updates to Compass Calibration Algorithm and UX - Updated compass calibration algorithm, based on iterative approximation - Updated calibration UX --- inc/drivers/MicroBitCompass.h | 18 +- inc/drivers/MicroBitCompassCalibrator.h | 54 +++- inc/types/CoordinateSystem.h | 6 + source/drivers/MicroBitCompass.cpp | 63 ++-- source/drivers/MicroBitCompassCalibrator.cpp | 311 +++++++++++++++---- 5 files changed, 361 insertions(+), 91 deletions(-) diff --git a/inc/drivers/MicroBitCompass.h b/inc/drivers/MicroBitCompass.h index 2047218..3b211db 100644 --- a/inc/drivers/MicroBitCompass.h +++ b/inc/drivers/MicroBitCompass.h @@ -47,6 +47,18 @@ DEALINGS IN THE SOFTWARE. #define MICROBIT_COMPASS_EVT_CALIBRATE 3 #define MICROBIT_COMPASS_EVT_CALIBRATION_NEEDED 4 +struct CompassCalibration +{ + Sample3D centre; // Zero offset of the compass. + Sample3D scale; // Scale factor to apply in each axis to accomodate 1st order directional fields. + int radius; // Indication of field strength - the "distance" from the centre to outmost sample. + + CompassCalibration() : centre(), scale(1024, 1024, 1024) + { + radius = 0; + } +}; + /** * Class definition for a general e-compass. */ @@ -55,7 +67,7 @@ class MicroBitCompass : public MicroBitComponent protected: uint16_t samplePeriod; // The time between samples, in milliseconds. - Sample3D average; // The zero offset of this compass (generated by calibration) + CompassCalibration calibration; // The calibration data of this compass Sample3D sample; // The last sample read, in the coordinate system specified by the coordinateSpace variable. Sample3D sampleENU; // The last sample read, in raw ENU format (stored in case requests are made for data in other coordinate spaces) CoordinateSpace &coordinateSpace; // The coordinate space transform (if any) to apply to the raw data from the hardware. @@ -149,7 +161,7 @@ class MicroBitCompass : public MicroBitComponent * * @param calibration A Sample3D containing the offsets for the x, y and z axis. */ - void setCalibration(Sample3D calibration); + void setCalibration(CompassCalibration calibration); /** * Provides the calibration data currently in use by the compass. @@ -158,7 +170,7 @@ class MicroBitCompass : public MicroBitComponent * * @return A Sample3D containing the offsets for the x, y and z axis. */ - Sample3D getCalibration(); + CompassCalibration getCalibration(); /** * Returns 0 or 1. 1 indicates that the compass is calibrated, zero means the compass requires calibration. diff --git a/inc/drivers/MicroBitCompassCalibrator.h b/inc/drivers/MicroBitCompassCalibrator.h index de707e2..e563539 100644 --- a/inc/drivers/MicroBitCompassCalibrator.h +++ b/inc/drivers/MicroBitCompassCalibrator.h @@ -78,7 +78,59 @@ class MicroBitCompassCalibrator * * This function is, by design, synchronous and only returns once calibration is complete. */ - void calibrate(MicroBitEvent); + void calibrateUX(MicroBitEvent); + /** + * Calculates an independent X, Y, Z scale factor and centre for a given set of data points, + * assumed to be on a bounding sphere + * + * @param data An array of all data points + * @param samples The number of samples in the 'data' array. + * + * This algorithm should be called with no fewer than 12 points, but testing has indicated >21 + * points provides a more robust calculation. + * + * @return A calibration structure containing the a calculated centre point, the radius of the + * minimum enclosing spere of points and a scaling factor for each axis that places those + * points as close as possible to the surface of the containing sphere. + */ + static CompassCalibration calibrate(Sample3D *data, int samples); + + private: + + /** + * Scoring function for a hill climb algorithm. + * + * @param c An approximated centre point + * @param data a collection of data points + * @param samples the number of samples in the 'data' array + * + * @return The deviation between the closest and further point in the data array from the point given. + */ + static int measureScore(Sample3D &c, Sample3D *data, int samples); + + /* + * Performs an interative approximation (hill descent) algorithm to determine an + * estimated centre point of a sphere upon which the given data points reside. + * + * @param data an array containing sample points + * @param samples the number of sample points in the 'data' array. + * + * @return the approximated centre point of the points in the 'data' array. + */ + static Sample3D approximateCentre(Sample3D *data, int samples); + + /** + * Calculates an independent scale factor for X,Y and Z axes that places the given data points on a bounding sphere + * + * @param centre A proviously calculated centre point of all data. + * @param data An array of all data points + * @param samples The number of samples in the 'data' array. + * + * @return A calibration structure containing the centre point provided, the radius of the minimum + * enclosing spere of points and a scaling factor for each axis that places those points as close as possible + * to the surface of the containing sphere. + */ + static CompassCalibration spherify(Sample3D centre, Sample3D *data, int samples); }; #endif diff --git a/inc/types/CoordinateSystem.h b/inc/types/CoordinateSystem.h index ebfca32..5d42ef2 100644 --- a/inc/types/CoordinateSystem.h +++ b/inc/types/CoordinateSystem.h @@ -121,6 +121,12 @@ struct Sample3D { return !(x == other.x && y == other.y && z == other.z); } + + int dSquared(Sample3D &s) + { + return (x - s.x)*(x - s.x) + (y - s.y)*(y - s.y) + (z - s.z)*(z - s.z); + } + }; diff --git a/source/drivers/MicroBitCompass.cpp b/source/drivers/MicroBitCompass.cpp index 627791e..ec33551 100644 --- a/source/drivers/MicroBitCompass.cpp +++ b/source/drivers/MicroBitCompass.cpp @@ -33,6 +33,10 @@ DEALINGS IN THE SOFTWARE. #include "LSM303Magnetometer.h" #include "FXOS8700.h" +// +// Internal convenience macro to apply calibration to a given sample. +// +#define CALIBRATED_SAMPLE(sample, axis) (((sample.axis - calibration.centre.axis) * calibration.scale.axis) >> 10) /** * Constructor. @@ -42,7 +46,7 @@ DEALINGS IN THE SOFTWARE. * @param coordinateSpace the orientation of the sensor. Defaults to: SIMPLE_CARTESIAN * */ -MicroBitCompass::MicroBitCompass(CoordinateSpace &cspace, uint16_t id) : sample(), sampleENU(), coordinateSpace(cspace) +MicroBitCompass::MicroBitCompass(CoordinateSpace &cspace, uint16_t id) : calibration(), sample(), sampleENU(), coordinateSpace(cspace) { accelerometer = NULL; init(id); @@ -57,7 +61,7 @@ MicroBitCompass::MicroBitCompass(CoordinateSpace &cspace, uint16_t id) : sample( * @param coordinateSpace the orientation of the sensor. Defaults to: SIMPLE_CARTESIAN * */ -MicroBitCompass::MicroBitCompass(MicroBitAccelerometer &accel, CoordinateSpace &cspace, uint16_t id) : sample(), sampleENU(), coordinateSpace(cspace) +MicroBitCompass::MicroBitCompass(MicroBitAccelerometer &accel, CoordinateSpace &cspace, uint16_t id) : calibration(), sample(), sampleENU(), coordinateSpace(cspace) { accelerometer = &accel; init(id); @@ -236,9 +240,9 @@ int MicroBitCompass::calibrate() * * @param calibration A Sample3D containing the offsets for the x, y and z axis. */ -void MicroBitCompass::setCalibration(Sample3D calibration) +void MicroBitCompass::setCalibration(CompassCalibration calibration) { - average = calibration; + this->calibration = calibration; status |= MICROBIT_COMPASS_STATUS_CALIBRATED; } @@ -249,9 +253,9 @@ void MicroBitCompass::setCalibration(Sample3D calibration) * * @return A Sample3D containing the offsets for the x, y and z axis. */ -Sample3D MicroBitCompass::getCalibration() +CompassCalibration MicroBitCompass::getCalibration() { - return average; + return calibration; } /** @@ -275,7 +279,7 @@ int MicroBitCompass::isCalibrating() */ void MicroBitCompass::clearCalibration() { - average = Sample3D(); + calibration = CompassCalibration(); status &= ~MICROBIT_COMPASS_STATUS_CALIBRATED; } @@ -356,7 +360,10 @@ int MicroBitCompass::requestUpdate() int MicroBitCompass::update() { // Store the new data, after performing any necessary coordinate transformations. - sample = coordinateSpace.transform(sampleENU - average); + sampleENU.x = CALIBRATED_SAMPLE(sampleENU, x); + sampleENU.y = CALIBRATED_SAMPLE(sampleENU, y); + sampleENU.z = CALIBRATED_SAMPLE(sampleENU, z); + sample = coordinateSpace.transform(sampleENU); // Indicate that a new sample is available MicroBitEvent e(id, MICROBIT_COMPASS_EVT_DATA_UPDATE); @@ -373,7 +380,7 @@ int MicroBitCompass::update() Sample3D MicroBitCompass::getSample(CoordinateSystem coordinateSystem) { requestUpdate(); - return coordinateSpace.transform(sampleENU - average, coordinateSystem); + return coordinateSpace.transform(sampleENU, coordinateSystem); } /** @@ -427,28 +434,36 @@ int MicroBitCompass::getZ() */ int MicroBitCompass::tiltCompensatedBearing() { - // Precompute the tilt compensation parameters to improve readability. - float phi = accelerometer->getRollRadians(); - float theta = accelerometer->getPitchRadians(); + Sample3D cs = this->getSample(NORTH_EAST_DOWN); + Sample3D as = accelerometer->getSample(NORTH_EAST_DOWN); - Sample3D s = getSample(NORTH_EAST_DOWN); + // Convert to floating point to reduce rounding errors + float x = (float) cs.x; + float y = (float) cs.y; + float z = (float) cs.z; - float x = (float) s.x; - float y = (float) s.y; - float z = (float) s.z; + float ax = (float) as.x; + float ay = (float) as.y; + float az = (float) as.z; - // Precompute cos and sin of pitch and roll angles to make the calculation a little more efficient. - float sinPhi = sin(phi); - float cosPhi = cos(phi); - float sinTheta = sin(theta); - float cosTheta = cos(theta); + // normalize the readings + float amag = sqrt(ax*ax + ay*ay + az*az); + ax = ax/amag; + ay = ay/amag; + az = az/amag; - float bearing = (360*atan2(z*sinPhi - y*cosPhi, x*cosTheta + y*sinTheta*sinPhi + z*sinTheta*cosPhi)) / (2*PI); + float ax2 = ax*ax; + float ay2 = ay*ay; + + float resultx = x*(1.0f - ax2) - y*ax*ay - z*ax*sqrt(1.0f-ax2-ay2); + float resulty = y*sqrt(1.0f-ax2-ay2) - z*ay; + + float bearing = (360*atan2(resulty,resultx)) / (2*PI); if (bearing < 0) bearing += 360.0; - return (int) bearing; + return (int) (360.0 - bearing); } /** @@ -456,7 +471,7 @@ int MicroBitCompass::tiltCompensatedBearing() */ int MicroBitCompass::basicBearing() { - float bearing = (atan2((double)(sample.y - average.y),(double)(sample.x - average.x)))*180/PI; + float bearing = (atan2((double)getY(),(double)getX()))*180/PI; if (bearing < 0) bearing += 360.0; diff --git a/source/drivers/MicroBitCompassCalibrator.cpp b/source/drivers/MicroBitCompassCalibrator.cpp index 75ed0c9..e738ee2 100644 --- a/source/drivers/MicroBitCompassCalibrator.cpp +++ b/source/drivers/MicroBitCompassCalibrator.cpp @@ -28,6 +28,8 @@ DEALINGS IN THE SOFTWARE. #include "EventModel.h" #include "Matrix4.h" +#define CALIBRATION_INCREMENT 10 + /** * Constructor. * @@ -48,61 +50,269 @@ DEALINGS IN THE SOFTWARE. MicroBitCompassCalibrator::MicroBitCompassCalibrator(MicroBitCompass& _compass, MicroBitAccelerometer& _accelerometer, MicroBitDisplay& _display) : compass(_compass), accelerometer(_accelerometer), display(_display) { if (EventModel::defaultEventBus) - EventModel::defaultEventBus->listen(MICROBIT_ID_COMPASS, MICROBIT_COMPASS_EVT_CALIBRATE, this, &MicroBitCompassCalibrator::calibrate, MESSAGE_BUS_LISTENER_IMMEDIATE); + EventModel::defaultEventBus->listen(MICROBIT_ID_COMPASS, MICROBIT_COMPASS_EVT_CALIBRATE, this, &MicroBitCompassCalibrator::calibrateUX, MESSAGE_BUS_LISTENER_IMMEDIATE); } /** - * Performs a simple game that in parallel, calibrates the compass. - * - * This function is executed automatically when the user requests a compass bearing, and compass calibration is required. - * - * This function is, by design, synchronous and only returns once calibration is complete. - */ -void MicroBitCompassCalibrator::calibrate(MicroBitEvent) + * Scoring function for a hill climb algorithm. + * + * @param c An approximated centre point + * @param data a collection of data points + * @param samples the number of samples in the 'data' array + * + * @return The deviation between the closest and further point in the data array from the point given. + */ +int MicroBitCompassCalibrator::measureScore(Sample3D &c, Sample3D *data, int samples) +{ + int minD; + int maxD; + + minD = maxD = c.dSquared(data[0]); + for (int i = 1; i < samples; i++) + { + int d = c.dSquared(data[i]); + + if (d < minD) + minD = d; + + if (d > maxD) + maxD = d; + } + + return (maxD - minD); +} + +/** + * Calculates an independent X, Y, Z scale factor and centre for a given set of data points, assumed to be on + * a bounding sphere + * + * @param data An array of all data points + * @param samples The number of samples in the 'data' array. + * + * This algorithm should be called with no fewer than 12 points, but testing has indicated >21 points provides + * a more robust calculation. + * + * @return A calibration structure containing the a calculated centre point, the radius of the minimum + * enclosing spere of points and a scaling factor for each axis that places those points as close as possible + * to the surface of the containing sphere. + */ +CompassCalibration MicroBitCompassCalibrator::calibrate(Sample3D *data, int samples) +{ + Sample3D centre = approximateCentre(data, samples); + return spherify(centre, data, samples); +} +/** + * Calculates an independent scale factor for X,Y and Z axes that places the given data points on a bounding sphere + * + * @param centre A proviously calculated centre point of all data. + * @param data An array of all data points + * @param samples The number of samples in the 'data' array. + * + * @return A calibration structure containing the centre point provided, the radius of the minimum + * enclosing spere of points and a scaling factor for each axis that places those points as close as possible + * to the surface of the containing sphere. + */ +CompassCalibration MicroBitCompassCalibrator::spherify(Sample3D centre, Sample3D *data, int samples) +{ + // First, determine the radius of the enclosing sphere from the given centre. + // n.b. this will likely be different to the radius from the centre of mass previously calculated. + // We use the same algorithm though. + CompassCalibration result; + + float radius = 0; + float scaleX = 0.0; + float scaleY = 0.0; + float scaleZ = 0.0; + + float scale = 0.0; + float weightX = 0.0; + float weightY = 0.0; + float weightZ = 0.0; + + for (int i = 0; i < samples; i++) + { + int d = sqrt((float)centre.dSquared(data[i])); + + if (d > radius) + radius = d; + } + + // Now, for each data point, determine a scalar multiplier for the vector between the centre and that point that + // takes the point onto the surface of the enclosing sphere. + for (int i = 0; i < samples; i++) + { + // Calculate the distance from this point to the centre of the sphere + float d = sqrt(centre.dSquared(data[i])); + + // Now determine a scalar multiplier that, when applied to the vector to the centre, + // will place this point on the surface of the sphere. + float s = (radius / d) - 1; + + scale = max(scale, s); + + // next, determine the scale effect this has on each of our components. + float dx = (data[i].x - centre.x); + float dy = (data[i].y - centre.y); + float dz = (data[i].z - centre.z); + + weightX += s * fabsf(dx / d); + weightY += s * fabsf(dy / d); + weightZ += s * fabsf(dz / d); + } + + float wmag = sqrt((weightX * weightX) + (weightY * weightY) + (weightZ * weightZ)); + + scaleX = 1.0 + scale * (weightX / wmag); + scaleY = 1.0 + scale * (weightY / wmag); + scaleZ = 1.0 + scale * (weightZ / wmag); + + result.scale.x = (int)(1024 * scaleX); + result.scale.y = (int)(1024 * scaleY); + result.scale.z = (int)(1024 * scaleZ); + + result.centre.x = centre.x; + result.centre.y = centre.y; + result.centre.z = centre.z; + + result.radius = radius; + + return result; +} + +/* + * Performs an interative approximation (hill descent) algorithm to determine an + * estimated centre point of a sphere upon which the given data points reside. + * + * @param data an array containing sample points + * @param samples the number of sample points in the 'data' array. + * + * @return the approximated centre point of the points in the 'data' array. + */ +Sample3D MicroBitCompassCalibrator::approximateCentre(Sample3D *data, int samples) +{ + Sample3D c,t; + Sample3D centre = { 0,0,0 }; + Sample3D best = { 0,0,0 }; + + int score; + + for (int i = 0; i < samples; i++) + { + centre.x += data[i].x; + centre.y += data[i].y; + centre.z += data[i].z; + } + + // Calclulate a centre of mass for our input samples. We only use this for validation purposes. + centre.x = centre.x / samples; + centre.y = centre.y / samples; + centre.z = centre.z / samples; + + // Start hill climb in the centre of mass. + c = centre; + + // calculate the nearest and furthest point to us. + score = measureScore(c, data, samples); + + // iteratively attempt to improve position... + while (1) + { + for (int x = -CALIBRATION_INCREMENT; x <= CALIBRATION_INCREMENT; x=x+CALIBRATION_INCREMENT) + { + for (int y = -CALIBRATION_INCREMENT; y <= CALIBRATION_INCREMENT; y=y+CALIBRATION_INCREMENT) + { + for (int z = -CALIBRATION_INCREMENT; z <= CALIBRATION_INCREMENT; z=z+CALIBRATION_INCREMENT) + { + t = c; + t.x += x; + t.y += y; + t.z += z; + + int s = measureScore(t, data, samples); + if (s < score) + { + score = s; + best = t; + } + } + } + } + + if (best.x == c.x && best.y == c.y && best.z == c.z) + break; + + c = best; + } + + return c; +} + +/** + * Performs a simple game that in parallel, calibrates the compass. + * + * This function is executed automatically when the user requests a compass bearing, and compass calibration is required. + * + * This function is, by design, synchronous and only returns once calibration is complete. + */ +void MicroBitCompassCalibrator::calibrateUX(MicroBitEvent) { struct Point { uint8_t x; uint8_t y; - uint8_t on; }; - const int PERIMETER_POINTS = 12; + const int PERIMETER_POINTS = 25; + const int PIXEL1_THRESHOLD = 200; - const int PIXEL2_THRESHOLD = 800; + const int PIXEL2_THRESHOLD = 680; + const int REDISPLAY_MSG_TIMEOUT_MS = 30000; + const int SAMPLES_END_MSG_COUNT = 15; + const int TIME_STEP = 100; + const int MSG_TIME = 155 * TIME_STEP; //We require MSG_TIME % TIME_STEP == 0 wait_ms(100); - Matrix4 X(PERIMETER_POINTS, 4); - Point perimeter[PERIMETER_POINTS] = {{1,0,0}, {2,0,0}, {3,0,0}, {4,1,0}, {4,2,0}, {4,3,0}, {3,4,0}, {2,4,0}, {1,4,0}, {0,3,0}, {0,2,0}, {0,1,0}}; - Point cursor = {2,2,0}; + static const Point perimeter[PERIMETER_POINTS] = {{0,0}, {1,0}, {2,0}, {3,0}, {4,0}, {0,1}, {1,1}, {2,1}, {3,1}, {4,1}, {0,2}, {1,2}, {2,2}, {3,2}, {4,2}, {0,3}, {1,3}, {2,3}, {3,3}, {4,3}, {0,4}, {1,4}, {2,4}, {3,4}, {4,4}}; + Point cursor = {2,2}; MicroBitImage img(5,5); MicroBitImage smiley("0,255,0,255,0\n0,255,0,255,0\n0,0,0,0,0\n255,0,0,0,255\n0,255,255,255,0\n"); - int samples = 0; + + Sample3D data[PERIMETER_POINTS]; + uint8_t visited[PERIMETER_POINTS] = { 0 }; + uint8_t cursor_on = 0; + uint8_t samples = 0; + uint8_t samples_this_period = 0; + int16_t remaining_scroll_time = MSG_TIME; // 32s maximum in uint16_t // Firstly, we need to take over the display. Ensure all active animations are paused. display.stopAnimation(); - display.scrollAsync("DRAW A CIRCLE"); - - for (int i=0; i<110; i++) - wait_ms(100); - - display.stopAnimation(); - display.clear(); while(samples < PERIMETER_POINTS) { + // Scroll a message the first time we enter this loop and every REDISPLAY_MSG_TIMEOUT_MS + if (remaining_scroll_time == MSG_TIME || remaining_scroll_time <= -REDISPLAY_MSG_TIMEOUT_MS) { + display.clear(); + display.scrollAsync("TILT TO FILL SCREEN "); // Takes about 14s + + remaining_scroll_time = MSG_TIME; + samples_this_period = 0; + } + else if (remaining_scroll_time == 0 || samples_this_period == SAMPLES_END_MSG_COUNT) + { + // This stops the scrolling at the end of the message. + // ...and it is the source of the ((MSG_TIME % TIME_STEP) == 0) requirement + display.stopAnimation(); + } + // update our model of the flash status of the user controlled pixel. - cursor.on = (cursor.on + 1) % 4; + cursor_on = (cursor_on + 1) % 4; // take a snapshot of the current accelerometer data. int x = accelerometer.getX(); int y = accelerometer.getY(); - // Wait a little whie for the button state to stabilise (one scheduler tick). - wait_ms(10); - // Deterine the position of the user controlled pixel on the screen. if (x < -PIXEL2_THRESHOLD) cursor.x = 0; @@ -130,60 +340,35 @@ void MicroBitCompassCalibrator::calibrate(MicroBitEvent) // Turn on any pixels that have been visited. for (int i=0; i SAMPLES_END_MSG_COUNT) + display.image.paste(img,0,0,0); // test if we need to update the state at the users position. for (int i=0; i Date: Fri, 6 Jul 2018 03:08:06 +0100 Subject: [PATCH 3/9] Updates to Compass Calibration and Compass Heading Calculation - Add mutator to allow accelerometer to be added to a compass after construction - Use floating point values for Sample3D:dSquared() operations to resmove possible overflow - Change compass calibration hill climb algorithm to use 100 unit steps to converge faster. - Update autodetect functions to register accelerometer instances with compass instances upon detection - Clean and fix axis alignments for tilt compensation algorithm --- inc/drivers/MicroBitCompass.h | 8 +++ inc/drivers/MicroBitCompassCalibrator.h | 2 +- inc/types/CoordinateSystem.h | 8 ++- source/drivers/MicroBitAccelerometer.cpp | 3 + source/drivers/MicroBitCompass.cpp | 60 ++++++++++++-------- source/drivers/MicroBitCompassCalibrator.cpp | 16 +++--- source/types/CoordinateSystem.cpp | 7 ++- 7 files changed, 68 insertions(+), 36 deletions(-) diff --git a/inc/drivers/MicroBitCompass.h b/inc/drivers/MicroBitCompass.h index 3b211db..2567943 100644 --- a/inc/drivers/MicroBitCompass.h +++ b/inc/drivers/MicroBitCompass.h @@ -198,6 +198,14 @@ class MicroBitCompass : public MicroBitComponent */ int configure(); + /** + * + * Defines the accelerometer to be used for tilt compensation. + * + * @param acceleromter Reference to the accelerometer to use. + */ + void setAccelerometer(MicroBitAccelerometer &accelerometer); + /** * Attempts to set the sample rate of the compass to the specified period value (in ms). * diff --git a/inc/drivers/MicroBitCompassCalibrator.h b/inc/drivers/MicroBitCompassCalibrator.h index e563539..9b410f7 100644 --- a/inc/drivers/MicroBitCompassCalibrator.h +++ b/inc/drivers/MicroBitCompassCalibrator.h @@ -106,7 +106,7 @@ class MicroBitCompassCalibrator * * @return The deviation between the closest and further point in the data array from the point given. */ - static int measureScore(Sample3D &c, Sample3D *data, int samples); + static float measureScore(Sample3D &c, Sample3D *data, int samples); /* * Performs an interative approximation (hill descent) algorithm to determine an diff --git a/inc/types/CoordinateSystem.h b/inc/types/CoordinateSystem.h index 5d42ef2..8c45562 100644 --- a/inc/types/CoordinateSystem.h +++ b/inc/types/CoordinateSystem.h @@ -122,9 +122,13 @@ struct Sample3D return !(x == other.x && y == other.y && z == other.z); } - int dSquared(Sample3D &s) + float dSquared(Sample3D &s) { - return (x - s.x)*(x - s.x) + (y - s.y)*(y - s.y) + (z - s.z)*(z - s.z); + float dx = x - s.x; + float dy = y - s.y; + float dz = z - s.z; + + return (dx*dx) + (dy*dy) + (dz*dz); } }; diff --git a/source/drivers/MicroBitAccelerometer.cpp b/source/drivers/MicroBitAccelerometer.cpp index c2f0c01..097a057 100644 --- a/source/drivers/MicroBitAccelerometer.cpp +++ b/source/drivers/MicroBitAccelerometer.cpp @@ -116,6 +116,9 @@ MicroBitAccelerometer& MicroBitAccelerometer::autoDetect(MicroBitI2C &i2c) } } + if (MicroBitCompass::detectedCompass) + MicroBitCompass::detectedCompass->setAccelerometer(*MicroBitAccelerometer::detectedAccelerometer); + return *MicroBitAccelerometer::detectedAccelerometer; } diff --git a/source/drivers/MicroBitCompass.cpp b/source/drivers/MicroBitCompass.cpp index ec33551..16c08e5 100644 --- a/source/drivers/MicroBitCompass.cpp +++ b/source/drivers/MicroBitCompass.cpp @@ -138,6 +138,10 @@ MicroBitCompass& MicroBitCompass::autoDetect(MicroBitI2C &i2c) } } + // If an accelerometer has been discovered, enable tilt compensation on the e-compass. + if (MicroBitAccelerometer::detectedAccelerometer) + MicroBitCompass::detectedCompass->setAccelerometer(*MicroBitAccelerometer::detectedAccelerometer); + return *MicroBitCompass::detectedCompass; } @@ -296,6 +300,17 @@ int MicroBitCompass::configure() return MICROBIT_NOT_SUPPORTED; } +/** + * + * Defines the accelerometer to be used for tilt compensation. + * + * @param acceleromter Reference to the accelerometer to use. + */ +void MicroBitCompass::setAccelerometer(MicroBitAccelerometer &accelerometer) +{ + this->accelerometer = &accelerometer; +} + /** * Attempts to set the sample rate of the compass to the specified period value (in ms). * @@ -359,10 +374,12 @@ int MicroBitCompass::requestUpdate() */ int MicroBitCompass::update() { - // Store the new data, after performing any necessary coordinate transformations. + // Store the raw data, and apply any calibration data we have. sampleENU.x = CALIBRATED_SAMPLE(sampleENU, x); sampleENU.y = CALIBRATED_SAMPLE(sampleENU, y); sampleENU.z = CALIBRATED_SAMPLE(sampleENU, z); + + // Store the user accessible data, in the requested coordinate space, and taking into account component placement of the sensor. sample = coordinateSpace.transform(sampleENU); // Indicate that a new sample is available @@ -434,36 +451,28 @@ int MicroBitCompass::getZ() */ int MicroBitCompass::tiltCompensatedBearing() { - Sample3D cs = this->getSample(NORTH_EAST_DOWN); - Sample3D as = accelerometer->getSample(NORTH_EAST_DOWN); + // Precompute the tilt compensation parameters to improve readability. + float phi = accelerometer->getRollRadians(); + float theta = accelerometer->getPitchRadians(); // Convert to floating point to reduce rounding errors + Sample3D cs = this->getSample(SIMPLE_CARTESIAN); float x = (float) cs.x; float y = (float) cs.y; float z = (float) cs.z; - float ax = (float) as.x; - float ay = (float) as.y; - float az = (float) as.z; + // Precompute cos and sin of pitch and roll angles to make the calculation a little more efficient. + float sinPhi = sin(phi); + float cosPhi = cos(phi); + float sinTheta = sin(theta); + float cosTheta = cos(theta); - // normalize the readings - float amag = sqrt(ax*ax + ay*ay + az*az); - ax = ax/amag; - ay = ay/amag; - az = az/amag; - - float ax2 = ax*ax; - float ay2 = ay*ay; - - float resultx = x*(1.0f - ax2) - y*ax*ay - z*ax*sqrt(1.0f-ax2-ay2); - float resulty = y*sqrt(1.0f-ax2-ay2) - z*ay; - - float bearing = (360*atan2(resulty,resultx)) / (2*PI); + float bearing = (360*atan2(x*cosTheta + y*sinTheta*sinPhi + z*sinTheta*cosPhi, z*sinPhi - y*cosPhi)) / (2*PI); if (bearing < 0) - bearing += 360.0; + bearing += 360.0f; - return (int) (360.0 - bearing); + return (int) (bearing); } /** @@ -471,12 +480,17 @@ int MicroBitCompass::tiltCompensatedBearing() */ int MicroBitCompass::basicBearing() { - float bearing = (atan2((double)getY(),(double)getX()))*180/PI; + // Convert to floating point to reduce rounding errors + Sample3D cs = this->getSample(SIMPLE_CARTESIAN); + float x = (float) cs.x; + float y = (float) cs.y; + + float bearing = (atan2(x,y))*180/PI; if (bearing < 0) bearing += 360.0; - return (int)(360.0 - bearing); + return (int)bearing; } /** diff --git a/source/drivers/MicroBitCompassCalibrator.cpp b/source/drivers/MicroBitCompassCalibrator.cpp index e738ee2..ee31527 100644 --- a/source/drivers/MicroBitCompassCalibrator.cpp +++ b/source/drivers/MicroBitCompassCalibrator.cpp @@ -28,7 +28,7 @@ DEALINGS IN THE SOFTWARE. #include "EventModel.h" #include "Matrix4.h" -#define CALIBRATION_INCREMENT 10 +#define CALIBRATION_INCREMENT 200 /** * Constructor. @@ -62,15 +62,15 @@ MicroBitCompassCalibrator::MicroBitCompassCalibrator(MicroBitCompass& _compass, * * @return The deviation between the closest and further point in the data array from the point given. */ -int MicroBitCompassCalibrator::measureScore(Sample3D &c, Sample3D *data, int samples) +float MicroBitCompassCalibrator::measureScore(Sample3D &c, Sample3D *data, int samples) { - int minD; - int maxD; + float minD; + float maxD; minD = maxD = c.dSquared(data[0]); for (int i = 1; i < samples; i++) { - int d = c.dSquared(data[i]); + float d = c.dSquared(data[i]); if (d < minD) minD = d; @@ -194,7 +194,7 @@ Sample3D MicroBitCompassCalibrator::approximateCentre(Sample3D *data, int sample Sample3D centre = { 0,0,0 }; Sample3D best = { 0,0,0 }; - int score; + float score; for (int i = 0; i < samples; i++) { @@ -228,7 +228,7 @@ Sample3D MicroBitCompassCalibrator::approximateCentre(Sample3D *data, int sample t.y += y; t.z += z; - int s = measureScore(t, data, samples); + float s = measureScore(t, data, samples); if (s < score) { score = s; @@ -356,7 +356,7 @@ void MicroBitCompassCalibrator::calibrateUX(MicroBitEvent) if (cursor.x == perimeter[i].x && cursor.y == perimeter[i].y && !(visited[i] == 1)) { // Record the sample data for later processing... - data[samples] = compass.getSample(); + data[samples] = compass.getSample(RAW); // Record that this pixel has been visited. visited[i] = 1; diff --git a/source/types/CoordinateSystem.cpp b/source/types/CoordinateSystem.cpp index 45a0a86..72a298b 100644 --- a/source/types/CoordinateSystem.cpp +++ b/source/types/CoordinateSystem.cpp @@ -65,6 +65,10 @@ Sample3D CoordinateSpace::transform(Sample3D s, CoordinateSystem system) Sample3D result = s; int temp; + // If we've been asked to supply raw data, simply return it. + if (system == RAW) + return result; + // Firstly, handle any inversions. // As we know the input is in ENU format, this means we flip the polarity of the X and Z axes. if(upsidedown) @@ -109,8 +113,7 @@ Sample3D CoordinateSpace::transform(Sample3D s, CoordinateSystem system) result.z = -result.z; break; - case EAST_NORTH_UP: - case RAW: + default: // EAST_NORTH_UP break; } From e770f346366da72d641c88cde86f567eec3e2b44 Mon Sep 17 00:00:00 2001 From: Joe Finney Date: Fri, 6 Jul 2018 03:17:03 +0100 Subject: [PATCH 4/9] Add Backward Compatibility for CompassSample type. --- inc/drivers/MicroBitCompass.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/inc/drivers/MicroBitCompass.h b/inc/drivers/MicroBitCompass.h index 2567943..0fa9680 100644 --- a/inc/drivers/MicroBitCompass.h +++ b/inc/drivers/MicroBitCompass.h @@ -316,4 +316,9 @@ class MicroBitCompass : public MicroBitComponent int basicBearing(); }; +// +// Backward Compatibility +// +typedef Sample3D CompassSample; + #endif From 2d46e9dabd1972af10f6a5bd0439c798562cfce4 Mon Sep 17 00:00:00 2001 From: Joe Finney Date: Fri, 6 Jul 2018 03:19:42 +0100 Subject: [PATCH 5/9] Remove reduntant code in MicroBitAccelerometer Also removes a potential recursive loop, as noted by @dpgeorge. --- source/drivers/MicroBitAccelerometer.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/source/drivers/MicroBitAccelerometer.cpp b/source/drivers/MicroBitAccelerometer.cpp index 097a057..da71ccb 100644 --- a/source/drivers/MicroBitAccelerometer.cpp +++ b/source/drivers/MicroBitAccelerometer.cpp @@ -163,8 +163,6 @@ int MicroBitAccelerometer::update() */ uint32_t MicroBitAccelerometer::instantaneousAccelerationSquared() { - requestUpdate(); - // Use pythagoras theorem to determine the combined force acting on the device. return (uint32_t)sample.x*(uint32_t)sample.x + (uint32_t)sample.y*(uint32_t)sample.y + (uint32_t)sample.z*(uint32_t)sample.z; } From 81b85e13351e9c205b6c9818a17781351c39cb46 Mon Sep 17 00:00:00 2001 From: Joe Finney Date: Fri, 6 Jul 2018 03:32:08 +0100 Subject: [PATCH 6/9] Backward compatibility for ::updateSample() methods in sensors. --- inc/drivers/MicroBitAccelerometer.h | 8 ++++++++ inc/drivers/MicroBitCompass.h | 8 ++++++++ 2 files changed, 16 insertions(+) diff --git a/inc/drivers/MicroBitAccelerometer.h b/inc/drivers/MicroBitAccelerometer.h index c6c73eb..c7b9f3f 100644 --- a/inc/drivers/MicroBitAccelerometer.h +++ b/inc/drivers/MicroBitAccelerometer.h @@ -316,6 +316,14 @@ class MicroBitAccelerometer : public MicroBitComponent */ uint16_t getGesture(); + /** + * updateSample() method maintained here as an inline method purely for backward compatibility. + */ + inline void updateSample() + { + getSample(); + } + /** * Destructor. */ diff --git a/inc/drivers/MicroBitCompass.h b/inc/drivers/MicroBitCompass.h index 0fa9680..f82e26f 100644 --- a/inc/drivers/MicroBitCompass.h +++ b/inc/drivers/MicroBitCompass.h @@ -291,6 +291,14 @@ class MicroBitCompass : public MicroBitComponent */ int getZ(); + /** + * updateSample() method maintained here as an inline method purely for backward compatibility. + */ + inline void updateSample() + { + getSample(); + } + /** * Destructor. From 5d4c39731164cfdd2289a45b04127b08d37ffd9f Mon Sep 17 00:00:00 2001 From: Joe Finney Date: Fri, 6 Jul 2018 03:39:02 +0100 Subject: [PATCH 7/9] Remove redundant Matrix4 Class. - Matrix4 was used only for the least mean squres calculation as part of the MicroBitCompassCalibrator. This has now been replace with an itereative approximation algorithm. --- inc/types/Matrix4.h | 202 ------------- source/CMakeLists.txt | 1 - source/drivers/MicroBitCompassCalibrator.cpp | 1 - source/types/Matrix4.cpp | 285 ------------------- 4 files changed, 489 deletions(-) delete mode 100644 inc/types/Matrix4.h delete mode 100644 source/types/Matrix4.cpp diff --git a/inc/types/Matrix4.h b/inc/types/Matrix4.h deleted file mode 100644 index 8490cc4..0000000 --- a/inc/types/Matrix4.h +++ /dev/null @@ -1,202 +0,0 @@ -/* -The MIT License (MIT) - -Copyright (c) 2016 British Broadcasting Corporation. -This software is provided by Lancaster University by arrangement with the BBC. - -Permission is hereby granted, free of charge, to any person obtaining a -copy of this software and associated documentation files (the "Software"), -to deal in the Software without restriction, including without limitation -the rights to use, copy, modify, merge, publish, distribute, sublicense, -and/or sell copies of the Software, and to permit persons to whom the -Software is furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER -DEALINGS IN THE SOFTWARE. -*/ - -#ifndef MICROBIT_MATRIX4_H -#define MICROBIT_MATRIX4_H - -#include "MicroBitConfig.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 -{ - float *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. - * - * @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. - * - * @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. - * - * @code - * int c = matrix.width(); - * @endcode - */ - int width(); - - /** - * Determines the number of rows in this matrix. - * - * @return The number of rows in the matrix. - * - * @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. 0 is returned if the given index is out of range. - * - * @code - * float v = matrix.get(1,2); - * @endcode - */ - float 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. - * - * @code - * matrix.set(1,2,42.0); - * @endcode - */ - void set(int row, int col, float v); - - /** - * Transposes this matrix. - * - * @return the resultant matrix. - * - * @code - * matrix.transpose(); - * @endcode - */ - Matrix4 transpose(); - - /** - * Multiplies this matrix with the given matrix (if possible). - * - * @param matrix the matrix to multiply this matrix's values against. - * - * @param transpose Transpose the matrices before multiplication. Defaults to false. - * - * @return the resultant matrix. An empty matrix is returned if the operation canot be completed. - * - * @code - * Matrix result = matrixA.multiply(matrixB); - * @endcode - */ - Matrix4 multiply(Matrix4 &matrix, bool transpose = false); - - /** - * Multiplies the transpose of this matrix with the given matrix (if possible). - * - * @param matrix the matrix to multiply this matrix's values against. - * - * @return the resultant matrix. An empty matrix is returned if the operation canot be completed. - * - * @code - * Matrix result = matrixA.multiplyT(matrixB); - * @endcode - */ - Matrix4 multiplyT(Matrix4 &matrix); - - /** - * 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. - * - * @code - * Matrix result = matrixA.invert(); - * @endcode - */ - Matrix4 invert(); - - /** - * Destructor. - * - * Frees any memory consumed by this Matrix4 instance. - */ - ~Matrix4(); -}; - -/** - * Multiplies the transpose of this matrix with the given matrix (if possible). - * - * @return the resultant matrix. An empty matrix is returned if the operation canot be completed. - * - * @code - * Matrix result = matrixA.multiplyT(matrixB); - * @endcode - */ -inline Matrix4 Matrix4::multiplyT(Matrix4 &matrix) -{ - return multiply(matrix, true); -} - -#endif diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index 3aecde3..6fffad8 100755 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -18,7 +18,6 @@ set(YOTTA_AUTO_MICROBIT-DAL_CPP_FILES "types/CoordinateSystem.cpp" "types/ManagedString.cpp" - "types/Matrix4.cpp" "types/MicroBitEvent.cpp" "types/MicroBitImage.cpp" "types/PacketBuffer.cpp" diff --git a/source/drivers/MicroBitCompassCalibrator.cpp b/source/drivers/MicroBitCompassCalibrator.cpp index ee31527..bdcb836 100644 --- a/source/drivers/MicroBitCompassCalibrator.cpp +++ b/source/drivers/MicroBitCompassCalibrator.cpp @@ -26,7 +26,6 @@ DEALINGS IN THE SOFTWARE. #include "MicroBitConfig.h" #include "MicroBitCompassCalibrator.h" #include "EventModel.h" -#include "Matrix4.h" #define CALIBRATION_INCREMENT 200 diff --git a/source/types/Matrix4.cpp b/source/types/Matrix4.cpp deleted file mode 100644 index cd060be..0000000 --- a/source/types/Matrix4.cpp +++ /dev/null @@ -1,285 +0,0 @@ -/* -The MIT License (MIT) - -Copyright (c) 2016 British Broadcasting Corporation. -This software is provided by Lancaster University by arrangement with the BBC. - -Permission is hereby granted, free of charge, to any person obtaining a -copy of this software and associated documentation files (the "Software"), -to deal in the Software without restriction, including without limitation -the rights to use, copy, modify, merge, publish, distribute, sublicense, -and/or sell copies of the Software, and to permit persons to whom the -Software is furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER -DEALINGS IN THE SOFTWARE. -*/ - -#include "MicroBitConfig.h" -#include "Matrix4.h" -#include "mbed.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. - * - * @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 float[size]; - else - data = NULL; -} - -/** - * Constructor. - * Create a matrix that is an identical copy of the given matrix. - * - * @param matrix The matrix to copy. - * - * @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 float[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. - * - * @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. - * - * @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. 0 is returned if the given index is out of range. - * - * @code - * float v = matrix.get(1,2); - * @endcode - */ -float 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. - * - * @code - * matrix.set(1,2,42.0); - * @endcode - */ -void Matrix4::set(int row, int col, float v) -{ - if (row < 0 || col < 0 || row >= rows || col >= cols) - return; - - data[width() * row + col] = v; -} - -/** - * Transposes this matrix. - * - * @return the resultant matrix. - * - * @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). - * - * @param matrix the matrix to multiply this matrix's values against. - * - * @param transpose Transpose the matrices before multiplication. Defaults to false. - * - * @return the resultant matrix. An empty matrix is returned if the operation canot be completed. - * - * @code - * Matrix result = matrixA.multiply(matrixB); - * @endcode - */ -Matrix4 Matrix4::multiply(Matrix4 &matrix, bool transpose) -{ - int w = transpose ? height() : width(); - int h = transpose ? width() : height(); - - if (w != matrix.height()) - return Matrix4(0, 0); - - Matrix4 result(h, matrix.width()); - - for (int r = 0; r < result.height(); r++) - { - for (int c = 0; c < result.width(); c++) - { - float v = 0.0; - - for (int i = 0; i < w; i++) - v += (transpose ? get(i, r) : 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. - * - * @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]; - - float 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; -} - -/** - * Destructor. - * - * Frees any memory consumed by this Matrix4 instance. - */ -Matrix4::~Matrix4() -{ - if (data != NULL) - { - delete data; - data = NULL; - } -} From 57e2485861d79de0a06805fab709f1d19dc9bfd3 Mon Sep 17 00:00:00 2001 From: Joe Finney Date: Fri, 6 Jul 2018 04:18:34 +0100 Subject: [PATCH 8/9] Introduce optional persistent storage functionality into MicroBitCompassCalibrator - A new form of constructor now allows for a persistent storage object to be provided. If present, compass calibration data will be automatically stored and retrieved from FLASH when needed. --- inc/drivers/MicroBitCompassCalibrator.h | 20 ++++++++++ source/drivers/MicroBitCompassCalibrator.cpp | 42 +++++++++++++++++++- 2 files changed, 61 insertions(+), 1 deletion(-) diff --git a/inc/drivers/MicroBitCompassCalibrator.h b/inc/drivers/MicroBitCompassCalibrator.h index 9b410f7..633f8a7 100644 --- a/inc/drivers/MicroBitCompassCalibrator.h +++ b/inc/drivers/MicroBitCompassCalibrator.h @@ -30,6 +30,7 @@ DEALINGS IN THE SOFTWARE. #include "MicroBitCompass.h" #include "MicroBitAccelerometer.h" #include "MicroBitDisplay.h" +#include "MicroBitStorage.h" /** @@ -49,6 +50,7 @@ class MicroBitCompassCalibrator MicroBitCompass& compass; MicroBitAccelerometer& accelerometer; MicroBitDisplay& display; + MicroBitStorage *storage; public: @@ -71,6 +73,24 @@ class MicroBitCompassCalibrator */ MicroBitCompassCalibrator(MicroBitCompass& _compass, MicroBitAccelerometer& _accelerometer, MicroBitDisplay& _display); + /** + * Constructor. + * + * Create an object capable of calibrating the compass. + * + * The algorithm uses an accelerometer to ensure that a broad range of sample data has been gathered + * from the compass module, then performs a least mean squares optimisation of the + * results to determine the calibration data for the compass. + * + * The LED matrix display is used to provide feedback to the user on the gestures required. + * + * @param compass The compass instance to calibrate. + * @param accelerometer The accelerometer to gather contextual data from. + * @param display The LED matrix to display user feedback on. + * @param storage The object to use for storing calibration data in persistent FLASH. + */ + MicroBitCompassCalibrator(MicroBitCompass& _compass, MicroBitAccelerometer& _accelerometer, MicroBitDisplay& _display, MicroBitStorage &storage); + /** * Performs a simple game that in parallel, calibrates the compass. * diff --git a/source/drivers/MicroBitCompassCalibrator.cpp b/source/drivers/MicroBitCompassCalibrator.cpp index bdcb836..6c3d875 100644 --- a/source/drivers/MicroBitCompassCalibrator.cpp +++ b/source/drivers/MicroBitCompassCalibrator.cpp @@ -48,10 +48,46 @@ DEALINGS IN THE SOFTWARE. */ MicroBitCompassCalibrator::MicroBitCompassCalibrator(MicroBitCompass& _compass, MicroBitAccelerometer& _accelerometer, MicroBitDisplay& _display) : compass(_compass), accelerometer(_accelerometer), display(_display) { + this->storage = NULL; + if (EventModel::defaultEventBus) EventModel::defaultEventBus->listen(MICROBIT_ID_COMPASS, MICROBIT_COMPASS_EVT_CALIBRATE, this, &MicroBitCompassCalibrator::calibrateUX, MESSAGE_BUS_LISTENER_IMMEDIATE); } +/** + * Constructor. + * + * Create an object capable of calibrating the compass. + * + * The algorithm uses an accelerometer to ensure that a broad range of sample data has been gathered + * from the compass module, then performs a least mean squares optimisation of the + * results to determine the calibration data for the compass. + * + * The LED matrix display is used to provide feedback to the user on the gestures required. + * + * @param compass The compass instance to calibrate. + * @param accelerometer The accelerometer to gather contextual data from. + * @param display The LED matrix to display user feedback on. + * @param storage The object to use for storing calibration data in persistent FLASH. + */ +MicroBitCompassCalibrator::MicroBitCompassCalibrator(MicroBitCompass& _compass, MicroBitAccelerometer& _accelerometer, MicroBitDisplay& _display, MicroBitStorage &storage) : compass(_compass), accelerometer(_accelerometer), display(_display) +{ + this->storage = &storage; + + //Attempt to load any stored calibration datafor the compass. + KeyValuePair *calibrationData = this->storage->get("compassCal"); + + if(calibrationData != NULL) + { + CompassCalibration cal = CompassCalibration(); + memcpy(&cal, calibrationData->value, sizeof(CompassCalibration)); + compass.setCalibration(cal); + delete calibrationData; + } + + if (EventModel::defaultEventBus) + EventModel::defaultEventBus->listen(MICROBIT_ID_COMPASS, MICROBIT_COMPASS_EVT_CALIBRATE, this, &MicroBitCompassCalibrator::calibrateUX, MESSAGE_BUS_LISTENER_IMMEDIATE); +} /** * Scoring function for a hill climb algorithm. * @@ -367,7 +403,11 @@ void MicroBitCompassCalibrator::calibrateUX(MicroBitEvent) remaining_scroll_time-=TIME_STEP; } - compass.setCalibration(calibrate(data, samples)); + CompassCalibration cal = calibrate(data, samples); + compass.setCalibration(cal); + + if(this->storage) + this->storage->put(ManagedString("compassCal"), (uint8_t *) &cal, sizeof(CompassCalibration)); // Show a smiley to indicate that we're done, and continue on with the user program. display.clear(); From 13459857a5181d3f2d9f030f10199bc5216c7a96 Mon Sep 17 00:00:00 2001 From: Joe Finney Date: Fri, 6 Jul 2018 04:28:13 +0100 Subject: [PATCH 9/9] Disable DEBUG, pending beta release --- inc/core/MicroBitConfig.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/inc/core/MicroBitConfig.h b/inc/core/MicroBitConfig.h index bce606d..783ccd0 100644 --- a/inc/core/MicroBitConfig.h +++ b/inc/core/MicroBitConfig.h @@ -196,7 +196,7 @@ extern uint32_t __etext; // Enable/Disable BLE during normal operation. // Set '1' to enable. #ifndef MICROBIT_BLE_ENABLED -#define MICROBIT_BLE_ENABLED 0 +#define MICROBIT_BLE_ENABLED 1 #endif // Enable/Disable BLE pairing mode mode at power up. @@ -416,7 +416,7 @@ extern uint32_t __etext; // n.b. This also disables the user serial port 'uBit.serial'. // Set '1' to enable. #ifndef MICROBIT_DBG -#define MICROBIT_DBG 1 +#define MICROBIT_DBG 0 #endif // Enable this to receive diagnostic messages from the heap allocator via the USB serial interface.