1 /*
2 * Author: Yevgeniy Kiveisha <yevgeniy.kiveisha@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 <stdlib.h>
30 #include <string.h>
31 #include <pthread.h>
32 #include <math.h>
33
34 #include "mma7455.h"
35
36 using namespace upm;
37
MMA7455(int bus,int devAddr)38 MMA7455::MMA7455 (int bus, int devAddr) : m_i2ControlCtx(bus) {
39 unsigned char data = 0;
40 int nBytes = 0;
41
42 m_name = "MMA7455";
43
44 m_controlAddr = devAddr;
45 m_bus = bus;
46
47 mraa::Result error = m_i2ControlCtx.address(m_controlAddr);
48 if (error != mraa::SUCCESS) {
49 throw std::runtime_error(std::string(__FUNCTION__) +
50 ": mraa_i2c_address() failed");
51 return;
52 }
53
54 // setting GLVL 0x1 (64LSB/g) and MODE 0x1 (Measurement Mode)
55 data = (BIT (MMA7455_GLVL0) | BIT (MMA7455_MODE0));
56 error = i2cWriteReg (MMA7455_MCTL, &data, 0x1);
57 if (error != mraa::SUCCESS) {
58 throw std::runtime_error(std::string(__FUNCTION__) +
59 ": writing mode register failed");
60 return;
61 }
62
63 if (mraa::SUCCESS != calibrate ()) {
64 throw std::runtime_error(std::string(__FUNCTION__) +
65 ": calibrate() failed");
66 return;
67 }
68 }
69
70 mraa::Result
calibrate()71 MMA7455::calibrate () {
72 mraa::Result error = mraa::SUCCESS;
73 int i = 0;
74
75 accelData xyz;
76 xyz.value.x = xyz.value.y = xyz.value.z = 0;
77
78 do {
79 error = readData (&xyz.value.x, &xyz.value.y, &xyz.value.z);
80 if (mraa::SUCCESS != error) {
81 return error;
82 }
83
84 xyz.value.x += 2 * -xyz.value.x;
85 xyz.value.y += 2 * -xyz.value.y;
86 xyz.value.z += 2 * -(xyz.value.z - 64);
87
88 error = i2cWriteReg (MMA7455_XOFFL, (unsigned char *) &xyz, 0x6);
89 if (error != mraa::SUCCESS) {
90 return error;
91 }
92
93 } while ( ++i < 3 );
94
95 return error;
96 }
97
98 mraa::Result
readData(short * ptrX,short * ptrY,short * ptrZ)99 MMA7455::readData (short * ptrX, short * ptrY, short * ptrZ) {
100 accelData xyz;
101 unsigned char data = 0;
102 int nBytes = 0;
103
104 /*do {
105 nBytes = i2cReadReg (MMA7455_STATUS, &data, 0x1);
106 } while ( !(data & MMA7455_DRDY) && nBytes == mraa::SUCCESS);
107
108 if (nBytes == mraa::SUCCESS) {
109 std::cout << "NO_GDB :: 1" << std::endl;
110 return mraa::SUCCESS;
111 }*/
112
113 nBytes = i2cReadReg (MMA7455_XOUTL, (unsigned char *) &xyz, 0x6);
114 if (nBytes == 0) {
115 std::cout << "NO_GDB :: 2" << std::endl;
116 return mraa::ERROR_UNSPECIFIED;
117 }
118
119 if (xyz.reg.x_msb & 0x02) {
120 xyz.reg.x_msb |= 0xFC;
121 }
122
123 if (xyz.reg.y_msb & 0x02) {
124 xyz.reg.y_msb |= 0xFC;
125 }
126
127 if (xyz.reg.z_msb & 0x02) {
128 xyz.reg.z_msb |= 0xFC;
129 }
130
131 // The result is the g-force in units of 64 per 'g'.
132 *ptrX = xyz.value.x;
133 *ptrY = xyz.value.y;
134 *ptrZ = xyz.value.z;
135
136 return mraa::SUCCESS;
137 }
138
139 #ifdef SWIGJAVA
readData()140 short *MMA7455::readData() {
141 short *v = new short[3];
142 readData(&v[0], &v[1], &v[2]);
143 return v;
144 }
145 #endif
146
147 int
i2cReadReg(unsigned char reg,uint8_t * buffer,int len)148 MMA7455::i2cReadReg (unsigned char reg, uint8_t *buffer, int len) {
149 if (mraa::SUCCESS != m_i2ControlCtx.address(m_controlAddr)) {
150 throw std::runtime_error(std::string(__FUNCTION__) +
151 ": mraa_i2c_address() failed");
152 return 0;
153 }
154
155 if (mraa::SUCCESS != m_i2ControlCtx.writeByte(reg)) {
156 throw std::runtime_error(std::string(__FUNCTION__) +
157 ": mraa_i2c_write_byte() failed");
158 return 0;
159 }
160
161 return (int) m_i2ControlCtx.read(buffer, len);
162 }
163
164 mraa::Result
i2cWriteReg(unsigned char reg,uint8_t * buffer,int len)165 MMA7455::i2cWriteReg (unsigned char reg, uint8_t *buffer, int len) {
166 mraa::Result error = mraa::SUCCESS;
167
168 uint8_t data[len + 1];
169 data[0] = reg;
170 memcpy(&data[1], buffer, len);
171
172 error = m_i2ControlCtx.address (m_controlAddr);
173 if (error != mraa::SUCCESS) {
174 throw std::runtime_error(std::string(__FUNCTION__) +
175 ": mraa_i2c_address() failed");
176 return error;
177 }
178 error = m_i2ControlCtx.write (data, len + 1);
179 if (error != mraa::SUCCESS) {
180 throw std::runtime_error(std::string(__FUNCTION__) +
181 ": mraa_i2c_write() failed");
182 return error;
183 }
184
185 return error;
186 }
187