mirror of
https://github.com/eclipse/upm.git
synced 2025-10-30 06:34:58 +03:00
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>
This commit is contained in:
Lay, Kuan Loon
5
src/l3gd20/CMakeLists.txt
Normal file
5
src/l3gd20/CMakeLists.txt
Normal file
@@ -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()
|
||||
8
src/l3gd20/jsupm_l3gd20.i
Normal file
8
src/l3gd20/jsupm_l3gd20.i
Normal file
@@ -0,0 +1,8 @@
|
||||
%module jsupm_l3gd20
|
||||
%include "../upm.i"
|
||||
|
||||
%{
|
||||
#include "l3gd20.hpp"
|
||||
%}
|
||||
|
||||
%include "l3gd20.hpp"
|
||||
462
src/l3gd20/l3gd20.cxx
Normal file
462
src/l3gd20/l3gd20.cxx
Normal file
@@ -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;
|
||||
}
|
||||
}
|
||||
210
src/l3gd20/l3gd20.hpp
Executable file
210
src/l3gd20/l3gd20.hpp
Executable file
@@ -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
|
||||
};
|
||||
}
|
||||
9
src/l3gd20/pyupm_l3gd20.i
Normal file
9
src/l3gd20/pyupm_l3gd20.i
Normal file
@@ -0,0 +1,9 @@
|
||||
%module pyupm_l3gd20
|
||||
%include "../upm.i"
|
||||
|
||||
%feature("autodoc", "3");
|
||||
|
||||
%include "l3gd20.hpp"
|
||||
%{
|
||||
#include "l3gd20.hpp"
|
||||
%}
|
||||
Reference in New Issue
Block a user