1 /* 2 * Author: Brendan Le Foll <brendan.le.foll (at) intel.com> 3 * Author: Thomas Ingleby <thomas.c.ingleby (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 <unistd.h> 27 #include <signal.h> 28 29 #include "mraa.hpp" 30 #include "math.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 83 int running = 0; 84 85 void 86 sig_handler(int signo) 87 { 88 if (signo == SIGINT) { 89 printf("closing nicely\n"); 90 running = -1; 91 } 92 } 93 94 int 95 main() 96 { 97 float direction = 0; 98 int16_t x = 0, y = 0, z = 0; 99 uint8_t rx_tx_buf[MAX_BUFFER_LENGTH]; 100 101 //! [Interesting] 102 mraa::I2c* i2c; 103 i2c = new mraa::I2c(0); 104 105 i2c->address(HMC5883L_I2C_ADDR); 106 rx_tx_buf[0] = HMC5883L_CONF_REG_B; 107 rx_tx_buf[1] = GA_1_3_REG; 108 i2c->write(rx_tx_buf, 2); 109 //! [Interesting] 110 111 i2c->address(HMC5883L_I2C_ADDR); 112 rx_tx_buf[0] = HMC5883L_MODE_REG; 113 rx_tx_buf[1] = HMC5883L_CONT_MODE; 114 i2c->write(rx_tx_buf, 2); 115 116 signal(SIGINT, sig_handler); 117 118 while (running == 0) { 119 i2c->address(HMC5883L_I2C_ADDR); 120 i2c->writeByte(HMC5883L_DATA_REG); 121 122 i2c->address(HMC5883L_I2C_ADDR); 123 i2c->read(rx_tx_buf, DATA_REG_SIZE); 124 125 x = (rx_tx_buf[HMC5883L_X_MSB_REG] << 8) | rx_tx_buf[HMC5883L_X_LSB_REG]; 126 z = (rx_tx_buf[HMC5883L_Z_MSB_REG] << 8) | rx_tx_buf[HMC5883L_Z_LSB_REG]; 127 y = (rx_tx_buf[HMC5883L_Y_MSB_REG] << 8) | rx_tx_buf[HMC5883L_Y_LSB_REG]; 128 129 // scale and calculate direction 130 direction = atan2(y * SCALE_0_92_MG, x * SCALE_0_92_MG); 131 132 // check if the signs are reversed 133 if (direction < 0) 134 direction += 2 * M_PI; 135 136 printf("Compass scaled data x : %f, y : %f, z : %f\n", x * SCALE_0_92_MG, y * SCALE_0_92_MG, 137 z * SCALE_0_92_MG); 138 printf("Heading : %f\n", direction * 180 / M_PI); 139 sleep(1); 140 } 141 delete i2c; 142 143 return MRAA_SUCCESS; 144 } 145