Home | History | Annotate | Download | only in hmc5883l 
      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