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 <math.h>
27 #include <iostream>
28 #include <stdexcept>
29 #include <string>
30
31 #include "h3lis331dl.h"
32
33 using namespace upm;
34 using namespace std;
35
36
H3LIS331DL(int bus,uint8_t address)37 H3LIS331DL::H3LIS331DL(int bus, uint8_t address):
38 m_i2c(bus)
39 {
40 m_addr = address;
41
42 mraa::Result rv;
43 if ( (rv = m_i2c.address(m_addr)) != mraa::SUCCESS)
44 {
45 throw std::runtime_error(std::string(__FUNCTION__) +
46 ": I2c.address() failed");
47 return;
48 }
49
50 m_rawX = m_rawY = m_rawZ = 0;
51 setAdjustmentOffsets(0, 0, 0);
52 }
53
~H3LIS331DL()54 H3LIS331DL::~H3LIS331DL()
55 {
56 }
57
init(DR_BITS_T odr,PM_BITS_T pm,FS_BITS_T fs)58 bool H3LIS331DL::init(DR_BITS_T odr, PM_BITS_T pm, FS_BITS_T fs)
59 {
60 if (!setDataRate(odr))
61 return false;
62 if (!setPowerMode(pm))
63 return false;
64 if (!setFullScale(fs))
65 return false;
66
67 // now enable X, Y, and Z axes
68 if (enableAxis(REG1_XEN | REG1_YEN | REG1_ZEN))
69 return false;
70
71 return true;
72 }
73
getChipID()74 uint8_t H3LIS331DL::getChipID()
75 {
76 return m_i2c.readReg(REG_WHOAMI);
77 }
78
setDataRate(DR_BITS_T odr)79 bool H3LIS331DL::setDataRate(DR_BITS_T odr)
80 {
81 uint8_t reg1 = m_i2c.readReg(REG_REG1);
82
83 reg1 &= ~(REG1_DR0 | REG1_DR1);
84 reg1 |= (odr << REG1_DR_SHIFT);
85
86 if (m_i2c.writeReg(REG_REG1, reg1))
87 {
88 throw std::runtime_error(std::string(__FUNCTION__) +
89 ": I2c.writeReg() failed");
90 return false;
91 }
92
93 return true;
94 }
95
setPowerMode(PM_BITS_T pm)96 bool H3LIS331DL::setPowerMode(PM_BITS_T pm)
97 {
98 uint8_t reg1 = m_i2c.readReg(REG_REG1);
99
100 reg1 &= ~(REG1_PM0 | REG1_PM1 | REG1_PM2);
101 reg1 |= (pm << REG1_PM_SHIFT);
102
103 if (m_i2c.writeReg(REG_REG1, reg1))
104 {
105 throw std::runtime_error(std::string(__FUNCTION__) +
106 ": I2c.writeReg() failed");
107 return false;
108 }
109
110 return true;
111 }
112
enableAxis(uint8_t axisEnable)113 bool H3LIS331DL::enableAxis(uint8_t axisEnable)
114 {
115 uint8_t reg1 = m_i2c.readReg(REG_REG1);
116
117 reg1 &= ~(REG1_XEN | REG1_YEN | REG1_ZEN);
118 reg1 |= (axisEnable & (REG1_XEN | REG1_YEN | REG1_ZEN));
119
120 if (m_i2c.writeReg(REG_REG1, reg1))
121 {
122 throw std::runtime_error(std::string(__FUNCTION__) +
123 ": I2c.writeReg() failed");
124 return false;
125 }
126
127 return true;
128 }
129
setFullScale(FS_BITS_T fs)130 bool H3LIS331DL::setFullScale(FS_BITS_T fs)
131 {
132 uint8_t reg4 = m_i2c.readReg(REG_REG4);
133
134 reg4 &= ~(REG4_FS0 | REG4_FS1);
135 reg4 |= (fs << REG4_FS_SHIFT);
136
137 if (m_i2c.writeReg(REG_REG4, reg4))
138 {
139 throw std::runtime_error(std::string(__FUNCTION__) +
140 ": I2c.writeReg() failed");
141 return false;
142 }
143
144 return true;
145 }
146
setHPCF(HPCF_BITS_T val)147 bool H3LIS331DL::setHPCF(HPCF_BITS_T val)
148 {
149 uint8_t reg = m_i2c.readReg(REG_REG2);
150
151 reg &= ~(REG2_HPCF0 | REG2_HPCF1);
152 reg |= (val << REG2_HPCF_SHIFT);
153
154 if (m_i2c.writeReg(REG_REG2, reg))
155 {
156 throw std::runtime_error(std::string(__FUNCTION__) +
157 ": I2c.writeReg() failed");
158 return false;
159 }
160
161 return true;
162 }
163
setHPM(HPM_BITS_T val)164 bool H3LIS331DL::setHPM(HPM_BITS_T val)
165 {
166 uint8_t reg = m_i2c.readReg(REG_REG2);
167
168 reg &= ~(REG2_HPM0 | REG2_HPM1);
169 reg |= (val << REG2_HPM_SHIFT);
170
171 if (m_i2c.writeReg(REG_REG2, reg))
172 {
173 throw std::runtime_error(std::string(__FUNCTION__) +
174 ": I2c.writeReg() failed");
175 return false;
176 }
177
178 return true;
179 }
180
boot()181 bool H3LIS331DL::boot()
182 {
183 uint8_t reg = m_i2c.readReg(REG_REG2);
184
185 reg |= REG2_BOOT;
186
187 if (m_i2c.writeReg(REG_REG2, reg))
188 {
189 throw std::runtime_error(std::string(__FUNCTION__) +
190 ": I2c.writeReg() failed");
191 return false;
192 }
193
194 // wait for the boot bit to clear
195 do {
196 reg = m_i2c.readReg(REG_REG2);
197 usleep(200000);
198 } while (reg & REG2_BOOT);
199
200 return true;
201 }
202
enableHPF1(bool enable)203 bool H3LIS331DL::enableHPF1(bool enable)
204 {
205 uint8_t reg = m_i2c.readReg(REG_REG2);
206
207 if (enable)
208 reg |= REG2_HPEN1;
209 else
210 reg &= ~REG2_HPEN1;
211
212 if (m_i2c.writeReg(REG_REG2, reg))
213 {
214 throw std::runtime_error(std::string(__FUNCTION__) +
215 ": I2c.writeReg() failed");
216 return false;
217 }
218
219 return true;
220 }
221
enableHPF2(bool enable)222 bool H3LIS331DL::enableHPF2(bool enable)
223 {
224 uint8_t reg = m_i2c.readReg(REG_REG2);
225
226 if (enable)
227 reg |= REG2_HPEN2;
228 else
229 reg &= ~REG2_HPEN2;
230
231 if (m_i2c.writeReg(REG_REG2, reg))
232 {
233 throw std::runtime_error(std::string(__FUNCTION__) +
234 ": I2c.writeReg() failed");
235 return false;
236 }
237
238 return true;
239 }
240
enableFDS(bool enable)241 bool H3LIS331DL::enableFDS(bool enable)
242 {
243 uint8_t reg = m_i2c.readReg(REG_REG2);
244
245 if (enable)
246 reg |= REG2_FDS;
247 else
248 reg &= ~REG2_FDS;
249
250 if (m_i2c.writeReg(REG_REG2, reg))
251 {
252 throw std::runtime_error(std::string(__FUNCTION__) +
253 ": I2c.writeReg() failed");
254 return false;
255 }
256
257 return true;
258 }
259
setInterruptActiveLow(bool enable)260 bool H3LIS331DL::setInterruptActiveLow(bool enable)
261 {
262 uint8_t reg = m_i2c.readReg(REG_REG3);
263
264 if (enable)
265 reg |= REG3_IHL;
266 else
267 reg &= ~REG3_IHL;
268
269 if (m_i2c.writeReg(REG_REG3, reg))
270 {
271 throw std::runtime_error(std::string(__FUNCTION__) +
272 ": I2c.writeReg() failed");
273 return false;
274 }
275
276 return true;
277 }
278
setInterruptOpenDrain(bool enable)279 bool H3LIS331DL::setInterruptOpenDrain(bool enable)
280 {
281 uint8_t reg = m_i2c.readReg(REG_REG3);
282
283 if (enable)
284 reg |= REG3_PP_OD;
285 else
286 reg &= ~REG3_PP_OD;
287
288 if (m_i2c.writeReg(REG_REG3, reg))
289 {
290 throw std::runtime_error(std::string(__FUNCTION__) +
291 ": I2c.writeReg() failed");
292 return false;
293 }
294
295 return true;
296 }
297
setInterrupt1Latch(bool enable)298 bool H3LIS331DL::setInterrupt1Latch(bool enable)
299 {
300 uint8_t reg = m_i2c.readReg(REG_REG3);
301
302 if (enable)
303 reg |= REG3_LIR1;
304 else
305 reg &= ~REG3_LIR1;
306
307 if (m_i2c.writeReg(REG_REG3, reg))
308 {
309 throw std::runtime_error(std::string(__FUNCTION__) +
310 ": I2c.writeReg() failed");
311 return false;
312 }
313
314 return true;
315 }
316
setInterrupt2Latch(bool enable)317 bool H3LIS331DL::setInterrupt2Latch(bool enable)
318 {
319 uint8_t reg = m_i2c.readReg(REG_REG3);
320
321 if (enable)
322 reg |= REG3_LIR2;
323 else
324 reg &= ~REG3_LIR2;
325
326 if (m_i2c.writeReg(REG_REG3, reg))
327 {
328 throw std::runtime_error(std::string(__FUNCTION__) +
329 ": I2c.writeReg() failed");
330 return false;
331 }
332
333 return true;
334 }
335
setInterrupt1PadConfig(I_CFG_BITS_T val)336 bool H3LIS331DL::setInterrupt1PadConfig(I_CFG_BITS_T val)
337 {
338 uint8_t reg = m_i2c.readReg(REG_REG3);
339
340 reg &= ~(REG3_I1_CFG0 | REG3_I1_CFG1);
341 reg |= (val << REG3_I1_CFG_SHIFT);
342
343 if (m_i2c.writeReg(REG_REG3, reg))
344 {
345 throw std::runtime_error(std::string(__FUNCTION__) +
346 ": I2c.writeReg() failed");
347 return false;
348 }
349
350 return true;
351 }
352
setInterrupt2PadConfig(I_CFG_BITS_T val)353 bool H3LIS331DL::setInterrupt2PadConfig(I_CFG_BITS_T val)
354 {
355 uint8_t reg = m_i2c.readReg(REG_REG3);
356
357 reg &= ~(REG3_I2_CFG0 | REG3_I2_CFG1);
358 reg |= (val << REG3_I2_CFG_SHIFT);
359
360 if (m_i2c.writeReg(REG_REG3, reg))
361 {
362 throw std::runtime_error(std::string(__FUNCTION__) +
363 ": I2c.writeReg() failed");
364 return false;
365 }
366
367 return true;
368 }
369
370
enableBDU(bool enable)371 bool H3LIS331DL::enableBDU(bool enable)
372 {
373 uint8_t reg = m_i2c.readReg(REG_REG4);
374
375 if (enable)
376 reg |= REG4_BDU;
377 else
378 reg &= ~REG4_BDU;
379
380 if (m_i2c.writeReg(REG_REG4, reg))
381 {
382 throw std::runtime_error(std::string(__FUNCTION__) +
383 ": I2c.writeReg() failed");
384 return false;
385 }
386
387 return true;
388 }
389
enableBLE(bool enable)390 bool H3LIS331DL::enableBLE(bool enable)
391 {
392 uint8_t reg = m_i2c.readReg(REG_REG4);
393
394 if (enable)
395 reg |= REG4_BLE;
396 else
397 reg &= ~REG4_BLE;
398
399 if (m_i2c.writeReg(REG_REG4, reg))
400 {
401 throw std::runtime_error(std::string(__FUNCTION__) +
402 ": I2c.writeReg() failed");
403 return false;
404 }
405
406 return true;
407 }
408
enableSleepToWake(bool enable)409 bool H3LIS331DL::enableSleepToWake(bool enable)
410 {
411 uint8_t reg = m_i2c.readReg(REG_REG5);
412
413 if (enable)
414 reg |= (REG5_TURNON0 | REG5_TURNON1);
415 else
416 reg &= ~(REG5_TURNON0 | REG5_TURNON1);
417
418 if (m_i2c.writeReg(REG_REG5, reg))
419 {
420 throw std::runtime_error(std::string(__FUNCTION__) +
421 ": I2c.writeReg() failed");
422 return false;
423 }
424
425 return true;
426 }
427
getStatus()428 uint8_t H3LIS331DL::getStatus()
429 {
430 return m_i2c.readReg(REG_STATUS);
431 }
432
setInterrupt1Config(uint8_t val)433 bool H3LIS331DL::setInterrupt1Config(uint8_t val)
434 {
435 uint8_t reg = m_i2c.readReg(REG_INT1_CFG);
436
437 // mask off reserved bit
438 reg = (val & ~0x40);
439
440 if (m_i2c.writeReg(REG_INT1_CFG, reg))
441 {
442 throw std::runtime_error(std::string(__FUNCTION__) +
443 ": I2c.writeReg() failed");
444 return false;
445 }
446
447 return true;
448 }
449
setInterrupt1Source(uint8_t val)450 bool H3LIS331DL::setInterrupt1Source(uint8_t val)
451 {
452 uint8_t reg = m_i2c.readReg(REG_INT1_SRC);
453
454 // mask off reserved bit
455 reg = (val & ~0x80);
456
457 if (m_i2c.writeReg(REG_INT1_SRC, reg))
458 {
459 throw std::runtime_error(std::string(__FUNCTION__) +
460 ": I2c.writeReg() failed");
461 return false;
462 }
463
464 return true;
465 }
466
setInterrupt1Threshold(uint8_t val)467 bool H3LIS331DL::setInterrupt1Threshold(uint8_t val)
468 {
469 if (m_i2c.writeReg(REG_INT1_THS, val))
470 {
471 throw std::runtime_error(std::string(__FUNCTION__) +
472 ": I2c.writeReg() failed");
473 return false;
474 }
475
476 return true;
477 }
478
setInterrupt1Duration(uint8_t val)479 bool H3LIS331DL::setInterrupt1Duration(uint8_t val)
480 {
481 if (m_i2c.writeReg(REG_INT1_DUR, val))
482 {
483 throw std::runtime_error(std::string(__FUNCTION__) +
484 ": I2c.writeReg() failed");
485 return false;
486 }
487
488 return true;
489 }
490
setInterrupt2Config(uint8_t val)491 bool H3LIS331DL::setInterrupt2Config(uint8_t val)
492 {
493 uint8_t reg = m_i2c.readReg(REG_INT2_CFG);
494
495 // mask off reserved bit
496 reg = (val & ~0x40);
497
498 if (m_i2c.writeReg(REG_INT2_CFG, reg))
499 {
500 throw std::runtime_error(std::string(__FUNCTION__) +
501 ": I2c.writeReg() failed");
502 return false;
503 }
504
505 return true;
506 }
507
setInterrupt2Source(uint8_t val)508 bool H3LIS331DL::setInterrupt2Source(uint8_t val)
509 {
510 uint8_t reg = m_i2c.readReg(REG_INT2_SRC);
511
512 // mask off reserved bit
513 reg = (val & ~0x80);
514
515 if (m_i2c.writeReg(REG_INT2_SRC, reg))
516 {
517 throw std::runtime_error(std::string(__FUNCTION__) +
518 ": I2c.writeReg() failed");
519 return false;
520 }
521
522 return true;
523 }
524
setInterrupt2Threshold(uint8_t val)525 bool H3LIS331DL::setInterrupt2Threshold(uint8_t val)
526 {
527 if (m_i2c.writeReg(REG_INT2_THS, val))
528 {
529 throw std::runtime_error(std::string(__FUNCTION__) +
530 ": I2c.writeReg() failed");
531 return false;
532 }
533
534 return true;
535 }
536
setInterrupt2Duration(uint8_t val)537 bool H3LIS331DL::setInterrupt2Duration(uint8_t val)
538 {
539 if (m_i2c.writeReg(REG_INT2_DUR, val))
540 {
541 throw std::runtime_error(std::string(__FUNCTION__) +
542 ": I2c.writeReg() failed");
543 return false;
544 }
545
546 return true;
547 }
548
update()549 void H3LIS331DL::update()
550 {
551 uint8_t low, high;
552
553 // X
554 low = m_i2c.readReg(REG_OUT_X_L);
555 high = m_i2c.readReg(REG_OUT_X_H);
556 m_rawX = ((high << 8) | low);
557
558 // Y
559 low = m_i2c.readReg(REG_OUT_Y_L);
560 high = m_i2c.readReg(REG_OUT_Y_H);
561 m_rawY = ((high << 8) | low);
562
563 // Z
564 low = m_i2c.readReg(REG_OUT_Z_L);
565 high = m_i2c.readReg(REG_OUT_Z_H);
566 m_rawZ = ((high << 8) | low);
567 }
568
setAdjustmentOffsets(int adjX,int adjY,int adjZ)569 void H3LIS331DL::setAdjustmentOffsets(int adjX, int adjY, int adjZ)
570 {
571 m_adjX = adjX;
572 m_adjY = adjY;
573 m_adjZ = adjZ;
574 }
575
getAcceleration(float * aX,float * aY,float * aZ)576 void H3LIS331DL::getAcceleration(float *aX, float *aY, float *aZ)
577 {
578 const float gains = 0.003; // Seeed magic number?
579
580 *aX = float(m_rawX - m_adjX) * gains;
581 *aY = float(m_rawY - m_adjY) * gains;
582 *aZ = float(m_rawZ - m_adjZ) * gains;
583 }
584
getRawXYZ(int * x,int * y,int * z)585 void H3LIS331DL::getRawXYZ(int *x, int *y, int*z)
586 {
587 *x = m_rawX;
588 *y = m_rawY;
589 *z = m_rawZ;
590 }
591
getXYZ(int * x,int * y,int * z)592 void H3LIS331DL::getXYZ(int *x, int *y, int*z)
593 {
594 *x = (m_rawX - m_adjX);
595 *y = (m_rawY - m_adjY);
596 *z = (m_rawZ - m_adjZ);
597 }
598
599 #ifdef SWIGJAVA
getAcceleration()600 float *H3LIS331DL::getAcceleration()
601 {
602 float *v = new float[3];
603 getAcceleration(&v[0], &v[1], &v[2]);
604 return v;
605 }
606
getRawXYZ()607 int *H3LIS331DL::getRawXYZ()
608 {
609 int *v = new int[3];
610 getRawXYZ(&v[0], &v[1], &v[2]);
611 return v;
612 }
613
getXYZ()614 int *H3LIS331DL::getXYZ()
615 {
616 int *v = new int[3];
617 getXYZ(&v[0], &v[1], &v[2]);
618 return v;
619 }
620 #endif
621