L3GD20: Enable L3GD20 3-axis digital gyroscope library and example

L3GD20 is tri-axis gyroscope from STMicroelectronics.
This sensor can measure angular velocity in degree per second.

The library provided is libupm-l3gd20.so.0.4.0.
The example provided is l3gd20-example where it will print x,y,z axis when
trigger buffer data is ready.

This sensor requires calibration to be done for 2 seconds. Please place the
sensor on level surface.

As the sensor data is noisy, we have implemented denoise algorithm within the
sensor library.

Signed-off-by: Lay, Kuan Loon <kuan.loon.lay@intel.com>
Signed-off-by: Noel Eck <noel.eck@intel.com>

examples/c++/CMakeLists.txt

@@ -268,6 +268,7 @@ add_example (ds2413)
 add_example (ds18b20)
 add_example (bmp280)
 add_example (bno055)
+add_example (l3gd20)
 
 # These are special cases where you specify example binary, source file and module(s)
 include_directories (${PROJECT_SOURCE_DIR}/src)

examples/c++/l3gd20.cxx

@@ -0,0 +1,74 @@
+/*
+ * Author: Lay, Kuan Loon <kuan.loon.lay@intel.com>
+ * Copyright (c) 2015 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.
+ */
+
+#include <unistd.h>
+#include <iostream>
+#include <signal.h>
+#include "l3gd20.hpp"
+
+using namespace std;
+
+int shouldRun = true;
+upm::L3GD20* gyroscope;
+
+void
+sig_handler(int signo)
+{
+    if (signo == SIGINT)
+        shouldRun = false;
+}
+
+void
+data_callback(char* data)
+{
+    float x, y, z;
+    if (gyroscope->extract3Axis(data, &x, &y, &z))
+        printf("% .2f               % .2f               % .2f\n", x, y, z);
+}
+
+int
+main()
+{
+    signal(SIGINT, sig_handler);
+    //! [Interesting]
+    // Instantiate a L3GD20 Gyroscope Sensor on iio device 3
+    gyroscope = new upm::L3GD20(3);
+    gyroscope->setScale(0.001222);
+    gyroscope->setSamplingFrequency(95.0);
+    gyroscope->enable3AxisChannel();
+    gyroscope->installISR(data_callback, NULL);
+    gyroscope->enableBuffer(16);
+
+    while (shouldRun) {
+        sleep(1);
+    }
+    gyroscope->disableBuffer();
+
+    //! [Interesting]
+    cout << "Exiting" << endl;
+
+    delete gyroscope;
+
+    return 0;
+}

src/javaswig_blacklist

@@ -1,2 +1,3 @@
 nrf8001
 kxcjk1013
+l3gd20

src/l3gd20/CMakeLists.txt

@@ -0,0 +1,5 @@
+set (libname "l3gd20")
+set (libdescription "upm l3gd20 sensor module")
+set (module_src ${libname}.cxx)
+set (module_hpp ${libname}.hpp)
+upm_module_init()

src/l3gd20/jsupm_l3gd20.i

@@ -0,0 +1,8 @@
+%module jsupm_l3gd20
+%include "../upm.i"
+
+%{
+    #include "l3gd20.hpp"
+%}
+
+%include "l3gd20.hpp"

src/l3gd20/l3gd20.cxx

@@ -0,0 +1,462 @@
+/*
+ * Author: Lay, Kuan Loon <kuan.loon.lay@intel.com>
+ * Copyright (c) 2015 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.
+ */
+
+#include <iostream>
+#include <string>
+#include <stdexcept>
+#include <string.h>
+#include <math.h>
+#include "l3gd20.hpp"
+
+#define NUMBER_OF_BITS_IN_BYTE 8
+#define GYRO_MIN_SAMPLES 5 /* Drop first few gyro samples after enable */
+#define GYRO_MAX_ERR 0.05
+#define GYRO_DS_SIZE 100
+
+#define GYRO_DENOISE_MAX_SAMPLES 5
+#define GYRO_DENOISE_NUM_FIELDS 3
+
+using namespace upm;
+
+L3GD20::L3GD20(int device)
+{
+    float gyro_scale;
+    char trigger[64];
+
+    if (!(m_iio = mraa_iio_init(device))) {
+        throw std::invalid_argument(std::string(__FUNCTION__) +
+                                    ": mraa_iio_init() failed, invalid device?");
+        return;
+    }
+    m_scale = 1;
+    m_iio_device_num = device;
+    sprintf(trigger, "hrtimer-l3gd20-hr-dev%d", device);
+
+    if (mraa_iio_create_trigger(m_iio, trigger) != MRAA_SUCCESS)
+        fprintf(stderr, "Create trigger %s failed\n", trigger);
+
+    if (mraa_iio_get_mounting_matrix(m_iio, m_mount_matrix) == MRAA_SUCCESS)
+        m_mount_matrix_exist = true;
+    else
+        m_mount_matrix_exist = false;
+
+    if (mraa_iio_read_float(m_iio, "in_anglvel_x_scale", &gyro_scale) == MRAA_SUCCESS)
+        m_scale = gyro_scale;
+
+    m_event_count = 0;
+
+    // initial calibrate data
+    initCalibrate();
+
+    // initial denoise data
+    m_filter.buff =
+    (float*) calloc(GYRO_DENOISE_MAX_SAMPLES, sizeof(float) * GYRO_DENOISE_NUM_FIELDS);
+    if (m_filter.buff == NULL) {
+        throw std::invalid_argument(std::string(__FUNCTION__) +
+                                    ": mraa_iio_init() failed, calloc denoise data");
+        return;
+    }
+    m_filter.sample_size = GYRO_DENOISE_MAX_SAMPLES;
+    m_filter.count = 0;
+    m_filter.idx = 0;
+}
+
+L3GD20::~L3GD20()
+{
+    if (m_filter.buff) {
+        free(m_filter.buff);
+        m_filter.buff = NULL;
+    }
+    if(m_iio)
+        mraa_iio_close(m_iio);
+}
+
+#ifdef JAVACALLBACK
+void
+L3GD20::installISR(IsrCallback* cb)
+{
+    installISR(generic_callback_isr, cb);
+}
+#endif
+
+void
+L3GD20::installISR(void (*isr)(char*), void* arg)
+{
+    mraa_iio_trigger_buffer(m_iio, isr, NULL);
+}
+
+int64_t
+L3GD20::getChannelValue(unsigned char* input, mraa_iio_channel* chan)
+{
+    uint64_t u64 = 0;
+    int i;
+    int storagebits = chan->bytes * NUMBER_OF_BITS_IN_BYTE;
+    int realbits = chan->bits_used;
+    int zeroed_bits = storagebits - realbits;
+    uint64_t sign_mask;
+    uint64_t value_mask;
+
+    if (!chan->lendian)
+        for (i = 0; i < storagebits / NUMBER_OF_BITS_IN_BYTE; i++)
+            u64 = (u64 << 8) | input[i];
+    else
+        for (i = storagebits / NUMBER_OF_BITS_IN_BYTE - 1; i >= 0; i--)
+            u64 = (u64 << 8) | input[i];
+
+    u64 = (u64 >> chan->shift) & (~0ULL >> zeroed_bits);
+
+    if (!chan->signedd)
+        return (int64_t) u64; /* We don't handle unsigned 64 bits int */
+
+    /* Signed integer */
+
+    switch (realbits) {
+        case 0 ... 1:
+            return 0;
+
+        case 8:
+            return (int64_t)(int8_t) u64;
+
+        case 16:
+            return (int64_t)(int16_t) u64;
+
+        case 32:
+            return (int64_t)(int32_t) u64;
+
+        case 64:
+            return (int64_t) u64;
+
+        default:
+            sign_mask = 1 << (realbits - 1);
+            value_mask = sign_mask - 1;
+
+            if (u64 & sign_mask)
+                return -((~u64 & value_mask) + 1); /* Negative value: return 2-complement */
+            else
+                return (int64_t) u64; /* Positive value */
+    }
+}
+
+bool
+L3GD20::enableBuffer(int length)
+{
+    mraa_iio_write_int(m_iio, "buffer/length", length);
+    // enable must be last step, else will have error in writing above config
+    mraa_iio_write_int(m_iio, "buffer/enable", 1);
+    return true;
+}
+
+bool
+L3GD20::disableBuffer()
+{
+    mraa_iio_write_int(m_iio, "buffer/enable", 0);
+    return true;
+}
+
+bool
+L3GD20::setScale(float scale)
+{
+    mraa_iio_write_float(m_iio, "in_anglvel_x_scale", scale);
+    mraa_iio_write_float(m_iio, "in_anglvel_y_scale", scale);
+    mraa_iio_write_float(m_iio, "in_anglvel_z_scale", scale);
+    return true;
+}
+
+bool
+L3GD20::setSamplingFrequency(float sampling_frequency)
+{
+    mraa_iio_write_float(m_iio, "sampling_frequency", sampling_frequency);
+    return true;
+}
+
+bool
+L3GD20::enable3AxisChannel()
+{
+    char trigger[64];
+    sprintf(trigger, "l3gd20-hr-dev%d", m_iio_device_num);
+
+    mraa_iio_write_string(m_iio, "trigger/current_trigger", trigger);
+    mraa_iio_write_int(m_iio, "scan_elements/in_anglvel_x_en", 1);
+    mraa_iio_write_int(m_iio, "scan_elements/in_anglvel_y_en", 1);
+    mraa_iio_write_int(m_iio, "scan_elements/in_anglvel_z_en", 1);
+
+    // need update channel data size after enable
+    mraa_iio_update_channels(m_iio);
+    return true;
+}
+
+bool
+L3GD20::extract3Axis(char* data, float* x, float* y, float* z)
+{
+    mraa_iio_channel* channels = mraa_iio_get_channels(m_iio);
+    float tmp[3];
+    int iio_x, iio_y, iio_z;
+
+    m_event_count++;
+
+    if (m_event_count < GYRO_MIN_SAMPLES) {
+        /* drop the sample */
+        return false;
+    }
+
+    iio_x = getChannelValue((unsigned char*) (data + channels[0].location), &channels[0]);
+    iio_y = getChannelValue((unsigned char*) (data + channels[1].location), &channels[1]);
+    iio_z = getChannelValue((unsigned char*) (data + channels[2].location), &channels[2]);
+
+    *x = (iio_x * m_scale);
+    *y = (iio_y * m_scale);
+    *z = (iio_z * m_scale);
+
+    if (m_mount_matrix_exist) {
+        tmp[0] = *x * m_mount_matrix[0] + *y * m_mount_matrix[1] + *z * m_mount_matrix[2];
+        tmp[1] = *x * m_mount_matrix[3] + *y * m_mount_matrix[4] + *z * m_mount_matrix[5];
+        tmp[2] = *x * m_mount_matrix[6] + *y * m_mount_matrix[7] + *z * m_mount_matrix[8];
+
+        *x = tmp[0];
+        *y = tmp[1];
+        *z = tmp[2];
+    }
+
+    /* Attempt gyroscope calibration if we have not reached this state */
+    if (m_calibrated == false)
+        m_calibrated = gyroCollect(*x, *y, *z);
+
+    *x = *x - m_cal_data.bias_x;
+    *y = *y - m_cal_data.bias_y;
+    *z = *z - m_cal_data.bias_z;
+
+
+    gyroDenoiseMedian(x, y, z);
+    clampGyroReadingsToZero(x, y, z);
+
+    return true;
+}
+
+void
+L3GD20::initCalibrate()
+{
+    m_calibrated = false;
+    m_cal_data.count = 0;
+    m_cal_data.bias_x = m_cal_data.bias_y = m_cal_data.bias_z = 0;
+    m_cal_data.min_x = m_cal_data.min_y = m_cal_data.min_z = 1.0;
+    m_cal_data.max_x = m_cal_data.max_y = m_cal_data.max_z = -1.0;
+}
+
+bool
+L3GD20::getCalibratedStatus()
+{
+    return m_calibrated;
+}
+
+void
+L3GD20::getCalibratedData(float* bias_x, float* bias_y, float* bias_z)
+{
+    *bias_x = m_cal_data.bias_x;
+    *bias_y = m_cal_data.bias_y;
+    *bias_z = m_cal_data.bias_z;
+}
+
+void
+L3GD20::loadCalibratedData(float bias_x, float bias_y, float bias_z)
+{
+    m_calibrated = true;
+    m_cal_data.bias_x = bias_x;
+    m_cal_data.bias_y = bias_y;
+    m_cal_data.bias_z = bias_z;
+}
+
+bool
+L3GD20::gyroCollect(float x, float y, float z)
+{
+    /* Analyze gyroscope data */
+
+    if (fabs(x) >= 1 || fabs(y) >= 1 || fabs(z) >= 1) {
+        /* We're supposed to be standing still ; start over */
+        m_cal_data.count = 0;
+        m_cal_data.bias_x = m_cal_data.bias_y = m_cal_data.bias_z = 0;
+        m_cal_data.min_x = m_cal_data.min_y = m_cal_data.min_z = 1.0;
+        m_cal_data.max_x = m_cal_data.max_y = m_cal_data.max_z = -1.0;
+
+        return false; /* Uncalibrated */
+    }
+
+    /* Thanks to https://github.com/01org/android-iio-sensors-hal for calibration algorithm */
+    if (m_cal_data.count < GYRO_DS_SIZE) {
+        if (x < m_cal_data.min_x)
+            m_cal_data.min_x = x;
+
+        if (y < m_cal_data.min_y)
+            m_cal_data.min_y = y;
+
+        if (z < m_cal_data.min_z)
+            m_cal_data.min_z = z;
+
+        if (x > m_cal_data.max_x)
+            m_cal_data.max_x = x;
+
+        if (y > m_cal_data.max_y)
+            m_cal_data.max_y = y;
+
+        if (z > m_cal_data.max_z)
+            m_cal_data.max_z = z;
+
+        if (fabs(m_cal_data.max_x - m_cal_data.min_x) <= GYRO_MAX_ERR &&
+            fabs(m_cal_data.max_y - m_cal_data.min_y) <= GYRO_MAX_ERR &&
+            fabs(m_cal_data.max_z - m_cal_data.min_z) <= GYRO_MAX_ERR)
+            m_cal_data.count++; /* One more conformant sample */
+        else {
+            /* Out of spec sample ; start over */
+            m_calibrated = false;
+            m_cal_data.count = 0;
+            m_cal_data.bias_x = m_cal_data.bias_y = m_cal_data.bias_z = 0;
+            m_cal_data.min_x = m_cal_data.min_y = m_cal_data.min_z = 1.0;
+            m_cal_data.max_x = m_cal_data.max_y = m_cal_data.max_z = -1.0;
+        }
+
+        return false; /* Still uncalibrated */
+    }
+
+    /* We got enough stable samples to estimate gyroscope bias */
+    m_cal_data.bias_x = (m_cal_data.max_x + m_cal_data.min_x) / 2;
+    m_cal_data.bias_y = (m_cal_data.max_y + m_cal_data.min_y) / 2;
+    m_cal_data.bias_z = (m_cal_data.max_z + m_cal_data.min_z) / 2;
+
+    return true; /* Calibrated! */
+}
+
+void
+L3GD20::gyroDenoiseMedian(float* x, float* y, float* z)
+{
+    /* Thanks to https://github.com/01org/android-iio-sensors-hal for denoise algorithm */
+    unsigned int offset;
+
+    /* If we are at event count 1 reset the indices */
+    if (m_event_count == 1) {
+        m_filter.count = 0;
+        m_filter.idx = 0;
+    }
+
+    if (m_filter.count < m_filter.sample_size)
+        m_filter.count++;
+
+    offset = 0;
+    m_filter.buff[offset + m_filter.idx] = *x;
+    *x = median(m_filter.buff + offset, m_filter.count);
+
+    offset = m_filter.sample_size * 1;
+    m_filter.buff[offset + m_filter.idx] = *y;
+    *y = median(m_filter.buff + offset, m_filter.count);
+
+    offset = m_filter.sample_size * 2;
+    m_filter.buff[offset + m_filter.idx] = *z;
+    *z = median(m_filter.buff + offset, m_filter.count);
+
+    m_filter.idx = (m_filter.idx + 1) % m_filter.sample_size;
+}
+
+float
+L3GD20::median(float* queue, unsigned int size)
+{
+    /* http://en.wikipedia.org/wiki/Quickselect */
+
+    unsigned int left = 0;
+    unsigned int right = size - 1;
+    unsigned int pivot_index;
+    unsigned int median_index = (right / 2);
+    float temp[size];
+
+    memcpy(temp, queue, size * sizeof(float));
+
+    /* If the list has only one element return it */
+    if (left == right)
+        return temp[left];
+
+    while (left < right) {
+        pivot_index = (left + right) / 2;
+        pivot_index = partition(temp, left, right, pivot_index);
+        if (pivot_index == median_index)
+            return temp[median_index];
+        else if (pivot_index > median_index)
+            right = pivot_index - 1;
+        else
+            left = pivot_index + 1;
+    }
+
+    return temp[left];
+}
+
+unsigned int
+L3GD20::partition(float* list, unsigned int left, unsigned int right, unsigned int pivot_index)
+{
+    unsigned int i;
+    unsigned int store_index = left;
+    float aux;
+    float pivot_value = list[pivot_index];
+
+    /* Swap list[pivotIndex] and list[right] */
+    aux = list[pivot_index];
+    list[pivot_index] = list[right];
+    list[right] = aux;
+
+    for (i = left; i < right; i++) {
+        if (list[i] < pivot_value) {
+            /* Swap list[store_index] and list[i] */
+            aux = list[store_index];
+            list[store_index] = list[i];
+            list[i] = aux;
+            store_index++;
+        }
+    }
+
+    /* Swap list[right] and list[store_index] */
+    aux = list[right];
+    list[right] = list[store_index];
+    list[store_index] = aux;
+    return store_index;
+}
+
+void
+L3GD20::clampGyroReadingsToZero(float* x, float* y, float* z)
+{
+    float near_zero;
+
+    /* If we're calibrated, don't filter out as much */
+    if (m_calibrated)
+        near_zero = 0.02; /* rad/s */
+    else
+        near_zero = 0.1;
+
+    /* If motion on all axes is small enough */
+    if (fabs(*x) < near_zero && fabs(*y) < near_zero && fabs(*z) < near_zero) {
+        /*
+         * Report that we're not moving at all... but not exactly zero as composite sensors
+         * (orientation, rotation vector) don't
+         * seem to react very well to it.
+         */
+
+        *x *= 0.000001;
+        *y *= 0.000001;
+        *z *= 0.000001;
+    }
+}

src/l3gd20/l3gd20.hpp

@@ -0,0 +1,210 @@
+/*
+ * Author: Lay, Kuan Loon <kuan.loon.lay@intel.com>
+ * Copyright (c) 2015 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
+{
+/**
+ * @brief L3GD20 Tri-axis Digital Gyroscope
+ * @defgroup l3gd20 libupm-l3gd20
+ * @ingroup STMicroelectronics iio i2c tri-axis digital gyroscope
+ */
+
+/**
+ * @library l3gd20
+ * @sensor l3gd20
+ * @comname L3GD20 Tri-axis Digital Gyroscope
+ * @type gyroscope
+ * @man STMicroelectronics
+ * @con iio i2c
+ *
+ * @brief L3GD20 Tri-axis Digital Gyroscope API
+ *
+ * The L3GD20 The L3GD20 is a low-power three-axis angular rate sensor.
+ *
+ * @snippet l3gd20.cxx Interesting
+ */
+
+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 Tri-axis Digital Gyroscope
+     *
+     * @param iio device number
+     */
+    L3GD20(int device);
+
+    /**
+     * L3GD20 destructor
+     */
+    ~L3GD20();
+
+    /**
+     * Installs an interrupt service routine (ISR) to be called when
+     * an interrupt occurs
+     *
+     * @param interrupt channel
+     * @param fptr Pointer to a function to be called on interrupt
+     * @param arg Pointer to an object to be supplied as an
+     * argument to the ISR.
+     */
+    void installISR(void (*isr)(char*), void* arg);
+
+    /**
+     * Extract the channel value based on channel type
+     * @param input Channel data
+     * @param chan MRAA iio-layer channel info
+     */
+    int64_t getChannelValue(unsigned char* input, mraa_iio_channel* chan);
+
+    /**
+     * Enable trigger buffer
+     * @param trigger buffer length in string
+     */
+    bool enableBuffer(int length);
+
+    /**
+     * Disable trigger buffer
+     */
+    bool disableBuffer();
+
+    /**
+     * Set scale
+     * @param scale in string
+     */
+    bool setScale(const float scale);
+
+    /**
+     * Set sampling frequency
+     * @param sampling frequency  in string
+     */
+    bool setSamplingFrequency(const float sampling_frequency);
+
+    /**
+     * Enable 3 axis scan element
+     */
+    bool enable3AxisChannel();
+
+    /**
+     * Process enabled channel buffer and return x, y, z axis
+     * @param data Enabled channel data, 6 bytes, each axis 2 bytes
+     * @param x X-Axis
+     * @param y Y-Axis
+     * @param z Z-Axis
+     */
+    bool extract3Axis(char* data, float* x, float* y, float* z);
+
+    /**
+     * Reset calibration data and start collect calibration data again
+     */
+    void initCalibrate();
+
+    /**
+     * Get calibrated status, return true if calibrate successfully
+     */
+    bool getCalibratedStatus();
+
+    /**
+     * Get calibrated data
+     */
+    void getCalibratedData(float* bias_x, float* bias_y, float* bias_z);
+
+    /**
+     * Load calibrated data
+     */
+    void loadCalibratedData(float bias_x, float bias_y, float bias_z);
+
+    /**
+     * Calibrate gyro
+     * @param x X-Axis
+     * @param y Y-Axis
+     * @param z Z-Axis
+     */
+    bool gyroCollect(float x, float y, float z);
+
+    /**
+     * Denoise gyro
+     * @param x X-Axis
+     * @param y Y-Axis
+     * @param z Z-Axis
+     */
+    void gyroDenoiseMedian(float* x, float* y, float* z);
+
+    /**
+     * median algorithm
+     * @param queue
+     * @param size
+     */
+    float median(float* queue, unsigned int size);
+
+    /**
+     * partition algorithm
+     * @param list
+     * @param left
+     * @param right
+     * @param pivot_index
+     */
+    unsigned int
+    partition(float* list, unsigned int left, unsigned int right, unsigned int pivot_index);
+
+    /**
+     * Clamp Gyro Readings to Zero
+     * @param x X-Axis
+     * @param y Y-Axis
+     * @param z Z-Axis
+     */
+    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
+};
+}

src/l3gd20/pyupm_l3gd20.i

@@ -0,0 +1,9 @@
+%module pyupm_l3gd20
+%include "../upm.i"
+
+%feature("autodoc", "3");
+
+%include "l3gd20.hpp"
+%{
+    #include "l3gd20.hpp"
+%}