航空研究会 / HMC5883L_2

強要

HMC5883L.cpp@6:5c0cebb61b0f, 2019-02-07 (annotated)

Committer:
taknokolat
Date:
Thu Feb 07 06:13:40 2019 +0000
Revision:
6:5c0cebb61b0f
Parent:
5:ea1b30a71280 
ghh

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tylerjw 0:8b84d61cee94 1 /*
tylerjw 0:8b84d61cee94 2 * @file HMC5883L.cpp
tylerjw 0:8b84d61cee94 3 * @author Tyler Weaver
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
tylerjw 0:8b84d61cee94 25 * For use with the Sparkfun 9 Degrees of Freedom - Sensor Stick
tylerjw 0:8b84d61cee94 26 *
tylerjw 0:8b84d61cee94 27 * Datasheet:
tylerjw 0:8b84d61cee94 28 *
tylerjw 0:8b84d61cee94 29 * http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/Magneto/HMC5883L-FDS.pdf
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
tylerjw 0:8b84d61cee94 54 setConfigurationB(0x20); // default
tylerjw 0:8b84d61cee94 55 setMode(CONTINUOUS_MODE); // continuous sample mode
tylerjw 0:8b84d61cee94 56 }
tylerjw 0:8b84d61cee94 57
tylerjw 0:8b84d61cee94 58 void HMC5883L::setConfigurationA(char config)
tylerjw 0:8b84d61cee94 59 {
tylerjw 0:8b84d61cee94 60 char cmd[2];
tylerjw 0:8b84d61cee94 61 cmd[0] = CONFIG_A_REG; // register a address
tylerjw 0:8b84d61cee94 62 cmd[1] = config;
tylerjw 0:8b84d61cee94 63
tylerjw 0:8b84d61cee94 64 i2c_.write(I2C_ADDRESS, cmd, 2);
tylerjw 0:8b84d61cee94 65 }
tylerjw 0:8b84d61cee94 66
tylerjw 0:8b84d61cee94 67 void HMC5883L::setConfigurationB(char config)
tylerjw 0:8b84d61cee94 68 {
tylerjw 0:8b84d61cee94 69 char cmd[2];
tylerjw 0:8b84d61cee94 70 cmd[0] = CONFIG_B_REG; // register b address
tylerjw 0:8b84d61cee94 71 cmd[1] = config;
tylerjw 0:8b84d61cee94 72
tylerjw 0:8b84d61cee94 73 i2c_.write(I2C_ADDRESS, cmd, 2);
tylerjw 0:8b84d61cee94 74 }
tylerjw 0:8b84d61cee94 75
tylerjw 0:8b84d61cee94 76 char HMC5883L::getConfigurationA()
tylerjw 0:8b84d61cee94 77 {
tylerjw 0:8b84d61cee94 78 char cmd[2];
tylerjw 0:8b84d61cee94 79 cmd[0] = CONFIG_A_REG; // register a address
tylerjw 0:8b84d61cee94 80 i2c_.write(I2C_ADDRESS, cmd, 1, true);
tylerjw 0:8b84d61cee94 81 i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
tylerjw 0:8b84d61cee94 82 return cmd[1];
tylerjw 0:8b84d61cee94 83 }
tylerjw 0:8b84d61cee94 84
tylerjw 0:8b84d61cee94 85 char HMC5883L::getConfigurationB()
tylerjw 0:8b84d61cee94 86 {
tylerjw 0:8b84d61cee94 87 char cmd[2];
taknokolat 6:5c0cebb61b0f 88 cmd[0] = CONFIG_B_REG; // register b address
tylerjw 0:8b84d61cee94 89 i2c_.write(I2C_ADDRESS, cmd, 1, true);
tylerjw 0:8b84d61cee94 90 i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
tylerjw 0:8b84d61cee94 91 return cmd[1];
tylerjw 0:8b84d61cee94 92 }
tylerjw 0:8b84d61cee94 93
tylerjw 0:8b84d61cee94 94 void HMC5883L::setMode(char mode = SINGLE_MODE)
tylerjw 0:8b84d61cee94 95 {
tylerjw 0:8b84d61cee94 96 char cmd[2];
tylerjw 0:8b84d61cee94 97 cmd[0] = MODE_REG; // mode register address
tylerjw 0:8b84d61cee94 98 cmd[1] = mode;
tylerjw 0:8b84d61cee94 99 i2c_.write(I2C_ADDRESS,cmd,2);
tylerjw 0:8b84d61cee94 100 }
tylerjw 0:8b84d61cee94 101
tylerjw 0:8b84d61cee94 102 char HMC5883L::getMode()
tylerjw 0:8b84d61cee94 103 {
tylerjw 0:8b84d61cee94 104 char cmd[2];
tylerjw 0:8b84d61cee94 105 cmd[0] = MODE_REG; // mode register
tylerjw 0:8b84d61cee94 106 i2c_.write(I2C_ADDRESS, cmd, 1, true);
tylerjw 0:8b84d61cee94 107 i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
tylerjw 0:8b84d61cee94 108 return cmd[1];
tylerjw 0:8b84d61cee94 109 }
tylerjw 0:8b84d61cee94 110
tylerjw 0:8b84d61cee94 111 char HMC5883L::getStatus()
tylerjw 0:8b84d61cee94 112 {
tylerjw 0:8b84d61cee94 113 char cmd[2];
tylerjw 0:8b84d61cee94 114 cmd[0] = STATUS_REG; // status register
tylerjw 0:8b84d61cee94 115 i2c_.write(I2C_ADDRESS, cmd, 1, true);
tylerjw 0:8b84d61cee94 116 i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
tylerjw 0:8b84d61cee94 117 return cmd[1];
tylerjw 0:8b84d61cee94 118 }
tylerjw 0:8b84d61cee94 119
tylerjw 3:14d274e0f9de 120 void HMC5883L::getXYZ(int16_t output[3])
tylerjw 0:8b84d61cee94 121 {
tylerjw 0:8b84d61cee94 122 char cmd[2];
tylerjw 0:8b84d61cee94 123 char data[6];
tylerjw 0:8b84d61cee94 124 cmd[0] = 0x03; // starting point for reading
tylerjw 0:8b84d61cee94 125 i2c_.write(I2C_ADDRESS, cmd, 1, true); // set the pointer to the start of x
tylerjw 0:8b84d61cee94 126 i2c_.read(I2C_ADDRESS, data, 6, false);
tylerjw 0:8b84d61cee94 127
tylerjw 0:8b84d61cee94 128 for(int i = 0; i < 3; i++) // fill the output variables
tylerjw 0:8b84d61cee94 129 output[i] = int16_t(((unsigned char)data[i*2] << 8) | (unsigned char)data[i*2+1]);
tylerjw 1:8a1357c351c6 130 }
tylerjw 1:8a1357c351c6 131
TUATBM 5:ea1b30a71280 132 double HMC5883L::getHeadingXY(int Calib_x,int Calib_y)
tylerjw 1:8a1357c351c6 133 {
tylerjw 4:bc4e1201e092 134 int16_t raw_data[3];
tylerjw 1:8a1357c351c6 135 getXYZ(raw_data);
taknokolat 6:5c0cebb61b0f 136 double heading = atan2(static_cast<double>(raw_data[2]+Calib_x), static_cast<double>(raw_data[0])+Calib_y); // heading = arctan(Y/X)
tylerjw 1:8a1357c351c6 137
tylerjw 1:8a1357c351c6 138 // TODO: declenation angle compensation
tylerjw 1:8a1357c351c6 139
tylerjw 1:8a1357c351c6 140 if(heading < 0.0) // fix sign
tylerjw 2:8eb755577f83 141 heading += PI2;
tylerjw 1:8a1357c351c6 142
tylerjw 2:8eb755577f83 143 if(heading > PI2) // fix overflow
tylerjw 2:8eb755577f83 144 heading -= PI2;
tylerjw 1:8a1357c351c6 145
tylerjw 1:8a1357c351c6 146 return heading;
tylerjw 0:8b84d61cee94 147 }