l3gd20.hpp

/*
 * Author: Lay, Kuan Loon <kuan.loon.lay@intel.com>
 *         Jon Trulson <jtrulson@ics.com>
 * Copyright (c) 2016 Intel Corporation.
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 * Thanks to https://github.com/01org/android-iio-sensors-hal for gyroscope
 * calibration and denoise algorithm.
 */
#pragma once

#include <string>
#include <mraa/iio.h>
#include <mraa/i2c.hpp>

#define L3GD20_DEFAULT_I2C_BUS                      0
// if SDO tied to GND
#define L3GD20_DEFAULT_I2C_ADDR                     0x6a
#define L3GD20_DEFAULT_CHIP_ID                      0xd4
// the 'H' variant uses a different chip id
#define L3GD20H_DEFAULT_CHIP_ID                     0xd7

namespace upm
{
class L3GD20
{
  public:
    typedef struct {
        float bias_x, bias_y, bias_z;
        int count;
        float min_x, min_y, min_z;
        float max_x, max_y, max_z;
    } gyro_cal_t;

    typedef struct {
        float* buff;
        unsigned int idx;
        unsigned int count;
        unsigned int sample_size;
    } filter_median_t;

    // NOTE: Reserved registers must not be written into or permanent
    // device damage can result.  Reading from them may return
    // indeterminate values.  Registers containing reserved bitfields
    // must be written as 0.  Reading reserved bitfields may return
    // indeterminate values.

    typedef enum {
      // 0x00-0x0e reserved

      REG_WHO_AM_I                            = 0x0f,

      // 0x10-0x1f reserved

      REG_CTRL_REG1                           = 0x20,
      REG_CTRL_REG2                           = 0x21,
      REG_CTRL_REG3                           = 0x22,
      REG_CTRL_REG4                           = 0x23,
      REG_CTRL_REG5                           = 0x24,

      REG_REFERENCE                           = 0x25,

      REG_OUT_TEMPERATURE                     = 0x26,

      REG_STATUS_REG                          = 0x27,

      // output registers (also for FIFO output)
      REG_OUT_X_L                             = 0x28,
      REG_OUT_X_H                             = 0x29,

      REG_OUT_Y_L                             = 0x2a,
      REG_OUT_Y_H                             = 0x2b,

      REG_OUT_Z_L                             = 0x2c,
      REG_OUT_Z_H                             = 0x2d,

      REG_FIFO_CTRL_REG                       = 0x2e,
      REG_FIFO_SRC_REG                        = 0x2f,

      REG_INT1_CFG                            = 0x30,
      REG_INT1_SRC                            = 0x31,

      REG_INT1_TSH_XH                         = 0x32,
      REG_INT1_TSH_XL                         = 0x33,

      REG_INT1_TSH_YH                         = 0x34,
      REG_INT1_TSH_YL                         = 0x35,

      REG_INT1_TSH_ZH                         = 0x36,
      REG_INT1_TSH_ZL                         = 0x37,
      REG_INT1_DURATION                       = 0x38
    } L3GD20_REGS_T;

    typedef enum {
      CTRL_REG1_YEN                           = 0x01,
      CTRL_REG1_XEN                           = 0x02,
      CTRL_REG1_ZEN                           = 0x04,
      CTRL_REG1_PD                            = 0x08,

      CTRL_REG1_BW0                           = 0x10, // bandwidth
      CTRL_REG1_BW1                           = 0x20,
      _CTRL_REG1_BW_MASK                      = 3,
      _CTRL_REG1_BW_SHIFT                     = 4,

      CTRL_REG1_DR0                           = 0x40, // data rate
      CTRL_REG1_DR1                           = 0x80,
      _CTRL_REG1_DR_MASK                      = 3,
      _CTRL_REG1_DR_SHIFT                     = 6,

      // together the BW and DR modes represent an output data rate
      // (ODR) and a filter cut-off.  So here, we will create a 'fake'
      // bitfield that can be used directly with the ODR_CUTOFF enum
      _CTRL_REG1_ODR_CUTOFF0                  = 0x10,
      _CTRL_REG1_ODR_CUTOFF1                  = 0x20,
      _CTRL_REG1_ODR_CUTOFF2                  = 0x40,
      _CTRL_REG1_ODR_CUTOFF3                  = 0x80,
      _CTRL_REG1_ODR_CUTOFF_MASK              = 15,
      _CTRL_REG1_ODR_CUTOFF_SHIFT             = 4

    } CTRL_REG1_BITS_T;

    typedef enum {
      ODR_CUTOFF_95_12_5                      = 0, // ODR 95Hz, CO 12.5
      ODR_CUTOFF_95_25                        = 1, // ODR 95Hz, CO 25
      // 2 and 3 same as 1

      ODR_CUTOFF_190_12_5                     = 4,
      ODR_CUTOFF_190_25                       = 5,
      ODR_CUTOFF_190_50                       = 6,
      ODR_CUTOFF_190_70                       = 7,

      ODR_CUTOFF_380_20                       = 8,
      ODR_CUTOFF_380_25                       = 9,
      ODR_CUTOFF_380_50                       = 10,
      ODR_CUTOFF_380_100                      = 11,

      ODR_CUTOFF_760_30                       = 12,
      ODR_CUTOFF_760_35                       = 13,
      ODR_CUTOFF_760_50                       = 14,
      ODR_CUTOFF_760_100                      = 15
    } ODR_CUTOFF_T;

    typedef enum {
      POWER_DOWN,
      POWER_SLEEP,
      POWER_NORMAL
    } POWER_MODES_T;

    typedef enum {
      _CTRL_REG2_RESERVED_BITS                = 0x40 | 0x80,

      CTRL_REG2_HPCF0                         = 0x01, // highpass filter cutoff
      CTRL_REG2_HPCF1                         = 0x02,
      CTRL_REG2_HPCF2                         = 0x04,
      CTRL_REG2_HPCF3                         = 0x08,
      _CTRL_REG2_HPCF_MASK                    = 15,
      _CTRL_REG2_HPCF_SHIFT                   = 0,

      CTRL_REG2_HPM0                          = 0x10, // highpass filter mode
      CTRL_REG2_HPM1                          = 0x20,
      _CTRL_REG2_HPM_MASK                     = 3,
      _CTRL_REG2_HPM_SHIFT                    = 4

      // 0x40-0x80 reserved
    } CTRL_REG2_BITS_T;


    typedef enum {
      HPCF_7_2                                = 0, // 7.2Hz CO (w/ ODR@95Hz)
      HPCF_3_5                                = 1,
      HPCF_1_8                                = 2,
      HPCF_0_9                                = 3,
      HPCF_0_45                               = 4,
      HPCF_0_18                               = 5,
      HPCF_0_09                               = 6,
      HPCF_0_045                              = 7,
      HPCF_0_018                              = 8,
      HPCF_0_009                              = 9
    } HPCF_T;

    typedef enum {
      HPM_NORMAL_RESET_FILTER                 = 0,
      HPM_REFERENCE_SIGNAL                    = 1,
      HPM_NORMAL                              = 2,
      HPM_AUTORESET_ON_INT                    = 3
    } HPM_T;

    typedef enum {
      CTRL_REG3_I2_EMPTY                      = 0x01,
      CTRL_REG3_I2_ORUN                       = 0x02,
      CTRL_REG3_I2_WTM                        = 0x04,
      CTRL_REG3_I2_DRDY                       = 0x08,
      CTRL_REG3_PP_OD                         = 0x10,
      CTRL_REG3_H_LACTIVE                     = 0x20,
      CTRL_REG3_I1_BOOT                       = 0x40,
      CTRL_REG3_I1_INT1                       = 0x80
    } CTRL_REG3_BITS_T;

    typedef enum {
      _CTRL_REG4_RESERVED_BITS                = 0x02 | 0x04 | 0x08,

      CTRL_REG4_SIM                           = 0x01, // SPI 3 or 4 wire
      // 0x02-0x08 reserved

      CTRL_REG4_FS0                           = 0x10, // full scale select
      CTRL_REG4_FS1                           = 0x20,
      _CTRL_REG4_FS_MASK                      = 3,
      _CTRL_REG4_FS_SHIFT                     = 4,

      CTRL_REG4_BLE                           = 0x40, // endian selection
      CTRL_REG4_BDU                           = 0x80  // block updating
    } CTRL_REG4_BITS_T;

    typedef enum {
      FS_250                                  = 0, // 250 deg/s
      FS_500                                  = 1,
      FS_2000                                 = 2
    } FS_T;

    typedef enum {
      _CTRL_REG5_RESERVED_BITS                = 0x20,

      CTRL_REG5_OUT_SEL0                      = 0x01,
      CTRL_REG5_OUT_SEL1                      = 0x02,
      _CTRL_REG5_OUT_SEL_MASK                 = 3,
      _CTRL_REG5_OUT_SEL_SHIFT                = 0,

      CTRL_REG5_INT1_SEL0                     = 0x04,
      CTRL_REG5_INT1_SEL1                     = 0x08,
      _CTRL_REG5_INT1_SEL_MASK                = 3,
      _CTRL_REG5_INT1_SEL_SHIFT               = 2,

      CTRL_REG5_HPEN                          = 0x10,

      // 0x20 reserved
      CTRL_REG5_FIFO_EN                       = 0x40,
      CTRL_REG5_BOOT                          = 0x80
    } CTRL_REG5_BITS_T;

    typedef enum {
      STATUS_REG_XDA                          = 0x01, // axis data avail
      STATUS_REG_YDA                          = 0x02,
      STATUS_REG_ZDA                          = 0x04,
      STATUS_REG_ZYXDA                        = 0x08,

      STATUS_REG_XOR                          = 0x10, // axis data overrun
      STATUS_REG_YOR                          = 0x20,
      STATUS_REG_ZOR                          = 0x40,
      STATUS_REG_ZYXOR                        = 0x80
    } STATUS_REG_BITS_T;

    typedef enum {
      FIFO_CTRL_REG_WTM0                      = 0x01, // FIFO watermark
      FIFO_CTRL_REG_WTM1                      = 0x02,
      FIFO_CTRL_REG_WTM2                      = 0x04,
      FIFO_CTRL_REG_WTM3                      = 0x08,
      FIFO_CTRL_REG_WTM4                      = 0x10,
      _FIFO_CTRL_REG_WTM_MASK                 = 31,
      _FIFO_CTRL_REG_WTM_SHIFT                = 0,

      FIFO_CTRL_REG_FM0                       = 0x20, // FIFO mode
      FIFO_CTRL_REG_FM1                       = 0x40,
      FIFO_CTRL_REG_FM2                       = 0x80,
      _FIFO_CTRL_REG_FM_MASK                  = 7,
      _FIFO_CTRL_REG_FM_SHIFT                 = 5
    } FIFO_CTRL_REG_BITS_T;

    typedef enum {
      FIFO_MODE_BYPASS                        = 0,
      FIFO_MODE_FIFO                          = 1,
      FIFO_MODE_STREAM                        = 2,
      FIFO_MODE_STREAM_TO_FIFO                = 3,
      FIFO_MODE_BYPASS_TO_STREAM              = 4
    } FIFO_MODE_T;


    typedef enum {
      FIFO_SRC_REG_FSS0                       = 0x01, // FIFO stored data level
      FIFO_SRC_REG_FSS1                       = 0x02,
      FIFO_SRC_REG_FSS2                       = 0x04,
      FIFO_SRC_REG_FSS3                       = 0x08,
      FIFO_SRC_REG_FSS4                       = 0x10,
      _FIFO_SRC_REG_FSS_MASK                  = 31,
      _FIFO_SRC_REG_FSS_SHIFT                 = 0,

      FIFO_SRC_REG_EMPTY                      = 0x20,
      FIFO_SRC_REG_OVRN                       = 0x40,
      FIFO_SRC_REG_WTM                        = 0x80
    } FIFO_SRC_BITS_T;

    typedef enum {
      INT1_CFG_XLIE                           = 0x01, // low intr en
      INT1_CFG_XHIE                           = 0x02, // high intr en

      INT1_CFG_YLIE                           = 0x04,
      INT1_CFG_YHIE                           = 0x08,

      INT1_CFG_ZLIE                           = 0x10,
      INT1_CFG_ZHIE                           = 0x20,

      INT1_CFG_LIR                            = 0x40,
      INT1_CFG_AND_OR                         = 0x80
    } INT1_CFG_BITS_T;

    typedef enum {
      _INT1_SRC_RESERVED_BITS                 = 0x80,

      INT1_SRC_XL                             = 0x01, // X low intr
      INT1_SRC_XH                             = 0x02, // X high intr

      INT1_SRC_YL                             = 0x04,
      INT1_SRC_YH                             = 0x08,

      INT1_SRC_ZL                             = 0x10,
      INT1_SRC_ZH                             = 0x20,

      INT1_SRC_IA                             = 0x40 // intr active

      // 0x80 reserved
    } INT1_SRC_BITS_T;

    typedef enum {
      INT1_DURATION_D0                        = 0x01,
      INT1_DURATION_D1                        = 0x02,
      INT1_DURATION_D2                        = 0x04,
      INT1_DURATION_D3                        = 0x08,
      INT1_DURATION_D4                        = 0x10,
      INT1_DURATION_D5                        = 0x20,
      INT1_DURATION_D6                        = 0x40,

      INT1_DURATION_WAIT                      = 0x80
    } INT1_DURATION_BITS_T;


    L3GD20(int device);

    L3GD20(int bus, int addr);

    ~L3GD20();

    uint8_t getChipID();

    void getGyroscope(float *x, float *y, float *z);

    void setPowerMode(POWER_MODES_T mode);

    void setRange(FS_T range);

    void update();

   float getTemperature(bool fahrenheit=false);

    void setODR(ODR_CUTOFF_T odr);

    void enableBDU(bool enable);

    uint8_t getStatusBits();

    void installISR(void (*isr)(char*, void*), void* arg);

    int64_t getChannelValue(unsigned char* input, mraa_iio_channel* chan);

    bool enableBuffer(int length);

    bool disableBuffer();

    bool setScale(const float scale);

    bool setSamplingFrequency(const float sampling_frequency);

    bool enable3AxisChannel();

    bool extract3Axis(char* data, float* x, float* y, float* z);

    void initCalibrate();

    bool getCalibratedStatus();

    void getCalibratedData(float* bias_x, float* bias_y, float* bias_z);

    void loadCalibratedData(float bias_x, float bias_y, float bias_z);

    uint8_t readReg(uint8_t reg);

    int readRegs(uint8_t reg, uint8_t *buffer, int len);

    void writeReg(uint8_t reg, uint8_t val);

    bool gyroCollect(float x, float y, float z);

    void gyroDenoiseMedian(float* x, float* y, float* z);

    float median(float* queue, unsigned int size);

    unsigned int
    partition(float* list, unsigned int left, unsigned int right, unsigned int pivot_index);

    void clampGyroReadingsToZero(float* x, float* y, float* z);

  protected:
    mraa::I2c *m_i2c;
    float m_gyrScale;
    float m_gyrX;
    float m_gyrY;
    float m_gyrZ;
    float m_temperature;

  private:
    mraa_iio_context m_iio;

    int m_iio_device_num;
    bool m_mount_matrix_exist; // is mount matrix exist
    float m_mount_matrix[9];   // mount matrix
    float m_scale;             // gyroscope data scale
    int m_event_count;         // sample data arrive
    bool m_calibrated;         // calibrate state
    gyro_cal_t m_cal_data;     // calibrate data
    filter_median_t m_filter;  // filter data
};
}