upm  1.6.0
Sensor/Actuator repository for libmraa (v1.9.0)
l3gd20.hpp
1 /*
2  * Author: Lay, Kuan Loon <kuan.loon.lay@intel.com>
3  * Jon Trulson <jtrulson@ics.com>
4  * Copyright (c) 2016 Intel Corporation.
5  *
6  * Permission is hereby granted, free of charge, to any person obtaining
7  * a copy of this software and associated documentation files (the
8  * "Software"), to deal in the Software without restriction, including
9  * without limitation the rights to use, copy, modify, merge, publish,
10  * distribute, sublicense, and/or sell copies of the Software, and to
11  * permit persons to whom the Software is furnished to do so, subject to
12  * the following conditions:
13  *
14  * The above copyright notice and this permission notice shall be
15  * included in all copies or substantial portions of the Software.
16  *
17  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
18  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
19  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
20  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
21  * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
22  * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
23  * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
24  *
25  * Thanks to https://github.com/01org/android-iio-sensors-hal for gyroscope
26  * calibration and denoise algorithm.
27  */
28 #pragma once
29 
30 #include <string>
31 #include <mraa/iio.h>
32 #include <mraa/i2c.hpp>
33 
34 #define L3GD20_DEFAULT_I2C_BUS 0
35 // if SDO tied to GND
36 #define L3GD20_DEFAULT_I2C_ADDR 0x6a
37 #define L3GD20_DEFAULT_CHIP_ID 0xd4
38 // the 'H' variant uses a different chip id
39 #define L3GD20H_DEFAULT_CHIP_ID 0xd7
40 
41 namespace upm
42 {
77 class L3GD20
78 {
79  public:
80  typedef struct {
81  float bias_x, bias_y, bias_z;
82  int count;
83  float min_x, min_y, min_z;
84  float max_x, max_y, max_z;
85  } gyro_cal_t;
86 
87  typedef struct {
88  float* buff;
89  unsigned int idx;
90  unsigned int count;
91  unsigned int sample_size;
93 
94  // NOTE: Reserved registers must not be written into or permanent
95  // device damage can result. Reading from them may return
96  // indeterminate values. Registers containing reserved bitfields
97  // must be written as 0. Reading reserved bitfields may return
98  // indeterminate values.
99 
103  typedef enum {
104  // 0x00-0x0e reserved
105 
106  REG_WHO_AM_I = 0x0f,
107 
108  // 0x10-0x1f reserved
109 
110  REG_CTRL_REG1 = 0x20,
111  REG_CTRL_REG2 = 0x21,
112  REG_CTRL_REG3 = 0x22,
113  REG_CTRL_REG4 = 0x23,
114  REG_CTRL_REG5 = 0x24,
115 
116  REG_REFERENCE = 0x25,
117 
118  REG_OUT_TEMPERATURE = 0x26,
119 
120  REG_STATUS_REG = 0x27,
121 
122  // output registers (also for FIFO output)
123  REG_OUT_X_L = 0x28,
124  REG_OUT_X_H = 0x29,
125 
126  REG_OUT_Y_L = 0x2a,
127  REG_OUT_Y_H = 0x2b,
128 
129  REG_OUT_Z_L = 0x2c,
130  REG_OUT_Z_H = 0x2d,
131 
132  REG_FIFO_CTRL_REG = 0x2e,
133  REG_FIFO_SRC_REG = 0x2f,
134 
135  REG_INT1_CFG = 0x30,
136  REG_INT1_SRC = 0x31,
137 
138  REG_INT1_TSH_XH = 0x32,
139  REG_INT1_TSH_XL = 0x33,
140 
141  REG_INT1_TSH_YH = 0x34,
142  REG_INT1_TSH_YL = 0x35,
143 
144  REG_INT1_TSH_ZH = 0x36,
145  REG_INT1_TSH_ZL = 0x37,
146  REG_INT1_DURATION = 0x38
147  } L3GD20_REGS_T;
148 
152  typedef enum {
153  CTRL_REG1_YEN = 0x01,
154  CTRL_REG1_XEN = 0x02,
155  CTRL_REG1_ZEN = 0x04,
156  CTRL_REG1_PD = 0x08,
157 
158  CTRL_REG1_BW0 = 0x10, // bandwidth
159  CTRL_REG1_BW1 = 0x20,
160  _CTRL_REG1_BW_MASK = 3,
161  _CTRL_REG1_BW_SHIFT = 4,
162 
163  CTRL_REG1_DR0 = 0x40, // data rate
164  CTRL_REG1_DR1 = 0x80,
165  _CTRL_REG1_DR_MASK = 3,
166  _CTRL_REG1_DR_SHIFT = 6,
167 
168  // together the BW and DR modes represent an output data rate
169  // (ODR) and a filter cut-off. So here, we will create a 'fake'
170  // bitfield that can be used directly with the ODR_CUTOFF enum
171  _CTRL_REG1_ODR_CUTOFF0 = 0x10,
172  _CTRL_REG1_ODR_CUTOFF1 = 0x20,
173  _CTRL_REG1_ODR_CUTOFF2 = 0x40,
174  _CTRL_REG1_ODR_CUTOFF3 = 0x80,
175  _CTRL_REG1_ODR_CUTOFF_MASK = 15,
176  _CTRL_REG1_ODR_CUTOFF_SHIFT = 4
177 
179 
183  typedef enum {
184  ODR_CUTOFF_95_12_5 = 0, // ODR 95Hz, CO 12.5
185  ODR_CUTOFF_95_25 = 1, // ODR 95Hz, CO 25
186  // 2 and 3 same as 1
187 
188  ODR_CUTOFF_190_12_5 = 4,
189  ODR_CUTOFF_190_25 = 5,
190  ODR_CUTOFF_190_50 = 6,
191  ODR_CUTOFF_190_70 = 7,
192 
193  ODR_CUTOFF_380_20 = 8,
194  ODR_CUTOFF_380_25 = 9,
195  ODR_CUTOFF_380_50 = 10,
196  ODR_CUTOFF_380_100 = 11,
197 
198  ODR_CUTOFF_760_30 = 12,
199  ODR_CUTOFF_760_35 = 13,
200  ODR_CUTOFF_760_50 = 14,
201  ODR_CUTOFF_760_100 = 15
202  } ODR_CUTOFF_T;
203 
208  typedef enum {
209  POWER_DOWN,
210  POWER_SLEEP,
211  POWER_NORMAL
212  } POWER_MODES_T;
213 
217  typedef enum {
218  _CTRL_REG2_RESERVED_BITS = 0x40 | 0x80,
219 
220  CTRL_REG2_HPCF0 = 0x01, // highpass filter cutoff
221  CTRL_REG2_HPCF1 = 0x02,
222  CTRL_REG2_HPCF2 = 0x04,
223  CTRL_REG2_HPCF3 = 0x08,
224  _CTRL_REG2_HPCF_MASK = 15,
225  _CTRL_REG2_HPCF_SHIFT = 0,
226 
227  CTRL_REG2_HPM0 = 0x10, // highpass filter mode
228  CTRL_REG2_HPM1 = 0x20,
229  _CTRL_REG2_HPM_MASK = 3,
230  _CTRL_REG2_HPM_SHIFT = 4
231 
232  // 0x40-0x80 reserved
234 
235 
239  typedef enum {
240  HPCF_7_2 = 0, // 7.2Hz CO (w/ ODR@95Hz)
241  HPCF_3_5 = 1,
242  HPCF_1_8 = 2,
243  HPCF_0_9 = 3,
244  HPCF_0_45 = 4,
245  HPCF_0_18 = 5,
246  HPCF_0_09 = 6,
247  HPCF_0_045 = 7,
248  HPCF_0_018 = 8,
249  HPCF_0_009 = 9
250  } HPCF_T;
251 
255  typedef enum {
256  HPM_NORMAL_RESET_FILTER = 0,
257  HPM_REFERENCE_SIGNAL = 1,
258  HPM_NORMAL = 2,
259  HPM_AUTORESET_ON_INT = 3
260  } HPM_T;
261 
265  typedef enum {
266  CTRL_REG3_I2_EMPTY = 0x01,
267  CTRL_REG3_I2_ORUN = 0x02,
268  CTRL_REG3_I2_WTM = 0x04,
269  CTRL_REG3_I2_DRDY = 0x08,
270  CTRL_REG3_PP_OD = 0x10,
271  CTRL_REG3_H_LACTIVE = 0x20,
272  CTRL_REG3_I1_BOOT = 0x40,
273  CTRL_REG3_I1_INT1 = 0x80
275 
279  typedef enum {
280  _CTRL_REG4_RESERVED_BITS = 0x02 | 0x04 | 0x08,
281 
282  CTRL_REG4_SIM = 0x01, // SPI 3 or 4 wire
283  // 0x02-0x08 reserved
284 
285  CTRL_REG4_FS0 = 0x10, // full scale select
286  CTRL_REG4_FS1 = 0x20,
287  _CTRL_REG4_FS_MASK = 3,
288  _CTRL_REG4_FS_SHIFT = 4,
289 
290  CTRL_REG4_BLE = 0x40, // endian selection
291  CTRL_REG4_BDU = 0x80 // block updating
293 
297  typedef enum {
298  FS_250 = 0, // 250 deg/s
299  FS_500 = 1,
300  FS_2000 = 2
301  } FS_T;
302 
306  typedef enum {
307  _CTRL_REG5_RESERVED_BITS = 0x20,
308 
309  CTRL_REG5_OUT_SEL0 = 0x01,
310  CTRL_REG5_OUT_SEL1 = 0x02,
311  _CTRL_REG5_OUT_SEL_MASK = 3,
312  _CTRL_REG5_OUT_SEL_SHIFT = 0,
313 
314  CTRL_REG5_INT1_SEL0 = 0x04,
315  CTRL_REG5_INT1_SEL1 = 0x08,
316  _CTRL_REG5_INT1_SEL_MASK = 3,
317  _CTRL_REG5_INT1_SEL_SHIFT = 2,
318 
319  CTRL_REG5_HPEN = 0x10,
320 
321  // 0x20 reserved
322  CTRL_REG5_FIFO_EN = 0x40,
323  CTRL_REG5_BOOT = 0x80
325 
329  typedef enum {
330  STATUS_REG_XDA = 0x01, // axis data avail
331  STATUS_REG_YDA = 0x02,
332  STATUS_REG_ZDA = 0x04,
333  STATUS_REG_ZYXDA = 0x08,
334 
335  STATUS_REG_XOR = 0x10, // axis data overrun
336  STATUS_REG_YOR = 0x20,
337  STATUS_REG_ZOR = 0x40,
338  STATUS_REG_ZYXOR = 0x80
340 
344  typedef enum {
345  FIFO_CTRL_REG_WTM0 = 0x01, // FIFO watermark
346  FIFO_CTRL_REG_WTM1 = 0x02,
347  FIFO_CTRL_REG_WTM2 = 0x04,
348  FIFO_CTRL_REG_WTM3 = 0x08,
349  FIFO_CTRL_REG_WTM4 = 0x10,
350  _FIFO_CTRL_REG_WTM_MASK = 31,
351  _FIFO_CTRL_REG_WTM_SHIFT = 0,
352 
353  FIFO_CTRL_REG_FM0 = 0x20, // FIFO mode
354  FIFO_CTRL_REG_FM1 = 0x40,
355  FIFO_CTRL_REG_FM2 = 0x80,
356  _FIFO_CTRL_REG_FM_MASK = 7,
357  _FIFO_CTRL_REG_FM_SHIFT = 5
359 
363  typedef enum {
364  FIFO_MODE_BYPASS = 0,
365  FIFO_MODE_FIFO = 1,
366  FIFO_MODE_STREAM = 2,
367  FIFO_MODE_STREAM_TO_FIFO = 3,
368  FIFO_MODE_BYPASS_TO_STREAM = 4
369  } FIFO_MODE_T;
370 
371 
375  typedef enum {
376  FIFO_SRC_REG_FSS0 = 0x01, // FIFO stored data level
377  FIFO_SRC_REG_FSS1 = 0x02,
378  FIFO_SRC_REG_FSS2 = 0x04,
379  FIFO_SRC_REG_FSS3 = 0x08,
380  FIFO_SRC_REG_FSS4 = 0x10,
381  _FIFO_SRC_REG_FSS_MASK = 31,
382  _FIFO_SRC_REG_FSS_SHIFT = 0,
383 
384  FIFO_SRC_REG_EMPTY = 0x20,
385  FIFO_SRC_REG_OVRN = 0x40,
386  FIFO_SRC_REG_WTM = 0x80
387  } FIFO_SRC_BITS_T;
388 
392  typedef enum {
393  INT1_CFG_XLIE = 0x01, // low intr en
394  INT1_CFG_XHIE = 0x02, // high intr en
395 
396  INT1_CFG_YLIE = 0x04,
397  INT1_CFG_YHIE = 0x08,
398 
399  INT1_CFG_ZLIE = 0x10,
400  INT1_CFG_ZHIE = 0x20,
401 
402  INT1_CFG_LIR = 0x40,
403  INT1_CFG_AND_OR = 0x80
404  } INT1_CFG_BITS_T;
405 
409  typedef enum {
410  _INT1_SRC_RESERVED_BITS = 0x80,
411 
412  INT1_SRC_XL = 0x01, // X low intr
413  INT1_SRC_XH = 0x02, // X high intr
414 
415  INT1_SRC_YL = 0x04,
416  INT1_SRC_YH = 0x08,
417 
418  INT1_SRC_ZL = 0x10,
419  INT1_SRC_ZH = 0x20,
420 
421  INT1_SRC_IA = 0x40 // intr active
422 
423  // 0x80 reserved
424  } INT1_SRC_BITS_T;
425 
429  typedef enum {
430  INT1_DURATION_D0 = 0x01,
431  INT1_DURATION_D1 = 0x02,
432  INT1_DURATION_D2 = 0x04,
433  INT1_DURATION_D3 = 0x08,
434  INT1_DURATION_D4 = 0x10,
435  INT1_DURATION_D5 = 0x20,
436  INT1_DURATION_D6 = 0x40,
437 
438  INT1_DURATION_WAIT = 0x80
440 
441 
447  L3GD20(int device);
448 
455  L3GD20(int bus, int addr);
456 
460  ~L3GD20();
461 
467  uint8_t getChipID();
468 
480  void getGyroscope(float *x, float *y, float *z);
481 
487  void setPowerMode(POWER_MODES_T mode);
488 
495  void setRange(FS_T range);
496 
502  void update();
503 
513  float getTemperature(bool fahrenheit=false);
514 
521  void setODR(ODR_CUTOFF_T odr);
522 
531  void enableBDU(bool enable);
532 
540  uint8_t getStatusBits();
541 
550  void installISR(void (*isr)(char*, void*), void* arg);
551 
558  int64_t getChannelValue(unsigned char* input, mraa_iio_channel* chan);
559 
565  bool enableBuffer(int length);
566 
570  bool disableBuffer();
571 
580  bool setScale(const float scale);
581 
590  bool setSamplingFrequency(const float sampling_frequency);
591 
595  bool enable3AxisChannel();
596 
604  bool extract3Axis(char* data, float* x, float* y, float* z);
605 
609  void initCalibrate();
610 
614  bool getCalibratedStatus();
615 
619  void getCalibratedData(float* bias_x, float* bias_y, float* bias_z);
620 
624  void loadCalibratedData(float bias_x, float bias_y, float bias_z);
625 
632  uint8_t readReg(uint8_t reg);
633 
641  int readRegs(uint8_t reg, uint8_t *buffer, int len);
642 
649  void writeReg(uint8_t reg, uint8_t val);
650 
657  bool gyroCollect(float x, float y, float z);
658 
665  void gyroDenoiseMedian(float* x, float* y, float* z);
666 
672  float median(float* queue, unsigned int size);
673 
681  unsigned int
682  partition(float* list, unsigned int left, unsigned int right, unsigned int pivot_index);
683 
690  void clampGyroReadingsToZero(float* x, float* y, float* z);
691 
692  protected:
693  mraa::I2c *m_i2c;
694  float m_gyrScale;
695  float m_gyrX;
696  float m_gyrY;
697  float m_gyrZ;
698  float m_temperature;
699 
700  private:
701  mraa_iio_context m_iio;
702 
703  int m_iio_device_num;
704  bool m_mount_matrix_exist; // is mount matrix exist
705  float m_mount_matrix[9]; // mount matrix
706  float m_scale; // gyroscope data scale
707  int m_event_count; // sample data arrive
708  bool m_calibrated; // calibrate state
709  gyro_cal_t m_cal_data; // calibrate data
710  filter_median_t m_filter; // filter data
711 };
712 }
uint8_t getStatusBits()
Definition: l3gd20.cxx:340
int64_t getChannelValue(unsigned char *input, mraa_iio_channel *chan)
Definition: l3gd20.cxx:353
ODR_CUTOFF_T
Definition: l3gd20.hpp:183
void writeReg(uint8_t reg, uint8_t val)
Definition: l3gd20.cxx:183
void update()
Definition: l3gd20.cxx:276
void initCalibrate()
Definition: l3gd20.cxx:504
~L3GD20()
Definition: l3gd20.cxx:162
bool setSamplingFrequency(const float sampling_frequency)
Definition: l3gd20.cxx:432
bool disableBuffer()
Definition: l3gd20.cxx:414
L3GD20(int device)
Definition: l3gd20.cxx:49
int readRegs(uint8_t reg, uint8_t *buffer, int len)
Definition: l3gd20.cxx:177
uint8_t readReg(uint8_t reg)
Definition: l3gd20.cxx:172
void setODR(ODR_CUTOFF_T odr)
Definition: l3gd20.cxx:330
bool enableBuffer(int length)
Definition: l3gd20.cxx:405
L3GD20_REGS_T
Definition: l3gd20.hpp:103
STATUS_REG_BITS_T
Definition: l3gd20.hpp:329
bool setScale(const float scale)
Definition: l3gd20.cxx:421
void gyroDenoiseMedian(float *x, float *y, float *z)
Definition: l3gd20.cxx:596
uint8_t getChipID()
Definition: l3gd20.cxx:192
C++ API for the kxtj3 driver.
Definition: a110x.hpp:29
bool getCalibratedStatus()
Definition: l3gd20.cxx:514
CTRL_REG3_BITS_T
Definition: l3gd20.hpp:265
float getTemperature(bool fahrenheit=false)
Definition: l3gd20.cxx:322
void enableBDU(bool enable)
Definition: l3gd20.cxx:252
INT1_CFG_BITS_T
Definition: l3gd20.hpp:392
FS_T
Definition: l3gd20.hpp:297
void getCalibratedData(float *bias_x, float *bias_y, float *bias_z)
Definition: l3gd20.cxx:520
void setRange(FS_T range)
Definition: l3gd20.cxx:226
HPM_T
Definition: l3gd20.hpp:255
Definition: l3gd20.hpp:80
FIFO_MODE_T
Definition: l3gd20.hpp:363
float median(float *queue, unsigned int size)
Definition: l3gd20.cxx:626
void setPowerMode(POWER_MODES_T mode)
Definition: l3gd20.cxx:197
void loadCalibratedData(float bias_x, float bias_y, float bias_z)
Definition: l3gd20.cxx:528
POWER_MODES_T
Definition: l3gd20.hpp:208
FIFO_SRC_BITS_T
Definition: l3gd20.hpp:375
bool gyroCollect(float x, float y, float z)
Definition: l3gd20.cxx:537
bool extract3Axis(char *data, float *x, float *y, float *z)
Definition: l3gd20.cxx:455
bool enable3AxisChannel()
Definition: l3gd20.cxx:439
FIFO_CTRL_REG_BITS_T
Definition: l3gd20.hpp:344
void clampGyroReadingsToZero(float *x, float *y, float *z)
Definition: l3gd20.cxx:687
CTRL_REG2_BITS_T
Definition: l3gd20.hpp:217
unsigned int partition(float *list, unsigned int left, unsigned int right, unsigned int pivot_index)
Definition: l3gd20.cxx:657
CTRL_REG4_BITS_T
Definition: l3gd20.hpp:279
void installISR(void(*isr)(char *, void *), void *arg)
Definition: l3gd20.cxx:347
Definition: l3gd20.hpp:87
L3GD20 Tri-axis Digital Gyroscope API.
Definition: l3gd20.hpp:77
HPCF_T
Definition: l3gd20.hpp:239
INT1_DURATION_BITS_T
Definition: l3gd20.hpp:429
CTRL_REG5_BITS_T
Definition: l3gd20.hpp:306
void getGyroscope(float *x, float *y, float *z)
Definition: l3gd20.cxx:264
INT1_SRC_BITS_T
Definition: l3gd20.hpp:409
CTRL_REG1_BITS_T
Definition: l3gd20.hpp:152