IMU 10dof MEMS from DR Robot adapted from HK10DOF Changed gyro to ITG3200

Fork of HK10DOF by Aloïs Wolff

WARNING: This project is not complete, but this library seems ok so far.

I have the module DFRobotics.com 10DOF MEMS IMU. I wanted a concise module for resolving direction and movement.

I found HK10DOF library (http://developer.mbed.org/users/pommzorz/code/HK10DOF/) with quaternions. But it used a different gyro. So I modified that code to use the same higher level calls but use the ITG3200 low level calls.

Committer:
svkatielee
Date:
Tue Nov 18 06:28:56 2014 +0000
Revision:
4:c4db4e2ffdd7
Parent:
3:450acaa4f52d
Changed gyro sensor to ITG3200

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 {
svkatielee 3:450acaa4f52d 52 i2c_.frequency(50000);
pommzorz 0:9a1682a09c50 53 // init - configure your setup here
pommzorz 0:9a1682a09c50 54 setConfigurationA(AVG8_SAMPLES | OUTPUT_RATE_15); // 8 sample average, 15Hz, normal mode
pommzorz 0:9a1682a09c50 55 setConfigurationB(0x20); // default
pommzorz 0:9a1682a09c50 56 setMode(CONTINUOUS_MODE); // continuous sample mode
pommzorz 0:9a1682a09c50 57 }
pommzorz 0:9a1682a09c50 58
pommzorz 0:9a1682a09c50 59 void HMC5883L::setConfigurationA(char config)
pommzorz 0:9a1682a09c50 60 {
pommzorz 0:9a1682a09c50 61 char cmd[2];
pommzorz 0:9a1682a09c50 62 cmd[0] = CONFIG_A_REG; // register a address
pommzorz 0:9a1682a09c50 63 cmd[1] = config;
pommzorz 0:9a1682a09c50 64
pommzorz 0:9a1682a09c50 65 i2c_.write(I2C_ADDRESS, cmd, 2);
pommzorz 0:9a1682a09c50 66 }
pommzorz 0:9a1682a09c50 67
pommzorz 0:9a1682a09c50 68 void HMC5883L::setConfigurationB(char config)
pommzorz 0:9a1682a09c50 69 {
pommzorz 0:9a1682a09c50 70 char cmd[2];
pommzorz 0:9a1682a09c50 71 cmd[0] = CONFIG_B_REG; // register b address
pommzorz 0:9a1682a09c50 72 cmd[1] = config;
pommzorz 0:9a1682a09c50 73
pommzorz 0:9a1682a09c50 74 i2c_.write(I2C_ADDRESS, cmd, 2);
pommzorz 0:9a1682a09c50 75 }
pommzorz 0:9a1682a09c50 76
pommzorz 0:9a1682a09c50 77 char HMC5883L::getConfigurationA()
pommzorz 0:9a1682a09c50 78 {
pommzorz 0:9a1682a09c50 79 char cmd[2];
pommzorz 0:9a1682a09c50 80 cmd[0] = CONFIG_A_REG; // register a address
pommzorz 0:9a1682a09c50 81 i2c_.write(I2C_ADDRESS, cmd, 1, true);
pommzorz 0:9a1682a09c50 82 i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
pommzorz 0:9a1682a09c50 83 return cmd[1];
pommzorz 0:9a1682a09c50 84 }
pommzorz 0:9a1682a09c50 85
pommzorz 0:9a1682a09c50 86 char HMC5883L::getConfigurationB()
pommzorz 0:9a1682a09c50 87 {
pommzorz 0:9a1682a09c50 88 char cmd[2];
pommzorz 0:9a1682a09c50 89 cmd[0] = CONFIG_A_REG; // register b address
pommzorz 0:9a1682a09c50 90 i2c_.write(I2C_ADDRESS, cmd, 1, true);
pommzorz 0:9a1682a09c50 91 i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
pommzorz 0:9a1682a09c50 92 return cmd[1];
pommzorz 0:9a1682a09c50 93 }
pommzorz 0:9a1682a09c50 94
pommzorz 0:9a1682a09c50 95 void HMC5883L::setMode(char mode = SINGLE_MODE)
pommzorz 0:9a1682a09c50 96 {
pommzorz 0:9a1682a09c50 97 char cmd[2];
pommzorz 0:9a1682a09c50 98 cmd[0] = MODE_REG; // mode register address
pommzorz 0:9a1682a09c50 99 cmd[1] = mode;
pommzorz 0:9a1682a09c50 100 i2c_.write(I2C_ADDRESS,cmd,2);
pommzorz 0:9a1682a09c50 101 }
pommzorz 0:9a1682a09c50 102
pommzorz 0:9a1682a09c50 103 char HMC5883L::getMode()
pommzorz 0:9a1682a09c50 104 {
pommzorz 0:9a1682a09c50 105 char cmd[2];
pommzorz 0:9a1682a09c50 106 cmd[0] = MODE_REG; // mode register
pommzorz 0:9a1682a09c50 107 i2c_.write(I2C_ADDRESS, cmd, 1, true);
pommzorz 0:9a1682a09c50 108 i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
pommzorz 0:9a1682a09c50 109 return cmd[1];
pommzorz 0:9a1682a09c50 110 }
pommzorz 0:9a1682a09c50 111
pommzorz 0:9a1682a09c50 112 char HMC5883L::getStatus()
pommzorz 0:9a1682a09c50 113 {
pommzorz 0:9a1682a09c50 114 char cmd[2];
pommzorz 0:9a1682a09c50 115 cmd[0] = STATUS_REG; // status register
pommzorz 0:9a1682a09c50 116 i2c_.write(I2C_ADDRESS, cmd, 1, true);
pommzorz 0:9a1682a09c50 117 i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
pommzorz 0:9a1682a09c50 118 return cmd[1];
pommzorz 0:9a1682a09c50 119 }
pommzorz 0:9a1682a09c50 120
pommzorz 0:9a1682a09c50 121 void HMC5883L::getXYZ(int16_t output[3])
pommzorz 0:9a1682a09c50 122 {
pommzorz 0:9a1682a09c50 123 char cmd[2];
pommzorz 0:9a1682a09c50 124 char data[6];
pommzorz 0:9a1682a09c50 125 cmd[0] = 0x03; // starting point for reading
pommzorz 0:9a1682a09c50 126 i2c_.write(I2C_ADDRESS, cmd, 1, true); // set the pointer to the start of x
pommzorz 0:9a1682a09c50 127 i2c_.read(I2C_ADDRESS, data, 6, false);
pommzorz 0:9a1682a09c50 128
pommzorz 0:9a1682a09c50 129 for(int i = 0; i < 3; i++) // fill the output variables
pommzorz 0:9a1682a09c50 130 output[i] = int16_t(((unsigned char)data[i*2] << 8) | (unsigned char)data[i*2+1]);
pommzorz 0:9a1682a09c50 131 }
pommzorz 0:9a1682a09c50 132
pommzorz 0:9a1682a09c50 133 double HMC5883L::getHeadingXY()
pommzorz 0:9a1682a09c50 134 {
pommzorz 0:9a1682a09c50 135 int16_t raw_data[3];
pommzorz 0:9a1682a09c50 136 getXYZ(raw_data);
pommzorz 0:9a1682a09c50 137 double heading = atan2(static_cast<double>(raw_data[2]), static_cast<double>(raw_data[0])); // heading = arctan(Y/X)
pommzorz 0:9a1682a09c50 138
pommzorz 0:9a1682a09c50 139 // TODO: declenation angle compensation
pommzorz 0:9a1682a09c50 140
pommzorz 0:9a1682a09c50 141 if(heading < 0.0) // fix sign
pommzorz 0:9a1682a09c50 142 heading += PI2;
pommzorz 0:9a1682a09c50 143
pommzorz 0:9a1682a09c50 144 if(heading > PI2) // fix overflow
pommzorz 0:9a1682a09c50 145 heading -= PI2;
pommzorz 0:9a1682a09c50 146
pommzorz 0:9a1682a09c50 147 return heading;
pommzorz 0:9a1682a09c50 148 }