HMC5883 Digital compass Library
Dependents: sensor Cansat_program Cansat_program2 compass_cal ... more
Fork of HMC5883L by
HMC5883L.cpp@5:c9ce1eeaf001, 2013-12-10 (annotated)
- Committer:
- xeta05
- Date:
- Tue Dec 10 10:58:28 2013 +0000
- Revision:
- 5:c9ce1eeaf001
- Parent:
- 4:bc4e1201e092
*Corrected the XZY order instead of the previous XYZ to match the datasheet.; * Added Declination compensation by a define
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tylerjw | 0:8b84d61cee94 | 1 | /* |
tylerjw | 0:8b84d61cee94 | 2 | * @file HMC5883L.cpp |
xeta05 | 5:c9ce1eeaf001 | 3 | * @author Oskar Lopez de Gamboa |
tylerjw | 0:8b84d61cee94 | 4 | * |
tylerjw | 0:8b84d61cee94 | 5 | * @section LICENSE |
tylerjw | 0:8b84d61cee94 | 6 | * |
tylerjw | 0:8b84d61cee94 | 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software |
tylerjw | 0:8b84d61cee94 | 8 | * and associated documentation files (the "Software"), to deal in the Software without restriction, |
tylerjw | 0:8b84d61cee94 | 9 | * including without limitation the rights to use, copy, modify, merge, publish, distribute, |
tylerjw | 0:8b84d61cee94 | 10 | * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is |
tylerjw | 0:8b84d61cee94 | 11 | * furnished to do so, subject to the following conditions: |
tylerjw | 0:8b84d61cee94 | 12 | * |
tylerjw | 0:8b84d61cee94 | 13 | * The above copyright notice and this permission notice shall be included in all copies or |
tylerjw | 0:8b84d61cee94 | 14 | * substantial portions of the Software. |
tylerjw | 0:8b84d61cee94 | 15 | * |
tylerjw | 0:8b84d61cee94 | 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING |
tylerjw | 0:8b84d61cee94 | 17 | * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND |
tylerjw | 0:8b84d61cee94 | 18 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, |
tylerjw | 0:8b84d61cee94 | 19 | * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
tylerjw | 0:8b84d61cee94 | 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. |
tylerjw | 0:8b84d61cee94 | 21 | * |
tylerjw | 0:8b84d61cee94 | 22 | * @section DESCRIPTION |
tylerjw | 0:8b84d61cee94 | 23 | * |
tylerjw | 0:8b84d61cee94 | 24 | * HMC5883L 3-Axis Digital Compas IC |
xeta05 | 5:c9ce1eeaf001 | 25 | * The library done by Tyler Weaver with: |
tylerjw | 0:8b84d61cee94 | 26 | * |
xeta05 | 5:c9ce1eeaf001 | 27 | * *Corrected the XZY order instead of the previous XYZ to match the datasheet. |
xeta05 | 5:c9ce1eeaf001 | 28 | * *Added Declination compensation by a define |
xeta05 | 5:c9ce1eeaf001 | 29 | * |
tylerjw | 0:8b84d61cee94 | 30 | */ |
tylerjw | 0:8b84d61cee94 | 31 | |
tylerjw | 0:8b84d61cee94 | 32 | #include "HMC5883L.h" |
tylerjw | 0:8b84d61cee94 | 33 | #include <new> |
tylerjw | 0:8b84d61cee94 | 34 | |
tylerjw | 0:8b84d61cee94 | 35 | HMC5883L::HMC5883L(PinName sda, PinName scl) : i2c_(*reinterpret_cast<I2C*>(i2cRaw)) |
tylerjw | 0:8b84d61cee94 | 36 | { |
tylerjw | 0:8b84d61cee94 | 37 | // Placement new to avoid additional heap memory allocation. |
tylerjw | 0:8b84d61cee94 | 38 | new(i2cRaw) I2C(sda, scl); |
tylerjw | 0:8b84d61cee94 | 39 | |
tylerjw | 0:8b84d61cee94 | 40 | init(); |
tylerjw | 0:8b84d61cee94 | 41 | } |
tylerjw | 0:8b84d61cee94 | 42 | |
tylerjw | 0:8b84d61cee94 | 43 | HMC5883L::~HMC5883L() |
tylerjw | 0:8b84d61cee94 | 44 | { |
tylerjw | 0:8b84d61cee94 | 45 | // If the I2C object is initialized in the buffer in this object, call destructor of it. |
tylerjw | 0:8b84d61cee94 | 46 | if(&i2c_ == reinterpret_cast<I2C*>(&i2cRaw)) |
tylerjw | 0:8b84d61cee94 | 47 | reinterpret_cast<I2C*>(&i2cRaw)->~I2C(); |
tylerjw | 0:8b84d61cee94 | 48 | } |
tylerjw | 0:8b84d61cee94 | 49 | |
tylerjw | 0:8b84d61cee94 | 50 | void HMC5883L::init() |
tylerjw | 0:8b84d61cee94 | 51 | { |
tylerjw | 0:8b84d61cee94 | 52 | // init - configure your setup here |
tylerjw | 0:8b84d61cee94 | 53 | setConfigurationA(AVG8_SAMPLES | OUTPUT_RATE_15); // 8 sample average, 15Hz, normal mode |
xeta05 | 5:c9ce1eeaf001 | 54 | setConfigurationB(0x20); // default gain |
tylerjw | 0:8b84d61cee94 | 55 | setMode(CONTINUOUS_MODE); // continuous sample mode |
xeta05 | 5:c9ce1eeaf001 | 56 | wait(0.006);//wait 6ms as told in the datasheet |
tylerjw | 0:8b84d61cee94 | 57 | } |
tylerjw | 0:8b84d61cee94 | 58 | |
tylerjw | 0:8b84d61cee94 | 59 | void HMC5883L::setConfigurationA(char config) |
tylerjw | 0:8b84d61cee94 | 60 | { |
tylerjw | 0:8b84d61cee94 | 61 | char cmd[2]; |
tylerjw | 0:8b84d61cee94 | 62 | cmd[0] = CONFIG_A_REG; // register a address |
tylerjw | 0:8b84d61cee94 | 63 | cmd[1] = config; |
tylerjw | 0:8b84d61cee94 | 64 | |
tylerjw | 0:8b84d61cee94 | 65 | i2c_.write(I2C_ADDRESS, cmd, 2); |
tylerjw | 0:8b84d61cee94 | 66 | } |
tylerjw | 0:8b84d61cee94 | 67 | |
tylerjw | 0:8b84d61cee94 | 68 | void HMC5883L::setConfigurationB(char config) |
tylerjw | 0:8b84d61cee94 | 69 | { |
tylerjw | 0:8b84d61cee94 | 70 | char cmd[2]; |
tylerjw | 0:8b84d61cee94 | 71 | cmd[0] = CONFIG_B_REG; // register b address |
tylerjw | 0:8b84d61cee94 | 72 | cmd[1] = config; |
tylerjw | 0:8b84d61cee94 | 73 | |
tylerjw | 0:8b84d61cee94 | 74 | i2c_.write(I2C_ADDRESS, cmd, 2); |
tylerjw | 0:8b84d61cee94 | 75 | } |
tylerjw | 0:8b84d61cee94 | 76 | |
tylerjw | 0:8b84d61cee94 | 77 | char HMC5883L::getConfigurationA() |
tylerjw | 0:8b84d61cee94 | 78 | { |
tylerjw | 0:8b84d61cee94 | 79 | char cmd[2]; |
tylerjw | 0:8b84d61cee94 | 80 | cmd[0] = CONFIG_A_REG; // register a address |
tylerjw | 0:8b84d61cee94 | 81 | i2c_.write(I2C_ADDRESS, cmd, 1, true); |
tylerjw | 0:8b84d61cee94 | 82 | i2c_.read(I2C_ADDRESS, &cmd[1], 1, false); |
tylerjw | 0:8b84d61cee94 | 83 | return cmd[1]; |
tylerjw | 0:8b84d61cee94 | 84 | } |
tylerjw | 0:8b84d61cee94 | 85 | |
tylerjw | 0:8b84d61cee94 | 86 | char HMC5883L::getConfigurationB() |
tylerjw | 0:8b84d61cee94 | 87 | { |
tylerjw | 0:8b84d61cee94 | 88 | char cmd[2]; |
tylerjw | 0:8b84d61cee94 | 89 | cmd[0] = CONFIG_A_REG; // register b address |
tylerjw | 0:8b84d61cee94 | 90 | i2c_.write(I2C_ADDRESS, cmd, 1, true); |
tylerjw | 0:8b84d61cee94 | 91 | i2c_.read(I2C_ADDRESS, &cmd[1], 1, false); |
tylerjw | 0:8b84d61cee94 | 92 | return cmd[1]; |
tylerjw | 0:8b84d61cee94 | 93 | } |
tylerjw | 0:8b84d61cee94 | 94 | |
tylerjw | 0:8b84d61cee94 | 95 | void HMC5883L::setMode(char mode = SINGLE_MODE) |
tylerjw | 0:8b84d61cee94 | 96 | { |
tylerjw | 0:8b84d61cee94 | 97 | char cmd[2]; |
tylerjw | 0:8b84d61cee94 | 98 | cmd[0] = MODE_REG; // mode register address |
tylerjw | 0:8b84d61cee94 | 99 | cmd[1] = mode; |
tylerjw | 0:8b84d61cee94 | 100 | i2c_.write(I2C_ADDRESS,cmd,2); |
tylerjw | 0:8b84d61cee94 | 101 | } |
tylerjw | 0:8b84d61cee94 | 102 | |
tylerjw | 0:8b84d61cee94 | 103 | char HMC5883L::getMode() |
tylerjw | 0:8b84d61cee94 | 104 | { |
tylerjw | 0:8b84d61cee94 | 105 | char cmd[2]; |
tylerjw | 0:8b84d61cee94 | 106 | cmd[0] = MODE_REG; // mode register |
tylerjw | 0:8b84d61cee94 | 107 | i2c_.write(I2C_ADDRESS, cmd, 1, true); |
tylerjw | 0:8b84d61cee94 | 108 | i2c_.read(I2C_ADDRESS, &cmd[1], 1, false); |
tylerjw | 0:8b84d61cee94 | 109 | return cmd[1]; |
tylerjw | 0:8b84d61cee94 | 110 | } |
tylerjw | 0:8b84d61cee94 | 111 | |
tylerjw | 0:8b84d61cee94 | 112 | char HMC5883L::getStatus() |
tylerjw | 0:8b84d61cee94 | 113 | { |
tylerjw | 0:8b84d61cee94 | 114 | char cmd[2]; |
tylerjw | 0:8b84d61cee94 | 115 | cmd[0] = STATUS_REG; // status register |
tylerjw | 0:8b84d61cee94 | 116 | i2c_.write(I2C_ADDRESS, cmd, 1, true); |
tylerjw | 0:8b84d61cee94 | 117 | i2c_.read(I2C_ADDRESS, &cmd[1], 1, false); |
tylerjw | 0:8b84d61cee94 | 118 | return cmd[1]; |
tylerjw | 0:8b84d61cee94 | 119 | } |
tylerjw | 0:8b84d61cee94 | 120 | |
tylerjw | 3:14d274e0f9de | 121 | void HMC5883L::getXYZ(int16_t output[3]) |
tylerjw | 0:8b84d61cee94 | 122 | { |
tylerjw | 0:8b84d61cee94 | 123 | char cmd[2]; |
tylerjw | 0:8b84d61cee94 | 124 | char data[6]; |
tylerjw | 0:8b84d61cee94 | 125 | cmd[0] = 0x03; // starting point for reading |
tylerjw | 0:8b84d61cee94 | 126 | i2c_.write(I2C_ADDRESS, cmd, 1, true); // set the pointer to the start of x |
tylerjw | 0:8b84d61cee94 | 127 | i2c_.read(I2C_ADDRESS, data, 6, false); |
tylerjw | 0:8b84d61cee94 | 128 | |
tylerjw | 0:8b84d61cee94 | 129 | for(int i = 0; i < 3; i++) // fill the output variables |
tylerjw | 0:8b84d61cee94 | 130 | output[i] = int16_t(((unsigned char)data[i*2] << 8) | (unsigned char)data[i*2+1]); |
tylerjw | 1:8a1357c351c6 | 131 | } |
tylerjw | 1:8a1357c351c6 | 132 | |
tylerjw | 1:8a1357c351c6 | 133 | double HMC5883L::getHeadingXY() |
tylerjw | 1:8a1357c351c6 | 134 | { |
tylerjw | 4:bc4e1201e092 | 135 | int16_t raw_data[3]; |
tylerjw | 1:8a1357c351c6 | 136 | getXYZ(raw_data); |
xeta05 | 5:c9ce1eeaf001 | 137 | //The HMC5883L gives X Z Y order |
xeta05 | 5:c9ce1eeaf001 | 138 | double heading = atan2(static_cast<double>(raw_data[2]), static_cast<double>(raw_data[0])); // heading = arctan(Y/X) |
tylerjw | 1:8a1357c351c6 | 139 | |
xeta05 | 5:c9ce1eeaf001 | 140 | |
xeta05 | 5:c9ce1eeaf001 | 141 | heading += DECLINATION_ANGLE; |
tylerjw | 1:8a1357c351c6 | 142 | |
tylerjw | 1:8a1357c351c6 | 143 | if(heading < 0.0) // fix sign |
tylerjw | 2:8eb755577f83 | 144 | heading += PI2; |
tylerjw | 1:8a1357c351c6 | 145 | |
tylerjw | 2:8eb755577f83 | 146 | if(heading > PI2) // fix overflow |
tylerjw | 2:8eb755577f83 | 147 | heading -= PI2; |
tylerjw | 1:8a1357c351c6 | 148 | |
tylerjw | 1:8a1357c351c6 | 149 | return heading; |
tylerjw | 0:8b84d61cee94 | 150 | } |