Thomas Butler / microbit-dal

fsdfds

Dependencies:   BLE_API mbed-dev-bin nRF51822

Fork of microbit-dal by Lancaster University

source/drivers/MicroBitCompass.cpp@45:23b71960fe6c, 2016-07-13 (annotated)

Committer:
LancasterUniversity
Date:
Wed Jul 13 12:18:24 2016 +0100
Revision:
45:23b71960fe6c
Parent:
44:bb4aa1985f37
Child:
48:34d1adb6771c
Synchronized with git rev e9353806

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jonathan Austin 1:8aa5cdb4ab67 1 /*
Jonathan Austin 1:8aa5cdb4ab67 2 The MIT License (MIT)
Jonathan Austin 1:8aa5cdb4ab67 3
Jonathan Austin 1:8aa5cdb4ab67 4 Copyright (c) 2016 British Broadcasting Corporation.
Jonathan Austin 1:8aa5cdb4ab67 5 This software is provided by Lancaster University by arrangement with the BBC.
Jonathan Austin 1:8aa5cdb4ab67 6
Jonathan Austin 1:8aa5cdb4ab67 7 Permission is hereby granted, free of charge, to any person obtaining a
Jonathan Austin 1:8aa5cdb4ab67 8 copy of this software and associated documentation files (the "Software"),
Jonathan Austin 1:8aa5cdb4ab67 9 to deal in the Software without restriction, including without limitation
Jonathan Austin 1:8aa5cdb4ab67 10 the rights to use, copy, modify, merge, publish, distribute, sublicense,
Jonathan Austin 1:8aa5cdb4ab67 11 and/or sell copies of the Software, and to permit persons to whom the
Jonathan Austin 1:8aa5cdb4ab67 12 Software is furnished to do so, subject to the following conditions:
Jonathan Austin 1:8aa5cdb4ab67 13
Jonathan Austin 1:8aa5cdb4ab67 14 The above copyright notice and this permission notice shall be included in
Jonathan Austin 1:8aa5cdb4ab67 15 all copies or substantial portions of the Software.
Jonathan Austin 1:8aa5cdb4ab67 16
Jonathan Austin 1:8aa5cdb4ab67 17 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
Jonathan Austin 1:8aa5cdb4ab67 18 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
Jonathan Austin 1:8aa5cdb4ab67 19 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
Jonathan Austin 1:8aa5cdb4ab67 20 THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
Jonathan Austin 1:8aa5cdb4ab67 21 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
Jonathan Austin 1:8aa5cdb4ab67 22 FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
Jonathan Austin 1:8aa5cdb4ab67 23 DEALINGS IN THE SOFTWARE.
Jonathan Austin 1:8aa5cdb4ab67 24 */
Jonathan Austin 1:8aa5cdb4ab67 25
Jonathan Austin 1:8aa5cdb4ab67 26 /**
Jonathan Austin 1:8aa5cdb4ab67 27 * Class definition for MicroBit Compass.
Jonathan Austin 1:8aa5cdb4ab67 28 *
Jonathan Austin 1:8aa5cdb4ab67 29 * Represents an implementation of the Freescale MAG3110 I2C Magnetmometer.
Jonathan Austin 1:8aa5cdb4ab67 30 * Also includes basic caching, calibration and on demand activation.
Jonathan Austin 1:8aa5cdb4ab67 31 */
Jonathan Austin 1:8aa5cdb4ab67 32 #include "MicroBitConfig.h"
Jonathan Austin 1:8aa5cdb4ab67 33 #include "MicroBitCompass.h"
Jonathan Austin 1:8aa5cdb4ab67 34 #include "MicroBitFiber.h"
Jonathan Austin 1:8aa5cdb4ab67 35 #include "ErrorNo.h"
Jonathan Austin 1:8aa5cdb4ab67 36
Jonathan Austin 1:8aa5cdb4ab67 37 /**
Jonathan Austin 1:8aa5cdb4ab67 38 * An initialisation member function used by the many constructors of MicroBitCompass.
Jonathan Austin 1:8aa5cdb4ab67 39 *
Jonathan Austin 1:8aa5cdb4ab67 40 * @param id the unique identifier for this compass instance.
Jonathan Austin 1:8aa5cdb4ab67 41 *
Jonathan Austin 1:8aa5cdb4ab67 42 * @param address the base address of the magnetometer on the i2c bus.
Jonathan Austin 1:8aa5cdb4ab67 43 */
Jonathan Austin 1:8aa5cdb4ab67 44 void MicroBitCompass::init(uint16_t id, uint16_t address)
Jonathan Austin 1:8aa5cdb4ab67 45 {
Jonathan Austin 1:8aa5cdb4ab67 46 this->id = id;
Jonathan Austin 1:8aa5cdb4ab67 47 this->address = address;
Jonathan Austin 1:8aa5cdb4ab67 48
Jonathan Austin 1:8aa5cdb4ab67 49 // Select 10Hz update rate, with oversampling, and enable the device.
Jonathan Austin 1:8aa5cdb4ab67 50 this->samplePeriod = 100;
Jonathan Austin 1:8aa5cdb4ab67 51 this->configure();
Jonathan Austin 1:8aa5cdb4ab67 52
Jonathan Austin 1:8aa5cdb4ab67 53 // Assume that we have no calibration information.
Jonathan Austin 1:8aa5cdb4ab67 54 status &= ~MICROBIT_COMPASS_STATUS_CALIBRATED;
Jonathan Austin 1:8aa5cdb4ab67 55
Jonathan Austin 1:8aa5cdb4ab67 56 if(this->storage != NULL)
Jonathan Austin 1:8aa5cdb4ab67 57 {
LancasterUniversity 45:23b71960fe6c 58 KeyValuePair *calibrationData = storage->get(ManagedString("compassCal"));
Jonathan Austin 1:8aa5cdb4ab67 59
Jonathan Austin 1:8aa5cdb4ab67 60 if(calibrationData != NULL)
Jonathan Austin 1:8aa5cdb4ab67 61 {
Jonathan Austin 1:8aa5cdb4ab67 62 CompassSample storedSample = CompassSample();
Jonathan Austin 1:8aa5cdb4ab67 63
Jonathan Austin 1:8aa5cdb4ab67 64 memcpy(&storedSample, calibrationData->value, sizeof(CompassSample));
Jonathan Austin 1:8aa5cdb4ab67 65
Jonathan Austin 1:8aa5cdb4ab67 66 setCalibration(storedSample);
Jonathan Austin 1:8aa5cdb4ab67 67
Jonathan Austin 1:8aa5cdb4ab67 68 delete calibrationData;
Jonathan Austin 1:8aa5cdb4ab67 69 }
Jonathan Austin 1:8aa5cdb4ab67 70 }
Jonathan Austin 1:8aa5cdb4ab67 71
Jonathan Austin 1:8aa5cdb4ab67 72 // Indicate that we're up and running.
Jonathan Austin 1:8aa5cdb4ab67 73 status |= MICROBIT_COMPONENT_RUNNING;
Jonathan Austin 1:8aa5cdb4ab67 74 }
Jonathan Austin 1:8aa5cdb4ab67 75
Jonathan Austin 1:8aa5cdb4ab67 76 /**
Jonathan Austin 1:8aa5cdb4ab67 77 * Constructor.
Jonathan Austin 1:8aa5cdb4ab67 78 * Create a software representation of an e-compass.
Jonathan Austin 1:8aa5cdb4ab67 79 *
Jonathan Austin 1:8aa5cdb4ab67 80 * @param _i2c an instance of i2c, which the compass is accessible from.
Jonathan Austin 1:8aa5cdb4ab67 81 *
Jonathan Austin 1:8aa5cdb4ab67 82 * @param _accelerometer an instance of the accelerometer, used for tilt compensation.
Jonathan Austin 1:8aa5cdb4ab67 83 *
Jonathan Austin 1:8aa5cdb4ab67 84 * @param _storage an instance of MicroBitStorage, used to persist calibration data across resets.
Jonathan Austin 1:8aa5cdb4ab67 85 *
Jonathan Austin 1:8aa5cdb4ab67 86 * @param address the default address for the compass register on the i2c bus. Defaults to MAG3110_DEFAULT_ADDR.
Jonathan Austin 1:8aa5cdb4ab67 87 *
Jonathan Austin 1:8aa5cdb4ab67 88 * @param id the ID of the new MicroBitCompass object. Defaults to MAG3110_DEFAULT_ADDR.
Jonathan Austin 1:8aa5cdb4ab67 89 *
Jonathan Austin 1:8aa5cdb4ab67 90 * @code
Jonathan Austin 1:8aa5cdb4ab67 91 * MicroBitI2C i2c(I2C_SDA0, I2C_SCL0);
Jonathan Austin 1:8aa5cdb4ab67 92 *
Jonathan Austin 1:8aa5cdb4ab67 93 * MicroBitAccelerometer accelerometer(i2c);
Jonathan Austin 1:8aa5cdb4ab67 94 *
Jonathan Austin 1:8aa5cdb4ab67 95 * MicroBitStorage storage;
Jonathan Austin 1:8aa5cdb4ab67 96 *
Jonathan Austin 1:8aa5cdb4ab67 97 * MicroBitCompass compass(i2c, accelerometer, storage);
Jonathan Austin 1:8aa5cdb4ab67 98 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 99 */
Jonathan Austin 1:8aa5cdb4ab67 100 MicroBitCompass::MicroBitCompass(MicroBitI2C& _i2c, MicroBitAccelerometer& _accelerometer, MicroBitStorage& _storage, uint16_t address, uint16_t id) :
Jonathan Austin 1:8aa5cdb4ab67 101 average(),
Jonathan Austin 1:8aa5cdb4ab67 102 sample(),
Jonathan Austin 1:8aa5cdb4ab67 103 int1(MICROBIT_PIN_COMPASS_DATA_READY),
Jonathan Austin 1:8aa5cdb4ab67 104 i2c(_i2c),
Jonathan Austin 1:8aa5cdb4ab67 105 accelerometer(&_accelerometer),
Jonathan Austin 1:8aa5cdb4ab67 106 storage(&_storage)
Jonathan Austin 1:8aa5cdb4ab67 107 {
Jonathan Austin 1:8aa5cdb4ab67 108 init(id, address);
Jonathan Austin 1:8aa5cdb4ab67 109 }
Jonathan Austin 1:8aa5cdb4ab67 110
Jonathan Austin 1:8aa5cdb4ab67 111 /**
Jonathan Austin 1:8aa5cdb4ab67 112 * Constructor.
Jonathan Austin 1:8aa5cdb4ab67 113 * Create a software representation of an e-compass.
Jonathan Austin 1:8aa5cdb4ab67 114 *
Jonathan Austin 1:8aa5cdb4ab67 115 * @param _i2c an instance of i2c, which the compass is accessible from.
Jonathan Austin 1:8aa5cdb4ab67 116 *
Jonathan Austin 1:8aa5cdb4ab67 117 * @param _accelerometer an instance of the accelerometer, used for tilt compensation.
Jonathan Austin 1:8aa5cdb4ab67 118 *
Jonathan Austin 1:8aa5cdb4ab67 119 * @param address the default address for the compass register on the i2c bus. Defaults to MAG3110_DEFAULT_ADDR.
Jonathan Austin 1:8aa5cdb4ab67 120 *
Jonathan Austin 1:8aa5cdb4ab67 121 * @param id the ID of the new MicroBitCompass object. Defaults to MAG3110_DEFAULT_ADDR.
Jonathan Austin 1:8aa5cdb4ab67 122 *
Jonathan Austin 1:8aa5cdb4ab67 123 * @code
Jonathan Austin 1:8aa5cdb4ab67 124 * MicroBitI2C i2c(I2C_SDA0, I2C_SCL0);
Jonathan Austin 1:8aa5cdb4ab67 125 *
Jonathan Austin 1:8aa5cdb4ab67 126 * MicroBitAccelerometer accelerometer(i2c);
Jonathan Austin 1:8aa5cdb4ab67 127 *
Jonathan Austin 1:8aa5cdb4ab67 128 * MicroBitCompass compass(i2c, accelerometer, storage);
Jonathan Austin 1:8aa5cdb4ab67 129 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 130 */
Jonathan Austin 1:8aa5cdb4ab67 131 MicroBitCompass::MicroBitCompass(MicroBitI2C& _i2c, MicroBitAccelerometer& _accelerometer, uint16_t address, uint16_t id) :
Jonathan Austin 1:8aa5cdb4ab67 132 average(),
Jonathan Austin 1:8aa5cdb4ab67 133 sample(),
Jonathan Austin 1:8aa5cdb4ab67 134 int1(MICROBIT_PIN_COMPASS_DATA_READY),
Jonathan Austin 1:8aa5cdb4ab67 135 i2c(_i2c),
Jonathan Austin 1:8aa5cdb4ab67 136 accelerometer(&_accelerometer),
Jonathan Austin 1:8aa5cdb4ab67 137 storage(NULL)
Jonathan Austin 1:8aa5cdb4ab67 138 {
Jonathan Austin 1:8aa5cdb4ab67 139 init(id, address);
Jonathan Austin 1:8aa5cdb4ab67 140 }
Jonathan Austin 1:8aa5cdb4ab67 141
Jonathan Austin 1:8aa5cdb4ab67 142 /**
Jonathan Austin 1:8aa5cdb4ab67 143 * Constructor.
Jonathan Austin 1:8aa5cdb4ab67 144 * Create a software representation of an e-compass.
Jonathan Austin 1:8aa5cdb4ab67 145 *
Jonathan Austin 1:8aa5cdb4ab67 146 * @param _i2c an instance of i2c, which the compass is accessible from.
Jonathan Austin 1:8aa5cdb4ab67 147 *
Jonathan Austin 1:8aa5cdb4ab67 148 * @param _storage an instance of MicroBitStorage, used to persist calibration data across resets.
Jonathan Austin 1:8aa5cdb4ab67 149 *
Jonathan Austin 1:8aa5cdb4ab67 150 * @param address the default address for the compass register on the i2c bus. Defaults to MAG3110_DEFAULT_ADDR.
Jonathan Austin 1:8aa5cdb4ab67 151 *
Jonathan Austin 1:8aa5cdb4ab67 152 * @param id the ID of the new MicroBitCompass object. Defaults to MAG3110_DEFAULT_ADDR.
Jonathan Austin 1:8aa5cdb4ab67 153 *
Jonathan Austin 1:8aa5cdb4ab67 154 * @code
Jonathan Austin 1:8aa5cdb4ab67 155 * MicroBitI2C i2c(I2C_SDA0, I2C_SCL0);
Jonathan Austin 1:8aa5cdb4ab67 156 *
Jonathan Austin 1:8aa5cdb4ab67 157 * MicroBitStorage storage;
Jonathan Austin 1:8aa5cdb4ab67 158 *
Jonathan Austin 1:8aa5cdb4ab67 159 * MicroBitCompass compass(i2c, storage);
Jonathan Austin 1:8aa5cdb4ab67 160 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 161 */
Jonathan Austin 1:8aa5cdb4ab67 162 MicroBitCompass::MicroBitCompass(MicroBitI2C& _i2c, MicroBitStorage& _storage, uint16_t address, uint16_t id) :
Jonathan Austin 1:8aa5cdb4ab67 163 average(),
Jonathan Austin 1:8aa5cdb4ab67 164 sample(),
Jonathan Austin 1:8aa5cdb4ab67 165 int1(MICROBIT_PIN_COMPASS_DATA_READY),
Jonathan Austin 1:8aa5cdb4ab67 166 i2c(_i2c),
Jonathan Austin 1:8aa5cdb4ab67 167 accelerometer(NULL),
Jonathan Austin 1:8aa5cdb4ab67 168 storage(&_storage)
Jonathan Austin 1:8aa5cdb4ab67 169 {
Jonathan Austin 1:8aa5cdb4ab67 170 init(id, address);
Jonathan Austin 1:8aa5cdb4ab67 171 }
Jonathan Austin 1:8aa5cdb4ab67 172
Jonathan Austin 1:8aa5cdb4ab67 173 /**
Jonathan Austin 1:8aa5cdb4ab67 174 * Constructor.
Jonathan Austin 1:8aa5cdb4ab67 175 * Create a software representation of an e-compass.
Jonathan Austin 1:8aa5cdb4ab67 176 *
Jonathan Austin 1:8aa5cdb4ab67 177 * @param _i2c an instance of i2c, which the compass is accessible from.
Jonathan Austin 1:8aa5cdb4ab67 178 *
Jonathan Austin 1:8aa5cdb4ab67 179 * @param address the default address for the compass register on the i2c bus. Defaults to MAG3110_DEFAULT_ADDR.
Jonathan Austin 1:8aa5cdb4ab67 180 *
Jonathan Austin 1:8aa5cdb4ab67 181 * @param id the ID of the new MicroBitCompass object. Defaults to MAG3110_DEFAULT_ADDR.
Jonathan Austin 1:8aa5cdb4ab67 182 *
Jonathan Austin 1:8aa5cdb4ab67 183 * @code
Jonathan Austin 1:8aa5cdb4ab67 184 * MicroBitI2C i2c(I2C_SDA0, I2C_SCL0);
Jonathan Austin 1:8aa5cdb4ab67 185 *
Jonathan Austin 1:8aa5cdb4ab67 186 * MicroBitCompass compass(i2c);
Jonathan Austin 1:8aa5cdb4ab67 187 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 188 */
Jonathan Austin 1:8aa5cdb4ab67 189 MicroBitCompass::MicroBitCompass(MicroBitI2C& _i2c, uint16_t address, uint16_t id) :
Jonathan Austin 1:8aa5cdb4ab67 190 average(),
Jonathan Austin 1:8aa5cdb4ab67 191 sample(),
Jonathan Austin 1:8aa5cdb4ab67 192 int1(MICROBIT_PIN_COMPASS_DATA_READY),
Jonathan Austin 1:8aa5cdb4ab67 193 i2c(_i2c),
Jonathan Austin 1:8aa5cdb4ab67 194 accelerometer(NULL),
Jonathan Austin 1:8aa5cdb4ab67 195 storage(NULL)
Jonathan Austin 1:8aa5cdb4ab67 196 {
Jonathan Austin 1:8aa5cdb4ab67 197 init(id, address);
Jonathan Austin 1:8aa5cdb4ab67 198 }
Jonathan Austin 1:8aa5cdb4ab67 199
Jonathan Austin 1:8aa5cdb4ab67 200 /**
Jonathan Austin 1:8aa5cdb4ab67 201 * Issues a standard, 2 byte I2C command write to the accelerometer.
Jonathan Austin 1:8aa5cdb4ab67 202 *
Jonathan Austin 1:8aa5cdb4ab67 203 * Blocks the calling thread until complete.
Jonathan Austin 1:8aa5cdb4ab67 204 *
Jonathan Austin 1:8aa5cdb4ab67 205 * @param reg The address of the register to write to.
Jonathan Austin 1:8aa5cdb4ab67 206 *
Jonathan Austin 1:8aa5cdb4ab67 207 * @param value The value to write.
Jonathan Austin 1:8aa5cdb4ab67 208 *
Jonathan Austin 1:8aa5cdb4ab67 209 * @return MICROBIT_OK on success, MICROBIT_I2C_ERROR if the the write request failed.
Jonathan Austin 1:8aa5cdb4ab67 210 */
Jonathan Austin 1:8aa5cdb4ab67 211 int MicroBitCompass::writeCommand(uint8_t reg, uint8_t value)
Jonathan Austin 1:8aa5cdb4ab67 212 {
Jonathan Austin 1:8aa5cdb4ab67 213 uint8_t command[2];
Jonathan Austin 1:8aa5cdb4ab67 214 command[0] = reg;
Jonathan Austin 1:8aa5cdb4ab67 215 command[1] = value;
Jonathan Austin 1:8aa5cdb4ab67 216
Jonathan Austin 1:8aa5cdb4ab67 217 return i2c.write(address, (const char *)command, 2);
Jonathan Austin 1:8aa5cdb4ab67 218 }
Jonathan Austin 1:8aa5cdb4ab67 219
Jonathan Austin 1:8aa5cdb4ab67 220 /**
Jonathan Austin 1:8aa5cdb4ab67 221 * Issues a read command, copying data into the specified buffer.
Jonathan Austin 1:8aa5cdb4ab67 222 *
Jonathan Austin 1:8aa5cdb4ab67 223 * Blocks the calling thread until complete.
Jonathan Austin 1:8aa5cdb4ab67 224 *
Jonathan Austin 1:8aa5cdb4ab67 225 * @param reg The address of the register to access.
Jonathan Austin 1:8aa5cdb4ab67 226 *
Jonathan Austin 1:8aa5cdb4ab67 227 * @param buffer Memory area to read the data into.
Jonathan Austin 1:8aa5cdb4ab67 228 *
Jonathan Austin 1:8aa5cdb4ab67 229 * @param length The number of bytes to read.
Jonathan Austin 1:8aa5cdb4ab67 230 *
Jonathan Austin 1:8aa5cdb4ab67 231 * @return MICROBIT_OK on success, MICROBIT_INVALID_PARAMETER or MICROBIT_I2C_ERROR if the the read request failed.
Jonathan Austin 1:8aa5cdb4ab67 232 */
Jonathan Austin 1:8aa5cdb4ab67 233 int MicroBitCompass::readCommand(uint8_t reg, uint8_t* buffer, int length)
Jonathan Austin 1:8aa5cdb4ab67 234 {
Jonathan Austin 1:8aa5cdb4ab67 235 int result;
Jonathan Austin 1:8aa5cdb4ab67 236
Jonathan Austin 1:8aa5cdb4ab67 237 if (buffer == NULL || length <= 0)
Jonathan Austin 1:8aa5cdb4ab67 238 return MICROBIT_INVALID_PARAMETER;
Jonathan Austin 1:8aa5cdb4ab67 239
Jonathan Austin 1:8aa5cdb4ab67 240 result = i2c.write(address, (const char *)&reg, 1, true);
Jonathan Austin 1:8aa5cdb4ab67 241 if (result !=0)
Jonathan Austin 1:8aa5cdb4ab67 242 return MICROBIT_I2C_ERROR;
Jonathan Austin 1:8aa5cdb4ab67 243
Jonathan Austin 1:8aa5cdb4ab67 244 result = i2c.read(address, (char *)buffer, length);
Jonathan Austin 1:8aa5cdb4ab67 245 if (result !=0)
Jonathan Austin 1:8aa5cdb4ab67 246 return MICROBIT_I2C_ERROR;
Jonathan Austin 1:8aa5cdb4ab67 247
Jonathan Austin 1:8aa5cdb4ab67 248 return MICROBIT_OK;
Jonathan Austin 1:8aa5cdb4ab67 249 }
Jonathan Austin 1:8aa5cdb4ab67 250
Jonathan Austin 1:8aa5cdb4ab67 251
Jonathan Austin 1:8aa5cdb4ab67 252 /**
Jonathan Austin 1:8aa5cdb4ab67 253 * Issues a read of a given address, and returns the value.
Jonathan Austin 1:8aa5cdb4ab67 254 *
Jonathan Austin 1:8aa5cdb4ab67 255 * Blocks the calling thread until complete.
Jonathan Austin 1:8aa5cdb4ab67 256 *
Jonathan Austin 1:8aa5cdb4ab67 257 * @param reg The address of the 16 bit register to access.
Jonathan Austin 1:8aa5cdb4ab67 258 *
Jonathan Austin 1:8aa5cdb4ab67 259 * @return The register value, interpreted as a 16 but signed value, or MICROBIT_I2C_ERROR if the magnetometer could not be accessed.
Jonathan Austin 1:8aa5cdb4ab67 260 */
Jonathan Austin 1:8aa5cdb4ab67 261 int MicroBitCompass::read16(uint8_t reg)
Jonathan Austin 1:8aa5cdb4ab67 262 {
Jonathan Austin 1:8aa5cdb4ab67 263 uint8_t cmd[2];
Jonathan Austin 1:8aa5cdb4ab67 264 int result;
Jonathan Austin 1:8aa5cdb4ab67 265
Jonathan Austin 1:8aa5cdb4ab67 266 cmd[0] = reg;
Jonathan Austin 1:8aa5cdb4ab67 267 result = i2c.write(address, (const char *)cmd, 1);
Jonathan Austin 1:8aa5cdb4ab67 268 if (result !=0)
Jonathan Austin 1:8aa5cdb4ab67 269 return MICROBIT_I2C_ERROR;
Jonathan Austin 1:8aa5cdb4ab67 270
Jonathan Austin 1:8aa5cdb4ab67 271 cmd[0] = 0x00;
Jonathan Austin 1:8aa5cdb4ab67 272 cmd[1] = 0x00;
Jonathan Austin 1:8aa5cdb4ab67 273
Jonathan Austin 1:8aa5cdb4ab67 274 result = i2c.read(address, (char *)cmd, 2);
Jonathan Austin 1:8aa5cdb4ab67 275 if (result !=0)
Jonathan Austin 1:8aa5cdb4ab67 276 return MICROBIT_I2C_ERROR;
Jonathan Austin 1:8aa5cdb4ab67 277
Jonathan Austin 1:8aa5cdb4ab67 278 return (int16_t) ((cmd[1] | (cmd[0] << 8))); //concatenate the MSB and LSB
Jonathan Austin 1:8aa5cdb4ab67 279 }
Jonathan Austin 1:8aa5cdb4ab67 280
Jonathan Austin 1:8aa5cdb4ab67 281 /**
Jonathan Austin 1:8aa5cdb4ab67 282 * Issues a read of a given address, and returns the value.
Jonathan Austin 1:8aa5cdb4ab67 283 *
Jonathan Austin 1:8aa5cdb4ab67 284 * Blocks the calling thread until complete.
Jonathan Austin 1:8aa5cdb4ab67 285 *
Jonathan Austin 1:8aa5cdb4ab67 286 * @param reg The address of the 16 bit register to access.
Jonathan Austin 1:8aa5cdb4ab67 287 *
Jonathan Austin 1:8aa5cdb4ab67 288 * @return The register value, interpreted as a 8 bit unsigned value, or MICROBIT_I2C_ERROR if the magnetometer could not be accessed.
Jonathan Austin 1:8aa5cdb4ab67 289 */
Jonathan Austin 1:8aa5cdb4ab67 290 int MicroBitCompass::read8(uint8_t reg)
Jonathan Austin 1:8aa5cdb4ab67 291 {
Jonathan Austin 1:8aa5cdb4ab67 292 uint8_t data;
Jonathan Austin 1:8aa5cdb4ab67 293 int result;
Jonathan Austin 1:8aa5cdb4ab67 294
Jonathan Austin 1:8aa5cdb4ab67 295 data = 0;
Jonathan Austin 1:8aa5cdb4ab67 296 result = readCommand(reg, (uint8_t*) &data, 1);
Jonathan Austin 1:8aa5cdb4ab67 297 if (result != MICROBIT_OK)
Jonathan Austin 1:8aa5cdb4ab67 298 return MICROBIT_I2C_ERROR;
Jonathan Austin 1:8aa5cdb4ab67 299
Jonathan Austin 1:8aa5cdb4ab67 300 return data;
Jonathan Austin 1:8aa5cdb4ab67 301 }
Jonathan Austin 1:8aa5cdb4ab67 302
Jonathan Austin 1:8aa5cdb4ab67 303 /**
Jonathan Austin 1:8aa5cdb4ab67 304 * Calculates a tilt compensated bearing of the device, using the accelerometer.
Jonathan Austin 1:8aa5cdb4ab67 305 */
Jonathan Austin 1:8aa5cdb4ab67 306 int MicroBitCompass::tiltCompensatedBearing()
Jonathan Austin 1:8aa5cdb4ab67 307 {
Jonathan Austin 1:8aa5cdb4ab67 308 // Precompute the tilt compensation parameters to improve readability.
Jonathan Austin 1:8aa5cdb4ab67 309 float phi = accelerometer->getRollRadians();
Jonathan Austin 1:8aa5cdb4ab67 310 float theta = accelerometer->getPitchRadians();
Jonathan Austin 1:8aa5cdb4ab67 311
Jonathan Austin 1:8aa5cdb4ab67 312 float x = (float) getX(NORTH_EAST_DOWN);
Jonathan Austin 1:8aa5cdb4ab67 313 float y = (float) getY(NORTH_EAST_DOWN);
Jonathan Austin 1:8aa5cdb4ab67 314 float z = (float) getZ(NORTH_EAST_DOWN);
Jonathan Austin 1:8aa5cdb4ab67 315
Jonathan Austin 1:8aa5cdb4ab67 316 // Precompute cos and sin of pitch and roll angles to make the calculation a little more efficient.
Jonathan Austin 1:8aa5cdb4ab67 317 float sinPhi = sin(phi);
Jonathan Austin 1:8aa5cdb4ab67 318 float cosPhi = cos(phi);
Jonathan Austin 1:8aa5cdb4ab67 319 float sinTheta = sin(theta);
Jonathan Austin 1:8aa5cdb4ab67 320 float cosTheta = cos(theta);
Jonathan Austin 1:8aa5cdb4ab67 321
Jonathan Austin 1:8aa5cdb4ab67 322 float bearing = (360*atan2(z*sinPhi - y*cosPhi, x*cosTheta + y*sinTheta*sinPhi + z*sinTheta*cosPhi)) / (2*PI);
Jonathan Austin 1:8aa5cdb4ab67 323
Jonathan Austin 1:8aa5cdb4ab67 324 if (bearing < 0)
Jonathan Austin 1:8aa5cdb4ab67 325 bearing += 360.0;
Jonathan Austin 1:8aa5cdb4ab67 326
Jonathan Austin 1:8aa5cdb4ab67 327 return (int) bearing;
Jonathan Austin 1:8aa5cdb4ab67 328 }
Jonathan Austin 1:8aa5cdb4ab67 329
Jonathan Austin 1:8aa5cdb4ab67 330 /**
Jonathan Austin 1:8aa5cdb4ab67 331 * Calculates a non-tilt compensated bearing of the device.
Jonathan Austin 1:8aa5cdb4ab67 332 */
Jonathan Austin 1:8aa5cdb4ab67 333 int MicroBitCompass::basicBearing()
Jonathan Austin 1:8aa5cdb4ab67 334 {
Jonathan Austin 1:8aa5cdb4ab67 335 updateSample();
Jonathan Austin 1:8aa5cdb4ab67 336
Jonathan Austin 1:8aa5cdb4ab67 337 float bearing = (atan2((double)(sample.y - average.y),(double)(sample.x - average.x)))*180/PI;
Jonathan Austin 1:8aa5cdb4ab67 338
Jonathan Austin 1:8aa5cdb4ab67 339 if (bearing < 0)
Jonathan Austin 1:8aa5cdb4ab67 340 bearing += 360.0;
Jonathan Austin 1:8aa5cdb4ab67 341
Jonathan Austin 1:8aa5cdb4ab67 342 return (int)(360.0 - bearing);
Jonathan Austin 1:8aa5cdb4ab67 343 }
Jonathan Austin 1:8aa5cdb4ab67 344
Jonathan Austin 1:8aa5cdb4ab67 345 /**
Jonathan Austin 1:8aa5cdb4ab67 346 * Gets the current heading of the device, relative to magnetic north.
Jonathan Austin 1:8aa5cdb4ab67 347 *
Jonathan Austin 1:8aa5cdb4ab67 348 * If the compass is not calibrated, it will raise the MICROBIT_COMPASS_EVT_CALIBRATE event.
Jonathan Austin 1:8aa5cdb4ab67 349 *
Jonathan Austin 1:8aa5cdb4ab67 350 * Users wishing to implement their own calibration algorithms should listen for this event,
Jonathan Austin 1:8aa5cdb4ab67 351 * using MESSAGE_BUS_LISTENER_IMMEDIATE model. This ensures that calibration is complete before
Jonathan Austin 1:8aa5cdb4ab67 352 * the user program continues.
Jonathan Austin 1:8aa5cdb4ab67 353 *
Jonathan Austin 1:8aa5cdb4ab67 354 * @return the current heading, in degrees. Or MICROBIT_CALIBRATION_IN_PROGRESS if the compass is calibrating.
Jonathan Austin 1:8aa5cdb4ab67 355 *
Jonathan Austin 1:8aa5cdb4ab67 356 * @code
Jonathan Austin 1:8aa5cdb4ab67 357 * compass.heading();
Jonathan Austin 1:8aa5cdb4ab67 358 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 359 */
Jonathan Austin 1:8aa5cdb4ab67 360 int MicroBitCompass::heading()
Jonathan Austin 1:8aa5cdb4ab67 361 {
Jonathan Austin 1:8aa5cdb4ab67 362 if(status & MICROBIT_COMPASS_STATUS_CALIBRATING)
Jonathan Austin 1:8aa5cdb4ab67 363 return MICROBIT_CALIBRATION_IN_PROGRESS;
Jonathan Austin 1:8aa5cdb4ab67 364
Jonathan Austin 1:8aa5cdb4ab67 365 if(!(status & MICROBIT_COMPASS_STATUS_CALIBRATED))
Jonathan Austin 1:8aa5cdb4ab67 366 calibrate();
Jonathan Austin 1:8aa5cdb4ab67 367
Jonathan Austin 1:8aa5cdb4ab67 368 if(accelerometer != NULL)
Jonathan Austin 1:8aa5cdb4ab67 369 return tiltCompensatedBearing();
Jonathan Austin 1:8aa5cdb4ab67 370
Jonathan Austin 1:8aa5cdb4ab67 371 return basicBearing();
Jonathan Austin 1:8aa5cdb4ab67 372 }
Jonathan Austin 1:8aa5cdb4ab67 373
Jonathan Austin 1:8aa5cdb4ab67 374 /**
Jonathan Austin 1:8aa5cdb4ab67 375 * Updates the local sample, only if the compass indicates that
Jonathan Austin 1:8aa5cdb4ab67 376 * data is stale.
Jonathan Austin 1:8aa5cdb4ab67 377 *
Jonathan Austin 1:8aa5cdb4ab67 378 * @note Can be used to trigger manual updates, if the device is running without a scheduler.
Jonathan Austin 1:8aa5cdb4ab67 379 * Also called internally by all get[X,Y,Z]() member functions.
Jonathan Austin 1:8aa5cdb4ab67 380 */
Jonathan Austin 1:8aa5cdb4ab67 381 int MicroBitCompass::updateSample()
Jonathan Austin 1:8aa5cdb4ab67 382 {
Jonathan Austin 1:8aa5cdb4ab67 383 /**
Jonathan Austin 1:8aa5cdb4ab67 384 * Adds the compass to idle, if it hasn't been added already.
Jonathan Austin 1:8aa5cdb4ab67 385 * This is an optimisation so that the compass is only added on first 'use'.
Jonathan Austin 1:8aa5cdb4ab67 386 */
Jonathan Austin 1:8aa5cdb4ab67 387 if(!(status & MICROBIT_COMPASS_STATUS_ADDED_TO_IDLE))
Jonathan Austin 1:8aa5cdb4ab67 388 {
Jonathan Austin 1:8aa5cdb4ab67 389 fiber_add_idle_component(this);
Jonathan Austin 1:8aa5cdb4ab67 390 status |= MICROBIT_COMPASS_STATUS_ADDED_TO_IDLE;
Jonathan Austin 1:8aa5cdb4ab67 391 }
Jonathan Austin 1:8aa5cdb4ab67 392
Jonathan Austin 1:8aa5cdb4ab67 393 // Poll interrupt line from compass (Active HI).
Jonathan Austin 1:8aa5cdb4ab67 394 // Interrupt is cleared on data read of MAG_OUT_X_MSB.
Jonathan Austin 1:8aa5cdb4ab67 395 if(int1)
Jonathan Austin 1:8aa5cdb4ab67 396 {
Jonathan Austin 1:8aa5cdb4ab67 397 sample.x = MAG3110_NORMALIZE_SAMPLE((int) read16(MAG_OUT_X_MSB));
Jonathan Austin 1:8aa5cdb4ab67 398 sample.y = MAG3110_NORMALIZE_SAMPLE((int) read16(MAG_OUT_Y_MSB));
Jonathan Austin 1:8aa5cdb4ab67 399 sample.z = MAG3110_NORMALIZE_SAMPLE((int) read16(MAG_OUT_Z_MSB));
Jonathan Austin 1:8aa5cdb4ab67 400
Jonathan Austin 1:8aa5cdb4ab67 401 // Indicate that a new sample is available
Jonathan Austin 1:8aa5cdb4ab67 402 MicroBitEvent e(id, MICROBIT_COMPASS_EVT_DATA_UPDATE);
Jonathan Austin 1:8aa5cdb4ab67 403 }
Jonathan Austin 1:8aa5cdb4ab67 404
Jonathan Austin 1:8aa5cdb4ab67 405 return MICROBIT_OK;
Jonathan Austin 1:8aa5cdb4ab67 406 }
Jonathan Austin 1:8aa5cdb4ab67 407
Jonathan Austin 1:8aa5cdb4ab67 408 /**
Jonathan Austin 1:8aa5cdb4ab67 409 * Periodic callback from MicroBit idle thread.
Jonathan Austin 1:8aa5cdb4ab67 410 *
Jonathan Austin 1:8aa5cdb4ab67 411 * Calls updateSample().
Jonathan Austin 1:8aa5cdb4ab67 412 */
Jonathan Austin 1:8aa5cdb4ab67 413 void MicroBitCompass::idleTick()
Jonathan Austin 1:8aa5cdb4ab67 414 {
Jonathan Austin 1:8aa5cdb4ab67 415 updateSample();
Jonathan Austin 1:8aa5cdb4ab67 416 }
Jonathan Austin 1:8aa5cdb4ab67 417
Jonathan Austin 1:8aa5cdb4ab67 418 /**
Jonathan Austin 1:8aa5cdb4ab67 419 * Reads the value of the X axis from the latest update retrieved from the magnetometer.
Jonathan Austin 1:8aa5cdb4ab67 420 *
Jonathan Austin 1:8aa5cdb4ab67 421 * @param system The coordinate system to use. By default, a simple cartesian system is provided.
Jonathan Austin 1:8aa5cdb4ab67 422 *
Jonathan Austin 1:8aa5cdb4ab67 423 * @return The magnetic force measured in the X axis, in nano teslas.
Jonathan Austin 1:8aa5cdb4ab67 424 *
Jonathan Austin 1:8aa5cdb4ab67 425 * @code
Jonathan Austin 1:8aa5cdb4ab67 426 * compass.getX();
Jonathan Austin 1:8aa5cdb4ab67 427 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 428 */
Jonathan Austin 1:8aa5cdb4ab67 429 int MicroBitCompass::getX(MicroBitCoordinateSystem system)
Jonathan Austin 1:8aa5cdb4ab67 430 {
Jonathan Austin 1:8aa5cdb4ab67 431 updateSample();
Jonathan Austin 1:8aa5cdb4ab67 432
Jonathan Austin 1:8aa5cdb4ab67 433 switch (system)
Jonathan Austin 1:8aa5cdb4ab67 434 {
Jonathan Austin 1:8aa5cdb4ab67 435 case SIMPLE_CARTESIAN:
Jonathan Austin 1:8aa5cdb4ab67 436 return sample.x - average.x;
Jonathan Austin 1:8aa5cdb4ab67 437
Jonathan Austin 1:8aa5cdb4ab67 438 case NORTH_EAST_DOWN:
Jonathan Austin 1:8aa5cdb4ab67 439 return -(sample.y - average.y);
Jonathan Austin 1:8aa5cdb4ab67 440
Jonathan Austin 1:8aa5cdb4ab67 441 case RAW:
Jonathan Austin 1:8aa5cdb4ab67 442 default:
Jonathan Austin 1:8aa5cdb4ab67 443 return sample.x;
Jonathan Austin 1:8aa5cdb4ab67 444 }
Jonathan Austin 1:8aa5cdb4ab67 445 }
Jonathan Austin 1:8aa5cdb4ab67 446
Jonathan Austin 1:8aa5cdb4ab67 447 /**
Jonathan Austin 1:8aa5cdb4ab67 448 * Reads the value of the Y axis from the latest update retrieved from the magnetometer.
Jonathan Austin 1:8aa5cdb4ab67 449 *
Jonathan Austin 1:8aa5cdb4ab67 450 * @param system The coordinate system to use. By default, a simple cartesian system is provided.
Jonathan Austin 1:8aa5cdb4ab67 451 *
Jonathan Austin 1:8aa5cdb4ab67 452 * @return The magnetic force measured in the Y axis, in nano teslas.
Jonathan Austin 1:8aa5cdb4ab67 453 *
Jonathan Austin 1:8aa5cdb4ab67 454 * @code
Jonathan Austin 1:8aa5cdb4ab67 455 * compass.getY();
Jonathan Austin 1:8aa5cdb4ab67 456 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 457 */
Jonathan Austin 1:8aa5cdb4ab67 458 int MicroBitCompass::getY(MicroBitCoordinateSystem system)
Jonathan Austin 1:8aa5cdb4ab67 459 {
Jonathan Austin 1:8aa5cdb4ab67 460 updateSample();
Jonathan Austin 1:8aa5cdb4ab67 461
Jonathan Austin 1:8aa5cdb4ab67 462 switch (system)
Jonathan Austin 1:8aa5cdb4ab67 463 {
Jonathan Austin 1:8aa5cdb4ab67 464 case SIMPLE_CARTESIAN:
Jonathan Austin 1:8aa5cdb4ab67 465 return -(sample.y - average.y);
Jonathan Austin 1:8aa5cdb4ab67 466
Jonathan Austin 1:8aa5cdb4ab67 467 case NORTH_EAST_DOWN:
Jonathan Austin 1:8aa5cdb4ab67 468 return (sample.x - average.x);
Jonathan Austin 1:8aa5cdb4ab67 469
Jonathan Austin 1:8aa5cdb4ab67 470 case RAW:
Jonathan Austin 1:8aa5cdb4ab67 471 default:
Jonathan Austin 1:8aa5cdb4ab67 472 return sample.y;
Jonathan Austin 1:8aa5cdb4ab67 473 }
Jonathan Austin 1:8aa5cdb4ab67 474 }
Jonathan Austin 1:8aa5cdb4ab67 475
Jonathan Austin 1:8aa5cdb4ab67 476 /**
Jonathan Austin 1:8aa5cdb4ab67 477 * Reads the value of the Z axis from the latest update retrieved from the magnetometer.
Jonathan Austin 1:8aa5cdb4ab67 478 *
Jonathan Austin 1:8aa5cdb4ab67 479 * @param system The coordinate system to use. By default, a simple cartesian system is provided.
Jonathan Austin 1:8aa5cdb4ab67 480 *
Jonathan Austin 1:8aa5cdb4ab67 481 * @return The magnetic force measured in the Z axis, in nano teslas.
Jonathan Austin 1:8aa5cdb4ab67 482 *
Jonathan Austin 1:8aa5cdb4ab67 483 * @code
Jonathan Austin 1:8aa5cdb4ab67 484 * compass.getZ();
Jonathan Austin 1:8aa5cdb4ab67 485 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 486 */
Jonathan Austin 1:8aa5cdb4ab67 487 int MicroBitCompass::getZ(MicroBitCoordinateSystem system)
Jonathan Austin 1:8aa5cdb4ab67 488 {
Jonathan Austin 1:8aa5cdb4ab67 489 updateSample();
Jonathan Austin 1:8aa5cdb4ab67 490
Jonathan Austin 1:8aa5cdb4ab67 491 switch (system)
Jonathan Austin 1:8aa5cdb4ab67 492 {
Jonathan Austin 1:8aa5cdb4ab67 493 case SIMPLE_CARTESIAN:
Jonathan Austin 1:8aa5cdb4ab67 494 case NORTH_EAST_DOWN:
Jonathan Austin 1:8aa5cdb4ab67 495 return -(sample.z - average.z);
Jonathan Austin 1:8aa5cdb4ab67 496
Jonathan Austin 1:8aa5cdb4ab67 497 case RAW:
Jonathan Austin 1:8aa5cdb4ab67 498 default:
Jonathan Austin 1:8aa5cdb4ab67 499 return sample.z;
Jonathan Austin 1:8aa5cdb4ab67 500 }
Jonathan Austin 1:8aa5cdb4ab67 501 }
Jonathan Austin 1:8aa5cdb4ab67 502
Jonathan Austin 1:8aa5cdb4ab67 503 /**
Jonathan Austin 1:8aa5cdb4ab67 504 * Determines the overall magnetic field strength based on the latest update from the magnetometer.
Jonathan Austin 1:8aa5cdb4ab67 505 *
Jonathan Austin 1:8aa5cdb4ab67 506 * @return The magnetic force measured across all axis, in nano teslas.
Jonathan Austin 1:8aa5cdb4ab67 507 *
Jonathan Austin 1:8aa5cdb4ab67 508 * @code
Jonathan Austin 1:8aa5cdb4ab67 509 * compass.getFieldStrength();
Jonathan Austin 1:8aa5cdb4ab67 510 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 511 */
Jonathan Austin 1:8aa5cdb4ab67 512 int MicroBitCompass::getFieldStrength()
Jonathan Austin 1:8aa5cdb4ab67 513 {
Jonathan Austin 1:8aa5cdb4ab67 514 double x = getX();
Jonathan Austin 1:8aa5cdb4ab67 515 double y = getY();
Jonathan Austin 1:8aa5cdb4ab67 516 double z = getZ();
Jonathan Austin 1:8aa5cdb4ab67 517
Jonathan Austin 1:8aa5cdb4ab67 518 return (int) sqrt(x*x + y*y + z*z);
Jonathan Austin 1:8aa5cdb4ab67 519 }
Jonathan Austin 1:8aa5cdb4ab67 520
Jonathan Austin 1:8aa5cdb4ab67 521 /**
Jonathan Austin 1:8aa5cdb4ab67 522 * Configures the compass for the sample rate defined in this object.
Jonathan Austin 1:8aa5cdb4ab67 523 * The nearest values are chosen to those defined that are supported by the hardware.
Jonathan Austin 1:8aa5cdb4ab67 524 * The instance variables are then updated to reflect reality.
Jonathan Austin 1:8aa5cdb4ab67 525 *
Jonathan Austin 1:8aa5cdb4ab67 526 * @return MICROBIT_OK or MICROBIT_I2C_ERROR if the magnetometer could not be configured.
Jonathan Austin 1:8aa5cdb4ab67 527 */
Jonathan Austin 1:8aa5cdb4ab67 528 int MicroBitCompass::configure()
Jonathan Austin 1:8aa5cdb4ab67 529 {
Jonathan Austin 1:8aa5cdb4ab67 530 const MAG3110SampleRateConfig *actualSampleRate;
Jonathan Austin 1:8aa5cdb4ab67 531 int result;
Jonathan Austin 1:8aa5cdb4ab67 532
Jonathan Austin 1:8aa5cdb4ab67 533 // First, take the device offline, so it can be configured.
Jonathan Austin 1:8aa5cdb4ab67 534 result = writeCommand(MAG_CTRL_REG1, 0x00);
Jonathan Austin 1:8aa5cdb4ab67 535 if (result != MICROBIT_OK)
Jonathan Austin 1:8aa5cdb4ab67 536 return MICROBIT_I2C_ERROR;
Jonathan Austin 1:8aa5cdb4ab67 537
Jonathan Austin 1:8aa5cdb4ab67 538 // Wait for the part to enter standby mode...
Jonathan Austin 1:8aa5cdb4ab67 539 while(1)
Jonathan Austin 1:8aa5cdb4ab67 540 {
Jonathan Austin 1:8aa5cdb4ab67 541 // Read the status of the part...
Jonathan Austin 1:8aa5cdb4ab67 542 // If we can't communicate with it over I2C, pass on the error.
Jonathan Austin 1:8aa5cdb4ab67 543 result = this->read8(MAG_SYSMOD);
Jonathan Austin 1:8aa5cdb4ab67 544 if (result == MICROBIT_I2C_ERROR)
Jonathan Austin 1:8aa5cdb4ab67 545 return MICROBIT_I2C_ERROR;
Jonathan Austin 1:8aa5cdb4ab67 546
Jonathan Austin 1:8aa5cdb4ab67 547 // if the part in in standby, we're good to carry on.
Jonathan Austin 1:8aa5cdb4ab67 548 if((result & 0x03) == 0)
Jonathan Austin 1:8aa5cdb4ab67 549 break;
Jonathan Austin 1:8aa5cdb4ab67 550
Jonathan Austin 1:8aa5cdb4ab67 551 // Perform a power efficient sleep...
Jonathan Austin 1:8aa5cdb4ab67 552 fiber_sleep(100);
Jonathan Austin 1:8aa5cdb4ab67 553 }
Jonathan Austin 1:8aa5cdb4ab67 554
Jonathan Austin 1:8aa5cdb4ab67 555 // Find the nearest sample rate to that specified.
Jonathan Austin 1:8aa5cdb4ab67 556 actualSampleRate = &MAG3110SampleRate[MAG3110_SAMPLE_RATES-1];
Jonathan Austin 1:8aa5cdb4ab67 557 for (int i=MAG3110_SAMPLE_RATES-1; i>=0; i--)
Jonathan Austin 1:8aa5cdb4ab67 558 {
Jonathan Austin 1:8aa5cdb4ab67 559 if(MAG3110SampleRate[i].sample_period < this->samplePeriod * 1000)
Jonathan Austin 1:8aa5cdb4ab67 560 break;
Jonathan Austin 1:8aa5cdb4ab67 561
Jonathan Austin 1:8aa5cdb4ab67 562 actualSampleRate = &MAG3110SampleRate[i];
Jonathan Austin 1:8aa5cdb4ab67 563 }
Jonathan Austin 1:8aa5cdb4ab67 564
Jonathan Austin 1:8aa5cdb4ab67 565 // OK, we have the correct data. Update our local state.
Jonathan Austin 1:8aa5cdb4ab67 566 this->samplePeriod = actualSampleRate->sample_period / 1000;
Jonathan Austin 1:8aa5cdb4ab67 567
Jonathan Austin 1:8aa5cdb4ab67 568 // Enable automatic reset after each sample;
Jonathan Austin 1:8aa5cdb4ab67 569 result = writeCommand(MAG_CTRL_REG2, 0xA0);
Jonathan Austin 1:8aa5cdb4ab67 570 if (result != MICROBIT_OK)
Jonathan Austin 1:8aa5cdb4ab67 571 return MICROBIT_I2C_ERROR;
Jonathan Austin 1:8aa5cdb4ab67 572
Jonathan Austin 1:8aa5cdb4ab67 573
Jonathan Austin 1:8aa5cdb4ab67 574 // Bring the device online, with the requested sample frequency.
Jonathan Austin 1:8aa5cdb4ab67 575 result = writeCommand(MAG_CTRL_REG1, actualSampleRate->ctrl_reg1 | 0x01);
Jonathan Austin 1:8aa5cdb4ab67 576 if (result != MICROBIT_OK)
Jonathan Austin 1:8aa5cdb4ab67 577 return MICROBIT_I2C_ERROR;
Jonathan Austin 1:8aa5cdb4ab67 578
Jonathan Austin 1:8aa5cdb4ab67 579 return MICROBIT_OK;
Jonathan Austin 1:8aa5cdb4ab67 580 }
Jonathan Austin 1:8aa5cdb4ab67 581
Jonathan Austin 1:8aa5cdb4ab67 582 /**
Jonathan Austin 1:8aa5cdb4ab67 583 * Attempts to set the sample rate of the compass to the specified value (in ms).
Jonathan Austin 1:8aa5cdb4ab67 584 *
Jonathan Austin 1:8aa5cdb4ab67 585 * @param period the requested time between samples, in milliseconds.
Jonathan Austin 1:8aa5cdb4ab67 586 *
Jonathan Austin 1:8aa5cdb4ab67 587 * @return MICROBIT_OK or MICROBIT_I2C_ERROR if the magnetometer could not be updated.
Jonathan Austin 1:8aa5cdb4ab67 588 *
Jonathan Austin 1:8aa5cdb4ab67 589 * @code
Jonathan Austin 1:8aa5cdb4ab67 590 * // sample rate is now 20 ms.
Jonathan Austin 1:8aa5cdb4ab67 591 * compass.setPeriod(20);
Jonathan Austin 1:8aa5cdb4ab67 592 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 593 *
Jonathan Austin 1:8aa5cdb4ab67 594 * @note The requested rate may not be possible on the hardware. In this case, the
Jonathan Austin 1:8aa5cdb4ab67 595 * nearest lower rate is chosen.
Jonathan Austin 1:8aa5cdb4ab67 596 */
Jonathan Austin 1:8aa5cdb4ab67 597 int MicroBitCompass::setPeriod(int period)
Jonathan Austin 1:8aa5cdb4ab67 598 {
Jonathan Austin 1:8aa5cdb4ab67 599 this->samplePeriod = period;
Jonathan Austin 1:8aa5cdb4ab67 600 return this->configure();
Jonathan Austin 1:8aa5cdb4ab67 601 }
Jonathan Austin 1:8aa5cdb4ab67 602
Jonathan Austin 1:8aa5cdb4ab67 603 /**
Jonathan Austin 1:8aa5cdb4ab67 604 * Reads the currently configured sample rate of the compass.
Jonathan Austin 1:8aa5cdb4ab67 605 *
Jonathan Austin 1:8aa5cdb4ab67 606 * @return The time between samples, in milliseconds.
Jonathan Austin 1:8aa5cdb4ab67 607 */
Jonathan Austin 1:8aa5cdb4ab67 608 int MicroBitCompass::getPeriod()
Jonathan Austin 1:8aa5cdb4ab67 609 {
Jonathan Austin 1:8aa5cdb4ab67 610 return (int)samplePeriod;
Jonathan Austin 1:8aa5cdb4ab67 611 }
Jonathan Austin 1:8aa5cdb4ab67 612
Jonathan Austin 1:8aa5cdb4ab67 613 /**
Jonathan Austin 1:8aa5cdb4ab67 614 * Attempts to read the 8 bit ID from the magnetometer, this can be used for
Jonathan Austin 1:8aa5cdb4ab67 615 * validation purposes.
Jonathan Austin 1:8aa5cdb4ab67 616 *
Jonathan Austin 1:8aa5cdb4ab67 617 * @return the 8 bit ID returned by the magnetometer, or MICROBIT_I2C_ERROR if the request fails.
Jonathan Austin 1:8aa5cdb4ab67 618 *
Jonathan Austin 1:8aa5cdb4ab67 619 * @code
Jonathan Austin 1:8aa5cdb4ab67 620 * compass.whoAmI();
Jonathan Austin 1:8aa5cdb4ab67 621 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 622 */
Jonathan Austin 1:8aa5cdb4ab67 623 int MicroBitCompass::whoAmI()
Jonathan Austin 1:8aa5cdb4ab67 624 {
Jonathan Austin 1:8aa5cdb4ab67 625 uint8_t data;
Jonathan Austin 1:8aa5cdb4ab67 626 int result;
Jonathan Austin 1:8aa5cdb4ab67 627
Jonathan Austin 1:8aa5cdb4ab67 628 result = readCommand(MAG_WHOAMI, &data, 1);
Jonathan Austin 1:8aa5cdb4ab67 629 if (result != MICROBIT_OK)
Jonathan Austin 1:8aa5cdb4ab67 630 return MICROBIT_I2C_ERROR;
Jonathan Austin 1:8aa5cdb4ab67 631
Jonathan Austin 1:8aa5cdb4ab67 632 return (int)data;
Jonathan Austin 1:8aa5cdb4ab67 633 }
Jonathan Austin 1:8aa5cdb4ab67 634
Jonathan Austin 1:8aa5cdb4ab67 635 /**
Jonathan Austin 1:8aa5cdb4ab67 636 * Reads the current die temperature of the compass.
Jonathan Austin 1:8aa5cdb4ab67 637 *
Jonathan Austin 1:8aa5cdb4ab67 638 * @return the temperature in degrees celsius, or MICROBIT_I2C_ERROR if the temperature reading could not be retreived
Jonathan Austin 1:8aa5cdb4ab67 639 * from the accelerometer.
Jonathan Austin 1:8aa5cdb4ab67 640 */
Jonathan Austin 1:8aa5cdb4ab67 641 int MicroBitCompass::readTemperature()
Jonathan Austin 1:8aa5cdb4ab67 642 {
Jonathan Austin 1:8aa5cdb4ab67 643 int8_t temperature;
Jonathan Austin 1:8aa5cdb4ab67 644 int result;
Jonathan Austin 1:8aa5cdb4ab67 645
Jonathan Austin 1:8aa5cdb4ab67 646 result = readCommand(MAG_DIE_TEMP, (uint8_t *)&temperature, 1);
Jonathan Austin 1:8aa5cdb4ab67 647 if (result != MICROBIT_OK)
Jonathan Austin 1:8aa5cdb4ab67 648 return MICROBIT_I2C_ERROR;
Jonathan Austin 1:8aa5cdb4ab67 649
Jonathan Austin 1:8aa5cdb4ab67 650 return temperature;
Jonathan Austin 1:8aa5cdb4ab67 651 }
Jonathan Austin 1:8aa5cdb4ab67 652
Jonathan Austin 1:8aa5cdb4ab67 653 /**
Jonathan Austin 1:8aa5cdb4ab67 654 * Perform a calibration of the compass.
Jonathan Austin 1:8aa5cdb4ab67 655 *
Jonathan Austin 1:8aa5cdb4ab67 656 * This method will be called automatically if a user attempts to read a compass value when
Jonathan Austin 1:8aa5cdb4ab67 657 * the compass is uncalibrated. It can also be called at any time by the user.
Jonathan Austin 1:8aa5cdb4ab67 658 *
Jonathan Austin 1:8aa5cdb4ab67 659 * The method will only return once the compass has been calibrated.
Jonathan Austin 1:8aa5cdb4ab67 660 *
Jonathan Austin 1:8aa5cdb4ab67 661 * @return MICROBIT_OK, MICROBIT_I2C_ERROR if the magnetometer could not be accessed,
Jonathan Austin 1:8aa5cdb4ab67 662 * or MICROBIT_CALIBRATION_REQUIRED if the calibration algorithm failed to complete successfully.
Jonathan Austin 1:8aa5cdb4ab67 663 *
Jonathan Austin 1:8aa5cdb4ab67 664 * @note THIS MUST BE CALLED TO GAIN RELIABLE VALUES FROM THE COMPASS
Jonathan Austin 1:8aa5cdb4ab67 665 */
Jonathan Austin 1:8aa5cdb4ab67 666 int MicroBitCompass::calibrate()
Jonathan Austin 1:8aa5cdb4ab67 667 {
Jonathan Austin 1:8aa5cdb4ab67 668 // Only perform one calibration process at a time.
Jonathan Austin 1:8aa5cdb4ab67 669 if(isCalibrating())
Jonathan Austin 1:8aa5cdb4ab67 670 return MICROBIT_CALIBRATION_IN_PROGRESS;
Jonathan Austin 1:8aa5cdb4ab67 671
Jonathan Austin 1:8aa5cdb4ab67 672 updateSample();
Jonathan Austin 1:8aa5cdb4ab67 673
Jonathan Austin 1:8aa5cdb4ab67 674 // Delete old calibration data
Jonathan Austin 1:8aa5cdb4ab67 675 clearCalibration();
Jonathan Austin 1:8aa5cdb4ab67 676
Jonathan Austin 1:8aa5cdb4ab67 677 // Record that we've started calibrating.
Jonathan Austin 1:8aa5cdb4ab67 678 status |= MICROBIT_COMPASS_STATUS_CALIBRATING;
Jonathan Austin 1:8aa5cdb4ab67 679
Jonathan Austin 1:8aa5cdb4ab67 680 // Launch any registred calibration alogrithm visialisation
Jonathan Austin 1:8aa5cdb4ab67 681 MicroBitEvent(id, MICROBIT_COMPASS_EVT_CALIBRATE);
Jonathan Austin 1:8aa5cdb4ab67 682
Jonathan Austin 1:8aa5cdb4ab67 683 // Record that we've finished calibrating.
Jonathan Austin 1:8aa5cdb4ab67 684 status &= ~MICROBIT_COMPASS_STATUS_CALIBRATING;
Jonathan Austin 1:8aa5cdb4ab67 685
Jonathan Austin 1:8aa5cdb4ab67 686 // If there are no changes to our sample data, we either have no calibration algorithm, or it couldn't complete succesfully.
Jonathan Austin 1:8aa5cdb4ab67 687 if(!(status & MICROBIT_COMPASS_STATUS_CALIBRATED))
Jonathan Austin 1:8aa5cdb4ab67 688 return MICROBIT_CALIBRATION_REQUIRED;
Jonathan Austin 1:8aa5cdb4ab67 689
Jonathan Austin 1:8aa5cdb4ab67 690 return MICROBIT_OK;
Jonathan Austin 1:8aa5cdb4ab67 691 }
Jonathan Austin 1:8aa5cdb4ab67 692
Jonathan Austin 1:8aa5cdb4ab67 693 /**
Jonathan Austin 1:8aa5cdb4ab67 694 * Configure the compass to use the calibration data that is supplied to this call.
Jonathan Austin 1:8aa5cdb4ab67 695 *
Jonathan Austin 1:8aa5cdb4ab67 696 * Calibration data is comprised of the perceived zero offset of each axis of the compass.
Jonathan Austin 1:8aa5cdb4ab67 697 *
Jonathan Austin 1:8aa5cdb4ab67 698 * After calibration this should now take into account trimming errors in the magnetometer,
Jonathan Austin 1:8aa5cdb4ab67 699 * and any "hard iron" offsets on the device.
Jonathan Austin 1:8aa5cdb4ab67 700 *
Jonathan Austin 1:8aa5cdb4ab67 701 * @param calibration A CompassSample containing the offsets for the x, y and z axis.
Jonathan Austin 1:8aa5cdb4ab67 702 */
Jonathan Austin 1:8aa5cdb4ab67 703 void MicroBitCompass::setCalibration(CompassSample calibration)
Jonathan Austin 1:8aa5cdb4ab67 704 {
Jonathan Austin 1:8aa5cdb4ab67 705 if(this->storage != NULL)
LancasterUniversity 37:b624ae5e94a5 706 this->storage->put(ManagedString("compassCal"), (uint8_t *)&calibration, sizeof(CompassSample));
Jonathan Austin 1:8aa5cdb4ab67 707
Jonathan Austin 1:8aa5cdb4ab67 708 average = calibration;
Jonathan Austin 1:8aa5cdb4ab67 709 status |= MICROBIT_COMPASS_STATUS_CALIBRATED;
Jonathan Austin 1:8aa5cdb4ab67 710 }
Jonathan Austin 1:8aa5cdb4ab67 711
Jonathan Austin 1:8aa5cdb4ab67 712 /**
Jonathan Austin 1:8aa5cdb4ab67 713 * Provides the calibration data currently in use by the compass.
Jonathan Austin 1:8aa5cdb4ab67 714 *
Jonathan Austin 1:8aa5cdb4ab67 715 * More specifically, the x, y and z zero offsets of the compass.
Jonathan Austin 1:8aa5cdb4ab67 716 *
Jonathan Austin 1:8aa5cdb4ab67 717 * @return calibration A CompassSample containing the offsets for the x, y and z axis.
Jonathan Austin 1:8aa5cdb4ab67 718 */
Jonathan Austin 1:8aa5cdb4ab67 719 CompassSample MicroBitCompass::getCalibration()
Jonathan Austin 1:8aa5cdb4ab67 720 {
Jonathan Austin 1:8aa5cdb4ab67 721 return average;
Jonathan Austin 1:8aa5cdb4ab67 722 }
Jonathan Austin 1:8aa5cdb4ab67 723
Jonathan Austin 1:8aa5cdb4ab67 724 /**
Jonathan Austin 1:8aa5cdb4ab67 725 * Returns 0 or 1. 1 indicates that the compass is calibrated, zero means the compass requires calibration.
Jonathan Austin 1:8aa5cdb4ab67 726 */
Jonathan Austin 1:8aa5cdb4ab67 727 int MicroBitCompass::isCalibrated()
Jonathan Austin 1:8aa5cdb4ab67 728 {
Jonathan Austin 1:8aa5cdb4ab67 729 return status & MICROBIT_COMPASS_STATUS_CALIBRATED;
Jonathan Austin 1:8aa5cdb4ab67 730 }
Jonathan Austin 1:8aa5cdb4ab67 731
Jonathan Austin 1:8aa5cdb4ab67 732 /**
Jonathan Austin 1:8aa5cdb4ab67 733 * Returns 0 or 1. 1 indicates that the compass is calibrating, zero means the compass is not currently calibrating.
Jonathan Austin 1:8aa5cdb4ab67 734 */
Jonathan Austin 1:8aa5cdb4ab67 735 int MicroBitCompass::isCalibrating()
Jonathan Austin 1:8aa5cdb4ab67 736 {
Jonathan Austin 1:8aa5cdb4ab67 737 return status & MICROBIT_COMPASS_STATUS_CALIBRATING;
Jonathan Austin 1:8aa5cdb4ab67 738 }
Jonathan Austin 1:8aa5cdb4ab67 739
Jonathan Austin 1:8aa5cdb4ab67 740 /**
Jonathan Austin 1:8aa5cdb4ab67 741 * Clears the calibration held in persistent storage, and sets the calibrated flag to zero.
Jonathan Austin 1:8aa5cdb4ab67 742 */
Jonathan Austin 1:8aa5cdb4ab67 743 void MicroBitCompass::clearCalibration()
Jonathan Austin 1:8aa5cdb4ab67 744 {
Jonathan Austin 1:8aa5cdb4ab67 745 status &= ~MICROBIT_COMPASS_STATUS_CALIBRATED;
Jonathan Austin 1:8aa5cdb4ab67 746 }
Jonathan Austin 1:8aa5cdb4ab67 747
Jonathan Austin 1:8aa5cdb4ab67 748 /**
Jonathan Austin 1:8aa5cdb4ab67 749 * Destructor for MicroBitCompass, where we deregister this instance from the array of fiber components.
Jonathan Austin 1:8aa5cdb4ab67 750 */
Jonathan Austin 1:8aa5cdb4ab67 751 MicroBitCompass::~MicroBitCompass()
Jonathan Austin 1:8aa5cdb4ab67 752 {
Jonathan Austin 1:8aa5cdb4ab67 753 fiber_remove_idle_component(this);
Jonathan Austin 1:8aa5cdb4ab67 754 }
Jonathan Austin 1:8aa5cdb4ab67 755
Jonathan Austin 1:8aa5cdb4ab67 756 const MAG3110SampleRateConfig MAG3110SampleRate[MAG3110_SAMPLE_RATES] = {
Jonathan Austin 1:8aa5cdb4ab67 757 {12500, 0x00}, // 80 Hz
Jonathan Austin 1:8aa5cdb4ab67 758 {25000, 0x20}, // 40 Hz
Jonathan Austin 1:8aa5cdb4ab67 759 {50000, 0x40}, // 20 Hz
Jonathan Austin 1:8aa5cdb4ab67 760 {100000, 0x60}, // 10 hz
Jonathan Austin 1:8aa5cdb4ab67 761 {200000, 0x80}, // 5 hz
Jonathan Austin 1:8aa5cdb4ab67 762 {400000, 0x88}, // 2.5 hz
Jonathan Austin 1:8aa5cdb4ab67 763 {800000, 0x90}, // 1.25 hz
Jonathan Austin 1:8aa5cdb4ab67 764 {1600000, 0xb0}, // 0.63 hz
Jonathan Austin 1:8aa5cdb4ab67 765 {3200000, 0xd0}, // 0.31 hz
Jonathan Austin 1:8aa5cdb4ab67 766 {6400000, 0xf0}, // 0.16 hz
Jonathan Austin 1:8aa5cdb4ab67 767 {12800000, 0xf8} // 0.08 hz
LancasterUniversity 30:db87179335d5 768 };