alexey.zholtikov/upm
1
0
Fork 0
mirror of https://github.com/eclipse/upm.git synced 2025-03-15 04:57:30 +03:00
Code Issues Actions Packages Projects Releases Wiki Activity

upm: enable MMC35240 3-axis magnetic sensor library and example

Browse Source 
MMC35240 is 3-axis magnetic sensor from MEMSIC.

This sensor can measure magnetic fields within the full scale range of
+-24 Gauss (G).

The library provided is libupm-mmc35240.so
The example provided is mmc35240-example-cxx where it will print x,y,z axis
when trigger buffer data is ready. It's also print azimuth value.

This sensor requires calibration. Please shake the sensor in figure 8 pattern,
mmc35240-example will print the calibrated level.

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

The azimuth formula is provided by Han, He <he.han@intel.com>.

Signed-off-by: Lay, Kuan Loon <kuan.loon.lay@intel.com>
Signed-off-by: Mihai Tudor Panu <mihai.tudor.panu@intel.com>
This commit is contained in:
Lay, Kuan Loon 2017-01-31 15:41:04 +08:00 committed by Mihai Tudor Panu
parent 4ea01180a1
commit fabf4287d6
11 changed files with 2124 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
examples/c++/CMakeLists.txt
View File

@ -333,6 +333,7 @@ add_example (sensortemplate)
add_example (p9813) add_example (p9813)
add_example (abpdrrt005pg2a5) add_example (abpdrrt005pg2a5)
add_example (lcdks) add_example (lcdks)
add_example (mmc35240)
# These are special cases where you specify example binary, source file and module(s) # These are special cases where you specify example binary, source file and module(s)
include_directories (${PROJECT_SOURCE_DIR}/src) include_directories (${PROJECT_SOURCE_DIR}/src)

111
examples/c++/mmc35240.cxx Normal file
View File

@ -0,0 +1,111 @@
/*
* 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.
*/
#include <iostream>
#include <unistd.h>
#include <signal.h>
#include <math.h>
#include "mmc35240.hpp"
using namespace std;
int shouldRun = true;
upm::MMC35240* magnetometer;
void
sig_handler(int signo)
{
if (signo == SIGINT)
shouldRun = false;
}
void
data_callback(char* data)
{
float x, y, z;
double azimuth;
int level;
magnetometer->extract3Axis(data, &x, &y, &z);
/* calibrated level
* UNRELIABLE = 0
* ACCURACY_LOW = 1
* ACCURACY_MEDIUM = 2
* ACCURACY_HIGH = >=3
*/
level = magnetometer->getCalibratedLevel();
if ((x == 0) && (y == 0)) {
cout << "Point (0, 0) is invalid!\n" << endl;
return;
}
if (x == 0) {
if (y > 0)
azimuth = 0;
else
azimuth = 180;
} else if (y == 0) {
if (x > 0)
azimuth = 90;
else
azimuth = 270;
} else {
if (x > 0)
azimuth = 90 - atan(y / x) * 180 / M_PI;
else
azimuth = 270 - atan(y / x) * 180 / M_PI;
}
cout << "[Calibrated Level:" << level << ']' << "[Azimuth:" << (int) (360 - azimuth) << ']'
<< '\t' << (int) x << '\t' << (int) y << '\t' << (int) z << "[uT]" << endl;
}
int
main()
{
signal(SIGINT, sig_handler);
//! [Interesting]
// Instantiate a MMC35240 Magnetic Sensor on iio device 5. This configuration is a reference and
// should be changed per platform/board type.
magnetometer = new upm::MMC35240(5);
// Kernel driver does not allow changing the value of scale at run-time, default scale is
// 0.001000
magnetometer->setScale(0.001000);
// Available sampling frequency are 1.5, 13, 25, 50
magnetometer->setSamplingFrequency(25.000000);
magnetometer->enable3AxisChannel();
magnetometer->installISR(data_callback, NULL);
magnetometer->enableBuffer(16);
while (shouldRun) {
sleep(1);
}
magnetometer->disableBuffer();
//! [Interesting]
cout << "Exiting" << endl;
delete magnetometer;
return 0;
}

1
src/javaswig_blacklist
View File

@ -1,3 +1,4 @@
nrf8001 nrf8001
kxcjk1013 kxcjk1013
l3gd20 l3gd20
mmc35240

5
src/mmc35240/CMakeLists.txt Normal file
View File

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

8
src/mmc35240/jsupm_mmc35240.i Normal file
View File

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

406
src/mmc35240/mat.h Normal file
View File

@ -0,0 +1,406 @@
/*
* Copyright (C) 2011 The Android Open Source Project
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef ANDROID_MAT_H
#define ANDROID_MAT_H
#include "vec.h"
#include "traits.h"
// -----------------------------------------------------------------------
namespace android {
template <typename TYPE, size_t C, size_t R>
class mat;
namespace helpers {
template <typename TYPE, size_t C, size_t R>
mat<TYPE, C, R>& doAssign(
mat<TYPE, C, R>& lhs,
typename TypeTraits<TYPE>::ParameterType rhs) {
for (size_t i=0 ; i<C ; i++)
for (size_t j=0 ; j<R ; j++)
lhs[i][j] = (i==j) ? rhs : 0;
return lhs;
}
template <typename TYPE, size_t C, size_t R, size_t D>
mat<TYPE, C, R> PURE doMul(
const mat<TYPE, D, R>& lhs,
const mat<TYPE, C, D>& rhs)
{
mat<TYPE, C, R> res;
for (size_t c=0 ; c<C ; c++) {
for (size_t r=0 ; r<R ; r++) {
TYPE v(0);
for (size_t k=0 ; k<D ; k++) {
v += lhs[k][r] * rhs[c][k];
}
res[c][r] = v;
}
}
return res;
}
template <typename TYPE, size_t R, size_t D>
vec<TYPE, R> PURE doMul(
const mat<TYPE, D, R>& lhs,
const vec<TYPE, D>& rhs)
{
vec<TYPE, R> res;
for (size_t r=0 ; r<R ; r++) {
TYPE v(0);
for (size_t k=0 ; k<D ; k++) {
v += lhs[k][r] * rhs[k];
}
res[r] = v;
}
return res;
}
template <typename TYPE, size_t C, size_t R>
mat<TYPE, C, R> PURE doMul(
const vec<TYPE, R>& lhs,
const mat<TYPE, C, 1>& rhs)
{
mat<TYPE, C, R> res;
for (size_t c=0 ; c<C ; c++) {
for (size_t r=0 ; r<R ; r++) {
res[c][r] = lhs[r] * rhs[c][0];
}
}
return res;
}
template <typename TYPE, size_t C, size_t R>
mat<TYPE, C, R> PURE doMul(
const mat<TYPE, C, R>& rhs,
typename TypeTraits<TYPE>::ParameterType v)
{
mat<TYPE, C, R> res;
for (size_t c=0 ; c<C ; c++) {
for (size_t r=0 ; r<R ; r++) {
res[c][r] = rhs[c][r] * v;
}
}
return res;
}
template <typename TYPE, size_t C, size_t R>
mat<TYPE, C, R> PURE doMul(
typename TypeTraits<TYPE>::ParameterType v,
const mat<TYPE, C, R>& rhs)
{
mat<TYPE, C, R> res;
for (size_t c=0 ; c<C ; c++) {
for (size_t r=0 ; r<R ; r++) {
res[c][r] = v * rhs[c][r];
}
}
return res;
}
}; // namespace helpers
// -----------------------------------------------------------------------
template <typename TYPE, size_t C, size_t R>
class mat : public vec< vec<TYPE, R>, C > {
typedef typename TypeTraits<TYPE>::ParameterType pTYPE;
typedef vec< vec<TYPE, R>, C > base;
public:
// STL-like interface.
typedef TYPE value_type;
typedef TYPE& reference;
typedef TYPE const& const_reference;
typedef size_t size_type;
size_type size() const { return R*C; }
enum { ROWS = R, COLS = C };
// -----------------------------------------------------------------------
// default constructors
mat() { }
mat(const mat& rhs) : base(rhs) { }
mat(const base& rhs) : base(rhs) { }
// -----------------------------------------------------------------------
// conversion constructors
// sets the diagonal to the value, off-diagonal to zero
mat(pTYPE rhs) {
helpers::doAssign(*this, rhs);
}
// -----------------------------------------------------------------------
// Assignment
mat& operator=(const mat& rhs) {
base::operator=(rhs);
return *this;
}
mat& operator=(const base& rhs) {
base::operator=(rhs);
return *this;
}
mat& operator=(pTYPE rhs) {
return helpers::doAssign(*this, rhs);
}
// -----------------------------------------------------------------------
// non-member function declaration and definition
friend inline mat PURE operator + (const mat& lhs, const mat& rhs) {
return helpers::doAdd(
static_cast<const base&>(lhs),
static_cast<const base&>(rhs));
}
friend inline mat PURE operator - (const mat& lhs, const mat& rhs) {
return helpers::doSub(
static_cast<const base&>(lhs),
static_cast<const base&>(rhs));
}
// matrix*matrix
template <size_t D>
friend mat PURE operator * (
const mat<TYPE, D, R>& lhs,
const mat<TYPE, C, D>& rhs) {
return helpers::doMul(lhs, rhs);
}
// matrix*vector
friend vec<TYPE, R> PURE operator * (
const mat& lhs, const vec<TYPE, C>& rhs) {
return helpers::doMul(lhs, rhs);
}
// vector*matrix
friend mat PURE operator * (
const vec<TYPE, R>& lhs, const mat<TYPE, C, 1>& rhs) {
return helpers::doMul(lhs, rhs);
}
// matrix*scalar
friend inline mat PURE operator * (const mat& lhs, pTYPE v) {
return helpers::doMul(lhs, v);
}
// scalar*matrix
friend inline mat PURE operator * (pTYPE v, const mat& rhs) {
return helpers::doMul(v, rhs);
}
// -----------------------------------------------------------------------
// streaming operator to set the columns of the matrix:
// example:
// mat33_t m;
// m << v0 << v1 << v2;
// column_builder<> stores the matrix and knows which column to set
template<size_t PREV_COLUMN>
struct column_builder {
mat& matrix;
column_builder(mat& matrix) : matrix(matrix) { }
};
// operator << is not a method of column_builder<> so we can
// overload it for unauthorized values (partial specialization
// not allowed in class-scope).
// we just set the column and return the next column_builder<>
template<size_t PREV_COLUMN>
friend column_builder<PREV_COLUMN+1> operator << (
const column_builder<PREV_COLUMN>& lhs,
const vec<TYPE, R>& rhs) {
lhs.matrix[PREV_COLUMN+1] = rhs;
return column_builder<PREV_COLUMN+1>(lhs.matrix);
}
// we return void here so we get a compile-time error if the
// user tries to set too many columns
friend void operator << (
const column_builder<C-2>& lhs,
const vec<TYPE, R>& rhs) {
lhs.matrix[C-1] = rhs;
}
// this is where the process starts. we set the first columns and
// return the next column_builder<>
column_builder<0> operator << (const vec<TYPE, R>& rhs) {
(*this)[0] = rhs;
return column_builder<0>(*this);
}
};
// Specialize column matrix so they're exactly equivalent to a vector
template <typename TYPE, size_t R>
class mat<TYPE, 1, R> : public vec<TYPE, R> {
typedef vec<TYPE, R> base;
public:
// STL-like interface.
typedef TYPE value_type;
typedef TYPE& reference;
typedef TYPE const& const_reference;
typedef size_t size_type;
size_type size() const { return R; }
enum { ROWS = R, COLS = 1 };
mat() { }
mat(const base& rhs) : base(rhs) { }
mat(const mat& rhs) : base(rhs) { }
mat(const TYPE& rhs) { helpers::doAssign(*this, rhs); }
mat& operator=(const mat& rhs) { base::operator=(rhs); return *this; }
mat& operator=(const base& rhs) { base::operator=(rhs); return *this; }
mat& operator=(const TYPE& rhs) { return helpers::doAssign(*this, rhs); }
// we only have one column, so ignore the index
const base& operator[](size_t) const { return *this; }
base& operator[](size_t) { return *this; }
void operator << (const vec<TYPE, R>& rhs) { base::operator[](0) = rhs; }
};
// -----------------------------------------------------------------------
// matrix functions
// transpose. this handles matrices of matrices
inline int PURE transpose(int v) { return v; }
inline float PURE transpose(float v) { return v; }
inline double PURE transpose(double v) { return v; }
// Transpose a matrix
template <typename TYPE, size_t C, size_t R>
mat<TYPE, R, C> PURE transpose(const mat<TYPE, C, R>& m) {
mat<TYPE, R, C> r;
for (size_t i=0 ; i<R ; i++)
for (size_t j=0 ; j<C ; j++)
r[i][j] = transpose(m[j][i]);
return r;
}
template <typename TYPE, size_t M, size_t N, size_t P>
mat<TYPE, M, P> PURE multiply(const mat<TYPE, M, N>& m1, const mat<TYPE, N, P>& m2)
{
mat<TYPE, M, P> r;
for (size_t i = 0; i < M; i++)
for (size_t k = 0; k < P; k++) {
r [i][k] = 0;
for (size_t j = 0; j < N; j++)
r [i][k] += m1[i][j] * m2 [j][k];
}
return r;
}
// Calculate the trace of a matrix
template <typename TYPE, size_t C> static TYPE trace(const mat<TYPE, C, C>& m) {
TYPE t;
for (size_t i=0 ; i<C ; i++)
t += m[i][i];
return t;
}
// Test positive-semidefiniteness of a matrix
template <typename TYPE, size_t C>
static bool isPositiveSemidefinite(const mat<TYPE, C, C>& m, TYPE tolerance) {
for (size_t i=0 ; i<C ; i++)
if (m[i][i] < 0)
return false;
for (size_t i=0 ; i<C ; i++)
for (size_t j=i+1 ; j<C ; j++)
if (fabs(m[i][j] - m[j][i]) > tolerance)
return false;
return true;
}
// Transpose a vector
template <
template<typename T, size_t S> class VEC,
typename TYPE,
size_t SIZE
>
mat<TYPE, SIZE, 1> PURE transpose(const VEC<TYPE, SIZE>& v) {
mat<TYPE, SIZE, 1> r;
for (size_t i=0 ; i<SIZE ; i++)
r[i][0] = transpose(v[i]);
return r;
}
// -----------------------------------------------------------------------
// "dumb" matrix inversion
template<typename T, size_t N>
mat<T, N, N> PURE invert(const mat<T, N, N>& src) {
T t;
size_t swap;
mat<T, N, N> tmp(src);
mat<T, N, N> inverse(1);
for (size_t i=0 ; i<N ; i++) {
// look for largest element in column
swap = i;
for (size_t j=i+1 ; j<N ; j++) {
if (fabs(tmp[j][i]) > fabs(tmp[i][i])) {
swap = j;
}
}
if (swap != i) {
/* swap rows. */
for (size_t k=0 ; k<N ; k++) {
t = tmp[i][k];
tmp[i][k] = tmp[swap][k];
tmp[swap][k] = t;
t = inverse[i][k];
inverse[i][k] = inverse[swap][k];
inverse[swap][k] = t;
}
}
t = 1 / tmp[i][i];
for (size_t k=0 ; k<N ; k++) {
tmp[i][k] *= t;
inverse[i][k] *= t;
}
for (size_t j=0 ; j<N ; j++) {
if (j != i) {
t = tmp[j][i];
for (size_t k=0 ; k<N ; k++) {
tmp[j][k] -= tmp[i][k] * t;
inverse[j][k] -= inverse[i][k] * t;
}
}
}
}
return inverse;
}
// -----------------------------------------------------------------------
typedef mat<float, 2, 2> mat22_t;
typedef mat<float, 3, 3> mat33_t;
typedef mat<float, 4, 4> mat44_t;
// -----------------------------------------------------------------------
}; // namespace android
#endif /* ANDROID_MAT_H */

811
src/mmc35240/mmc35240.cxx Normal file
View File

@ -0,0 +1,811 @@
/*
* 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.
*/
#include <iostream>
#include <string>
#include <stdexcept>
#include <string.h>
#include <math.h>
#include "mmc35240.hpp"
#define NUMBER_OF_BITS_IN_BYTE 8
/* Compass defines */
#define CONVERT_GAUSS_TO_MICROTESLA(x) ((x) *100)
#define EPSILON 0.000000001
#define MAGNETIC_LOW 960 /* 31 micro tesla squared */
#define CAL_STEPS 5
/* Filter defines */
#define FILTER_MAX_SAMPLE 20
#define FILTER_NUM_FILED 3
using namespace upm;
using namespace android;
/* We'll have multiple calibration levels so that we can provide an estimation as fast as possible
*/
static const float min_diffs[CAL_STEPS] = { 0.2, 0.25, 0.4, 0.6, 1.0 };
static const float max_sqr_errs[CAL_STEPS] = { 10.0, 10.0, 8.0, 5.0, 3.5 };
static const unsigned int lookback_counts[CAL_STEPS] = { 2, 3, 4, 5, 6 };
MMC35240::MMC35240(int device)
{
float mag_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-mmc35240-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_mount_matrix(m_iio, "in_mount_matrix", m_mount_matrix) == MRAA_SUCCESS)
m_mount_matrix_exist = true;
else
m_mount_matrix_exist = false;
if (mraa_iio_read_float(m_iio, "in_magn_scale", &mag_scale) == MRAA_SUCCESS)
m_scale = mag_scale;
// calibration init data
initCalibrate();
// filter init data
memset(&m_filter, 0, sizeof(filter_average_t));
m_filter.max_samples = FILTER_MAX_SAMPLE;
m_filter.num_fields = FILTER_NUM_FILED;
}
MMC35240::~MMC35240()
{
if (m_filter.history) {
free(m_filter.history);
m_filter.history = NULL;
}
if (m_filter.history_sum) {
free(m_filter.history_sum);
m_filter.history_sum = NULL;
}
if (m_iio)
mraa_iio_close(m_iio);
}
void
MMC35240::installISR(void (*isr)(char*), void* arg)
{
mraa_iio_trigger_buffer(m_iio, isr, NULL);
}
int64_t
MMC35240::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
MMC35240::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
MMC35240::disableBuffer()
{
mraa_iio_write_int(m_iio, "buffer/enable", 0);
return true;
}
bool
MMC35240::setScale(float scale)
{
m_scale = scale;
mraa_iio_write_float(m_iio, "in_magn_scale", scale);
return true;
}
bool
MMC35240::setSamplingFrequency(float sampling_frequency)
{
m_sampling_frequency = sampling_frequency;
mraa_iio_write_float(m_iio, "in_magn_sampling_frequency", sampling_frequency);
return true;
}
bool
MMC35240::enable3AxisChannel()
{
char trigger[64];
sprintf(trigger, "mmc35240-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_magn_x_en", 1);
mraa_iio_write_int(m_iio, "scan_elements/in_magn_y_en", 1);
mraa_iio_write_int(m_iio, "scan_elements/in_magn_z_en", 1);
// need update channel data size after enable
mraa_iio_update_channels(m_iio);
return true;
}
void
MMC35240::extract3Axis(char* data, float* x, float* y, float* z)
{
mraa_iio_channel* channels = mraa_iio_get_channels(m_iio);
float tmp[3];
int64_t iio_x, iio_y, iio_z;
iio_x = getChannelValue((unsigned char*) (data), &channels[0]);
iio_y = getChannelValue((unsigned char*) (data + 4), &channels[1]);
iio_z = getChannelValue((unsigned char*) (data + 8), &channels[2]);
// Raw data is magnetic field along axis x, y, and z. Units after application of scale are Gauss
*x = CONVERT_GAUSS_TO_MICROTESLA(iio_x * m_scale);
*y = CONVERT_GAUSS_TO_MICROTESLA(iio_y * m_scale);
*z = CONVERT_GAUSS_TO_MICROTESLA(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];
}
calibrateCompass(x, y, z, &m_cal_data);
denoise_average(x, y, z);
}
int
MMC35240::getCalibratedLevel()
{
return m_cal_level;
}
void
MMC35240::initCalibrate()
{
m_cal_level = 0;
resetSample(&m_cal_data);
m_cal_data.offset[0][0] = 0;
m_cal_data.offset[1][0] = 0;
m_cal_data.offset[2][0] = 0;
m_cal_data.w_invert[0][0] = 1;
m_cal_data.w_invert[1][0] = 0;
m_cal_data.w_invert[2][0] = 0;
m_cal_data.w_invert[0][1] = 0;
m_cal_data.w_invert[1][1] = 1;
m_cal_data.w_invert[2][1] = 0;
m_cal_data.w_invert[0][2] = 0;
m_cal_data.w_invert[1][2] = 0;
m_cal_data.w_invert[2][2] = 1;
m_cal_data.bfield = 0;
}
void
MMC35240::getCalibratedData(int* cal_level, double offset[3][1], double w_invert[3][3], double* bfield)
{
*cal_level = m_cal_level;
offset[0][0] = m_cal_data.offset[0][0];
offset[1][0] = m_cal_data.offset[1][0];
offset[2][0] = m_cal_data.offset[2][0];
w_invert[0][0] = m_cal_data.w_invert[0][0];
w_invert[1][0] = m_cal_data.w_invert[1][0];
w_invert[2][0] = m_cal_data.w_invert[2][0];
w_invert[0][1] = m_cal_data.w_invert[0][1];
w_invert[1][1] = m_cal_data.w_invert[1][1];
w_invert[2][1] = m_cal_data.w_invert[2][1];
w_invert[0][2] = m_cal_data.w_invert[0][2];
w_invert[1][2] = m_cal_data.w_invert[1][2];
w_invert[2][2] = m_cal_data.w_invert[2][2];
*bfield = m_cal_data.bfield;
}
void
MMC35240::loadCalibratedData(int cal_level, double offset[3][1], double w_invert[3][3], double bfield)
{
m_cal_level = cal_level;
m_cal_data.offset[0][0] = offset[0][0];
m_cal_data.offset[1][0] = offset[1][0];
m_cal_data.offset[2][0] = offset[2][0];
m_cal_data.w_invert[0][0] = w_invert[0][0];
m_cal_data.w_invert[1][0] = w_invert[1][0];
m_cal_data.w_invert[2][0] = w_invert[2][0];
m_cal_data.w_invert[0][1] = w_invert[0][1];
m_cal_data.w_invert[1][1] = w_invert[1][1];
m_cal_data.w_invert[2][1] = w_invert[2][1];
m_cal_data.w_invert[0][2] = w_invert[0][2];
m_cal_data.w_invert[1][2] = w_invert[1][2];
m_cal_data.w_invert[2][2] = w_invert[2][2];
m_cal_data.bfield = bfield;
}
void
MMC35240::resetSample(compass_cal_t* data)
{
int i, j;
data->sample_count = 0;
for (i = 0; i < MAGN_DS_SIZE; i++)
for (j = 0; j < 3; j++)
data->sample[i][j] = 0;
data->average[0] = data->average[1] = data->average[2] = 0;
}
void
MMC35240::calibrateCompass(float* x, float* y, float* z, compass_cal_t* cal_data)
{
int cal_level;
/* Calibration is continuous */
compassCollect(x, y, z, cal_data);
cal_level = compassReady(cal_data);
switch (cal_level) {
case 0:
scale(x, y, z);
break;
default:
compassComputeCal(x, y, z, cal_data);
break;
}
}
int
MMC35240::compassCollect(float* x, float* y, float* z, compass_cal_t* cal_data)
{
float data[3] = { *x, *y, *z };
unsigned int index, j;
unsigned int lookback_count;
float min_diff;
/* Discard the point if not valid */
if (data[0] == 0 || data[1] == 0 || data[2] == 0)
return -1;
lookback_count = lookback_counts[m_cal_level];
min_diff = min_diffs[m_cal_level];
/* For the current point to be accepted, each x/y/z value must be different enough to the last
* several collected points */
if (cal_data->sample_count > 0 && cal_data->sample_count < MAGN_DS_SIZE) {
unsigned int lookback =
lookback_count < cal_data->sample_count ? lookback_count : cal_data->sample_count;
for (index = 0; index < lookback; index++)
for (j = 0; j < 3; j++)
if (fabsf(data[j] - cal_data->sample[cal_data->sample_count - 1 - index][j]) <
min_diff) {
#ifdef DEBUG
printf("CompassCalibration:point reject: [%f,%f,%f], selected_count=%d\n",
data[0],
data[1],
data[2],
cal_data->sample_count);
#endif
return 0;
}
}
if (cal_data->sample_count < MAGN_DS_SIZE) {
memcpy(cal_data->sample[cal_data->sample_count], data, sizeof(float) * 3);
cal_data->sample_count++;
cal_data->average[0] += data[0];
cal_data->average[1] += data[1];
cal_data->average[2] += data[2];
#ifdef DEBUG
printf("CompassCalibration:point collected [%f,%f,%f], selected_count=%d\n",
(double) data[0],
(double) data[1],
(double) data[2],
cal_data->sample_count);
#endif
}
return 1;
}
int
MMC35240::compassReady(compass_cal_t* cal_data)
{
mat_input_t mat;
int i;
float max_sqr_err;
compass_cal_t new_cal_data;
/*
* Some sensors take unrealistically long to calibrate at higher levels. We'll use a
* max_cal_level if we have such a property setup,
* or go with the default settings if not.
*/
int cal_steps = CAL_STEPS;
if (cal_data->sample_count < MAGN_DS_SIZE)
return m_cal_level;
max_sqr_err = max_sqr_errs[m_cal_level];
/* Enough points have been collected, do the ellipsoid calibration */
/* Compute average per axis */
cal_data->average[0] /= MAGN_DS_SIZE;
cal_data->average[1] /= MAGN_DS_SIZE;
cal_data->average[2] /= MAGN_DS_SIZE;
for (i = 0; i < MAGN_DS_SIZE; i++) {
mat[i][0] = cal_data->sample[i][0];
mat[i][1] = cal_data->sample[i][1];
mat[i][2] = cal_data->sample[i][2];
}
/* Check if result is good. The sample data must remain the same */
new_cal_data = *cal_data;
if (ellipsoidFit(mat, new_cal_data.offset, new_cal_data.w_invert, &new_cal_data.bfield)) {
double new_err = calcSquareErr(&new_cal_data);
#ifdef DEBUG
printf("new err is %f, max sqr err id %f\n", new_err, max_sqr_err);
#endif
if (new_err < max_sqr_err) {
double err = calcSquareErr(cal_data);
if (new_err < err) {
/* New cal data is better, so we switch to the new */
cal_data->offset = new_cal_data.offset;
cal_data->w_invert = new_cal_data.w_invert;
cal_data->bfield = new_cal_data.bfield;
if (m_cal_level < (cal_steps - 1))
m_cal_level++;
#ifdef DEBUG
printf("CompassCalibration: ready check success, caldata: %f %f %f %f %f %f %f %f "
"%f %f %f %f %f, err %f\n",
cal_data->offset[0][0],
cal_data->offset[1][0],
cal_data->offset[2][0],
cal_data->w_invert[0][0],
cal_data->w_invert[0][1],
cal_data->w_invert[0][2],
cal_data->w_invert[1][0],
cal_data->w_invert[1][1],
cal_data->w_invert[1][2],
cal_data->w_invert[2][0],
cal_data->w_invert[2][1],
cal_data->w_invert[2][2],
cal_data->bfield,
new_err);
#endif
}
}
}
resetSample(cal_data);
return m_cal_level;
}
double
MMC35240::calcSquareErr(compass_cal_t* data)
{
double err = 0;
mat<double, 3, 1> raw, result, mat_diff;
int i;
float stdev[3] = { 0, 0, 0 };
double diff;
for (i = 0; i < MAGN_DS_SIZE; i++) {
raw[0][0] = data->sample[i][0];
raw[1][0] = data->sample[i][1];
raw[2][0] = data->sample[i][2];
stdev[0] += (raw[0][0] - data->average[0]) * (raw[0][0] - data->average[0]);
stdev[1] += (raw[1][0] - data->average[1]) * (raw[1][0] - data->average[1]);
stdev[2] += (raw[2][0] - data->average[2]) * (raw[2][0] - data->average[2]);
mat_diff = raw - data->offset;
result = multiply(data->w_invert, mat_diff);
diff = sqrt(result[0][0] * result[0][0] + result[1][0] * result[1][0] +
result[2][0] * result[2][0]) -
data->bfield;
err += diff * diff;
}
stdev[0] = sqrt(stdev[0] / MAGN_DS_SIZE);
stdev[1] = sqrt(stdev[1] / MAGN_DS_SIZE);
stdev[2] = sqrt(stdev[2] / MAGN_DS_SIZE);
/* A sanity check - if we have too little variation for an axis it's best to reject the
* calibration than risking a wrong calibration */
if (stdev[0] <= 1 || stdev[1] <= 1 || stdev[2] <= 1)
return max_sqr_errs[0];
err /= MAGN_DS_SIZE;
return err;
}
int
MMC35240::ellipsoidFit(mat_input_t& m,
mat<double, 3, 1>& offset,
mat<double, 3, 3>& w_invert,
double* bfield)
{
int i;
mat<double, MAGN_DS_SIZE, 9> h;
mat<double, MAGN_DS_SIZE, 1> w;
mat<double, 9, MAGN_DS_SIZE> h_trans;
mat<double, 9, 9> p_temp1;
mat<double, 9, MAGN_DS_SIZE> p_temp2;
mat<double, 3, 3> temp1, temp;
mat<double, 3, 3> temp1_inv;
mat<double, 3, 1> temp2;
mat<double, 9, 9> result;
mat<double, 9, 1> p;
mat<double, 3, 3> a, sqrt_evals, evecs, evecs_trans;
mat<double, 3, 1> evec1, evec2, evec3;
for (i = 0; i < MAGN_DS_SIZE; i++) {
w[i][0] = m[i][0] * m[i][0];
h[i][0] = m[i][0];
h[i][1] = m[i][1];
h[i][2] = m[i][2];
h[i][3] = -1 * m[i][0] * m[i][1];
h[i][4] = -1 * m[i][0] * m[i][2];
h[i][5] = -1 * m[i][1] * m[i][2];
h[i][6] = -1 * m[i][1] * m[i][1];
h[i][7] = -1 * m[i][2] * m[i][2];
h[i][8] = 1;
}
h_trans = transpose(h);
result = multiply(h_trans, h);
p_temp1 = invert(result);
p_temp2 = multiply(p_temp1, h_trans);
p = multiply(p_temp2, w);
temp1[0][0] = 2;
temp1[0][1] = p[3][0];
temp1[0][2] = p[4][0];
temp1[1][0] = p[3][0];
temp1[1][1] = 2 * p[6][0];
temp1[1][2] = p[5][0];
temp1[2][0] = p[4][0];
temp1[2][1] = p[5][0];
temp1[2][2] = 2 * p[7][0];
temp2[0][0] = p[0][0];
temp2[1][0] = p[1][0];
temp2[2][0] = p[2][0];
temp1_inv = invert(temp1);
offset = multiply(temp1_inv, temp2);
double off_x = offset[0][0];
double off_y = offset[1][0];
double off_z = offset[2][0];
a[0][0] = 1.0 / (p[8][0] + off_x * off_x + p[6][0] * off_y * off_y + p[7][0] * off_z * off_z +
p[3][0] * off_x * off_y + p[4][0] * off_x * off_z + p[5][0] * off_y * off_z);
a[0][1] = p[3][0] * a[0][0] / 2;
a[0][2] = p[4][0] * a[0][0] / 2;
a[1][2] = p[5][0] * a[0][0] / 2;
a[1][1] = p[6][0] * a[0][0];
a[2][2] = p[7][0] * a[0][0];
a[2][1] = a[1][2];
a[1][0] = a[0][1];
a[2][0] = a[0][2];
double eig1 = 0, eig2 = 0, eig3 = 0;
computeEigenvalues(a, &eig1, &eig2, &eig3);
if (eig1 <= 0 || eig2 <= 0 || eig3 <= 0)
return 0;
sqrt_evals[0][0] = sqrt(eig1);
sqrt_evals[1][0] = 0;
sqrt_evals[2][0] = 0;
sqrt_evals[0][1] = 0;
sqrt_evals[1][1] = sqrt(eig2);
sqrt_evals[2][1] = 0;
sqrt_evals[0][2] = 0;
sqrt_evals[1][2] = 0;
sqrt_evals[2][2] = sqrt(eig3);
calcEvector(a, eig1, evec1);
calcEvector(a, eig2, evec2);
calcEvector(a, eig3, evec3);
evecs[0][0] = evec1[0][0];
evecs[1][0] = evec1[1][0];
evecs[2][0] = evec1[2][0];
evecs[0][1] = evec2[0][0];
evecs[1][1] = evec2[1][0];
evecs[2][1] = evec2[2][0];
evecs[0][2] = evec3[0][0];
evecs[1][2] = evec3[1][0];
evecs[2][2] = evec3[2][0];
temp1 = multiply(evecs, sqrt_evals);
evecs_trans = transpose(evecs);
temp = multiply(temp1, evecs_trans);
w_invert = transpose(temp);
*bfield = pow(sqrt(1 / eig1) * sqrt(1 / eig2) * sqrt(1 / eig3), 1.0 / 3.0);
if (*bfield < 0)
return 0;
w_invert = w_invert * (*bfield);
return 1;
}
/* Given an real symmetric 3x3 matrix A, compute the eigenvalues */
void
MMC35240::computeEigenvalues(mat<double, 3, 3>& A, double* eig1, double* eig2, double* eig3)
{
double p = A[0][1] * A[0][1] + A[0][2] * A[0][2] + A[1][2] * A[1][2];
if (p < EPSILON) {
*eig1 = A[0][0];
*eig2 = A[1][1];
*eig3 = A[2][2];
return;
}
double q = (A[0][0] + A[1][1] + A[2][2]) / 3;
double temp1 = A[0][0] - q;
double temp2 = A[1][1] - q;
double temp3 = A[2][2] - q;
p = temp1 * temp1 + temp2 * temp2 + temp3 * temp3 + 2 * p;
p = sqrt(p / 6);
mat<double, 3, 3> B = A;
B[0][0] -= q;
B[1][1] -= q;
B[2][2] -= q;
B = (1 / p) * B;
double r =
(B[0][0] * B[1][1] * B[2][2] + B[0][1] * B[1][2] * B[2][0] + B[0][2] * B[1][0] * B[2][1] -
B[0][2] * B[1][1] * B[2][0] - B[0][0] * B[1][2] * B[2][1] - B[0][1] * B[1][0] * B[2][2]) /
2;
double phi;
if (r <= -1.0)
phi = M_PI / 3;
else if (r >= 1.0)
phi = 0;
else
phi = acos(r) / 3;
*eig3 = q + 2 * p* cos(phi);
*eig1 = q + 2 * p* cos(phi + 2 * M_PI / 3);
*eig2 = 3 * q - *eig1 - *eig3;
}
void
MMC35240::calcEvector(mat<double, 3, 3>& A, double eig, mat<double, 3, 1>& vec)
{
mat<double, 3, 3> h;
mat<double, 2, 2> x_tmp;
h = A;
h[0][0] -= eig;
h[1][1] -= eig;
h[2][2] -= eig;
mat<double, 2, 2> x;
x[0][0] = h[1][1];
x[0][1] = h[1][2];
x[1][0] = h[2][1];
x[1][1] = h[2][2];
x_tmp = invert(x);
x = x_tmp;
double temp1 = x[0][0] * (-h[1][0]) + x[0][1] * (-h[2][0]);
double temp2 = x[1][0] * (-h[1][0]) + x[1][1] * (-h[2][0]);
double norm = sqrt(1 + temp1 * temp1 + temp2 * temp2);
vec[0][0] = 1.0 / norm;
vec[1][0] = temp1 / norm;
vec[2][0] = temp2 / norm;
}
void
MMC35240::scale(float* x, float* y, float* z)
{
float sqr_norm = 0;
float sanity_norm = 0;
float scale = 1;
sqr_norm = (*x * *x + *y * *y + *z * *z);
if (sqr_norm < MAGNETIC_LOW)
sanity_norm = MAGNETIC_LOW;
if (sanity_norm && sqr_norm) {
scale = sanity_norm / sqr_norm;
scale = sqrt(scale);
*x = *x* scale;
*y = *y* scale;
*z = *z* scale;
}
}
void
MMC35240::compassComputeCal(float* x, float* y, float* z, compass_cal_t* cal_data)
{
mat<double, 3, 1> result, raw, diff;
if (!m_cal_level)
return;
raw[0][0] = *x;
raw[1][0] = *y;
raw[2][0] = *z;
diff = raw - cal_data->offset;
result = multiply(cal_data->w_invert, diff);
*x = result[0][0];
*y = result[1][0];
*z = result[2][0];
scale(x, y, z);
}
void
MMC35240::denoise_average(float* x, float* y, float* z)
{
/*
* Smooth out incoming data using a moving average over a number of
* samples. We accumulate one second worth of samples, or max_samples,
* depending on which is lower.
*/
float* data[3];
int f;
int history_size;
int history_full = 0;
filter_average_t* filter;
data[0] = x;
data[1] = y;
data[2] = z;
/* Don't denoise anything if we have less than two samples per second */
if (m_sampling_frequency < 2)
return;
filter = (filter_average_t*) &m_filter;
if (!filter)
return;
/* Restrict window size to the min of sampling_rate and max_samples */
if (m_sampling_frequency > filter->max_samples)
history_size = filter->max_samples;
else
history_size = m_sampling_frequency;
/* Reset history if we're operating on an incorrect window size */
if (filter->history_size != history_size) {
filter->history_size = history_size;
filter->history_entries = 0;
filter->history_index = 0;
filter->history =
(float*) realloc(filter->history, filter->history_size * filter->num_fields * sizeof(float));
if (filter->history) {
filter->history_sum =
(float*) realloc(filter->history_sum, filter->num_fields * sizeof(float));
if (filter->history_sum)
memset(filter->history_sum, 0, filter->num_fields * sizeof(float));
}
}
if (!filter->history || !filter->history_sum)
return; /* Unlikely, but still... */
/* Update initialized samples count */
if (filter->history_entries < filter->history_size)
filter->history_entries++;
else
history_full = 1;
/* Record new sample and calculate the moving sum */
for (f = 0; f < filter->num_fields; f++) {
/** A field is going to be overwritten if history is full, so decrease the history sum */
if (history_full)
filter->history_sum[f] -=
filter->history[filter->history_index * filter->num_fields + f];
filter->history[filter->history_index * filter->num_fields + f] = *data[f];
filter->history_sum[f] += *data[f];
/* For now simply compute a mobile mean for each field and output filtered data */
*data[f] = filter->history_sum[f] / filter->history_entries;
}
/* Update our rolling index (next evicted cell) */
filter->history_index = (filter->history_index + 1) % filter->history_size;
}

213
src/mmc35240/mmc35240.hpp Normal file
View File

@ -0,0 +1,213 @@
/*
* 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 magnetometer
* calibration and denoise algorithm.
*/
#pragma once
#include <string>
#include <mraa/iio.h>
// Adopt
// https://android.googlesource.com/platform/frameworks/native/+/refs/heads/master/services/sensorservice/mat.h
#include "mat.h"
#define MAGN_DS_SIZE 32
namespace upm
{
/**
* @brief MMC35240 Tri-axis Magnetic Sensor
* @defgroup mmc35240 libupm-mmc35240
* @ingroup STMicroelectronics iio i2c tri-axis magnetic sensor
*/
/**
* @library mmc35240
* @sensor mmc35240
* @comname MMC35240 Tri-axis Magnetic Sensor
* @type compass
* @man Memsic
* @con iio i2c
*
* @brief MMC35240 Tri-axis Magnetic Sensor API
*
* The MMC3524xPJ is a complete 3-axis magnetic sensor
*
* @snippet mmc35240.cxx Interesting
*/
class MMC35240
{
public:
typedef struct {
/* hard iron offsets */
android::mat<double, 3, 1> offset;
/* soft iron matrix */
android::mat<double, 3, 3> w_invert;
/* geomagnetic strength */
double bfield;
/* selection data */
float sample[MAGN_DS_SIZE][3];
unsigned int sample_count;
float average[3];
} compass_cal_t;
typedef double mat_input_t[MAGN_DS_SIZE][3];
typedef struct {
int max_samples; /* Maximum averaging window size */
int num_fields; /* Number of fields per sample (usually 3) */
float* history; /* Working buffer containing recorded samples */
float* history_sum; /* The current sum of the history elements */
int history_size; /* Number of recorded samples */
int history_entries; /* How many of these are initialized */
int history_index; /* Index of sample to evict next time */
} filter_average_t;
/**
* MMC35240 Tri-axis Magnetic Sensor
*
* @param iio device number
*/
MMC35240(int device);
/**
* MMC35240 destructor
*/
~MMC35240();
/**
* 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 integer
*/
bool enableBuffer(int length);
/**
* Disable trigger buffer
*/
bool disableBuffer();
/**
* Set scale
* @param scale in float
* Kernel driver does not support changing the value of scale on run-time
* Default scale is 0.001000
*/
bool setScale(const float scale);
/**
* Set sampling frequency
* @param sampling frequency in float
* Available sampling frequency are 1.5, 13, 25, 50
* Default sampling frequency is 1.500000
*/
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
*/
void extract3Axis(char* data, float* x, float* y, float* z);
/**
* Get calibrated level
*/
int getCalibratedLevel();
/**
* Reset calibration data and start collect calibration data again
*/
void initCalibrate();
/**
* Get calibrated data
*/
void
getCalibratedData(int* cal_level, double offset[3][1], double w_invert[3][3], double* bfield);
/**
* Load calibrated data
*/
void
loadCalibratedData(int cal_level, double offset[3][1], double w_invert[3][3], double bfield);
private:
/* Adopt https://github.com/01org/android-iio-sensors-hal/blob/master/compass-calibration.c */
void resetSample(compass_cal_t* data);
void calibrateCompass(float* x, float* y, float* z, compass_cal_t* cal_data);
int compassCollect(float* x, float* y, float* z, compass_cal_t* cal_data);
int compassReady(compass_cal_t* cal_data);
double calcSquareErr(compass_cal_t* data);
int ellipsoidFit(mat_input_t& m,
android::mat<double, 3, 1>& offset,
android::mat<double, 3, 3>& w_invert,
double* bfield);
void computeEigenvalues(android::mat<double, 3, 3>& A, double* eig1, double* eig2, double* eig3);
void calcEvector(android::mat<double, 3, 3>& A, double eig, android::mat<double, 3, 1>& vec);
void scale(float* x, float* y, float* z);
void compassComputeCal(float* x, float* y, float* z, compass_cal_t* cal_data);
/* Adopt https://github.com/01org/android-iio-sensors-hal/blob/master/filtering.c */
void denoise_average(float* x, float* y, float* z);
mraa_iio_context m_iio;
int m_iio_device_num;
float m_sampling_frequency; // sampling frequency
bool m_mount_matrix_exist; // is mount matrix exist
float m_mount_matrix[9]; // mount matrix
float m_scale; // data scale
compass_cal_t m_cal_data; // calibrate data
int m_cal_level; // calibrated level
filter_average_t m_filter; // filter data
};
}

12
src/mmc35240/pyupm_mmc35240.i Normal file
View File

@ -0,0 +1,12 @@
%module pyupm_mmc35240
// Include doxygen-generated documentation
%include "pyupm_doxy2swig.i"
%include "../upm.i"
%feature("autodoc", "3");
%include "mmc35240.hpp"
%{
#include "mmc35240.hpp"
%}

118
src/mmc35240/traits.h Normal file
View File

@ -0,0 +1,118 @@
/*
* Copyright (C) 2011 The Android Open Source Project
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef ANDROID_TRAITS_H
#define ANDROID_TRAITS_H
// -----------------------------------------------------------------------
// Typelists
namespace android {
// end-of-list marker
class NullType {};
// type-list node
template <typename T, typename U>
struct TypeList {
typedef T Head;
typedef U Tail;
};
// helpers to build typelists
#define TYPELIST_1(T1) TypeList<T1, NullType>
#define TYPELIST_2(T1, T2) TypeList<T1, TYPELIST_1(T2)>
#define TYPELIST_3(T1, T2, T3) TypeList<T1, TYPELIST_2(T2, T3)>
#define TYPELIST_4(T1, T2, T3, T4) TypeList<T1, TYPELIST_3(T2, T3, T4)>
// typelists algorithms
namespace TL {
template <typename TList, typename T> struct IndexOf;
template <typename T>
struct IndexOf<NullType, T> {
enum { value = -1 };
};
template <typename T, typename Tail>
struct IndexOf<TypeList<T, Tail>, T> {
enum { value = 0 };
};
template <typename Head, typename Tail, typename T>
struct IndexOf<TypeList<Head, Tail>, T> {
private:
enum { temp = IndexOf<Tail, T>::value };
public:
enum { value = temp == -1 ? -1 : 1 + temp };
};
}; // namespace TL
// type selection based on a boolean
template <bool flag, typename T, typename U>
struct Select {
typedef T Result;
};
template <typename T, typename U>
struct Select<false, T, U> {
typedef U Result;
};
// -----------------------------------------------------------------------
// Type traits
template <typename T>
class TypeTraits {
typedef TYPELIST_4(
unsigned char, unsigned short,
unsigned int, unsigned long int) UnsignedInts;
typedef TYPELIST_4(
signed char, signed short,
signed int, signed long int) SignedInts;
typedef TYPELIST_1(
bool) OtherInts;
typedef TYPELIST_3(
float, double, long double) Floats;
template<typename U> struct PointerTraits {
enum { result = false };
typedef NullType PointeeType;
};
template<typename U> struct PointerTraits<U*> {
enum { result = true };
typedef U PointeeType;
};
public:
enum { isStdUnsignedInt = TL::IndexOf<UnsignedInts, T>::value >= 0 };
enum { isStdSignedInt = TL::IndexOf<SignedInts, T>::value >= 0 };
enum { isStdIntegral = TL::IndexOf<OtherInts, T>::value >= 0 || isStdUnsignedInt || isStdSignedInt };
enum { isStdFloat = TL::IndexOf<Floats, T>::value >= 0 };
enum { isPointer = PointerTraits<T>::result };
enum { isStdArith = isStdIntegral || isStdFloat };
// best parameter type for given type
typedef typename Select<isStdArith || isPointer, T, const T&>::Result ParameterType;
};
// -----------------------------------------------------------------------
}; // namespace android
#endif /* ANDROID_TRAITS_H */

438
src/mmc35240/vec.h Normal file
View File

@ -0,0 +1,438 @@
/*
* Copyright (C) 2011 The Android Open Source Project
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef ANDROID_VEC_H
#define ANDROID_VEC_H
#include <math.h>
#include <stdint.h>
#include <stddef.h>
#include "traits.h"
// -----------------------------------------------------------------------
#define PURE __attribute__((pure))
namespace android {
// -----------------------------------------------------------------------
// non-inline helpers
template <typename TYPE, size_t SIZE>
class vec;
template <typename TYPE, size_t SIZE>
struct vbase;
namespace helpers {
template <typename T> inline T min(T a, T b) { return a<b ? a : b; }
template <typename T> inline T max(T a, T b) { return a>b ? a : b; }
template < template<typename T, size_t S> class VEC,
typename TYPE, size_t SIZE, size_t S>
vec<TYPE, SIZE>& doAssign(
vec<TYPE, SIZE>& lhs, const VEC<TYPE, S>& rhs) {
const size_t minSize = min(SIZE, S);
const size_t maxSize = max(SIZE, S);
for (size_t i=0 ; i<minSize ; i++)
lhs[i] = rhs[i];
for (size_t i=minSize ; i<maxSize ; i++)
lhs[i] = 0;
return lhs;
}
template <
template<typename T, size_t S> class VLHS,
template<typename T, size_t S> class VRHS,
typename TYPE,
size_t SIZE
>
VLHS<TYPE, SIZE> PURE doAdd(
const VLHS<TYPE, SIZE>& lhs,
const VRHS<TYPE, SIZE>& rhs) {
VLHS<TYPE, SIZE> r;
for (size_t i=0 ; i<SIZE ; i++)
r[i] = lhs[i] + rhs[i];
return r;
}
template <
template<typename T, size_t S> class VLHS,
template<typename T, size_t S> class VRHS,
typename TYPE,
size_t SIZE
>
VLHS<TYPE, SIZE> PURE doSub(
const VLHS<TYPE, SIZE>& lhs,
const VRHS<TYPE, SIZE>& rhs) {
VLHS<TYPE, SIZE> r;
for (size_t i=0 ; i<SIZE ; i++)
r[i] = lhs[i] - rhs[i];
return r;
}
template <
template<typename T, size_t S> class VEC,
typename TYPE,
size_t SIZE
>
VEC<TYPE, SIZE> PURE doMulScalar(
const VEC<TYPE, SIZE>& lhs,
typename TypeTraits<TYPE>::ParameterType rhs) {
VEC<TYPE, SIZE> r;
for (size_t i=0 ; i<SIZE ; i++)
r[i] = lhs[i] * rhs;
return r;
}
template <
template<typename T, size_t S> class VEC,
typename TYPE,
size_t SIZE
>
VEC<TYPE, SIZE> PURE doScalarMul(
typename TypeTraits<TYPE>::ParameterType lhs,
const VEC<TYPE, SIZE>& rhs) {
VEC<TYPE, SIZE> r;
for (size_t i=0 ; i<SIZE ; i++)
r[i] = lhs * rhs[i];
return r;
}
}; // namespace helpers
// -----------------------------------------------------------------------
// Below we define the mathematical operators for vectors.
// We use template template arguments so we can generically
// handle the case where the right-hand-size and left-hand-side are
// different vector types (but with same value_type and size).
// This is needed for performance when using ".xy{z}" element access
// on vec<>. Without this, an extra conversion to vec<> would be needed.
//
// example:
// vec4_t a;
// vec3_t b;
// vec3_t c = a.xyz + b;
//
// "a.xyz + b" is a mixed-operation between a vbase<> and a vec<>, requiring
// a conversion of vbase<> to vec<>. The template gunk below avoids this,
// by allowing the addition on these different vector types directly
//
template <
template<typename T, size_t S> class VLHS,
template<typename T, size_t S> class VRHS,
typename TYPE,
size_t SIZE
>
inline VLHS<TYPE, SIZE> PURE operator + (
const VLHS<TYPE, SIZE>& lhs,
const VRHS<TYPE, SIZE>& rhs) {
return helpers::doAdd(lhs, rhs);
}
template <
template<typename T, size_t S> class VLHS,
template<typename T, size_t S> class VRHS,
typename TYPE,
size_t SIZE
>
inline VLHS<TYPE, SIZE> PURE operator - (
const VLHS<TYPE, SIZE>& lhs,
const VRHS<TYPE, SIZE>& rhs) {
return helpers::doSub(lhs, rhs);
}
template <
template<typename T, size_t S> class VEC,
typename TYPE,
size_t SIZE
>
inline VEC<TYPE, SIZE> PURE operator * (
const VEC<TYPE, SIZE>& lhs,
typename TypeTraits<TYPE>::ParameterType rhs) {
return helpers::doMulScalar(lhs, rhs);
}
template <
template<typename T, size_t S> class VEC,
typename TYPE,
size_t SIZE
>
inline VEC<TYPE, SIZE> PURE operator * (
typename TypeTraits<TYPE>::ParameterType lhs,
const VEC<TYPE, SIZE>& rhs) {
return helpers::doScalarMul(lhs, rhs);
}
template <
template<typename T, size_t S> class VLHS,
template<typename T, size_t S> class VRHS,
typename TYPE,
size_t SIZE
>
TYPE PURE dot_product(
const VLHS<TYPE, SIZE>& lhs,
const VRHS<TYPE, SIZE>& rhs) {
TYPE r(0);
for (size_t i=0 ; i<SIZE ; i++)
r += lhs[i] * rhs[i];
return r;
}
template <
template<typename T, size_t S> class V,
typename TYPE,
size_t SIZE
>
TYPE PURE length(const V<TYPE, SIZE>& v) {
return sqrt(dot_product(v, v));
}
template <
template<typename T, size_t S> class V,
typename TYPE,
size_t SIZE
>
TYPE PURE length_squared(const V<TYPE, SIZE>& v) {
return dot_product(v, v);
}
template <
template<typename T, size_t S> class V,
typename TYPE,
size_t SIZE
>
V<TYPE, SIZE> PURE normalize(const V<TYPE, SIZE>& v) {
return v * (1/length(v));
}
template <
template<typename T, size_t S> class VLHS,
template<typename T, size_t S> class VRHS,
typename TYPE
>
VLHS<TYPE, 3> PURE cross_product(
const VLHS<TYPE, 3>& u,
const VRHS<TYPE, 3>& v) {
VLHS<TYPE, 3> r;
r.x = u.y*v.z - u.z*v.y;
r.y = u.z*v.x - u.x*v.z;
r.z = u.x*v.y - u.y*v.x;
return r;
}
template <typename TYPE, size_t SIZE>
vec<TYPE, SIZE> PURE operator - (const vec<TYPE, SIZE>& lhs) {
vec<TYPE, SIZE> r;
for (size_t i=0 ; i<SIZE ; i++)
r[i] = -lhs[i];
return r;
}
// -----------------------------------------------------------------------
// This our basic vector type, it just implements the data storage
// and accessors.
template <typename TYPE, size_t SIZE>
struct vbase {
TYPE v[SIZE];
inline const TYPE& operator[](size_t i) const { return v[i]; }
inline TYPE& operator[](size_t i) { return v[i]; }
};
template<> struct vbase<float, 2> {
union {
float v[2];
struct { float x, y; };
struct { float s, t; };
};
inline const float& operator[](size_t i) const { return v[i]; }
inline float& operator[](size_t i) { return v[i]; }
};
template<> struct vbase<float, 3> {
union {
float v[3];
struct { float x, y, z; };
struct { float s, t, r; };
vbase<float, 2> xy;
vbase<float, 2> st;
};
inline const float& operator[](size_t i) const { return v[i]; }
inline float& operator[](size_t i) { return v[i]; }
};
template<> struct vbase<float, 4> {
union {
float v[4];
struct { float x, y, z, w; };
struct { float s, t, r, q; };
vbase<float, 3> xyz;
vbase<float, 3> str;
vbase<float, 2> xy;
vbase<float, 2> st;
};
inline const float& operator[](size_t i) const { return v[i]; }
inline float& operator[](size_t i) { return v[i]; }
};
// -----------------------------------------------------------------------
template <typename TYPE, size_t SIZE>
class vec : public vbase<TYPE, SIZE>
{
typedef typename TypeTraits<TYPE>::ParameterType pTYPE;
typedef vbase<TYPE, SIZE> base;
public:
// STL-like interface.
typedef TYPE value_type;
typedef TYPE& reference;
typedef TYPE const& const_reference;
typedef size_t size_type;
typedef TYPE* iterator;
typedef TYPE const* const_iterator;
iterator begin() { return base::v; }
iterator end() { return base::v + SIZE; }
const_iterator begin() const { return base::v; }
const_iterator end() const { return base::v + SIZE; }
size_type size() const { return SIZE; }
// -----------------------------------------------------------------------
// default constructors
vec() { }
vec(const vec& rhs) : base(rhs) { }
vec(const base& rhs) : base(rhs) { }
// -----------------------------------------------------------------------
// conversion constructors
vec(pTYPE rhs) {
for (size_t i=0 ; i<SIZE ; i++)
base::operator[](i) = rhs;
}
template < template<typename T, size_t S> class VEC, size_t S>
explicit vec(const VEC<TYPE, S>& rhs) {
helpers::doAssign(*this, rhs);
}
explicit vec(TYPE const* array) {
for (size_t i=0 ; i<SIZE ; i++)
base::operator[](i) = array[i];
}
// -----------------------------------------------------------------------
// Assignment
vec& operator = (const vec& rhs) {
base::operator=(rhs);
return *this;
}
vec& operator = (const base& rhs) {
base::operator=(rhs);
return *this;
}
vec& operator = (pTYPE rhs) {
for (size_t i=0 ; i<SIZE ; i++)
base::operator[](i) = rhs;
return *this;
}
template < template<typename T, size_t S> class VEC, size_t S>
vec& operator = (const VEC<TYPE, S>& rhs) {
return helpers::doAssign(*this, rhs);
}
// -----------------------------------------------------------------------
// operation-assignment
vec& operator += (const vec& rhs);
vec& operator -= (const vec& rhs);
vec& operator *= (pTYPE rhs);
// -----------------------------------------------------------------------
// non-member function declaration and definition
// NOTE: we declare the non-member function as friend inside the class
// so that they are known to the compiler when the class is instantiated.
// This helps the compiler doing template argument deduction when the
// passed types are not identical. Essentially this helps with
// type conversion so that you can multiply a vec<float> by an scalar int
// (for instance).
friend inline vec PURE operator + (const vec& lhs, const vec& rhs) {
return helpers::doAdd(lhs, rhs);
}
friend inline vec PURE operator - (const vec& lhs, const vec& rhs) {
return helpers::doSub(lhs, rhs);
}
friend inline vec PURE operator * (const vec& lhs, pTYPE v) {
return helpers::doMulScalar(lhs, v);
}
friend inline vec PURE operator * (pTYPE v, const vec& rhs) {
return helpers::doScalarMul(v, rhs);
}
friend inline TYPE PURE dot_product(const vec& lhs, const vec& rhs) {
return android::dot_product(lhs, rhs);
}
};
// -----------------------------------------------------------------------
template <typename TYPE, size_t SIZE>
vec<TYPE, SIZE>& vec<TYPE, SIZE>::operator += (const vec<TYPE, SIZE>& rhs) {
vec<TYPE, SIZE>& lhs(*this);
for (size_t i=0 ; i<SIZE ; i++)
lhs[i] += rhs[i];
return lhs;
}
template <typename TYPE, size_t SIZE>
vec<TYPE, SIZE>& vec<TYPE, SIZE>::operator -= (const vec<TYPE, SIZE>& rhs) {
vec<TYPE, SIZE>& lhs(*this);
for (size_t i=0 ; i<SIZE ; i++)
lhs[i] -= rhs[i];
return lhs;
}
template <typename TYPE, size_t SIZE>
vec<TYPE, SIZE>& vec<TYPE, SIZE>::operator *= (vec<TYPE, SIZE>::pTYPE rhs) {
vec<TYPE, SIZE>& lhs(*this);
for (size_t i=0 ; i<SIZE ; i++)
lhs[i] *= rhs;
return lhs;
}
// -----------------------------------------------------------------------
typedef vec<float, 2> vec2_t;
typedef vec<float, 3> vec3_t;
typedef vec<float, 4> vec4_t;
// -----------------------------------------------------------------------
}; // namespace android
#endif /* ANDROID_VEC_H */