Aloïs Wolff / HK10DOF

mbed implementation of the FreeIMU IMU for HobbyKing's 10DOF board

Dependents:   testHK10DOF

HMC5883L.cpp@0:9a1682a09c50, 2013-07-17 (annotated)

Committer:
pommzorz
Date:
Wed Jul 17 18:50:28 2013 +0000
Revision:
0:9a1682a09c50
mbed implementation of the FreeIMU imu for the HobbyKing 10DOF board

Who changed what in which revision?

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