upm: enable MMC35240 3-axis magnetic sensor library and example
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
committed by
Mihai Tudor Panu
Mihai Tudor Panu
parent
4ea01180a1
commit
fabf4287d6
438
src/mmc35240/vec.h
Normal file
438
src/mmc35240/vec.h
Normal 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 */
|
||||
Reference in New Issue
Block a user