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
33using namespace upm;
34using namespace std;
35
36
37H3LIS331DL::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
54H3LIS331DL::~H3LIS331DL()
55{
56}
57
58bool 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
74uint8_t H3LIS331DL::getChipID()
75{
76  return m_i2c.readReg(REG_WHOAMI);
77}
78
79bool 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
96bool 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
113bool 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
130bool 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
147bool 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
164bool 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
181bool 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
203bool 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
222bool 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
241bool 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
260bool 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
279bool 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
298bool 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
317bool 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
336bool 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
353bool 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
371bool 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
390bool 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
409bool 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
428uint8_t H3LIS331DL::getStatus()
429{
430  return m_i2c.readReg(REG_STATUS);
431}
432
433bool 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
450bool 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
467bool 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
479bool 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
491bool 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
508bool 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
525bool 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
537bool 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
549void 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
569void H3LIS331DL::setAdjustmentOffsets(int adjX, int adjY, int adjZ)
570{
571  m_adjX = adjX;
572  m_adjY = adjY;
573  m_adjZ = adjZ;
574}
575
576void 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
585void H3LIS331DL::getRawXYZ(int *x, int *y, int*z)
586{
587  *x = m_rawX;
588  *y = m_rawY;
589  *z = m_rawZ;
590}
591
592void 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
600float *H3LIS331DL::getAcceleration()
601{
602  float *v = new float[3];
603  getAcceleration(&v[0], &v[1], &v[2]);
604  return v;
605}
606
607int *H3LIS331DL::getRawXYZ()
608{
609  int *v = new int[3];
610  getRawXYZ(&v[0], &v[1], &v[2]);
611  return v;
612}
613
614int *H3LIS331DL::getXYZ()
615{
616  int *v = new int[3];
617  getXYZ(&v[0], &v[1], &v[2]);
618  return v;
619}
620#endif
621