1 /*
2 * Author: Mihai Tudor Panu <mihai.tudor.panu@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
Itg3200(int bus)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
calibrate(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
getTemperature()103 Itg3200::getTemperature()
104 {
105 return 35.0 + (m_temperature + 13200.0) / 280.0;
106 }
107
108 float*
getRotation()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*
getRawValues()118 Itg3200::getRawValues()
119 {
120 return &m_rotation[0];
121 }
122
123 int16_t
getRawTemp()124 Itg3200::getRawTemp()
125 {
126 return m_temperature;
127 }
128
129 mraa::Result
update(void)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