1765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang/* 2765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Author: Brendan Le Foll <brendan.le.foll@intel.com> 3765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Contributions: Mihai Tudor Panu <mihai.tudor.panu@intel.com> 4765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Copyright (c) 2014 Intel Corporation. 5765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * 6765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Permission is hereby granted, free of charge, to any person obtaining 7765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * a copy of this software and associated documentation files (the 8765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * "Software"), to deal in the Software without restriction, including 9765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * without limitation the rights to use, copy, modify, merge, publish, 10765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * distribute, sublicense, and/or sell copies of the Software, and to 11765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * permit persons to whom the Software is furnished to do so, subject to 12765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * the following conditions: 13765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * 14765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * The above copyright notice and this permission notice shall be 15765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * included in all copies or substantial portions of the Software. 16765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * 17765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 18765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 19765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 20765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 21765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 22765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 23765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 24765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang */ 25765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 26765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <string> 27765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <stdexcept> 28765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 29765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include "math.h" 30765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include "hmc5883l.h" 31765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 32765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define MAX_BUFFER_LENGTH 6 33765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define HMC5883L_I2C_ADDR 0x1E 34765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 35765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang//configuration registers 36765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define HMC5883L_CONF_REG_A 0x00 37765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define HMC5883L_CONF_REG_B 0x01 38765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 39765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang//mode register 40765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define HMC5883L_MODE_REG 0x02 41765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 42765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang//data register 43765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define HMC5883L_X_MSB_REG 0 44765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define HMC5883L_X_LSB_REG 1 45765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define HMC5883L_Z_MSB_REG 2 46765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define HMC5883L_Z_LSB_REG 3 47765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define HMC5883L_Y_MSB_REG 4 48765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define HMC5883L_Y_LSB_REG 5 49765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define DATA_REG_SIZE 6 50765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 51765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang//status register 52765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define HMC5883L_STATUS_REG 0x09 53765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 54765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang//ID registers 55765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define HMC5883L_ID_A_REG 0x0A 56765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define HMC5883L_ID_B_REG 0x0B 57765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define HMC5883L_ID_C_REG 0x0C 58765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 59765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define HMC5883L_CONT_MODE 0x00 60765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define HMC5883L_DATA_REG 0x03 61765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 62765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang//scales 63765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define GA_0_88_REG 0x00 << 5 64765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define GA_1_3_REG 0x01 << 5 65765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define GA_1_9_REG 0x02 << 5 66765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define GA_2_5_REG 0x03 << 5 67765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define GA_4_0_REG 0x04 << 5 68765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define GA_4_7_REG 0x05 << 5 69765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define GA_5_6_REG 0x06 << 5 70765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define GA_8_1_REG 0x07 << 5 71765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 72765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang//digital resolutions 73765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define SCALE_0_73_MG 0.73 74765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define SCALE_0_92_MG 0.92 75765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define SCALE_1_22_MG 1.22 76765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define SCALE_1_52_MG 1.52 77765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define SCALE_2_27_MG 2.27 78765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define SCALE_2_56_MG 2.56 79765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define SCALE_3_03_MG 3.03 80765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#define SCALE_4_35_MG 4.35 81765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 82765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangusing namespace upm; 83765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 84765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangHmc5883l::Hmc5883l(int bus) : m_i2c(bus) 85765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 86765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa::Result error; 87765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang error = m_i2c.address(HMC5883L_I2C_ADDR); 88765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if(error != mraa::SUCCESS){ 89765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::invalid_argument(std::string(__FUNCTION__) + 90765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": i2c.address() failed"); 91765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return; 92765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 93765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_rx_tx_buf[0] = HMC5883L_CONF_REG_B; 94765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_rx_tx_buf[1] = GA_1_3_REG; 95765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang error = m_i2c.write(m_rx_tx_buf, 2); 96765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if(error != mraa::SUCCESS){ 97765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::runtime_error(std::string(__FUNCTION__) + 98765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": i2c.write() configuration failed"); 99765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return; 100765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 101765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 102765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang error = m_i2c.address(HMC5883L_I2C_ADDR); 103765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if(error != mraa::SUCCESS){ 104765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::invalid_argument(std::string(__FUNCTION__) + 105765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": i2c.address() failed"); 106765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return; 107765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 108765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_rx_tx_buf[0] = HMC5883L_MODE_REG; 109765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_rx_tx_buf[1] = HMC5883L_CONT_MODE; 110765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang error = m_i2c.write(m_rx_tx_buf, 2); 111765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if(error != mraa::SUCCESS){ 112765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::runtime_error(std::string(__FUNCTION__) + 113765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": i2c.write() mode failed"); 114765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return; 115765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 116765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 117765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang Hmc5883l::update(); 118765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 119765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 120765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangmraa::Result 121765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangHmc5883l::update(void) 122765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 123765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_i2c.address(HMC5883L_I2C_ADDR); 124765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_i2c.writeByte(HMC5883L_DATA_REG); 125765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 126765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_i2c.address(HMC5883L_I2C_ADDR); 127765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_i2c.read(m_rx_tx_buf, DATA_REG_SIZE); 128765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 129765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // x 130765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_coor[0] = (m_rx_tx_buf[HMC5883L_X_MSB_REG] << 8 ) | m_rx_tx_buf[HMC5883L_X_LSB_REG]; 131765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // z 132765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_coor[2] = (m_rx_tx_buf[HMC5883L_Z_MSB_REG] << 8 ) | m_rx_tx_buf[HMC5883L_Z_LSB_REG]; 133765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // y 134765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_coor[1] = (m_rx_tx_buf[HMC5883L_Y_MSB_REG] << 8 ) | m_rx_tx_buf[HMC5883L_Y_LSB_REG]; 135765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 136765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return mraa::SUCCESS; 137765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 138765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 139765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangfloat 140765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangHmc5883l::direction(void) 141765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 142765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return atan2(m_coor[1] * SCALE_0_92_MG, m_coor[0] * SCALE_0_92_MG) + m_declination; 143765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 144765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 145765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangfloat 146765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangHmc5883l::heading(void) 147765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 148765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang float dir = Hmc5883l::direction() * 180/M_PI; 149765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if(dir < 0){ 150765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang dir += 360.0; 151765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 152765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return dir; 153765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 154765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 155765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangint16_t* 156765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangHmc5883l::coordinates(void) 157765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 158765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return &m_coor[0]; 159765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 160765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 161765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid 162765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangHmc5883l::set_declination(float dec) 163765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 164765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_declination = dec; 165765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 166765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 167765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangfloat 168765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangHmc5883l::get_declination() 169765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 170765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return m_declination; 171765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 172