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
65using namespace upm;
66
67Itg3200::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
78void
79Itg3200::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
102float
103Itg3200::getTemperature()
104{
105    return 35.0 + (m_temperature + 13200.0) / 280.0;
106}
107
108float*
109Itg3200::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
117int16_t*
118Itg3200::getRawValues()
119{
120    return &m_rotation[0];
121}
122
123int16_t
124Itg3200::getRawTemp()
125{
126    return m_temperature;
127}
128
129mraa::Result
130Itg3200::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