fsdfds
Dependencies: BLE_API mbed-dev-bin nRF51822
Fork of microbit-dal by
MicroBitCompass.cpp
00001 /* 00002 The MIT License (MIT) 00003 00004 Copyright (c) 2016 British Broadcasting Corporation. 00005 This software is provided by Lancaster University by arrangement with the BBC. 00006 00007 Permission is hereby granted, free of charge, to any person obtaining a 00008 copy of this software and associated documentation files (the "Software"), 00009 to deal in the Software without restriction, including without limitation 00010 the rights to use, copy, modify, merge, publish, distribute, sublicense, 00011 and/or sell copies of the Software, and to permit persons to whom the 00012 Software is furnished to do so, subject to the following conditions: 00013 00014 The above copyright notice and this permission notice shall be included in 00015 all copies or substantial portions of the Software. 00016 00017 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 00018 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 00019 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 00020 THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 00021 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 00022 FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 00023 DEALINGS IN THE SOFTWARE. 00024 */ 00025 00026 /** 00027 * Class definition for MicroBit Compass. 00028 * 00029 * Represents an implementation of the Freescale MAG3110 I2C Magnetmometer. 00030 * Also includes basic caching, calibration and on demand activation. 00031 */ 00032 #include "MicroBitConfig.h" 00033 #include "MicroBitCompass.h" 00034 #include "MicroBitFiber.h" 00035 #include "ErrorNo.h" 00036 00037 /** 00038 * An initialisation member function used by the many constructors of MicroBitCompass. 00039 * 00040 * @param id the unique identifier for this compass instance. 00041 * 00042 * @param address the base address of the magnetometer on the i2c bus. 00043 */ 00044 void MicroBitCompass::init(uint16_t id, uint16_t address) 00045 { 00046 this->id = id; 00047 this->address = address; 00048 00049 // Select 10Hz update rate, with oversampling, and enable the device. 00050 this->samplePeriod = 100; 00051 this->configure(); 00052 00053 // Assume that we have no calibration information. 00054 status &= ~MICROBIT_COMPASS_STATUS_CALIBRATED; 00055 00056 if(this->storage != NULL) 00057 { 00058 KeyValuePair *calibrationData = storage->get("compassCal"); 00059 00060 if(calibrationData != NULL) 00061 { 00062 CompassSample storedSample = CompassSample(); 00063 00064 memcpy(&storedSample, calibrationData->value, sizeof(CompassSample)); 00065 00066 setCalibration(storedSample); 00067 00068 delete calibrationData; 00069 } 00070 } 00071 00072 // Indicate that we're up and running. 00073 status |= MICROBIT_COMPONENT_RUNNING; 00074 } 00075 00076 /** 00077 * Constructor. 00078 * Create a software representation of an e-compass. 00079 * 00080 * @param _i2c an instance of i2c, which the compass is accessible from. 00081 * 00082 * @param _accelerometer an instance of the accelerometer, used for tilt compensation. 00083 * 00084 * @param _storage an instance of MicroBitStorage, used to persist calibration data across resets. 00085 * 00086 * @param address the default address for the compass register on the i2c bus. Defaults to MAG3110_DEFAULT_ADDR. 00087 * 00088 * @param id the ID of the new MicroBitCompass object. Defaults to MAG3110_DEFAULT_ADDR. 00089 * 00090 * @code 00091 * MicroBitI2C i2c(I2C_SDA0, I2C_SCL0); 00092 * 00093 * MicroBitAccelerometer accelerometer(i2c); 00094 * 00095 * MicroBitStorage storage; 00096 * 00097 * MicroBitCompass compass(i2c, accelerometer, storage); 00098 * @endcode 00099 */ 00100 MicroBitCompass::MicroBitCompass(MicroBitI2C& _i2c, MicroBitAccelerometer& _accelerometer, MicroBitStorage& _storage, uint16_t address, uint16_t id) : 00101 average(), 00102 sample(), 00103 int1(MICROBIT_PIN_COMPASS_DATA_READY), 00104 i2c(_i2c), 00105 accelerometer(&_accelerometer), 00106 storage(&_storage) 00107 { 00108 init(id, address); 00109 } 00110 00111 /** 00112 * Constructor. 00113 * Create a software representation of an e-compass. 00114 * 00115 * @param _i2c an instance of i2c, which the compass is accessible from. 00116 * 00117 * @param _accelerometer an instance of the accelerometer, used for tilt compensation. 00118 * 00119 * @param address the default address for the compass register on the i2c bus. Defaults to MAG3110_DEFAULT_ADDR. 00120 * 00121 * @param id the ID of the new MicroBitCompass object. Defaults to MAG3110_DEFAULT_ADDR. 00122 * 00123 * @code 00124 * MicroBitI2C i2c(I2C_SDA0, I2C_SCL0); 00125 * 00126 * MicroBitAccelerometer accelerometer(i2c); 00127 * 00128 * MicroBitCompass compass(i2c, accelerometer, storage); 00129 * @endcode 00130 */ 00131 MicroBitCompass::MicroBitCompass(MicroBitI2C& _i2c, MicroBitAccelerometer& _accelerometer, uint16_t address, uint16_t id) : 00132 average(), 00133 sample(), 00134 int1(MICROBIT_PIN_COMPASS_DATA_READY), 00135 i2c(_i2c), 00136 accelerometer(&_accelerometer), 00137 storage(NULL) 00138 { 00139 init(id, address); 00140 } 00141 00142 /** 00143 * Constructor. 00144 * Create a software representation of an e-compass. 00145 * 00146 * @param _i2c an instance of i2c, which the compass is accessible from. 00147 * 00148 * @param _storage an instance of MicroBitStorage, used to persist calibration data across resets. 00149 * 00150 * @param address the default address for the compass register on the i2c bus. Defaults to MAG3110_DEFAULT_ADDR. 00151 * 00152 * @param id the ID of the new MicroBitCompass object. Defaults to MAG3110_DEFAULT_ADDR. 00153 * 00154 * @code 00155 * MicroBitI2C i2c(I2C_SDA0, I2C_SCL0); 00156 * 00157 * MicroBitStorage storage; 00158 * 00159 * MicroBitCompass compass(i2c, storage); 00160 * @endcode 00161 */ 00162 MicroBitCompass::MicroBitCompass(MicroBitI2C& _i2c, MicroBitStorage& _storage, uint16_t address, uint16_t id) : 00163 average(), 00164 sample(), 00165 int1(MICROBIT_PIN_COMPASS_DATA_READY), 00166 i2c(_i2c), 00167 accelerometer(NULL), 00168 storage(&_storage) 00169 { 00170 init(id, address); 00171 } 00172 00173 /** 00174 * Constructor. 00175 * Create a software representation of an e-compass. 00176 * 00177 * @param _i2c an instance of i2c, which the compass is accessible from. 00178 * 00179 * @param address the default address for the compass register on the i2c bus. Defaults to MAG3110_DEFAULT_ADDR. 00180 * 00181 * @param id the ID of the new MicroBitCompass object. Defaults to MAG3110_DEFAULT_ADDR. 00182 * 00183 * @code 00184 * MicroBitI2C i2c(I2C_SDA0, I2C_SCL0); 00185 * 00186 * MicroBitCompass compass(i2c); 00187 * @endcode 00188 */ 00189 MicroBitCompass::MicroBitCompass(MicroBitI2C& _i2c, uint16_t address, uint16_t id) : 00190 average(), 00191 sample(), 00192 int1(MICROBIT_PIN_COMPASS_DATA_READY), 00193 i2c(_i2c), 00194 accelerometer(NULL), 00195 storage(NULL) 00196 { 00197 init(id, address); 00198 } 00199 00200 /** 00201 * Issues a standard, 2 byte I2C command write to the accelerometer. 00202 * 00203 * Blocks the calling thread until complete. 00204 * 00205 * @param reg The address of the register to write to. 00206 * 00207 * @param value The value to write. 00208 * 00209 * @return MICROBIT_OK on success, MICROBIT_I2C_ERROR if the the write request failed. 00210 */ 00211 int MicroBitCompass::writeCommand(uint8_t reg, uint8_t value) 00212 { 00213 uint8_t command[2]; 00214 command[0] = reg; 00215 command[1] = value; 00216 00217 return i2c.write(address, (const char *)command, 2); 00218 } 00219 00220 /** 00221 * Issues a read command, copying data into the specified buffer. 00222 * 00223 * Blocks the calling thread until complete. 00224 * 00225 * @param reg The address of the register to access. 00226 * 00227 * @param buffer Memory area to read the data into. 00228 * 00229 * @param length The number of bytes to read. 00230 * 00231 * @return MICROBIT_OK on success, MICROBIT_INVALID_PARAMETER or MICROBIT_I2C_ERROR if the the read request failed. 00232 */ 00233 int MicroBitCompass::readCommand(uint8_t reg, uint8_t* buffer, int length) 00234 { 00235 int result; 00236 00237 if (buffer == NULL || length <= 0) 00238 return MICROBIT_INVALID_PARAMETER; 00239 00240 result = i2c.write(address, (const char *)®, 1, true); 00241 if (result !=0) 00242 return MICROBIT_I2C_ERROR; 00243 00244 result = i2c.read(address, (char *)buffer, length); 00245 if (result !=0) 00246 return MICROBIT_I2C_ERROR; 00247 00248 return MICROBIT_OK; 00249 } 00250 00251 00252 /** 00253 * Issues a read of a given address, and returns the value. 00254 * 00255 * Blocks the calling thread until complete. 00256 * 00257 * @param reg The address of the 16 bit register to access. 00258 * 00259 * @return The register value, interpreted as a 16 but signed value, or MICROBIT_I2C_ERROR if the magnetometer could not be accessed. 00260 */ 00261 int MicroBitCompass::read16(uint8_t reg) 00262 { 00263 uint8_t cmd[2]; 00264 int result; 00265 00266 cmd[0] = reg; 00267 result = i2c.write(address, (const char *)cmd, 1); 00268 if (result !=0) 00269 return MICROBIT_I2C_ERROR; 00270 00271 cmd[0] = 0x00; 00272 cmd[1] = 0x00; 00273 00274 result = i2c.read(address, (char *)cmd, 2); 00275 if (result !=0) 00276 return MICROBIT_I2C_ERROR; 00277 00278 return (int16_t) ((cmd[1] | (cmd[0] << 8))); //concatenate the MSB and LSB 00279 } 00280 00281 /** 00282 * Issues a read of a given address, and returns the value. 00283 * 00284 * Blocks the calling thread until complete. 00285 * 00286 * @param reg The address of the 16 bit register to access. 00287 * 00288 * @return The register value, interpreted as a 8 bit unsigned value, or MICROBIT_I2C_ERROR if the magnetometer could not be accessed. 00289 */ 00290 int MicroBitCompass::read8(uint8_t reg) 00291 { 00292 uint8_t data; 00293 int result; 00294 00295 data = 0; 00296 result = readCommand(reg, (uint8_t*) &data, 1); 00297 if (result != MICROBIT_OK) 00298 return MICROBIT_I2C_ERROR; 00299 00300 return data; 00301 } 00302 00303 /** 00304 * Calculates a tilt compensated bearing of the device, using the accelerometer. 00305 */ 00306 int MicroBitCompass::tiltCompensatedBearing() 00307 { 00308 // Precompute the tilt compensation parameters to improve readability. 00309 float phi = accelerometer->getRollRadians(); 00310 float theta = accelerometer->getPitchRadians(); 00311 00312 float x = (float) getX(NORTH_EAST_DOWN); 00313 float y = (float) getY(NORTH_EAST_DOWN); 00314 float z = (float) getZ(NORTH_EAST_DOWN); 00315 00316 // Precompute cos and sin of pitch and roll angles to make the calculation a little more efficient. 00317 float sinPhi = sin(phi); 00318 float cosPhi = cos(phi); 00319 float sinTheta = sin(theta); 00320 float cosTheta = cos(theta); 00321 00322 float bearing = (360*atan2(z*sinPhi - y*cosPhi, x*cosTheta + y*sinTheta*sinPhi + z*sinTheta*cosPhi)) / (2*PI); 00323 00324 if (bearing < 0) 00325 bearing += 360.0; 00326 00327 return (int) bearing; 00328 } 00329 00330 /** 00331 * Calculates a non-tilt compensated bearing of the device. 00332 */ 00333 int MicroBitCompass::basicBearing() 00334 { 00335 updateSample(); 00336 00337 float bearing = (atan2((double)(sample.y - average.y),(double)(sample.x - average.x)))*180/PI; 00338 00339 if (bearing < 0) 00340 bearing += 360.0; 00341 00342 return (int)(360.0 - bearing); 00343 } 00344 00345 /** 00346 * Gets the current heading of the device, relative to magnetic north. 00347 * 00348 * If the compass is not calibrated, it will raise the MICROBIT_COMPASS_EVT_CALIBRATE event. 00349 * 00350 * Users wishing to implement their own calibration algorithms should listen for this event, 00351 * using MESSAGE_BUS_LISTENER_IMMEDIATE model. This ensures that calibration is complete before 00352 * the user program continues. 00353 * 00354 * @return the current heading, in degrees. Or MICROBIT_CALIBRATION_IN_PROGRESS if the compass is calibrating. 00355 * 00356 * @code 00357 * compass.heading(); 00358 * @endcode 00359 */ 00360 int MicroBitCompass::heading() 00361 { 00362 if(status & MICROBIT_COMPASS_STATUS_CALIBRATING) 00363 return MICROBIT_CALIBRATION_IN_PROGRESS; 00364 00365 if(!(status & MICROBIT_COMPASS_STATUS_CALIBRATED)) 00366 calibrate(); 00367 00368 if(accelerometer != NULL) 00369 return tiltCompensatedBearing(); 00370 00371 return basicBearing(); 00372 } 00373 00374 /** 00375 * Updates the local sample, only if the compass indicates that 00376 * data is stale. 00377 * 00378 * @note Can be used to trigger manual updates, if the device is running without a scheduler. 00379 * Also called internally by all get[X,Y,Z]() member functions. 00380 */ 00381 int MicroBitCompass::updateSample() 00382 { 00383 /** 00384 * Adds the compass to idle, if it hasn't been added already. 00385 * This is an optimisation so that the compass is only added on first 'use'. 00386 */ 00387 if(!(status & MICROBIT_COMPASS_STATUS_ADDED_TO_IDLE)) 00388 { 00389 fiber_add_idle_component(this); 00390 status |= MICROBIT_COMPASS_STATUS_ADDED_TO_IDLE; 00391 } 00392 00393 // Poll interrupt line from compass (Active HI). 00394 // Interrupt is cleared on data read of MAG_OUT_X_MSB. 00395 if(int1) 00396 { 00397 sample.x = MAG3110_NORMALIZE_SAMPLE((int) read16(MAG_OUT_X_MSB)); 00398 sample.y = MAG3110_NORMALIZE_SAMPLE((int) read16(MAG_OUT_Y_MSB)); 00399 sample.z = MAG3110_NORMALIZE_SAMPLE((int) read16(MAG_OUT_Z_MSB)); 00400 00401 // Indicate that a new sample is available 00402 MicroBitEvent e(id, MICROBIT_COMPASS_EVT_DATA_UPDATE); 00403 } 00404 00405 return MICROBIT_OK; 00406 } 00407 00408 /** 00409 * Periodic callback from MicroBit idle thread. 00410 * 00411 * Calls updateSample(). 00412 */ 00413 void MicroBitCompass::idleTick() 00414 { 00415 updateSample(); 00416 } 00417 00418 /** 00419 * Reads the value of the X axis from the latest update retrieved from the magnetometer. 00420 * 00421 * @param system The coordinate system to use. By default, a simple cartesian system is provided. 00422 * 00423 * @return The magnetic force measured in the X axis, in nano teslas. 00424 * 00425 * @code 00426 * compass.getX(); 00427 * @endcode 00428 */ 00429 int MicroBitCompass::getX(MicroBitCoordinateSystem system) 00430 { 00431 updateSample(); 00432 00433 switch (system) 00434 { 00435 case SIMPLE_CARTESIAN: 00436 return sample.x - average.x; 00437 00438 case NORTH_EAST_DOWN: 00439 return -(sample.y - average.y); 00440 00441 case RAW: 00442 default: 00443 return sample.x; 00444 } 00445 } 00446 00447 /** 00448 * Reads the value of the Y axis from the latest update retrieved from the magnetometer. 00449 * 00450 * @param system The coordinate system to use. By default, a simple cartesian system is provided. 00451 * 00452 * @return The magnetic force measured in the Y axis, in nano teslas. 00453 * 00454 * @code 00455 * compass.getY(); 00456 * @endcode 00457 */ 00458 int MicroBitCompass::getY(MicroBitCoordinateSystem system) 00459 { 00460 updateSample(); 00461 00462 switch (system) 00463 { 00464 case SIMPLE_CARTESIAN: 00465 return -(sample.y - average.y); 00466 00467 case NORTH_EAST_DOWN: 00468 return (sample.x - average.x); 00469 00470 case RAW: 00471 default: 00472 return sample.y; 00473 } 00474 } 00475 00476 /** 00477 * Reads the value of the Z axis from the latest update retrieved from the magnetometer. 00478 * 00479 * @param system The coordinate system to use. By default, a simple cartesian system is provided. 00480 * 00481 * @return The magnetic force measured in the Z axis, in nano teslas. 00482 * 00483 * @code 00484 * compass.getZ(); 00485 * @endcode 00486 */ 00487 int MicroBitCompass::getZ(MicroBitCoordinateSystem system) 00488 { 00489 updateSample(); 00490 00491 switch (system) 00492 { 00493 case SIMPLE_CARTESIAN: 00494 case NORTH_EAST_DOWN: 00495 return -(sample.z - average.z); 00496 00497 case RAW: 00498 default: 00499 return sample.z; 00500 } 00501 } 00502 00503 /** 00504 * Determines the overall magnetic field strength based on the latest update from the magnetometer. 00505 * 00506 * @return The magnetic force measured across all axis, in nano teslas. 00507 * 00508 * @code 00509 * compass.getFieldStrength(); 00510 * @endcode 00511 */ 00512 int MicroBitCompass::getFieldStrength() 00513 { 00514 double x = getX(); 00515 double y = getY(); 00516 double z = getZ(); 00517 00518 return (int) sqrt(x*x + y*y + z*z); 00519 } 00520 00521 /** 00522 * Configures the compass for the sample rate defined in this object. 00523 * The nearest values are chosen to those defined that are supported by the hardware. 00524 * The instance variables are then updated to reflect reality. 00525 * 00526 * @return MICROBIT_OK or MICROBIT_I2C_ERROR if the magnetometer could not be configured. 00527 */ 00528 int MicroBitCompass::configure() 00529 { 00530 const MAG3110SampleRateConfig *actualSampleRate; 00531 int result; 00532 00533 // First, take the device offline, so it can be configured. 00534 result = writeCommand(MAG_CTRL_REG1, 0x00); 00535 if (result != MICROBIT_OK) 00536 return MICROBIT_I2C_ERROR; 00537 00538 // Wait for the part to enter standby mode... 00539 while(1) 00540 { 00541 // Read the status of the part... 00542 // If we can't communicate with it over I2C, pass on the error. 00543 result = this->read8(MAG_SYSMOD); 00544 if (result == MICROBIT_I2C_ERROR) 00545 return MICROBIT_I2C_ERROR; 00546 00547 // if the part in in standby, we're good to carry on. 00548 if((result & 0x03) == 0) 00549 break; 00550 00551 // Perform a power efficient sleep... 00552 fiber_sleep(100); 00553 } 00554 00555 // Find the nearest sample rate to that specified. 00556 actualSampleRate = &MAG3110SampleRate[MAG3110_SAMPLE_RATES-1]; 00557 for (int i=MAG3110_SAMPLE_RATES-1; i>=0; i--) 00558 { 00559 if(MAG3110SampleRate[i].sample_period < this->samplePeriod * 1000) 00560 break; 00561 00562 actualSampleRate = &MAG3110SampleRate[i]; 00563 } 00564 00565 // OK, we have the correct data. Update our local state. 00566 this->samplePeriod = actualSampleRate->sample_period / 1000; 00567 00568 // Enable automatic reset after each sample; 00569 result = writeCommand(MAG_CTRL_REG2, 0xA0); 00570 if (result != MICROBIT_OK) 00571 return MICROBIT_I2C_ERROR; 00572 00573 00574 // Bring the device online, with the requested sample frequency. 00575 result = writeCommand(MAG_CTRL_REG1, actualSampleRate->ctrl_reg1 | 0x01); 00576 if (result != MICROBIT_OK) 00577 return MICROBIT_I2C_ERROR; 00578 00579 return MICROBIT_OK; 00580 } 00581 00582 /** 00583 * Attempts to set the sample rate of the compass to the specified value (in ms). 00584 * 00585 * @param period the requested time between samples, in milliseconds. 00586 * 00587 * @return MICROBIT_OK or MICROBIT_I2C_ERROR if the magnetometer could not be updated. 00588 * 00589 * @code 00590 * // sample rate is now 20 ms. 00591 * compass.setPeriod(20); 00592 * @endcode 00593 * 00594 * @note The requested rate may not be possible on the hardware. In this case, the 00595 * nearest lower rate is chosen. 00596 */ 00597 int MicroBitCompass::setPeriod(int period) 00598 { 00599 this->samplePeriod = period; 00600 return this->configure(); 00601 } 00602 00603 /** 00604 * Reads the currently configured sample rate of the compass. 00605 * 00606 * @return The time between samples, in milliseconds. 00607 */ 00608 int MicroBitCompass::getPeriod() 00609 { 00610 return (int)samplePeriod; 00611 } 00612 00613 /** 00614 * Attempts to read the 8 bit ID from the magnetometer, this can be used for 00615 * validation purposes. 00616 * 00617 * @return the 8 bit ID returned by the magnetometer, or MICROBIT_I2C_ERROR if the request fails. 00618 * 00619 * @code 00620 * compass.whoAmI(); 00621 * @endcode 00622 */ 00623 int MicroBitCompass::whoAmI() 00624 { 00625 uint8_t data; 00626 int result; 00627 00628 result = readCommand(MAG_WHOAMI, &data, 1); 00629 if (result != MICROBIT_OK) 00630 return MICROBIT_I2C_ERROR; 00631 00632 return (int)data; 00633 } 00634 00635 /** 00636 * Reads the current die temperature of the compass. 00637 * 00638 * @return the temperature in degrees celsius, or MICROBIT_I2C_ERROR if the temperature reading could not be retreived 00639 * from the accelerometer. 00640 */ 00641 int MicroBitCompass::readTemperature() 00642 { 00643 int8_t temperature; 00644 int result; 00645 00646 result = readCommand(MAG_DIE_TEMP, (uint8_t *)&temperature, 1); 00647 if (result != MICROBIT_OK) 00648 return MICROBIT_I2C_ERROR; 00649 00650 return temperature; 00651 } 00652 00653 /** 00654 * Perform a calibration of the compass. 00655 * 00656 * This method will be called automatically if a user attempts to read a compass value when 00657 * the compass is uncalibrated. It can also be called at any time by the user. 00658 * 00659 * The method will only return once the compass has been calibrated. 00660 * 00661 * @return MICROBIT_OK, MICROBIT_I2C_ERROR if the magnetometer could not be accessed, 00662 * or MICROBIT_CALIBRATION_REQUIRED if the calibration algorithm failed to complete successfully. 00663 * 00664 * @note THIS MUST BE CALLED TO GAIN RELIABLE VALUES FROM THE COMPASS 00665 */ 00666 int MicroBitCompass::calibrate() 00667 { 00668 // Only perform one calibration process at a time. 00669 if(isCalibrating()) 00670 return MICROBIT_CALIBRATION_IN_PROGRESS; 00671 00672 updateSample(); 00673 00674 // Delete old calibration data 00675 clearCalibration(); 00676 00677 // Record that we've started calibrating. 00678 status |= MICROBIT_COMPASS_STATUS_CALIBRATING; 00679 00680 // Launch any registred calibration alogrithm visialisation 00681 MicroBitEvent(id, MICROBIT_COMPASS_EVT_CALIBRATE); 00682 00683 // Record that we've finished calibrating. 00684 status &= ~MICROBIT_COMPASS_STATUS_CALIBRATING; 00685 00686 // If there are no changes to our sample data, we either have no calibration algorithm, or it couldn't complete succesfully. 00687 if(!(status & MICROBIT_COMPASS_STATUS_CALIBRATED)) 00688 return MICROBIT_CALIBRATION_REQUIRED; 00689 00690 return MICROBIT_OK; 00691 } 00692 00693 /** 00694 * Configure the compass to use the calibration data that is supplied to this call. 00695 * 00696 * Calibration data is comprised of the perceived zero offset of each axis of the compass. 00697 * 00698 * After calibration this should now take into account trimming errors in the magnetometer, 00699 * and any "hard iron" offsets on the device. 00700 * 00701 * @param calibration A CompassSample containing the offsets for the x, y and z axis. 00702 */ 00703 void MicroBitCompass::setCalibration(CompassSample calibration) 00704 { 00705 if(this->storage != NULL) 00706 this->storage->put(ManagedString("compassCal"), (uint8_t *)&calibration, sizeof(CompassSample)); 00707 00708 average = calibration; 00709 status |= MICROBIT_COMPASS_STATUS_CALIBRATED; 00710 } 00711 00712 /** 00713 * Provides the calibration data currently in use by the compass. 00714 * 00715 * More specifically, the x, y and z zero offsets of the compass. 00716 * 00717 * @return calibration A CompassSample containing the offsets for the x, y and z axis. 00718 */ 00719 CompassSample MicroBitCompass::getCalibration() 00720 { 00721 return average; 00722 } 00723 00724 /** 00725 * Returns 0 or 1. 1 indicates that the compass is calibrated, zero means the compass requires calibration. 00726 */ 00727 int MicroBitCompass::isCalibrated() 00728 { 00729 return status & MICROBIT_COMPASS_STATUS_CALIBRATED; 00730 } 00731 00732 /** 00733 * Returns 0 or 1. 1 indicates that the compass is calibrating, zero means the compass is not currently calibrating. 00734 */ 00735 int MicroBitCompass::isCalibrating() 00736 { 00737 return status & MICROBIT_COMPASS_STATUS_CALIBRATING; 00738 } 00739 00740 /** 00741 * Clears the calibration held in persistent storage, and sets the calibrated flag to zero. 00742 */ 00743 void MicroBitCompass::clearCalibration() 00744 { 00745 status &= ~MICROBIT_COMPASS_STATUS_CALIBRATED; 00746 } 00747 00748 /** 00749 * Destructor for MicroBitCompass, where we deregister this instance from the array of fiber components. 00750 */ 00751 MicroBitCompass::~MicroBitCompass() 00752 { 00753 fiber_remove_idle_component(this); 00754 } 00755 00756 const MAG3110SampleRateConfig MAG3110SampleRate[MAG3110_SAMPLE_RATES] = { 00757 {12500, 0x00}, // 80 Hz 00758 {25000, 0x20}, // 40 Hz 00759 {50000, 0x40}, // 20 Hz 00760 {100000, 0x60}, // 10 hz 00761 {200000, 0x80}, // 5 hz 00762 {400000, 0x88}, // 2.5 hz 00763 {800000, 0x90}, // 1.25 hz 00764 {1600000, 0xb0}, // 0.63 hz 00765 {3200000, 0xd0}, // 0.31 hz 00766 {6400000, 0xf0}, // 0.16 hz 00767 {12800000, 0xf8} // 0.08 hz 00768 };
Generated on Wed Jul 13 2022 00:58:03 by 1.7.2