l3gd20.hpp

/*
 * Author: Lay, Kuan Loon <kuan.loon.lay@intel.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>

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;
    L3GD20(int device);

    ~L3GD20();

    void installISR(void (*isr)(char*), 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);

    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);

  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
};
}