1 /*
2 * Author: Jon Trulson <jtrulson@ics.com>
3 * Copyright (c) 2015 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 <unistd.h>
26 #include <iostream>
27 #include <string.h>
28
29 #include "mpu60x0.h"
30
31 using namespace upm;
32 using namespace std;
33
34
MPU60X0(int bus,uint8_t address)35 MPU60X0::MPU60X0(int bus, uint8_t address) :
36 m_i2c(bus), m_gpioIRQ(0)
37 {
38 m_addr = address;
39
40 m_accelX = 0.0;
41 m_accelY = 0.0;
42 m_accelZ = 0.0;
43
44 m_gyroX = 0.0;
45 m_gyroY = 0.0;
46 m_gyroZ = 0.0;
47
48 m_temp = 0.0;
49
50 m_accelScale = 1.0;
51 m_gyroScale = 1.0;
52
53 mraa::Result rv;
54 if ( (rv = m_i2c.address(m_addr)) != mraa::SUCCESS)
55 {
56 throw std::runtime_error(std::string(__FUNCTION__) +
57 ": I2c.address() failed");
58 return;
59 }
60 }
61
~MPU60X0()62 MPU60X0::~MPU60X0()
63 {
64 uninstallISR();
65 }
66
init()67 bool MPU60X0::init()
68 {
69 // first, take the device out of sleep mode
70 if (!setSleep(false))
71 {
72 throw std::runtime_error(std::string(__FUNCTION__) +
73 ": Unable to wake up device");
74 return false;
75 }
76
77 // set the clock source to use the gyroscope PLL rather than the
78 // internal clock for stability
79 if (!setClockSource(PLL_XG))
80 {
81 throw std::runtime_error(std::string(__FUNCTION__) +
82 ": Unable to set clock source");
83 return false;
84 }
85
86 usleep(5000);
87
88 // enable temperature measurement (default on power up, but let's be sure)
89 enableTemperatureSensor(true);
90
91 // set the gyro and accel scale bits to reasonable values
92 setGyroscopeScale(FS_500);
93 setAccelerometerScale(AFS_2);
94
95 // enable the DLPF
96 setDigitalLowPassFilter(DLPF_94_98);
97
98 // let things stabilize...
99 usleep(100000);
100
101 return true;
102 }
103
104
update()105 void MPU60X0::update()
106 {
107 // read all of the sensor registers - accel, gyro, and temp
108 uint8_t buffer[14];
109
110 memset(buffer, 0, 14);
111 readRegs(REG_ACCEL_XOUT_H, buffer, 14);
112
113 int16_t ax, ay, az;
114 int16_t temp;
115 int16_t gx, gy, gz;
116
117 ax = ( (buffer[0] << 8) | buffer[1] );
118 ay = ( (buffer[2] << 8) | buffer[3] );
119 az = ( (buffer[4] << 8) | buffer[5] );
120
121 temp = ( (buffer[6] << 8) | buffer[7] );
122
123 gx = ( (buffer[8] << 8) | buffer[9] );
124 gy = ( (buffer[10] << 8) | buffer[11] );
125 gz = ( (buffer[12] << 8) | buffer[13] );
126
127 // now stash them
128 m_accelX = float(ax);
129 m_accelY = float(ay);
130 m_accelZ = float(az);
131
132 m_temp = float(temp);
133
134 m_gyroX = float(gx);
135 m_gyroY = float(gy);
136 m_gyroZ = float(gz);
137 }
138
readReg(uint8_t reg)139 uint8_t MPU60X0::readReg(uint8_t reg)
140 {
141 return m_i2c.readReg(reg);
142 }
143
readRegs(uint8_t reg,uint8_t * buffer,int len)144 void MPU60X0::readRegs(uint8_t reg, uint8_t *buffer, int len)
145 {
146 m_i2c.readBytesReg(reg, buffer, len);
147 }
148
writeReg(uint8_t reg,uint8_t val)149 bool MPU60X0::writeReg(uint8_t reg, uint8_t val)
150 {
151 mraa::Result rv;
152 if ((rv = m_i2c.writeReg(reg, val)) != mraa::SUCCESS)
153 {
154 throw std::runtime_error(std::string(__FUNCTION__) +
155 ": I2c.writeReg() failed");
156 return false;
157 }
158
159 return true;
160 }
161
setSleep(bool enable)162 bool MPU60X0::setSleep(bool enable)
163 {
164 uint8_t reg = readReg(REG_PWR_MGMT_1);
165
166 if (enable)
167 reg |= PWR_SLEEP;
168 else
169 reg &= ~PWR_SLEEP;
170
171 return writeReg(REG_PWR_MGMT_1, reg);
172 }
173
setClockSource(CLKSEL_T clk)174 bool MPU60X0::setClockSource(CLKSEL_T clk)
175 {
176 uint8_t reg = readReg(REG_PWR_MGMT_1);
177
178 reg &= ~(_CLKSEL_MASK << _CLKSEL_SHIFT);
179
180 reg |= (clk << _CLKSEL_SHIFT);
181
182 return writeReg(REG_PWR_MGMT_1, reg);
183 }
184
setGyroscopeScale(FS_SEL_T scale)185 bool MPU60X0::setGyroscopeScale(FS_SEL_T scale)
186 {
187 uint8_t reg = readReg(REG_GYRO_CONFIG);
188
189 reg &= ~(_FS_SEL_MASK << _FS_SEL_SHIFT);
190
191 reg |= (scale << _FS_SEL_SHIFT);
192
193 if (!writeReg(REG_GYRO_CONFIG, reg))
194 {
195 return false;
196 }
197
198 // store scaling factor
199
200 switch (scale)
201 {
202 case FS_250:
203 m_gyroScale = 131.0;
204 break;
205
206 case FS_500:
207 m_gyroScale = 65.5;
208 break;
209
210 case FS_1000:
211 m_gyroScale = 32.8;
212 break;
213
214 case FS_2000:
215 m_gyroScale = 16.4;
216 break;
217
218 default: // should never occur, but...
219 m_gyroScale = 1.0; // set a safe, though incorrect value
220 throw std::logic_error(string(__FUNCTION__) +
221 ": internal error, unsupported scale");
222 break;
223 }
224
225 return true;
226 }
227
setAccelerometerScale(AFS_SEL_T scale)228 bool MPU60X0::setAccelerometerScale(AFS_SEL_T scale)
229 {
230 uint8_t reg = readReg(REG_ACCEL_CONFIG);
231
232 reg &= ~(_AFS_SEL_MASK << _AFS_SEL_SHIFT);
233
234 reg |= (scale << _AFS_SEL_SHIFT);
235
236 if (!writeReg(REG_ACCEL_CONFIG, reg))
237 {
238 return false;
239 }
240
241 // store scaling factor
242
243 switch (scale)
244 {
245 case AFS_2:
246 m_accelScale = 16384.0;
247 break;
248
249 case AFS_4:
250 m_accelScale = 8192.0;
251 break;
252
253 case AFS_8:
254 m_accelScale = 4096.0;
255 break;
256
257 case AFS_16:
258 m_accelScale = 2048.0;
259 break;
260
261 default: // should never occur, but...
262 m_accelScale = 1.0; // set a safe, though incorrect value
263 throw std::logic_error(string(__FUNCTION__) +
264 ": internal error, unsupported scale");
265 break;
266 }
267
268 return true;
269 }
270
setDigitalLowPassFilter(DLPF_CFG_T dlp)271 bool MPU60X0::setDigitalLowPassFilter(DLPF_CFG_T dlp)
272 {
273 uint8_t reg = readReg(REG_CONFIG);
274
275 reg &= ~(_CONFIG_DLPF_MASK << _CONFIG_DLPF_SHIFT);
276
277 reg |= (dlp << _CONFIG_DLPF_SHIFT);
278
279 return writeReg(REG_CONFIG, reg);
280 }
281
setSampleRateDivider(uint8_t div)282 bool MPU60X0::setSampleRateDivider(uint8_t div)
283 {
284 return writeReg(REG_SMPLRT_DIV, div);
285 }
286
getSampleRateDivider()287 uint8_t MPU60X0::getSampleRateDivider()
288 {
289 return readReg(REG_SMPLRT_DIV);
290 }
291
getAccelerometer(float * x,float * y,float * z)292 void MPU60X0::getAccelerometer(float *x, float *y, float *z)
293 {
294 if (x)
295 *x = m_accelX / m_accelScale;
296
297 if (y)
298 *y = m_accelY / m_accelScale;
299
300 if (z)
301 *z = m_accelZ / m_accelScale;
302 }
303
getGyroscope(float * x,float * y,float * z)304 void MPU60X0::getGyroscope(float *x, float *y, float *z)
305 {
306 if (x)
307 *x = m_gyroX / m_gyroScale;
308
309 if (y)
310 *y = m_gyroY / m_gyroScale;
311
312 if (z)
313 *z = m_gyroZ / m_gyroScale;
314 }
315
getTemperature()316 float MPU60X0::getTemperature()
317 {
318 // this equation is taken from the datasheet
319 return (m_temp / 340.0) + 36.53;
320 }
321
enableTemperatureSensor(bool enable)322 bool MPU60X0::enableTemperatureSensor(bool enable)
323 {
324 uint8_t reg = readReg(REG_PWR_MGMT_1);
325
326 if (enable)
327 reg &= ~TEMP_DIS;
328 else
329 reg |= TEMP_DIS;
330
331 return writeReg(REG_PWR_MGMT_1, reg);
332 }
333
setExternalSync(EXT_SYNC_SET_T val)334 bool MPU60X0::setExternalSync(EXT_SYNC_SET_T val)
335 {
336 uint8_t reg = readReg(REG_CONFIG);
337
338 reg &= ~(_CONFIG_EXT_SYNC_SET_MASK << _CONFIG_EXT_SYNC_SET_SHIFT);
339
340 reg |= (val << _CONFIG_EXT_SYNC_SET_SHIFT);
341
342 return writeReg(REG_CONFIG, reg);
343 }
344
enableI2CBypass(bool enable)345 bool MPU60X0::enableI2CBypass(bool enable)
346 {
347 uint8_t reg = readReg(REG_INT_PIN_CFG);
348
349 if (enable)
350 reg |= I2C_BYPASS_ENABLE;
351 else
352 reg &= ~I2C_BYPASS_ENABLE;
353
354 return writeReg(REG_INT_PIN_CFG, reg);
355 }
356
setMotionDetectionThreshold(uint8_t thr)357 bool MPU60X0::setMotionDetectionThreshold(uint8_t thr)
358 {
359 return writeReg(REG_MOT_THR, thr);
360 }
361
getInterruptStatus()362 uint8_t MPU60X0::getInterruptStatus()
363 {
364 return readReg(REG_INT_STATUS);
365 }
366
setInterruptEnables(uint8_t enables)367 bool MPU60X0::setInterruptEnables(uint8_t enables)
368 {
369 return writeReg(REG_INT_ENABLE, enables);
370 }
371
getInterruptEnables()372 uint8_t MPU60X0::getInterruptEnables()
373 {
374 return readReg(REG_INT_ENABLE);
375 }
376
setInterruptPinConfig(uint8_t cfg)377 bool MPU60X0::setInterruptPinConfig(uint8_t cfg)
378 {
379 return writeReg(REG_INT_PIN_CFG, cfg);
380 }
381
getInterruptPinConfig()382 uint8_t MPU60X0::getInterruptPinConfig()
383 {
384 return readReg(REG_INT_PIN_CFG);
385 }
386
387 #ifdef JAVACALLBACK
installISR(int gpio,mraa::Edge level,IsrCallback * cb)388 void MPU60X0::installISR(int gpio, mraa::Edge level,
389 IsrCallback *cb)
390 {
391 installISR(gpio, level, generic_callback_isr, cb);
392 }
393 #endif
394
installISR(int gpio,mraa::Edge level,void (* isr)(void *),void * arg)395 void MPU60X0::installISR(int gpio, mraa::Edge level,
396 void (*isr)(void *), void *arg)
397 {
398 // delete any existing ISR and GPIO context
399 uninstallISR();
400
401 // greate gpio context
402 m_gpioIRQ = new mraa::Gpio(gpio);
403
404 m_gpioIRQ->dir(mraa::DIR_IN);
405 m_gpioIRQ->isr(level, isr, arg);
406 }
407
uninstallISR()408 void MPU60X0::uninstallISR()
409 {
410 if (m_gpioIRQ)
411 {
412 m_gpioIRQ->isrExit();
413 delete m_gpioIRQ;
414
415 m_gpioIRQ = 0;
416 }
417 }
418