1 /* 2 * Author: Mihai Tudor Panu <mihai.tudor.panu (at) intel.com> 3 * Copyright (c) 2014 Intel Corporation. 4 * 5 * Permission is hereby granted, free of charge, to any person obtaining 6 * a copy of this software and associated documentation files (the 7 * "Software"), to deal in the Software without restriction, including 8 * without limitation the rights to use, copy, modify, merge, publish, 9 * distribute, sublicense, and/or sell copies of the Software, and to 10 * permit persons to whom the Software is furnished to do so, subject to 11 * the following conditions: 12 * 13 * The above copyright notice and this permission notice shall be 14 * included in all copies or substantial portions of the Software. 15 * 16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 17 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 20 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 22 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 */ 24 25 #include <iostream> 26 #include <string> 27 #include <stdexcept> 28 #include <unistd.h> 29 #include "math.h" 30 #include "itg3200.h" 31 32 #define READ_BUFFER_LENGTH 8 33 34 //address and id 35 #define ITG3200_I2C_ADDR 0x68 36 #define ITG3200_ID 0x00 37 38 //configuration registers 39 #define ITG3200_SMPLRT_DIV 0x15 40 #define ITG3200_DLPF_FS 0x16 41 42 //interrupt registers 43 #define ITG3200_INT_CFG 0x17 44 #define ITG3200_INT_STATUS 0x1A 45 46 //data registers (read only) 47 #define ITG3200_TEMP_H 0x1B 48 #define ITG3200_TEMP_L 0x1C 49 #define ITG3200_XOUT_H 0x1D 50 #define ITG3200_XOUT_L 0x1E 51 #define ITG3200_YOUT_H 0x1F 52 #define ITG3200_YOUT_L 0x20 53 #define ITG3200_ZOUT_H 0x21 54 #define ITG3200_ZOUT_L 0x22 55 #define DATA_REG_SIZE 8 56 57 //power management 58 #define ITG3200_PWR_MGM 0x3E 59 60 //useful values 61 #define ITG3200_RESET 0x80 62 #define ITG3200_SLEEP 0x40 63 #define ITG3200_WAKEUP 0x00 64 65 using namespace upm; 66 67 Itg3200::Itg3200(int bus) : m_i2c(bus) 68 { 69 m_i2c.address(ITG3200_I2C_ADDR); 70 m_buffer[0] = ITG3200_PWR_MGM; 71 m_buffer[1] = ITG3200_RESET; 72 m_i2c.write(m_buffer, 2); 73 74 Itg3200::calibrate(); 75 Itg3200::update(); 76 } 77 78 void 79 Itg3200::calibrate(void) 80 { 81 int reads = 600; 82 int delay = 4000; // 4 milliseconds 83 int skip = 5; // initial samples to skip 84 int temp[3] = {0}; 85 86 for(int i = 0; i < reads; i++){ 87 88 Itg3200::update(); 89 if (i > skip){ 90 for (int j = 0; j < 3; j++){ 91 temp[j] += m_rotation[j]; 92 } 93 } 94 usleep(delay); 95 } 96 97 for(int i = 0; i < 3; i++){ 98 m_offsets[i] = (-1) * temp[i] / (reads - skip); 99 } 100 } 101 102 float 103 Itg3200::getTemperature() 104 { 105 return 35.0 + (m_temperature + 13200.0) / 280.0; 106 } 107 108 float* 109 Itg3200::getRotation() 110 { 111 for(int i = 0; i < 3; i++){ 112 m_angle[i] = m_rotation[i]/14.375; 113 } 114 return &m_angle[0]; 115 } 116 117 int16_t* 118 Itg3200::getRawValues() 119 { 120 return &m_rotation[0]; 121 } 122 123 int16_t 124 Itg3200::getRawTemp() 125 { 126 return m_temperature; 127 } 128 129 mraa::Result 130 Itg3200::update(void) 131 { 132 m_i2c.address(ITG3200_I2C_ADDR); 133 m_i2c.writeByte(ITG3200_TEMP_H); 134 135 m_i2c.address(ITG3200_I2C_ADDR); 136 m_i2c.read(m_buffer, DATA_REG_SIZE); 137 138 //temp 139 // 140 m_temperature = (m_buffer[0] << 8 ) | m_buffer[1]; 141 // x 142 m_rotation[0] = ((m_buffer[2] << 8 ) | m_buffer[3]) + m_offsets[0]; 143 // y 144 m_rotation[1] = ((m_buffer[4] << 8 ) | m_buffer[5]) + m_offsets[1]; 145 // z 146 m_rotation[2] = ((m_buffer[6] << 8 ) | m_buffer[7]) + m_offsets[2]; 147 148 return mraa::SUCCESS; 149 } 150
