1 /* 2 * Author: Brendan Le Foll <brendan.le.foll (at) intel.com> 3 * Contributions: Mihai Tudor Panu <mihai.tudor.panu (at) intel.com> 4 * Copyright (c) 2014 Intel Corporation. 5 * 6 * Permission is hereby granted, free of charge, to any person obtaining 7 * a copy of this software and associated documentation files (the 8 * "Software"), to deal in the Software without restriction, including 9 * without limitation the rights to use, copy, modify, merge, publish, 10 * distribute, sublicense, and/or sell copies of the Software, and to 11 * permit persons to whom the Software is furnished to do so, subject to 12 * the following conditions: 13 * 14 * The above copyright notice and this permission notice shall be 15 * included in all copies or substantial portions of the Software. 16 * 17 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 18 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 19 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 20 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 21 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 22 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 23 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 24 */ 25 26 #include <string> 27 #include <stdexcept> 28 29 #include "math.h" 30 #include "hmc5883l.h" 31 32 #define MAX_BUFFER_LENGTH 6 33 #define HMC5883L_I2C_ADDR 0x1E 34 35 //configuration registers 36 #define HMC5883L_CONF_REG_A 0x00 37 #define HMC5883L_CONF_REG_B 0x01 38 39 //mode register 40 #define HMC5883L_MODE_REG 0x02 41 42 //data register 43 #define HMC5883L_X_MSB_REG 0 44 #define HMC5883L_X_LSB_REG 1 45 #define HMC5883L_Z_MSB_REG 2 46 #define HMC5883L_Z_LSB_REG 3 47 #define HMC5883L_Y_MSB_REG 4 48 #define HMC5883L_Y_LSB_REG 5 49 #define DATA_REG_SIZE 6 50 51 //status register 52 #define HMC5883L_STATUS_REG 0x09 53 54 //ID registers 55 #define HMC5883L_ID_A_REG 0x0A 56 #define HMC5883L_ID_B_REG 0x0B 57 #define HMC5883L_ID_C_REG 0x0C 58 59 #define HMC5883L_CONT_MODE 0x00 60 #define HMC5883L_DATA_REG 0x03 61 62 //scales 63 #define GA_0_88_REG 0x00 << 5 64 #define GA_1_3_REG 0x01 << 5 65 #define GA_1_9_REG 0x02 << 5 66 #define GA_2_5_REG 0x03 << 5 67 #define GA_4_0_REG 0x04 << 5 68 #define GA_4_7_REG 0x05 << 5 69 #define GA_5_6_REG 0x06 << 5 70 #define GA_8_1_REG 0x07 << 5 71 72 //digital resolutions 73 #define SCALE_0_73_MG 0.73 74 #define SCALE_0_92_MG 0.92 75 #define SCALE_1_22_MG 1.22 76 #define SCALE_1_52_MG 1.52 77 #define SCALE_2_27_MG 2.27 78 #define SCALE_2_56_MG 2.56 79 #define SCALE_3_03_MG 3.03 80 #define SCALE_4_35_MG 4.35 81 82 using namespace upm; 83 84 Hmc5883l::Hmc5883l(int bus) : m_i2c(bus) 85 { 86 mraa::Result error; 87 error = m_i2c.address(HMC5883L_I2C_ADDR); 88 if(error != mraa::SUCCESS){ 89 throw std::invalid_argument(std::string(__FUNCTION__) + 90 ": i2c.address() failed"); 91 return; 92 } 93 m_rx_tx_buf[0] = HMC5883L_CONF_REG_B; 94 m_rx_tx_buf[1] = GA_1_3_REG; 95 error = m_i2c.write(m_rx_tx_buf, 2); 96 if(error != mraa::SUCCESS){ 97 throw std::runtime_error(std::string(__FUNCTION__) + 98 ": i2c.write() configuration failed"); 99 return; 100 } 101 102 error = m_i2c.address(HMC5883L_I2C_ADDR); 103 if(error != mraa::SUCCESS){ 104 throw std::invalid_argument(std::string(__FUNCTION__) + 105 ": i2c.address() failed"); 106 return; 107 } 108 m_rx_tx_buf[0] = HMC5883L_MODE_REG; 109 m_rx_tx_buf[1] = HMC5883L_CONT_MODE; 110 error = m_i2c.write(m_rx_tx_buf, 2); 111 if(error != mraa::SUCCESS){ 112 throw std::runtime_error(std::string(__FUNCTION__) + 113 ": i2c.write() mode failed"); 114 return; 115 } 116 117 Hmc5883l::update(); 118 } 119 120 mraa::Result 121 Hmc5883l::update(void) 122 { 123 m_i2c.address(HMC5883L_I2C_ADDR); 124 m_i2c.writeByte(HMC5883L_DATA_REG); 125 126 m_i2c.address(HMC5883L_I2C_ADDR); 127 m_i2c.read(m_rx_tx_buf, DATA_REG_SIZE); 128 129 // x 130 m_coor[0] = (m_rx_tx_buf[HMC5883L_X_MSB_REG] << 8 ) | m_rx_tx_buf[HMC5883L_X_LSB_REG]; 131 // z 132 m_coor[2] = (m_rx_tx_buf[HMC5883L_Z_MSB_REG] << 8 ) | m_rx_tx_buf[HMC5883L_Z_LSB_REG]; 133 // y 134 m_coor[1] = (m_rx_tx_buf[HMC5883L_Y_MSB_REG] << 8 ) | m_rx_tx_buf[HMC5883L_Y_LSB_REG]; 135 136 return mraa::SUCCESS; 137 } 138 139 float 140 Hmc5883l::direction(void) 141 { 142 return atan2(m_coor[1] * SCALE_0_92_MG, m_coor[0] * SCALE_0_92_MG) + m_declination; 143 } 144 145 float 146 Hmc5883l::heading(void) 147 { 148 float dir = Hmc5883l::direction() * 180/M_PI; 149 if(dir < 0){ 150 dir += 360.0; 151 } 152 return dir; 153 } 154 155 int16_t* 156 Hmc5883l::coordinates(void) 157 { 158 return &m_coor[0]; 159 } 160 161 void 162 Hmc5883l::set_declination(float dec) 163 { 164 m_declination = dec; 165 } 166 167 float 168 Hmc5883l::get_declination() 169 { 170 return m_declination; 171 } 172