IMU 10dof MEMS from DR Robot adapted from HK10DOF Changed gyro to ITG3200
Fork of HK10DOF by
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.
HMC5883L.cpp@3:450acaa4f52d, 2014-11-18 (annotated)
- Committer:
- svkatielee
- Date:
- Tue Nov 18 06:25:57 2014 +0000
- Revision:
- 3:450acaa4f52d
- Parent:
- 0:9a1682a09c50
Updated the code from off-line, for switching platform to NUCLEO-F401RE and IMU from HK to DFRobot
Who changed what in which revision?
User | Revision | Line number | New 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 | } |