航空研究会 / HMC5883L_kai_3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

HMC5883L.cpp@8:8b6e984df00b, 2020-02-24 (annotated)

Committer:
imanomadao
Date:
Mon Feb 24 07:45:19 2020 +0000
Revision:
8:8b6e984df00b
Parent:
7:38050f18f60a 
a

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
imanomadao 8:8b6e984df00b 64 i2c_.write(0x3C, cmd, 2);
imanomadao 8:8b6e984df00b 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
imanomadao 8:8b6e984df00b 74 i2c_.write(0x3C, cmd, 2);
imanomadao 8:8b6e984df00b 75 //i2c_.write(I2C_ADDRESS, cmd, 2);
tylerjw 0:8b84d61cee94 76 }
tylerjw 0:8b84d61cee94 77
tylerjw 0:8b84d61cee94 78 char HMC5883L::getConfigurationA()
tylerjw 0:8b84d61cee94 79 {
tylerjw 0:8b84d61cee94 80 char cmd[2];
tylerjw 0:8b84d61cee94 81 cmd[0] = CONFIG_A_REG; // register a address
imanomadao 8:8b6e984df00b 82 i2c_.write(0x3C, cmd, 1, true);
imanomadao 8:8b6e984df00b 83 //i2c_.write(I2C_ADDRESS, cmd, 1, true);
tylerjw 0:8b84d61cee94 84 i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
tylerjw 0:8b84d61cee94 85 return cmd[1];
tylerjw 0:8b84d61cee94 86 }
tylerjw 0:8b84d61cee94 87
tylerjw 0:8b84d61cee94 88 char HMC5883L::getConfigurationB()
tylerjw 0:8b84d61cee94 89 {
tylerjw 0:8b84d61cee94 90 char cmd[2];
taknokolat 6:5c0cebb61b0f 91 cmd[0] = CONFIG_B_REG; // register b address
imanomadao 8:8b6e984df00b 92 i2c_.write(0x3C, cmd, 1, true);
imanomadao 8:8b6e984df00b 93 //i2c_.write(I2C_ADDRESS, cmd, 1, true);
tylerjw 0:8b84d61cee94 94 i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
tylerjw 0:8b84d61cee94 95 return cmd[1];
tylerjw 0:8b84d61cee94 96 }
tylerjw 0:8b84d61cee94 97
tylerjw 0:8b84d61cee94 98 void HMC5883L::setMode(char mode = SINGLE_MODE)
tylerjw 0:8b84d61cee94 99 {
tylerjw 0:8b84d61cee94 100 char cmd[2];
tylerjw 0:8b84d61cee94 101 cmd[0] = MODE_REG; // mode register address
tylerjw 0:8b84d61cee94 102 cmd[1] = mode;
imanomadao 8:8b6e984df00b 103 i2c_.write(0x3C, cmd, 2);
imanomadao 8:8b6e984df00b 104 //i2c_.write(I2C_ADDRESS,cmd,2);
tylerjw 0:8b84d61cee94 105 }
tylerjw 0:8b84d61cee94 106
tylerjw 0:8b84d61cee94 107 char HMC5883L::getMode()
tylerjw 0:8b84d61cee94 108 {
tylerjw 0:8b84d61cee94 109 char cmd[2];
tylerjw 0:8b84d61cee94 110 cmd[0] = MODE_REG; // mode register
imanomadao 8:8b6e984df00b 111 i2c_.write(0x3C, cmd, 1, true);
imanomadao 8:8b6e984df00b 112 //i2c_.write(I2C_ADDRESS, cmd, 1, true);
tylerjw 0:8b84d61cee94 113 i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
tylerjw 0:8b84d61cee94 114 return cmd[1];
tylerjw 0:8b84d61cee94 115 }
tylerjw 0:8b84d61cee94 116
tylerjw 0:8b84d61cee94 117 char HMC5883L::getStatus()
tylerjw 0:8b84d61cee94 118 {
tylerjw 0:8b84d61cee94 119 char cmd[2];
tylerjw 0:8b84d61cee94 120 cmd[0] = STATUS_REG; // status register
imanomadao 8:8b6e984df00b 121 i2c_.write(0x3C, cmd, 1, true);
imanomadao 8:8b6e984df00b 122 //i2c_.write(I2C_ADDRESS, cmd, 1, true);
tylerjw 0:8b84d61cee94 123 i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
tylerjw 0:8b84d61cee94 124 return cmd[1];
tylerjw 0:8b84d61cee94 125 }
tylerjw 0:8b84d61cee94 126
tylerjw 3:14d274e0f9de 127 void HMC5883L::getXYZ(int16_t output[3])
tylerjw 0:8b84d61cee94 128 {
tylerjw 0:8b84d61cee94 129 char cmd[2];
tylerjw 0:8b84d61cee94 130 char data[6];
tylerjw 0:8b84d61cee94 131 cmd[0] = 0x03; // starting point for reading
imanomadao 8:8b6e984df00b 132 //i2c_.write(0x3c, cmd, 1, true);
imanomadao 8:8b6e984df00b 133 //i2c_.write(I2C_ADDRESS, cmd, 1, true); // set the pointer to the start of x
tylerjw 0:8b84d61cee94 134 i2c_.read(I2C_ADDRESS, data, 6, false);
tylerjw 0:8b84d61cee94 135
tylerjw 0:8b84d61cee94 136 for(int i = 0; i < 3; i++) // fill the output variables
tylerjw 0:8b84d61cee94 137 output[i] = int16_t(((unsigned char)data[i*2] << 8) | (unsigned char)data[i*2+1]);
imanomadao 8:8b6e984df00b 138
imanomadao 8:8b6e984df00b 139 i2c_.write(0x3c, cmd, 1, true);
imanomadao 8:8b6e984df00b 140
tylerjw 1:8a1357c351c6 141 }
tylerjw 1:8a1357c351c6 142
TUATBM 5:ea1b30a71280 143 double HMC5883L::getHeadingXY(int Calib_x,int Calib_y)
tylerjw 1:8a1357c351c6 144 {
tylerjw 4:bc4e1201e092 145 int16_t raw_data[3];
tylerjw 1:8a1357c351c6 146 getXYZ(raw_data);
taknokolat 7:38050f18f60a 147 double heading = atan2(static_cast<double>(raw_data[0]+Calib_x), static_cast<double>(raw_data[2])+Calib_y); // heading = arctan(Y/X)
tylerjw 1:8a1357c351c6 148
tylerjw 1:8a1357c351c6 149 // TODO: declenation angle compensation
tylerjw 1:8a1357c351c6 150
tylerjw 1:8a1357c351c6 151 if(heading < 0.0) // fix sign
tylerjw 2:8eb755577f83 152 heading += PI2;
tylerjw 1:8a1357c351c6 153
tylerjw 2:8eb755577f83 154 if(heading > PI2) // fix overflow
tylerjw 2:8eb755577f83 155 heading -= PI2;
tylerjw 1:8a1357c351c6 156
tylerjw 1:8a1357c351c6 157 return heading;
tylerjw 0:8b84d61cee94 158 }