int quat_to_orientation(const float *quat, float &azimuth, float &pitch, float &roll)
{
int error;
- float g[3];
float R[9];
error = quat_to_matrix(quat, R);
if (error < 0)
return error;
- float xyz_z = ARCTAN(R[3], R[0]);
- float yxz_x = asinf(R[7]);
- float yxz_y = ARCTAN(-R[6], R[8]);
- float yxz_z = ARCTAN(-R[1], R[4]);
-
- float a = fabs(yxz_x / HALF);
- a = a * a;
-
- float p = (fabs(yxz_y) / HALF - 1.0);
-
- if (p < 0)
- p = 0;
-
- float v = 1 + (1 - a) / a * p;
-
- if (v > 20)
- v = 20;
-
- if (yxz_x * yxz_y > 0) {
- if (yxz_z > 0 && xyz_z < 0)
- xyz_z += M_PI * 2;
- } else {
- if (yxz_z < 0 && xyz_z > 0)
- xyz_z -= M_PI * 2;
- }
-
- g[0] = (1 - a * v) * yxz_z + (a * v) * xyz_z;
- g[0] *= -1;
-
- float tmp = R[7];
-
- if (tmp > 1.0f)
- tmp = 1.0f;
- else if (tmp < -1.0f)
- tmp = -1.0f;
-
- g[1] = -asinf(tmp);
- if (R[8] < 0)
- g[1] = M_PI - g[1];
-
- if (g[1] > M_PI)
- g[1] -= M_PI * 2;
-
- if ((fabs(R[7]) > QUAT))
- g[2] = (float) atan2f(R[6], R[7]);
- else
- g[2] = (float) atan2f(R[6], R[8]);
-
- if (g[2] > HALF)
- g[2] = M_PI - g[2];
- else if (g[2] < -HALF)
- g[2] = -M_PI - g[2];
-
- g[0] *= RAD2DEGREE;
- g[1] *= RAD2DEGREE;
- g[2] *= RAD2DEGREE;
-
- if (g[0] < 0)
- g[0] += 360;
-
- azimuth = g[0];
- pitch = g[1];
- roll = g[2];
+ azimuth = atan2f(-R[3], R[0]) * RAD2DEGREE;
+ pitch = atan2f(-R[7], R[8]) * RAD2DEGREE;
+ roll = asinf (R[6]) * RAD2DEGREE;
+ if (azimuth < 0)
+ azimuth += 360;
return 0;
}
#include <sensor_log.h>
#include <sensor_types.h>
-#include <fusion_util.h>
#include <cmath>
#define NAME_SENSOR "http://tizen.org/sensor/general/gravity/tizen_complementary"
#include <sensor_log.h>
#include <sensor_types.h>
-#include <fusion_util.h>
#include <cmath>
#define NAME_SENSOR "http://tizen.org/sensor/general/gravity/tizen_lowpass"
#include <sensor_log.h>
#include <sensor_types.h>
-#include <fusion_util.h>
#define NAME_SENSOR "http://tizen.org/sensor/general/linear_acceleration/tizen_default"
#define NAME_VENDOR "tizen.org"
#include <sensor_log.h>
#include <sensor_types.h>
-#include <fusion_util.h>
#include <cmath>
#define NAME_SENSOR "http://samsung.com/sensor/healthinfo/pedometer/samsung_pedometer"
#include <cmath>
#include "fusion_base.h"
-#define ACCEL_COMPENSATION -1
-#define GYRO_COMPENSATION 1
-#define MAG_COMPENSATION -1
+const float RAD2DEG = 57.29577951;
+const float US2S = 1000000.0f;
fusion_base::fusion_base()
: m_enable_accel(false)
, m_z(0)
, m_w(0)
, m_timestamp(0)
+, m_timestamp_accel(0)
+, m_timestamp_gyro(0)
+, m_timestamp_mag(0)
{
}
void fusion_base::push_accel(sensor_data_t &data)
{
//_I("[fusion_sensor] : Pushing accel");
- pre_process_data(m_accel, data.values, ACCEL_COMPENSATION, ACCEL_SCALE);
- m_accel.m_time_stamp = data.timestamp;
+ android::vec3_t v(data.values);
+
+ float dT = (data.timestamp - m_timestamp_accel) / US2S;
+ m_timestamp_accel = data.timestamp;
+ m_timestamp = data.timestamp;
+
m_enable_accel = true;
- if (get_orientation())
- store_orientation();
+ m_orientation_filter.handleAcc(v, dT);
+ store_orientation();
}
void fusion_base::push_gyro(sensor_data_t &data)
{
- //_I("[fusion_sensor] : Pushing mag");
- pre_process_data(m_gyro, data.values, GYRO_COMPENSATION, GYRO_SCALE);
- m_gyro.m_time_stamp = data.timestamp;
+ //_I("[fusion_sensor] : Pushing gyro");
+ android::vec3_t v(data.values);
+ v[0] /= RAD2DEG;
+ v[1] /= RAD2DEG;
+ v[2] /= RAD2DEG;
+
+ float dT = (data.timestamp - m_timestamp_gyro) / US2S;
+ m_timestamp_gyro = data.timestamp;
+ m_timestamp = data.timestamp;
+
m_enable_gyro = true;
- if (get_orientation())
- store_orientation();
+ m_orientation_filter.handleGyro(v, dT);
+ store_orientation();
}
void fusion_base::push_mag(sensor_data_t &data)
{
- //_I("[fusion_sensor] : Pushing gyro");
- pre_process_data(m_magnetic, data.values, MAG_COMPENSATION, GEOMAGNETIC_SCALE);
- m_magnetic.m_time_stamp = data.timestamp;
+ //_I("[fusion_sensor] : Pushing mag");
+ android::vec3_t v(data.values);
+
+ m_timestamp_mag = data.timestamp;
+ m_timestamp = data.timestamp;
+
m_enable_magnetic = true;
- if (get_orientation())
- store_orientation();
+ m_orientation_filter.handleMag(v);
+ store_orientation();
}
bool fusion_base::get_rv(unsigned long long ×tamp, float &x, float &y, float &z, float &w)
void fusion_base::store_orientation(void)
{
- m_x = m_orientation_filter.m_quaternion.m_quat.m_vec[0];
- m_y = m_orientation_filter.m_quaternion.m_quat.m_vec[1];
- m_z = m_orientation_filter.m_quaternion.m_quat.m_vec[2];
- m_w = m_orientation_filter.m_quaternion.m_quat.m_vec[3];
+ android::quat_t q = m_orientation_filter.getAttitude();
+ m_x = q[0];
+ m_y = q[1];
+ m_z = q[2];
+ m_w = q[3];
if (std::isnan(m_x) || std::isnan(m_y) || std::isnan(m_z) || std::isnan(m_w)) {
- m_timestamp = 0;
- m_orientation_filter = orientation_filter<float>();
+ m_timestamp = m_timestamp_accel = m_timestamp_gyro = m_timestamp_mag = 0;
+ m_orientation_filter = android::orientation_filter();
}
clear();
}
virtual bool get_rv(unsigned long long ×tamp, float &w, float &x, float &y, float &z);
protected:
-
- sensor_data<float> m_accel;
- sensor_data<float> m_gyro;
- sensor_data<float> m_magnetic;
-
- orientation_filter<float> m_orientation_filter;
+ android::orientation_filter m_orientation_filter;
bool m_enable_accel;
bool m_enable_gyro;
float m_y;
float m_z;
float m_w;
+
float m_timestamp;
+ float m_timestamp_accel;
+ float m_timestamp_gyro;
+ float m_timestamp_mag;
void clear();
void store_orientation(void);
- virtual bool get_orientation(void) = 0;
};
--- /dev/null
+/*
+ * Copyright (C) 2007 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.
+ */
+
+// released in android-11.0.0_r9
+
+#ifndef ANDROID_ERRORS_H
+#define ANDROID_ERRORS_H
+#include <sys/types.h>
+#include <errno.h>
+namespace android {
+// use this type to return error codes
+#ifdef HAVE_MS_C_RUNTIME
+typedef int status_t;
+#else
+typedef int32_t status_t;
+#endif
+/* the MS C runtime lacks a few error codes */
+/*
+ * Error codes.
+ * All error codes are negative values.
+ */
+// Win32 #defines NO_ERROR as well. It has the same value, so there's no
+// real conflict, though it's a bit awkward.
+#ifdef _WIN32
+# undef NO_ERROR
+#endif
+
+enum {
+ OK = 0, // Everything's swell.
+ NO_ERROR = 0, // No errors.
+
+ UNKNOWN_ERROR = 0x80000000,
+ NO_MEMORY = -ENOMEM,
+ INVALID_OPERATION = -ENOSYS,
+ BAD_VALUE = -EINVAL,
+ BAD_TYPE = 0x80000001,
+ NAME_NOT_FOUND = -ENOENT,
+ PERMISSION_DENIED = -EPERM,
+ NO_INIT = -ENODEV,
+ ALREADY_EXISTS = -EEXIST,
+ DEAD_OBJECT = -EPIPE,
+ FAILED_TRANSACTION = 0x80000002,
+ JPARKS_BROKE_IT = -EPIPE,
+#if !defined(HAVE_MS_C_RUNTIME)
+ BAD_INDEX = -EOVERFLOW,
+ NOT_ENOUGH_DATA = -ENODATA,
+ WOULD_BLOCK = -EWOULDBLOCK,
+ TIMED_OUT = -ETIMEDOUT,
+ UNKNOWN_TRANSACTION = -EBADMSG,
+#else
+ BAD_INDEX = -E2BIG,
+ NOT_ENOUGH_DATA = 0x80000003,
+ WOULD_BLOCK = 0x80000004,
+ TIMED_OUT = 0x80000005,
+ UNKNOWN_TRANSACTION = 0x80000006,
+#endif
+ FDS_NOT_ALLOWED = 0x80000007,
+};
+// Restore define; enumeration is in "android" namespace, so the value defined
+// there won't work for Win32 code in a different namespace.
+#ifdef _WIN32
+# define NO_ERROR 0L
+#endif
+}; // namespace android
+
+// ---------------------------------------------------------------------------
+
+#endif // ANDROID_ERRORS_H
\ No newline at end of file
+++ /dev/null
-/*
- * sensord
- *
- * Copyright (c) 2014 Samsung Electronics Co., Ltd.
- *
- * 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.
- *
- */
-#if defined (_EULER_ANGLES_H_) && defined (_VECTOR_H_)
-
-#include <math.h>
-
-#define RAD2DEG 57.2957795
-#define DEG2RAD 0.0174532925
-
-template <typename TYPE>
-euler_angles<TYPE>::euler_angles() : m_ang()
-{
-}
-
-template <typename TYPE>
-euler_angles<TYPE>::euler_angles(const TYPE roll, const TYPE pitch, const TYPE azimuth)
-{
- TYPE euler_data[EULER_SIZE] = {roll, pitch, azimuth};
-
- vect<TYPE, EULER_SIZE> v(euler_data);
- m_ang = v;
-}
-
-template <typename TYPE>
-euler_angles<TYPE>::euler_angles(const vect<TYPE, EULER_SIZE> v)
-{
- m_ang = v;
-}
-
-template <typename TYPE>
-euler_angles<TYPE>::euler_angles(const euler_angles<TYPE>& e)
-{
- m_ang = e.m_ang;
-}
-
-template <typename TYPE>
-euler_angles<TYPE>::~euler_angles()
-{
-}
-
-template <typename TYPE>
-euler_angles<TYPE> euler_angles<TYPE>::operator =(const euler_angles<TYPE>& e)
-{
- m_ang = e.m_ang;
-
- return *this;
-}
-
-template <typename T>
-euler_angles<T> quat2euler(const quaternion<T> q)
-{
- T w, x, y, z;
- T R11, R21, R31, R32, R33;
- T phi, theta, psi;
-
- w = q.m_quat.m_vec[0];
- x = q.m_quat.m_vec[1];
- y = q.m_quat.m_vec[2];
- z = q.m_quat.m_vec[3];
-
- R11 = 2 * (w * w) - 1 + 2 * (x * x);
- R21 = 2 * ((x * y) - (w * z));
- R31 = 2 * ((x * z) + (w * y));
- R32 = 2 * ((y * z) - (w * x));
- R33 = 2 * (w * w) - 1 + 2 * (z * z);
-
- phi = atan2(R32, R33);
- theta = atan2(-R31 , sqrt((R32 * R32) + (R33 * R33)));
- psi = atan2(R21, R11);
-
- euler_angles<T> e(phi, theta, psi);
-
- return e;
-}
-
-template <typename T>
-euler_angles<T> rad2deg(const euler_angles<T> e)
-{
- return (e.m_ang * (T) RAD2DEG);
-}
-
-template <typename T>
-euler_angles<T> deg2rad(const euler_angles<T> e)
-{
- return (e.m_ang * (T) DEG2RAD);
-}
-
-template<typename T>
-quaternion<T> euler2quat(euler_angles<T> euler) {
- T theta = euler.m_ang.m_vec[0];
- T phi = euler.m_ang.m_vec[1];
- T psi = euler.m_ang.m_vec[2];
-
- T R[ROT_MAT_ROWS][ROT_MAT_COLS];
- R[0][0] = cos(psi)*cos(theta);
- R[0][1] = -sin(psi)*cos(phi) + cos(psi)*sin(theta)*sin(phi);
- R[0][2] = sin(psi)*sin(phi) + cos(psi)*sin(theta)*cos(phi);
- R[1][0] = sin(psi)*cos(theta);
- R[1][1] = cos(psi)*cos(phi) + sin(psi)*sin(theta)*sin(phi);
- R[1][2] = -cos(psi)*sin(phi) + sin(psi)*sin(theta)*cos(phi);
- R[2][0] = -sin(theta);
- R[2][1] = cos(theta)*sin(phi);
- R[2][2] = cos(theta)*cos(phi);
-
- rotation_matrix<T> rot_mat(R);
- quaternion<T> q = rot_mat2quat(rot_mat);
- return q;
-}
-
-#endif //_EULER_ANGLES_H_
+++ /dev/null
-/*
- * sensord
- *
- * Copyright (c) 2014 Samsung Electronics Co., Ltd.
- *
- * 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 _EULER_ANGLES_H_
-#define _EULER_ANGLES_H_
-
-#include "vector.h"
-#include "quaternion.h"
-#include "rotation_matrix.h"
-
-#define EULER_SIZE 3
-
-template <typename TYPE>
-class euler_angles {
-public:
- vect<TYPE, EULER_SIZE> m_ang;
-
- euler_angles();
- euler_angles(const TYPE roll, const TYPE pitch, const TYPE azimuth);
- euler_angles(const vect<TYPE, EULER_SIZE> v);
- euler_angles(const euler_angles<TYPE>& e);
- ~euler_angles();
-
- euler_angles<TYPE> operator =(const euler_angles<TYPE>& e);
-
- template<typename T> friend euler_angles<T> quat2euler(const quaternion<T> q);
- template<typename T> friend quaternion<T> euler2quat(euler_angles<T> euler);
- template<typename T> friend euler_angles<T> rad2deg(const euler_angles<T> e);
- template<typename T> friend euler_angles<T> deg2rad(const euler_angles<T> e);
-};
-
-#include "euler_angles.cpp"
-
-#endif //_EULER_ANGLES_H_
--- /dev/null
+/*
+ * 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.
+ */
+
+// released in android-11.0.0_r9
+
+#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) { } // NOLINT(google-explicit-constructor)
+
+ // -----------------------------------------------------------------------
+ // conversion constructors
+
+ // sets the diagonal to the value, off-diagonal to zero
+ mat(pTYPE rhs) { // NOLINT(google-explicit-constructor)
+ 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;
+ explicit 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() { }
+ explicit mat(const base& rhs) : base(rhs) { }
+ mat(const mat& rhs) : base(rhs) { }
+ explicit 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;
+}
+
+// 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 */
+++ /dev/null
-/*
- * sensord
- *
- * Copyright (c) 2014 Samsung Electronics Co., Ltd.
- *
- * 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.
- *
- */
-
-#ifdef _MATRIX_H_
-
-TYPE_ROW_COL matrix<TYPE, ROW, COL>::matrix(void)
-{
- for (int i = 0; i < ROW; i++)
- for (int j = 0; j < COL; j++)
- m_mat[i][j] = 0;
-}
-
-TYPE_ROW_COL matrix<TYPE, ROW, COL>::matrix(const matrix<TYPE, ROW, COL> &m)
-{
- for (int p = 0; p < ROW; p++)
- for (int q = 0; q < COL; q++)
- m_mat[p][q] = m.m_mat[p][q];
-}
-
-TYPE_ROW_COL matrix<TYPE, ROW, COL>::matrix(TYPE mat_data[ROW][COL])
-{
- for (int i = 0; i < ROW; i++)
- for (int j = 0; j < COL; j++)
- m_mat[i][j] = mat_data[i][j];
-}
-
-TYPE_ROW_COL matrix<TYPE, ROW, COL>::~matrix()
-{
-}
-
-TYPE_ROW_COL matrix<TYPE, ROW, COL> matrix<TYPE, ROW, COL>::operator =(const matrix<TYPE, ROW, COL> &m)
-{
- if (this == &m)
- {
- return *this;
- }
-
- for (int i = 0; i < ROW; i++)
- for (int j = 0; j < COL; j++)
- m_mat[i][j] = m.m_mat[i][j];
-
- return *this;
-}
-
-T_R_C ostream& operator <<(ostream& dout, matrix<T, R, C> &m)
-{
- for (int i = 0; i < R; i++)
- {
- for (int j = 0; j < C; j++)
- {
- dout << m.m_mat[i][j] << "\t";
- }
- dout << endl;
- }
- return dout;
-}
-
-T_R_C matrix<T, R, C> operator +(const matrix<T, R, C> &m1, const matrix<T, R, C> &m2)
-{
- matrix<T, R, C> m3;
-
- for (int i = 0; i < R; i++)
- for (int j = 0; j < C; j++)
- m3.m_mat[i][j] = m1.m_mat[i][j] + m2.m_mat[i][j];
-
- return m3;
-}
-
-T_R_C matrix<T, R, C> operator +(const matrix<T, R, C> &m, const T &val)
-{
- matrix<T, R, C> m1;
-
- for (int i = 0; i < R; i++)
- for (int j = 0; j < C; j++)
- m1.m_mat[i][j] = m.m_mat[i][j] + val;
-
- return m1;
-}
-
-T_R_C matrix<T, R, C> operator -(const matrix<T, R, C> &m1, const matrix<T, R, C> &m2)
-{
- matrix<T, R, C> m3;
-
- for (int i = 0; i < R; i++)
- for (int j = 0; j < C; j++)
- m3.m_mat[i][j] = m1.m_mat[i][j] - m2.m_mat[i][j];
-
- return m3;
-}
-
-T_R_C matrix<T, R, C> operator -(const matrix<T, R, C> &m, const T &val)
-{
- matrix<T, R, C> m1;
-
- for (int i = 0; i < R; i++)
- for (int j = 0; j < C; j++)
- m1.m_mat[i][j] = m.m_mat[i][j] - val;
-
- return m1;
-}
-
-T_R_C_C2 matrix<T, R, C2> operator *(const matrix<T, R, C> &m1, const matrix<T, C, C2> &m2)
-{
- matrix<T, R, C2> m3;
-
- for (int i = 0; i < R; i++)
- {
- for (int j = 0; j < C2; j++)
- {
- m3.m_mat[i][j] = 0;
- for (int k = 0; k < C; k++)
- m3.m_mat[i][j] += m1.m_mat[i][k] * m2.m_mat[k][j];
- }
- }
-
- return m3;
-}
-
-T_R_C matrix<T, R, C> operator *(const matrix<T, R, C> &m, const T &val)
-{
- matrix<T, R, C> m1;
-
- for (int i = 0; i < R; i++)
- for (int j = 0; j < C; j++)
- m1.m_mat[i][j] = m.m_mat[i][j] * val;
-
- return m1;
-}
-
-T_R_C matrix<T, R, C> operator /(const matrix<T, R, C> &m1, const T &val)
-{
- matrix<T, R, C> m3;
-
- for (int i = 0; i < R; i++)
- for (int j = 0; j < C; j++)
- m3.m_mat[i][j] = m1.m_mat[i][j] / val;
-
- return m3;
-}
-
-T_R1_C1_R2_C2 bool operator ==(const matrix<T, R1, C1> &m1, const matrix<T, R2, C2> &m2)
-{
- if ((R1 == R2) && (C1 == C2)) {
- for (int i = 0; i < R1; i++)
- for (int j = 0; j < C2; j++)
- if (m1.m_mat[i][j] != m2.m_mat[i][j])
- return false;
- return true;
- }
-
- return false;
-}
-
-T_R1_C1_R2_C2 bool operator !=(const matrix<T, R1, C1> &m1, const matrix<T, R2, C2> &m2)
-{
- return (!(m1 == m2));
-}
-
-T_R_C matrix<T, R, C> tran(const matrix<T, R, C> &m)
-{
- matrix<T, R, C> m1;
-
- for (int i = 0; i < R; i++)
- for (int j = 0; j < C; j++)
- m1.m_mat[j][i] = m.m_mat[i][j];
-
- return m1;
-}
-
-#endif //_MATRIX_H_
+++ /dev/null
-/*
- * sensord
- *
- * Copyright (c) 2014 Samsung Electronics Co., Ltd.
- *
- * 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 _MATRIX_H_
-#define _MATRIX_H_
-
-#include <assert.h>
-#include <iostream>
-using namespace std;
-
-#define TYPE_ROW_COL template<typename TYPE, int ROW, int COL>
-#define T_R_C template<typename T, int R, int C>
-#define T_R_C_C2 template<typename T, int R, int C, int C2>
-#define T_R1_C1_R2_C2 template<typename T, int R1, int C1, int R2, int C2>
-
-TYPE_ROW_COL class matrix {
-public:
- TYPE m_mat[ROW][COL];
-
- matrix(void);
- matrix(TYPE mat_data[ROW][COL]);
- matrix(const matrix<TYPE, ROW, COL>& m);
- ~matrix();
-
- matrix<TYPE, ROW, COL> operator =(const matrix<TYPE, ROW, COL> &m);
-
- T_R_C friend ostream& operator << (ostream& dout, matrix<T, R, C> &m);
- T_R_C friend matrix<T, R, C> operator +(const matrix<T, R, C> &m1, const matrix<T, R, C> &m2);
- T_R_C friend matrix<T, R, C> operator +(const matrix<T, R, C> &m, const T &val);
- T_R_C friend matrix<T, R, C> operator -(const matrix<T, R, C> &m1, const matrix<T, R, C> &m2);
- T_R_C friend matrix<T, R, C> operator -(const matrix<T, R, C> &m, const T &val);
- T_R_C_C2 friend matrix<T, R, C2> operator *(const matrix<T, R, C> &m1, const matrix<T, C, C2> &m2);
- T_R_C friend matrix<T, R, C> operator *(const matrix<T, R, C> &m, const T &val);
- T_R_C friend matrix<T, R, C> operator /(const matrix<T, R, C> &m1, const T &val);
- T_R1_C1_R2_C2 friend bool operator ==(const matrix<T, R1, C1> &m1, const matrix<T, R2, C2> &m2);
- T_R1_C1_R2_C2 friend bool operator !=(const matrix<T, R1, C1> &m1, const matrix<T, R2, C2> &m2);
- T_R_C friend matrix<T, R, C> tran(const matrix<T, R, C> &m);
-};
-
-#include "matrix.cpp"
-
-#endif //_MATRIX_H_
/*
- * sensord
- *
- * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ * 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
+ * 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.
- *
*/
-
-#ifdef _ORIENTATION_FILTER_H_
+// released in android-11.0.0_r9
#include "orientation_filter.h"
-//Windowing is used for buffering of previous samples for statistical analysis
-#define MOVING_AVERAGE_WINDOW_LENGTH 20
-//Earth's Gravity
-#define GRAVITY 9.80665
-#define PI 3.141593
-//Needed for non-zero initialization for statistical analysis
-#define NON_ZERO_VAL 0.1
-//microseconds to seconds
-#define US2S (1.0 / 1000000.0)
-//Initialize sampling interval to 100000microseconds
-#define SAMPLE_INTV 100000
-#define ACCEL_THRESHOLD 0.2
-#define GYRO_THRESHOLD (0.01 * PI)
-
-// constants for computation of covariance and transition matrices
-#define ZIGMA_W (0.05 * DEG2RAD)
-#define TAU_W 1000
-#define QWB_CONST ((2 * (ZIGMA_W * ZIGMA_W)) / TAU_W)
-#define F_CONST (-1.0 / TAU_W)
-#define SQUARE(T) (T * T)
-
-#define NEGLIGIBLE_VAL 0.0000001
-
-#define ABS(val) (((val) < 0) ? -(val) : (val))
-
-
-template <typename TYPE>
-orientation_filter<TYPE>::orientation_filter()
-{
- TYPE arr[MOVING_AVERAGE_WINDOW_LENGTH];
-
- std::fill_n(arr, MOVING_AVERAGE_WINDOW_LENGTH, NON_ZERO_VAL);
-
- vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> vec(arr);
-
- m_var_gyr_x = vec;
- m_var_gyr_y = vec;
- m_var_gyr_z = vec;
- m_var_roll = vec;
- m_var_pitch = vec;
- m_var_azimuth = vec;
- m_gyro_dt = TYPE();
-}
-
-template <typename TYPE>
-orientation_filter<TYPE>::~orientation_filter()
-{
-}
-
-template <typename TYPE>
-inline void orientation_filter<TYPE>::initialize_sensor_data(const sensor_data<TYPE> *accel,
- const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic)
-{
- m_accel.m_data = accel->m_data;
- m_accel.m_time_stamp = accel->m_time_stamp;
- normalize(m_accel);
-
- if (gyro != NULL) {
- unsigned long long sample_interval_gyro = SAMPLE_INTV;
+using namespace android;
- if (m_gyro.m_time_stamp != 0 && gyro->m_time_stamp != 0)
- sample_interval_gyro = gyro->m_time_stamp - m_gyro.m_time_stamp;
+// -----------------------------------------------------------------------
- m_gyro_dt = sample_interval_gyro * US2S;
- m_gyro.m_time_stamp = gyro->m_time_stamp;
+/*==================== BEGIN FUSION SENSOR PARAMETER =========================*/
- m_gyro.m_data = gyro->m_data * (TYPE) PI;
+/* Note:
+ * If a platform uses software fusion, it is necessary to tune the following
+ * parameters to fit the hardware sensors prior to release.
+ *
+ * The DEFAULT_ parameters will be used in FUSION_9AXIS and FUSION_NOMAG mode.
+ * The GEOMAG_ parameters will be used in FUSION_NOGYRO mode.
+ */
- m_gyro.m_data = m_gyro.m_data - m_bias_correction;
- }
+/*
+ * GYRO_VAR gives the measured variance of the gyro's output per
+ * Hz (or variance at 1 Hz). This is an "intrinsic" parameter of the gyro,
+ * which is independent of the sampling frequency.
+ *
+ * The variance of gyro's output at a given sampling period can be
+ * calculated as:
+ * variance(T) = GYRO_VAR / T
+ *
+ * The variance of the INTEGRATED OUTPUT at a given sampling period can be
+ * calculated as:
+ * variance_integrate_output(T) = GYRO_VAR * T
+ */
+static const float DEFAULT_GYRO_VAR = 1e-7; // (rad/s)^2 / Hz
+static const float DEFAULT_GYRO_BIAS_VAR = 1e-12; // (rad/s)^2 / s (guessed)
+static const float GEOMAG_GYRO_VAR = 1e-4; // (rad/s)^2 / Hz
+static const float GEOMAG_GYRO_BIAS_VAR = 1e-8; // (rad/s)^2 / s (guessed)
- if (magnetic != NULL) {
- m_magnetic.m_data = magnetic->m_data;
- m_magnetic.m_time_stamp = magnetic->m_time_stamp;
- }
-}
+/*
+ * Standard deviations of accelerometer and magnetometer
+ */
+static const float DEFAULT_ACC_STDEV = 0.015f; // m/s^2 (measured 0.08 / CDD 0.05)
+static const float DEFAULT_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5)
+static const float GEOMAG_ACC_STDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05)
+static const float GEOMAG_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5)
-template <typename TYPE>
-inline void orientation_filter<TYPE>::orientation_triad_algorithm()
-{
- TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0};
- TYPE arr_mag_e[V1x3S] = {0.0, 1.0, 0.0};
- vect<TYPE, V1x3S> acc_e(arr_acc_e);
- vect<TYPE, V1x3S> mag_e(arr_mag_e);
+/* ====================== END FUSION SENSOR PARAMETER ========================*/
- vect<TYPE, SENSOR_DATA_SIZE> acc_b_x_mag_b = cross(m_accel.m_data, m_magnetic.m_data);
- vect<TYPE, V1x3S> acc_e_x_mag_e = cross(acc_e, mag_e);
+static const float SYMMETRY_TOLERANCE = 1e-10f;
- vect<TYPE, SENSOR_DATA_SIZE> cross1 = cross(acc_b_x_mag_b, m_accel.m_data);
- vect<TYPE, V1x3S> cross2 = cross(acc_e_x_mag_e, acc_e);
+/*
+ * Accelerometer updates will not be performed near free fall to avoid
+ * ill-conditioning and div by zeros.
+ * Threshhold: 10% of g, in m/s^2
+ */
+static const float NOMINAL_GRAVITY = 9.81f;
+static const float FREE_FALL_THRESHOLD = 0.1f * (NOMINAL_GRAVITY);
- matrix<TYPE, M3X3R, M3X3C> mat_b;
- matrix<TYPE, M3X3R, M3X3C> mat_e;
+/*
+ * The geomagnetic-field should be between 30uT and 60uT.
+ * Fields strengths greater than this likely indicate a local magnetic
+ * disturbance which we do not want to update into the fused frame.
+ */
+static const float MAX_VALID_MAGNETIC_FIELD = 100; // uT
+static const float MAX_VALID_MAGNETIC_FIELD_SQ =
+ MAX_VALID_MAGNETIC_FIELD*MAX_VALID_MAGNETIC_FIELD;
- for(int i = 0; i < M3X3R; i++)
- {
- mat_b.m_mat[i][0] = m_accel.m_data.m_vec[i];
- mat_b.m_mat[i][1] = acc_b_x_mag_b.m_vec[i];
- mat_b.m_mat[i][2] = cross1.m_vec[i];
- mat_e.m_mat[i][0] = acc_e.m_vec[i];
- mat_e.m_mat[i][1] = acc_e_x_mag_e.m_vec[i];
- mat_e.m_mat[i][2] = cross2.m_vec[i];
- }
+/*
+ * Values of the field smaller than this should be ignored in fusion to avoid
+ * ill-conditioning. This state can happen with anomalous local magnetic
+ * disturbances canceling the Earth field.
+ */
+static const float MIN_VALID_MAGNETIC_FIELD = 10; // uT
+static const float MIN_VALID_MAGNETIC_FIELD_SQ =
+ MIN_VALID_MAGNETIC_FIELD*MIN_VALID_MAGNETIC_FIELD;
- matrix<TYPE, M3X3R, M3X3C> mat_b_t = tran(mat_b);
- rotation_matrix<TYPE> rot_mat(mat_e * mat_b_t);
+/*
+ * If the cross product of two vectors has magnitude squared less than this,
+ * we reject it as invalid due to alignment of the vectors.
+ * This threshold is used to check for the case where the magnetic field sample
+ * is parallel to the gravity field, which can happen in certain places due
+ * to magnetic field disturbances.
+ */
+static const float MIN_VALID_CROSS_PRODUCT_MAG = 1.0e-3;
+static const float MIN_VALID_CROSS_PRODUCT_MAG_SQ =
+ MIN_VALID_CROSS_PRODUCT_MAG*MIN_VALID_CROSS_PRODUCT_MAG;
+
+static const float SQRT_3 = 1.732f;
+static const float WVEC_EPS = 1e-4f/SQRT_3;
+// -----------------------------------------------------------------------
+
+template <typename TYPE, size_t C, size_t R>
+static mat<TYPE, R, R> scaleCovariance(
+ const mat<TYPE, C, R>& A,
+ const mat<TYPE, C, C>& P) {
+ // A*P*transpose(A);
+ mat<TYPE, R, R> APAt;
+ for (size_t r=0 ; r<R ; r++) {
+ for (size_t j=r ; j<R ; j++) {
+ double apat(0);
+ for (size_t c=0 ; c<C ; c++) {
+ double v(A[c][r]*P[c][c]*0.5);
+ for (size_t k=c+1 ; k<C ; k++)
+ v += A[k][r] * P[c][k];
+ apat += 2 * v * A[c][j];
+ }
+ APAt[j][r] = apat;
+ APAt[r][j] = apat;
+ }
+ }
+ return APAt;
+}
- m_quat_aid = rot_mat2quat(rot_mat);
+template <typename TYPE, typename OTHER_TYPE>
+static mat<TYPE, 3, 3> crossMatrix(const vec<TYPE, 3>& p, OTHER_TYPE diag) {
+ mat<TYPE, 3, 3> r;
+ r[0][0] = diag;
+ r[1][1] = diag;
+ r[2][2] = diag;
+ r[0][1] = p.z;
+ r[1][0] =-p.z;
+ r[0][2] =-p.y;
+ r[2][0] = p.y;
+ r[1][2] = p.x;
+ r[2][1] =-p.x;
+ return r;
}
-template <typename TYPE>
-inline void orientation_filter<TYPE>::compute_accel_orientation()
-{
- TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0};
- vect<TYPE, V1x3S> acc_e(arr_acc_e);
+template<typename TYPE, size_t SIZE>
+class Covariance {
+ mat<TYPE, SIZE, SIZE> mSumXX;
+ vec<TYPE, SIZE> mSumX;
+ size_t mN;
+public:
+ Covariance() : mSumXX(0.0f), mSumX(0.0f), mN(0) { }
+ void update(const vec<TYPE, SIZE>& x) {
+ mSumXX += x*transpose(x);
+ mSumX += x;
+ mN++;
+ }
+ mat<TYPE, SIZE, SIZE> operator()() const {
+ const float N = 1.0f / mN;
+ return mSumXX*N - (mSumX*transpose(mSumX))*(N*N);
+ }
+ void reset() {
+ mN = 0;
+ mSumXX = 0;
+ mSumX = 0;
+ }
+ size_t getCount() const {
+ return mN;
+ }
+};
+
+// -----------------------------------------------------------------------
+
+orientation_filter::orientation_filter() {
+ Phi[0][1] = 0;
+ Phi[1][1] = 1;
+
+ Ba.x = 0;
+ Ba.y = 0;
+ Ba.z = 1;
+
+ Bm.x = 0;
+ Bm.y = 1;
+ Bm.z = 0;
+
+ x0 = 0;
+ x1 = 0;
+
+ init();
+}
- m_quat_aid = sensor_data2quat(m_accel, acc_e);
+void orientation_filter::init(int mode) {
+ mInitState = 0;
+
+ mGyroRate = 0;
+
+ mCount[0] = 0;
+ mCount[1] = 0;
+ mCount[2] = 0;
+
+ mData = 0;
+ mMode = mode;
+
+ if (mMode != FUSION_NOGYRO) { //normal or game rotation
+ mParam.gyroVar = DEFAULT_GYRO_VAR;
+ mParam.gyroBiasVar = DEFAULT_GYRO_BIAS_VAR;
+ mParam.accStdev = DEFAULT_ACC_STDEV;
+ mParam.magStdev = DEFAULT_MAG_STDEV;
+ } else {
+ mParam.gyroVar = GEOMAG_GYRO_VAR;
+ mParam.gyroBiasVar = GEOMAG_GYRO_BIAS_VAR;
+ mParam.accStdev = GEOMAG_ACC_STDEV;
+ mParam.magStdev = GEOMAG_MAG_STDEV;
+ }
}
-template <typename TYPE>
-inline void orientation_filter<TYPE>::compute_covariance()
+void orientation_filter::initFusion(const vec4_t& q, float dT)
{
- TYPE var_gyr_x, var_gyr_y, var_gyr_z;
- TYPE var_roll, var_pitch, var_azimuth;
- quaternion<TYPE> quat_diff, quat_error;
-
- if(!is_initialized(m_quat_driv.m_quat))
- m_quat_driv = m_quat_aid;
-
- quaternion<TYPE> quat_rot_inc(0, m_gyro.m_data.m_vec[0], m_gyro.m_data.m_vec[1],
- m_gyro.m_data.m_vec[2]);
-
- quat_diff = (m_quat_driv * quat_rot_inc) * (TYPE) 0.5;
-
- m_quat_driv = m_quat_driv + (quat_diff * (TYPE) m_gyro_dt * (TYPE) PI);
- m_quat_driv.quat_normalize();
-
- m_quat_output = phase_correction(m_quat_driv, m_quat_aid);
-
- m_orientation = quat2euler(m_quat_output);
-
- quat_error = m_quat_aid * m_quat_driv;
-
- m_euler_error = (quat2euler(quat_error)).m_ang;
-
- m_gyro.m_data = m_gyro.m_data - m_euler_error.m_ang;
-
- m_euler_error.m_ang = m_euler_error.m_ang / (TYPE) PI;
-
- m_gyro_bias = m_euler_error.m_ang * (TYPE) PI;
+ // initial estimate: E{ x(t0) }
+ x0 = q;
+ x1 = 0;
+
+ // process noise covariance matrix: G.Q.Gt, with
+ //
+ // G = | -1 0 | Q = | q00 q10 |
+ // | 0 1 | | q01 q11 |
+ //
+ // q00 = sv^2.dt + 1/3.su^2.dt^3
+ // q10 = q01 = 1/2.su^2.dt^2
+ // q11 = su^2.dt
+ //
+
+ const float dT2 = dT*dT;
+ const float dT3 = dT2*dT;
+
+ // variance of integrated output at 1/dT Hz (random drift)
+ const float q00 = mParam.gyroVar * dT + 0.33333f * mParam.gyroBiasVar * dT3;
+
+ // variance of drift rate ramp
+ const float q11 = mParam.gyroBiasVar * dT;
+ const float q10 = 0.5f * mParam.gyroBiasVar * dT2;
+ const float q01 = q10;
+
+ GQGt[0][0] = q00; // rad^2
+ GQGt[1][0] = -q10;
+ GQGt[0][1] = -q01;
+ GQGt[1][1] = q11; // (rad/s)^2
+
+ // initial covariance: Var{ x(t0) }
+ // TODO: initialize P correctly
+ P = 0;
+}
- insert_end(m_var_gyr_x, m_gyro.m_data.m_vec[0]);
- insert_end(m_var_gyr_y, m_gyro.m_data.m_vec[1]);
- insert_end(m_var_gyr_z, m_gyro.m_data.m_vec[2]);
- insert_end(m_var_roll, m_orientation.m_ang.m_vec[0]);
- insert_end(m_var_pitch, m_orientation.m_ang.m_vec[1]);
- insert_end(m_var_azimuth, m_orientation.m_ang.m_vec[2]);
+bool orientation_filter::hasEstimate() const {
+ return ((mInitState & MAG) || (mMode == FUSION_NOMAG)) &&
+ ((mInitState & GYRO) || (mMode == FUSION_NOGYRO)) &&
+ (mInitState & ACC);
+}
- var_gyr_x = var(m_var_gyr_x);
- var_gyr_y = var(m_var_gyr_y);
- var_gyr_z = var(m_var_gyr_z);
- var_roll = var(m_var_roll);
- var_pitch = var(m_var_pitch);
- var_azimuth = var(m_var_azimuth);
+bool orientation_filter::checkInitComplete(int what, const vec3_t& d, float dT) {
+ if (hasEstimate())
+ return true;
+
+ if (what == ACC) {
+ mData[0] += d * (1/length(d));
+ mCount[0]++;
+ mInitState |= ACC;
+ if (mMode == FUSION_NOGYRO ) {
+ mGyroRate = dT;
+ }
+ } else if (what == MAG) {
+ mData[1] += d * (1/length(d));
+ mCount[1]++;
+ mInitState |= MAG;
+ } else if (what == GYRO) {
+ mGyroRate = dT;
+ mData[2] += d*dT;
+ mCount[2]++;
+ mInitState |= GYRO;
+ }
+
+ if (hasEstimate()) {
+ // Average all the values we collected so far
+ mData[0] *= 1.0f/mCount[0];
+ if (mMode != FUSION_NOMAG) {
+ mData[1] *= 1.0f/mCount[1];
+ }
+ mData[2] *= 1.0f/mCount[2];
+
+ // calculate the MRPs from the data collection, this gives us
+ // a rough estimate of our initial state
+ mat33_t R;
+ vec3_t up(mData[0]);
+ vec3_t east;
+
+ if (mMode != FUSION_NOMAG) {
+ east = normalize(cross_product(mData[1], up));
+ } else {
+ east = getOrthogonal(up);
+ }
+
+ vec3_t north(cross_product(up, east));
+ R << east << north << up;
+ const vec4_t q = matrixToQuat(R);
+
+ initFusion(q, mGyroRate);
+ }
+
+ return false;
+}
- m_driv_cov.m_mat[0][0] = var_gyr_x;
- m_driv_cov.m_mat[1][1] = var_gyr_y;
- m_driv_cov.m_mat[2][2] = var_gyr_z;
- m_driv_cov.m_mat[3][3] = (TYPE) QWB_CONST;
- m_driv_cov.m_mat[4][4] = (TYPE) QWB_CONST;
- m_driv_cov.m_mat[5][5] = (TYPE) QWB_CONST;
+void orientation_filter::handleGyro(const vec3_t& w, float dT) {
+ if (!checkInitComplete(GYRO, w, dT))
+ return;
- m_aid_cov.m_mat[0][0] = var_roll;
- m_aid_cov.m_mat[1][1] = var_pitch;
- m_aid_cov.m_mat[2][2] = var_azimuth;
+ predict(w, dT);
}
-template <typename TYPE>
-inline void orientation_filter<TYPE>::time_update()
-{
- euler_angles<TYPE> orientation;
-
- m_tran_mat.m_mat[0][1] = m_gyro.m_data.m_vec[2];
- m_tran_mat.m_mat[0][2] = -m_gyro.m_data.m_vec[1];
- m_tran_mat.m_mat[1][0] = -m_gyro.m_data.m_vec[2];
- m_tran_mat.m_mat[1][2] = m_gyro.m_data.m_vec[0];
- m_tran_mat.m_mat[2][0] = m_gyro.m_data.m_vec[1];
- m_tran_mat.m_mat[2][1] = -m_gyro.m_data.m_vec[0];
- m_tran_mat.m_mat[3][3] = (TYPE) F_CONST;
- m_tran_mat.m_mat[4][4] = (TYPE) F_CONST;
- m_tran_mat.m_mat[5][5] = (TYPE) F_CONST;
-
- m_measure_mat.m_mat[0][0] = 1;
- m_measure_mat.m_mat[1][1] = 1;
- m_measure_mat.m_mat[2][2] = 1;
-
- if (is_initialized(m_state_old))
- m_state_new = transpose(m_tran_mat * transpose(m_state_old));
-
- m_pred_cov = (m_tran_mat * m_pred_cov * tran(m_tran_mat)) + m_driv_cov;
-
- for (int j = 0; j < M6X6C; ++j) {
- for (int i = 0; i < M6X6R; ++i) {
- if (ABS(m_pred_cov.m_mat[i][j]) < NEGLIGIBLE_VAL)
- m_pred_cov.m_mat[i][j] = NEGLIGIBLE_VAL;
- }
- if (ABS(m_state_new.m_vec[j]) < NEGLIGIBLE_VAL)
- m_state_new.m_vec[j] = NEGLIGIBLE_VAL;
- }
-
- m_quat_9axis = m_quat_output;
- m_quat_gaming_rv = m_quat_9axis;
-
- m_rot_matrix = quat2rot_mat(m_quat_driv);
-
- quaternion<TYPE> quat_eu_er(1, m_euler_error.m_ang.m_vec[0], m_euler_error.m_ang.m_vec[1],
- m_euler_error.m_ang.m_vec[2]);
-
- m_quat_driv = (m_quat_driv * quat_eu_er) * (TYPE) PI;
- m_quat_driv.quat_normalize();
-
- if (is_initialized(m_state_new)) {
- m_state_error.m_vec[0] = m_euler_error.m_ang.m_vec[0];
- m_state_error.m_vec[1] = m_euler_error.m_ang.m_vec[1];
- m_state_error.m_vec[2] = m_euler_error.m_ang.m_vec[2];
- m_state_error.m_vec[3] = m_state_new.m_vec[3];
- m_state_error.m_vec[4] = m_state_new.m_vec[4];
- m_state_error.m_vec[5] = m_state_new.m_vec[5];
- }
+status_t orientation_filter::handleAcc(const vec3_t& a, float dT) {
+ if (!checkInitComplete(ACC, a, dT))
+ return BAD_VALUE;
+
+ // ignore acceleration data if we're close to free-fall
+ const float l = length(a);
+ if (l < FREE_FALL_THRESHOLD) {
+ return BAD_VALUE;
+ }
+
+ const float l_inv = 1.0f/l;
+
+ if ( mMode == FUSION_NOGYRO ) {
+ //geo mag
+ vec3_t w_dummy;
+ w_dummy = x1; //bias
+ predict(w_dummy, dT);
+ }
+
+ if ( mMode == FUSION_NOMAG) {
+ vec3_t m;
+ m = getRotationMatrix()*Bm;
+ update(m, Bm, mParam.magStdev);
+ }
+
+ vec3_t unityA = a * l_inv;
+ const float d = sqrtf(fabsf(l- NOMINAL_GRAVITY));
+ const float p = l_inv * mParam.accStdev*expf(d);
+
+ update(unityA, Ba, p);
+ return NO_ERROR;
}
-template <typename TYPE>
-inline void orientation_filter<TYPE>::time_update_gaming_rv()
-{
- euler_angles<TYPE> orientation;
- euler_angles<TYPE> euler_aid;
- euler_angles<TYPE> euler_driv;
-
- m_tran_mat.m_mat[0][1] = m_gyro.m_data.m_vec[2];
- m_tran_mat.m_mat[0][2] = -m_gyro.m_data.m_vec[1];
- m_tran_mat.m_mat[1][0] = -m_gyro.m_data.m_vec[2];
- m_tran_mat.m_mat[1][2] = m_gyro.m_data.m_vec[0];
- m_tran_mat.m_mat[2][0] = m_gyro.m_data.m_vec[1];
- m_tran_mat.m_mat[2][1] = -m_gyro.m_data.m_vec[0];
- m_tran_mat.m_mat[3][3] = (TYPE) F_CONST;
- m_tran_mat.m_mat[4][4] = (TYPE) F_CONST;
- m_tran_mat.m_mat[5][5] = (TYPE) F_CONST;
-
- m_measure_mat.m_mat[0][0] = 1;
- m_measure_mat.m_mat[1][1] = 1;
- m_measure_mat.m_mat[2][2] = 1;
-
- if (is_initialized(m_state_old))
- m_state_new = transpose(m_tran_mat * transpose(m_state_old));
-
- m_pred_cov = (m_tran_mat * m_pred_cov * tran(m_tran_mat)) + m_driv_cov;
-
- euler_aid = quat2euler(m_quat_aid);
- euler_driv = quat2euler(m_quat_output);
-
- euler_angles<TYPE> euler_gaming_rv(euler_aid.m_ang.m_vec[0], euler_aid.m_ang.m_vec[1],
- euler_driv.m_ang.m_vec[2]);
- m_quat_gaming_rv = euler2quat(euler_gaming_rv);
-
- if (is_initialized(m_state_new)) {
- m_state_error.m_vec[0] = m_euler_error.m_ang.m_vec[0];
- m_state_error.m_vec[1] = m_euler_error.m_ang.m_vec[1];
- m_state_error.m_vec[2] = m_euler_error.m_ang.m_vec[2];
- m_state_error.m_vec[3] = m_state_new.m_vec[3];
- m_state_error.m_vec[4] = m_state_new.m_vec[4];
- m_state_error.m_vec[5] = m_state_new.m_vec[5];
- }
+status_t orientation_filter::handleMag(const vec3_t& m) {
+ if (!checkInitComplete(MAG, m))
+ return BAD_VALUE;
+
+ // the geomagnetic-field should be between 30uT and 60uT
+ // reject if too large to avoid spurious magnetic sources
+ const float magFieldSq = length_squared(m);
+ if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ) {
+ return BAD_VALUE;
+ } else if (magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) {
+ // Also reject if too small since we will get ill-defined (zero mag)
+ // cross-products below
+ return BAD_VALUE;
+ }
+
+ // Orthogonalize the magnetic field to the gravity field, mapping it into
+ // tangent to Earth.
+ const vec3_t up( getRotationMatrix() * Ba );
+ const vec3_t east( cross_product(m, up) );
+
+ // If the m and up vectors align, the cross product magnitude will
+ // approach 0.
+ // Reject this case as well to avoid div by zero problems and
+ // ill-conditioning below.
+ if (length_squared(east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) {
+ return BAD_VALUE;
+ }
+
+ // If we have created an orthogonal magnetic field successfully,
+ // then pass it in as the update.
+ vec3_t north( cross_product(up, east) );
+
+ const float l_inv = 1 / length(north);
+ north *= l_inv;
+
+ update(north, Bm, mParam.magStdev*l_inv);
+ return NO_ERROR;
}
-template <typename TYPE>
-inline void orientation_filter<TYPE>::measurement_update()
-{
- matrix<TYPE, M6X6R, M6X6C> gain;
- matrix<TYPE, M6X6R, M6X6C> iden;
- iden.m_mat[0][0] = iden.m_mat[1][1] = iden.m_mat[2][2] = 1;
- iden.m_mat[3][3] = iden.m_mat[4][4] = iden.m_mat[5][5] = 1;
+void orientation_filter::checkState() {
+ // P needs to stay positive semidefinite or the fusion diverges. When we
+ // detect divergence, we reset the fusion.
+ // TODO(braun): Instead, find the reason for the divergence and fix it.
- for (int j = 0; j < M6X6C; ++j) {
- for (int i = 0; i < M6X6R; ++i) {
- gain.m_mat[i][j] = m_pred_cov.m_mat[j][i] / (m_pred_cov.m_mat[j][j] + m_aid_cov.m_mat[j][j]);
- m_state_new.m_vec[i] = m_state_new.m_vec[i] + gain.m_mat[i][j] * m_state_error.m_vec[j];
- }
+ if (!isPositiveSemidefinite(P[0][0], SYMMETRY_TOLERANCE) ||
+ !isPositiveSemidefinite(P[1][1], SYMMETRY_TOLERANCE)) {
+ P = 0;
+ }
+}
- matrix<TYPE, M6X6R, M6X6C> temp = iden;
+vec4_t orientation_filter::getAttitude() const {
+ return x0;
+}
- for (int i = 0; i < M6X6R; ++i)
- temp.m_mat[i][j] = iden.m_mat[i][j] - (gain.m_mat[i][j] * m_measure_mat.m_mat[j][i]);
+vec3_t orientation_filter::getBias() const {
+ return x1;
+}
- m_pred_cov = temp * m_pred_cov;
- }
+mat33_t orientation_filter::getRotationMatrix() const {
+ return quatToMatrix(x0);
+}
- for (int j = 0; j < M6X6C; ++j) {
- for (int i = 0; i < M6X6R; ++i) {
- if (ABS(m_pred_cov.m_mat[i][j]) < NEGLIGIBLE_VAL)
- m_pred_cov.m_mat[i][j] = NEGLIGIBLE_VAL;
- }
- }
+mat34_t orientation_filter::getF(const vec4_t& q) {
+ mat34_t F;
- m_state_old = m_state_new;
+ // This is used to compute the derivative of q
+ // F = | [q.xyz]x |
+ // | -q.xyz |
- TYPE arr_bias[V1x3S] = {m_state_new.m_vec[3], m_state_new.m_vec[4], m_state_new.m_vec[5]};
- vect<TYPE, V1x3S> vec(arr_bias);
+ F[0].x = q.w; F[1].x =-q.z; F[2].x = q.y;
+ F[0].y = q.z; F[1].y = q.w; F[2].y =-q.x;
+ F[0].z =-q.y; F[1].z = q.x; F[2].z = q.w;
+ F[0].w =-q.x; F[1].w =-q.y; F[2].w =-q.z;
+ return F;
+}
- m_bias_correction = vec;
+void orientation_filter::predict(const vec3_t& w, float dT) {
+ const vec4_t q = x0;
+ const vec3_t b = x1;
+ vec3_t we = w - b;
+
+ if (length(we) < WVEC_EPS) {
+ we = (we[0]>0.f)?WVEC_EPS:-WVEC_EPS;
+ }
+ // q(k+1) = O(we)*q(k)
+ // --------------------
+ //
+ // O(w) = | cos(0.5*||w||*dT)*I33 - [psi]x psi |
+ // | -psi' cos(0.5*||w||*dT) |
+ //
+ // psi = sin(0.5*||w||*dT)*w / ||w||
+ //
+ //
+ // P(k+1) = Phi(k)*P(k)*Phi(k)' + G*Q(k)*G'
+ // ----------------------------------------
+ //
+ // G = | -I33 0 |
+ // | 0 I33 |
+ //
+ // Phi = | Phi00 Phi10 |
+ // | 0 1 |
+ //
+ // Phi00 = I33
+ // - [w]x * sin(||w||*dt)/||w||
+ // + [w]x^2 * (1-cos(||w||*dT))/||w||^2
+ //
+ // Phi10 = [w]x * (1 - cos(||w||*dt))/||w||^2
+ // - [w]x^2 * (||w||*dT - sin(||w||*dt))/||w||^3
+ // - I33*dT
+
+ const mat33_t I33(1);
+ const mat33_t I33dT(dT);
+ const mat33_t wx(crossMatrix(we, 0));
+ const mat33_t wx2(wx*wx);
+ const float lwedT = length(we)*dT;
+ const float hlwedT = 0.5f*lwedT;
+ const float ilwe = 1.f/length(we);
+ const float k0 = (1-cosf(lwedT))*(ilwe*ilwe);
+ const float k1 = sinf(lwedT);
+ const float k2 = cosf(hlwedT);
+ const vec3_t psi(sinf(hlwedT)*ilwe*we);
+ const mat33_t O33(crossMatrix(-psi, k2));
+ mat44_t O;
+ O[0].xyz = O33[0]; O[0].w = -psi.x;
+ O[1].xyz = O33[1]; O[1].w = -psi.y;
+ O[2].xyz = O33[2]; O[2].w = -psi.z;
+ O[3].xyz = psi; O[3].w = k2;
+
+ Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0;
+ Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);
+
+ x0 = O*q;
+
+ if (x0.w < 0)
+ x0 = -x0;
+
+ P = Phi*P*transpose(Phi) + GQGt;
+
+ checkState();
+}
- m_gyro_bias = m_gyro_bias + vec;
+void orientation_filter::update(const vec3_t& z, const vec3_t& Bi, float sigma) {
+ vec4_t q(x0);
+ // measured vector in body space: h(p) = A(p)*Bi
+ const mat33_t A(quatToMatrix(q));
+ const vec3_t Bb(A*Bi);
+
+ // Sensitivity matrix H = dh(p)/dp
+ // H = [ L 0 ]
+ const mat33_t L(crossMatrix(Bb, 0));
+
+ // gain...
+ // K = P*Ht / [H*P*Ht + R]
+ vec<mat33_t, 2> K;
+ const mat33_t R(sigma*sigma);
+ const mat33_t S(scaleCovariance(L, P[0][0]) + R);
+ const mat33_t Si(invert(S));
+ const mat33_t LtSi(transpose(L)*Si);
+ K[0] = P[0][0] * LtSi;
+ K[1] = transpose(P[1][0])*LtSi;
+
+ // update...
+ // P = (I-K*H) * P
+ // P -= K*H*P
+ // | K0 | * | L 0 | * P = | K0*L 0 | * | P00 P10 | = | K0*L*P00 K0*L*P10 |
+ // | K1 | | K1*L 0 | | P01 P11 | | K1*L*P00 K1*L*P10 |
+ // Note: the Joseph form is numerically more stable and given by:
+ // P = (I-KH) * P * (I-KH)' + K*R*R'
+ const mat33_t K0L(K[0] * L);
+ const mat33_t K1L(K[1] * L);
+ P[0][0] -= K0L*P[0][0];
+ P[1][1] -= K1L*P[1][0];
+ P[1][0] -= K0L*P[1][0];
+ P[0][1] = transpose(P[1][0]);
+
+ const vec3_t e(z - Bb);
+ const vec3_t dq(K[0]*e);
+
+ q += getF(q)*(0.5f*dq);
+ x0 = normalize_quat(q);
+
+ if (mMode != FUSION_NOMAG) {
+ const vec3_t db(K[1]*e);
+ x1 += db;
+ }
+
+ checkState();
}
-template <typename TYPE>
-void orientation_filter<TYPE>::get_device_orientation(const sensor_data<TYPE> *accel,
- const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic)
-{
- initialize_sensor_data(accel, gyro, magnetic);
-
- if (gyro != NULL && magnetic != NULL) {
- orientation_triad_algorithm();
- compute_covariance();
- time_update();
- measurement_update();
- m_quaternion = m_quat_9axis;
- } else if (!gyro && !magnetic) {
- compute_accel_orientation();
- m_quaternion = m_quat_aid;
- } else if (!gyro) {
- orientation_triad_algorithm();
- m_quaternion = m_quat_aid;
- } else if (!magnetic) {
- compute_accel_orientation();
- compute_covariance();
- time_update_gaming_rv();
- measurement_update();
- m_quaternion = m_quat_gaming_rv;
- }
+vec3_t orientation_filter::getOrthogonal(const vec3_t &v) {
+ vec3_t w;
+ if (fabsf(v[0])<= fabsf(v[1]) && fabsf(v[0]) <= fabsf(v[2])) {
+ w[0]=0.f;
+ w[1] = v[2];
+ w[2] = -v[1];
+ } else if (fabsf(v[1]) <= fabsf(v[2])) {
+ w[0] = v[2];
+ w[1] = 0.f;
+ w[2] = -v[0];
+ }else {
+ w[0] = v[1];
+ w[1] = -v[0];
+ w[2] = 0.f;
+ }
+ return normalize(w);
}
-#endif //_ORIENTATION_FILTER_H_
+
+// -----------------------------------------------------------------------
+
/*
- * sensord
- *
- * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ * 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
+ * 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 _ORIENTATION_FILTER_H_
-#define _ORIENTATION_FILTER_H_
+// released in android-11.0.0_r9
+
+#ifndef ANDROID_FUSION_H
+#define ANDROID_FUSION_H
+
+#include "quat.h"
+#include "mat.h"
+#include "vec.h"
+#include "errors.h"
-#include "matrix.h"
-#include "vector.h"
-#include "sensor_data.h"
-#include "quaternion.h"
-#include "euler_angles.h"
-#include "rotation_matrix.h"
+namespace android {
-#define MOVING_AVERAGE_WINDOW_LENGTH 20
+typedef mat<float, 3, 4> mat34_t;
-#define M3X3R 3
-#define M3X3C 3
-#define M6X6R 6
-#define M6X6C 6
-#define V1x3S 3
-#define V1x4S 4
-#define V1x6S 6
-#define ACCEL_SCALE 1
-#define GYRO_SCALE 1146
-#define GEOMAGNETIC_SCALE 1
+enum FUSION_MODE{
+ FUSION_9AXIS, // use accel gyro mag
+ FUSION_NOMAG, // use accel gyro (game rotation, gravity)
+ FUSION_NOGYRO, // use accel mag (geomag rotation)
+ NUM_FUSION_MODE
+};
-template <typename TYPE>
class orientation_filter {
+ /*
+ * the state vector is made of two sub-vector containing respectively:
+ * - modified Rodrigues parameters
+ * - the estimated gyro bias
+ */
+ quat_t x0;
+ vec3_t x1;
+
+ /*
+ * the predicated covariance matrix is made of 4 3x3 sub-matrices and it is
+ * semi-definite positive.
+ *
+ * P = | P00 P10 | = | P00 P10 |
+ * | P01 P11 | | P10t P11 |
+ *
+ * Since P01 = transpose(P10), the code below never calculates or
+ * stores P01.
+ */
+ mat<mat33_t, 2, 2> P;
+
+ /*
+ * the process noise covariance matrix
+ */
+ mat<mat33_t, 2, 2> GQGt;
+
public:
- sensor_data<TYPE> m_accel;
- sensor_data<TYPE> m_gyro;
- sensor_data<TYPE> m_magnetic;
- vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_gyr_x;
- vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_gyr_y;
- vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_gyr_z;
- vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_roll;
- vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_pitch;
- vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_azimuth;
- matrix<TYPE, M6X6R, M6X6C> m_driv_cov;
- matrix<TYPE, M6X6R, M6X6C> m_aid_cov;
- matrix<TYPE, M6X6R, M6X6C> m_tran_mat;
- matrix<TYPE, M6X6R, M6X6C> m_measure_mat;
- matrix<TYPE, M6X6R, M6X6C> m_pred_cov;
- vect<TYPE, V1x6S> m_state_new;
- vect<TYPE, V1x6S> m_state_old;
- vect<TYPE, V1x6S> m_state_error;
- vect<TYPE, V1x3S> m_bias_correction;
- vect<TYPE, V1x3S> m_gyro_bias;
- quaternion<TYPE> m_quat_aid;
- quaternion<TYPE> m_quat_driv;
- rotation_matrix<TYPE> m_rot_matrix;
- euler_angles<TYPE> m_orientation;
- quaternion<TYPE> m_quat_9axis;
- quaternion<TYPE> m_quat_gaming_rv;
- quaternion<TYPE> m_quaternion;
- quaternion<TYPE> m_quat_output;
- euler_angles<TYPE> m_euler_error;
- TYPE m_gyro_dt;
+ orientation_filter();
+ void init(int mode = FUSION_9AXIS);
+ void handleGyro(const vec3_t& w, float dT);
+ status_t handleAcc(const vec3_t& a, float dT);
+ status_t handleMag(const vec3_t& m);
+ vec4_t getAttitude() const;
+ vec3_t getBias() const;
+ mat33_t getRotationMatrix() const;
+ bool hasEstimate() const;
- orientation_filter(void);
- ~orientation_filter(void);
+private:
+ struct Parameter {
+ float gyroVar;
+ float gyroBiasVar;
+ float accStdev;
+ float magStdev;
+ } mParam;
- inline void initialize_sensor_data(const sensor_data<TYPE> *accel,
- const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic);
- inline void orientation_triad_algorithm(void);
- inline void compute_accel_orientation(void);
- inline void compute_covariance(void);
- inline void time_update(void);
- inline void time_update_gaming_rv(void);
- inline void measurement_update(void);
+ mat<mat33_t, 2, 2> Phi;
+ vec3_t Ba, Bm;
+ uint32_t mInitState;
+ float mGyroRate;
+ vec<vec3_t, 3> mData;
+ size_t mCount[3];
+ int mMode;
- void get_device_orientation(const sensor_data<TYPE> *accel,
- const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic);
+ enum { ACC=0x1, MAG=0x2, GYRO=0x4 };
+ bool checkInitComplete(int, const vec3_t& w, float d = 0);
+ void initFusion(const vec4_t& q0, float dT);
+ void checkState();
+ void predict(const vec3_t& w, float dT);
+ void update(const vec3_t& z, const vec3_t& Bi, float sigma);
+ static mat34_t getF(const vec4_t& p);
+ static vec3_t getOrthogonal(const vec3_t &v);
};
-#include "orientation_filter.cpp"
+}
-#endif /* _ORIENTATION_FILTER_H_ */
+#endif // ANDROID_FUSION_H
--- /dev/null
+/*
+ * 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.
+ */
+
+// released in android-11.0.0_r9
+
+#ifndef ANDROID_QUAT_H
+#define ANDROID_QUAT_H
+
+#include <math.h>
+
+#include "vec.h"
+#include "mat.h"
+
+// -----------------------------------------------------------------------
+namespace android {
+// -----------------------------------------------------------------------
+
+template <typename TYPE>
+mat<TYPE, 3, 3> quatToMatrix(const vec<TYPE, 4>& q) {
+ mat<TYPE, 3, 3> R;
+ TYPE q0(q.w);
+ TYPE q1(q.x);
+ TYPE q2(q.y);
+ TYPE q3(q.z);
+ TYPE sq_q1 = 2 * q1 * q1;
+ TYPE sq_q2 = 2 * q2 * q2;
+ TYPE sq_q3 = 2 * q3 * q3;
+ TYPE q1_q2 = 2 * q1 * q2;
+ TYPE q3_q0 = 2 * q3 * q0;
+ TYPE q1_q3 = 2 * q1 * q3;
+ TYPE q2_q0 = 2 * q2 * q0;
+ TYPE q2_q3 = 2 * q2 * q3;
+ TYPE q1_q0 = 2 * q1 * q0;
+ R[0][0] = 1 - sq_q2 - sq_q3;
+ R[0][1] = q1_q2 - q3_q0;
+ R[0][2] = q1_q3 + q2_q0;
+ R[1][0] = q1_q2 + q3_q0;
+ R[1][1] = 1 - sq_q1 - sq_q3;
+ R[1][2] = q2_q3 - q1_q0;
+ R[2][0] = q1_q3 - q2_q0;
+ R[2][1] = q2_q3 + q1_q0;
+ R[2][2] = 1 - sq_q1 - sq_q2;
+ return R;
+}
+
+template <typename TYPE>
+vec<TYPE, 4> matrixToQuat(const mat<TYPE, 3, 3>& R) {
+ // matrix to quaternion
+
+ struct {
+ inline TYPE operator()(TYPE v) {
+ return v < 0 ? 0 : v;
+ }
+ } clamp;
+
+ vec<TYPE, 4> q;
+ const float Hx = R[0].x;
+ const float My = R[1].y;
+ const float Az = R[2].z;
+ q.x = sqrtf( clamp( Hx - My - Az + 1) * 0.25f );
+ q.y = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f );
+ q.z = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f );
+ q.w = sqrtf( clamp( Hx + My + Az + 1) * 0.25f );
+ q.x = copysignf(q.x, R[2].y - R[1].z);
+ q.y = copysignf(q.y, R[0].z - R[2].x);
+ q.z = copysignf(q.z, R[1].x - R[0].y);
+ // guaranteed to be unit-quaternion
+ return q;
+}
+
+template <typename TYPE>
+vec<TYPE, 4> normalize_quat(const vec<TYPE, 4>& q) {
+ vec<TYPE, 4> r(q);
+ if (r.w < 0) {
+ r = -r;
+ }
+ return normalize(r);
+}
+
+// -----------------------------------------------------------------------
+
+typedef vec4_t quat_t;
+
+// -----------------------------------------------------------------------
+}; // namespace android
+
+#endif /* ANDROID_QUAT_H */
+++ /dev/null
-/*
- * sensord
- *
- * Copyright (c) 2014 Samsung Electronics Co., Ltd.
- *
- * 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.
- *
- */
-
-#if defined (_QUATERNION_H_) && defined (_VECTOR_H_)
-
-#include <math.h>
-
-template <typename T> int sgn(T val) {
- if (val >= 0)
- return 1;
- else
- return -1;
-}
-
-template <typename T> T mag(T val) {
- if (val < 0)
- return val * static_cast<T>(-1);
- else
- return val;
-}
-
-template <typename TYPE>
-quaternion<TYPE>::quaternion() : m_quat()
-{
-}
-
-template <typename TYPE>
-quaternion<TYPE>::quaternion(const TYPE w, const TYPE x, const TYPE y, const TYPE z)
-{
- TYPE vec_data[QUAT_SIZE] = {w, x, y, z};
-
- vect<TYPE, QUAT_SIZE> v(vec_data);
- m_quat = v;
-}
-
-template <typename TYPE>
-quaternion<TYPE>::quaternion(const vect<TYPE, QUAT_SIZE> v)
-{
- m_quat = v;
-}
-
-template <typename TYPE>
-quaternion<TYPE>::quaternion(const quaternion<TYPE>& q)
-{
- m_quat = q.m_quat;
-}
-
-template <typename TYPE>
-quaternion<TYPE>::~quaternion()
-{
-}
-
-template <typename TYPE>
-quaternion<TYPE> quaternion<TYPE>::operator =(const quaternion<TYPE>& q)
-{
- m_quat = q.m_quat;
-
- return *this;
-}
-
-template <typename TYPE>
-void quaternion<TYPE>::quat_normalize()
-{
- TYPE w, x, y, z;
- TYPE val;
-
- w = m_quat.m_vec[0] * m_quat.m_vec[0];
- x = m_quat.m_vec[1] * m_quat.m_vec[1];
- y = m_quat.m_vec[2] * m_quat.m_vec[2];
- z = m_quat.m_vec[3] * m_quat.m_vec[3];
-
- val = sqrt(w + x + y + z);
-
- m_quat = m_quat / val;
-}
-
-template <typename T>
-quaternion<T> operator *(const quaternion<T> q, const T val)
-{
- return (q.m_quat * val);
-}
-
-template <typename T>
-quaternion<T> operator *(const quaternion<T> q1, const quaternion<T> q2)
-{
- T w, x, y, z;
- T w1, x1, y1, z1;
- T w2, x2, y2, z2;
-
- w1 = q1.m_quat.m_vec[0];
- x1 = q1.m_quat.m_vec[1];
- y1 = q1.m_quat.m_vec[2];
- z1 = q1.m_quat.m_vec[3];
-
- w2 = q2.m_quat.m_vec[0];
- x2 = q2.m_quat.m_vec[1];
- y2 = q2.m_quat.m_vec[2];
- z2 = q2.m_quat.m_vec[3];
-
- x = x1*w2 + y1*z2 - z1*y2 + w1*x2;
- y = -x1*z2 + y1*w2 + z1*x2 + w1*y2;
- z = x1*y2 - y1*x2 + z1*w2 + w1*z2;
- w = -x1*x2 - y1*y2 - z1*z2 + w1*w2;
-
- quaternion<T> q(w, x, y, z);
-
- return q;
-}
-
-template <typename T>
-quaternion<T> operator +(const quaternion<T> q1, const quaternion<T> q2)
-{
- return (q1.m_quat + q2.m_quat);
-}
-
-
-template<typename T>
-quaternion<T> phase_correction(const quaternion<T> q1, const quaternion<T> q2)
-{
- T w, x, y, z;
- w = mag(q1.m_quat.m_vec[0]) * sgn(q2.m_quat.m_vec[0]);
- x = mag(q1.m_quat.m_vec[1]) * sgn(q2.m_quat.m_vec[1]);
- y = mag(q1.m_quat.m_vec[2]) * sgn(q2.m_quat.m_vec[2]);
- z = mag(q1.m_quat.m_vec[3]) * sgn(q2.m_quat.m_vec[3]);
-
- quaternion<T> q(w, x, y, z);
-
- return q;
-}
-
-template<typename T>
-quaternion<T> axis2quat(const vect<T, REF_VEC_SIZE> axis, const T angle)
-{
- T w;
- vect<T, REF_VEC_SIZE> imag;
-
- w = cos(angle/2);
- imag = axis * (T)(-sin(angle/2));
-
- quaternion<T> q(w, imag.m_vec[0], imag.m_vec[1], imag.m_vec[2]);
-
- return q;
-}
-#endif //_QUATERNION_H_
+++ /dev/null
-/*
- * sensord
- *
- * Copyright (c) 2014 Samsung Electronics Co., Ltd.
- *
- * 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 _QUATERNION_H_
-#define _QUATERNION_H_
-
-#include "vector.h"
-
-#define QUAT_SIZE 4
-#define REF_VEC_SIZE 3
-
-template <typename TYPE>
-class quaternion {
-public:
- vect<TYPE, QUAT_SIZE> m_quat;
-
- quaternion();
- quaternion(const TYPE w, const TYPE x, const TYPE y, const TYPE z);
- quaternion(const vect<TYPE, QUAT_SIZE> v);
- quaternion(const quaternion<TYPE>& q);
- ~quaternion();
-
- quaternion<TYPE> operator =(const quaternion<TYPE>& q);
- void quat_normalize(void);
-
- template<typename T> friend quaternion<T> operator *(const quaternion<T> q,
- const T val);
- template<typename T> friend quaternion<T> operator *(const quaternion<T> q1,
- const quaternion<T> q2);
- template<typename T> friend quaternion<T> operator +(const quaternion<T> q1,
- const quaternion<T> q2);
- template<typename T> friend quaternion<T> phase_correction(const quaternion<T> q1,
- const quaternion<T> q2);
- template<typename T> friend quaternion<T> axis2quat(const vect<T, REF_VEC_SIZE> axis,
- const T angle);
-};
-
-#include "quaternion.cpp"
-
-#endif //_QUATERNION_H_
+++ /dev/null
-/*
- * sensord
- *
- * Copyright (c) 2014 Samsung Electronics Co., Ltd.
- *
- * 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.
- *
- */
-
-#if defined (_ROTATION_MATRIX_H_) && defined (_MATRIX_H_)
-
-#define QUAT_LEN 4
-
-template <typename T> T get_sign(T val)
-{
- return (val >= (T) 0) ? (T) 1 : (T) -1;
-}
-
-template <typename TYPE>
-rotation_matrix<TYPE>::rotation_matrix() : m_rot_mat()
-{
-}
-
-template <typename TYPE>
-rotation_matrix<TYPE>::rotation_matrix(const matrix<TYPE, ROT_MAT_ROWS, ROT_MAT_COLS> m)
-{
- m_rot_mat = m;
-}
-
-template <typename TYPE>
-rotation_matrix<TYPE>::rotation_matrix(TYPE mat_data[ROT_MAT_ROWS][ROT_MAT_COLS])
-{
- matrix<TYPE, ROT_MAT_ROWS, ROT_MAT_COLS> m(mat_data);
- m_rot_mat = m;
-}
-
-template <typename TYPE>
-rotation_matrix<TYPE>::rotation_matrix(const rotation_matrix<TYPE>& rm)
-{
- m_rot_mat = rm.m_rot_mat;
-}
-
-template <typename TYPE>
-rotation_matrix<TYPE>::~rotation_matrix()
-{
-}
-
-template <typename TYPE>
-rotation_matrix<TYPE> rotation_matrix<TYPE>::operator =(const rotation_matrix<TYPE>& rm)
-{
- m_rot_mat = rm.m_rot_mat;
-
- return *this;
-}
-
-template <typename T>
-rotation_matrix<T> quat2rot_mat(quaternion<T> q)
-{
- T w, x, y, z;
- T R[ROT_MAT_ROWS][ROT_MAT_COLS];
-
- w = q.m_quat.m_vec[0];
- x = q.m_quat.m_vec[1];
- y = q.m_quat.m_vec[2];
- z = q.m_quat.m_vec[3];
-
- R[0][0] = (2 * w * w) - 1 + (2 * x * x);
- R[0][1] = 2 * ((x * y) + (w * z));
- R[0][2] = 2 * ((x * z) - (w * y));
- R[1][0] = 2 * ((x * y) - (w * z));
- R[1][1] = (2 * w * w) - 1 + (2 * y * y);
- R[1][2] = 2 * ((y * z) + (w * x));
- R[2][0] = 2 * ((x * z) + (w * y));
- R[2][1] = 2 * ((y * z) - (w * x));
- R[2][2] = (2 * w * w) - 1 + (2 * z * z);
-
- rotation_matrix<T> rm(R);
-
- return rm;
-}
-
-template <typename T>
-quaternion<T> rot_mat2quat(rotation_matrix<T> rm)
-{
- T q0, q1, q2, q3;
- T phi, theta, psi;
-
- phi = atan2(rm.m_rot_mat.m_mat[2][1], rm.m_rot_mat.m_mat[2][2]);
- theta = atan2(-rm.m_rot_mat.m_mat[2][0],
- sqrt((rm.m_rot_mat.m_mat[2][1] * rm.m_rot_mat.m_mat[2][1]) +
- (rm.m_rot_mat.m_mat[2][2] * rm.m_rot_mat.m_mat[2][2])));
- psi = atan2(rm.m_rot_mat.m_mat[1][0], rm.m_rot_mat.m_mat[0][0]);
-
- q0 = (cos(phi/2) * cos(theta/2) * cos(psi/2)) +
- (sin(phi/2) * sin(theta/2) * sin(psi/2));
- q1 = (-cos(phi/2) * sin(theta/2) * sin(psi/2)) +
- (sin(phi/2) * cos(theta/2) * cos(psi/2));
- q2 = (cos(phi/2) * sin(theta/2) * cos(psi/2)) +
- (sin(phi/2) * cos(theta/2) * sin(psi/2));
- q3 = (cos(phi/2) * cos(theta/2) * sin(psi/2)) -
- (sin(phi/2) * sin(theta/2) * cos(psi/2));
-
- quaternion<T> q(q0, q1, q2, q3);
-
- q.quat_normalize();
-
- return q;
-}
-
-#endif /* _ROTATION_MATRIX_H_ */
+++ /dev/null
-/*
- * sensord
- *
- * Copyright (c) 2014 Samsung Electronics Co., Ltd.
- *
- * 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 _ROTATION_MATRIX_H_
-#define _ROTATION_MATRIX_H_
-
-#include "matrix.h"
-#include "quaternion.h"
-
-#define ROT_MAT_ROWS 3
-#define ROT_MAT_COLS 3
-
-template <typename TYPE>
-class rotation_matrix {
-public:
- matrix<TYPE, ROT_MAT_ROWS, ROT_MAT_COLS> m_rot_mat;
-
- rotation_matrix();
- rotation_matrix(const matrix<TYPE, ROT_MAT_ROWS, ROT_MAT_COLS> m);
- rotation_matrix(TYPE mat_data[ROT_MAT_ROWS][ROT_MAT_COLS]);
- rotation_matrix(const rotation_matrix<TYPE>& rm);
- ~rotation_matrix();
-
- rotation_matrix<TYPE> operator =(const rotation_matrix<TYPE>& rm);
-
- template<typename T> friend rotation_matrix<T> quat2rot_mat(quaternion<T> q);
- template<typename T> friend quaternion<T> rot_mat2quat(rotation_matrix<T> rm);
-};
-
-#include "rotation_matrix.cpp"
-
-#endif /* _ROTATION_MATRIX_H_ */
+++ /dev/null
-/*
- * sensord
- *
- * Copyright (c) 2014 Samsung Electronics Co., Ltd.
- *
- * 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.
- *
- */
-
-#if defined (_SENSOR_DATA_H_) && defined (_VECTOR_H_)
-
-#include "math.h"
-
-template <typename TYPE>
-sensor_data<TYPE>::sensor_data() : m_data(), m_time_stamp(0)
-{
-}
-
-template <typename TYPE>
-sensor_data<TYPE>::sensor_data(const TYPE x, const TYPE y,
- const TYPE z, const unsigned long long time_stamp)
-{
- TYPE vec_data[SENSOR_DATA_SIZE] = {x, y, z};
-
- vect<TYPE, SENSOR_DATA_SIZE> v(vec_data);
- m_data = v;
- m_time_stamp = time_stamp;
-}
-
-template <typename TYPE>
-sensor_data<TYPE>::sensor_data(const vect<TYPE, SENSOR_DATA_SIZE> v,
- const unsigned long long time_stamp)
-{
- m_data = v;
- m_time_stamp = time_stamp;
-}
-
-template <typename TYPE>
-sensor_data<TYPE>::sensor_data(const sensor_data<TYPE>& s)
-{
- m_data = s.m_data;
- m_time_stamp = s.m_time_stamp;
-}
-
-template <typename TYPE>
-sensor_data<TYPE>::~sensor_data()
-{
-}
-
-template <typename TYPE>
-sensor_data<TYPE> sensor_data<TYPE>::operator =(const sensor_data<TYPE>& s)
-{
- m_data = s.m_data;
- m_time_stamp = s.m_time_stamp;
-
- return *this;
-}
-
-template <typename T>
-sensor_data<T> operator +(sensor_data<T> data1, sensor_data<T> data2)
-{
- sensor_data<T> result(data1.m_data + data2.m_data, 0);
- return result;
-}
-
-template <typename T>
-void normalize(sensor_data<T>& data)
-{
- T x, y, z;
-
- x = data.m_data.m_vec[0];
- y = data.m_data.m_vec[1];
- z = data.m_data.m_vec[2];
-
- T val = sqrt(x*x + y*y + z*z);
-
- data.m_data.m_vec[0] = x / val;
- data.m_data.m_vec[1] = y / val;
- data.m_data.m_vec[2] = z / val;
-}
-
-template <typename T>
-sensor_data<T> scale_data(sensor_data<T> data, T scaling_factor)
-{
- T x, y, z;
-
- x = data.m_data.m_vec[0] / scaling_factor;
- y = data.m_data.m_vec[1] / scaling_factor;
- z = data.m_data.m_vec[2] / scaling_factor;
-
- sensor_data<T> s(x, y, z, data.m_time_stamp);
-
- return s;
-}
-
-
-template<typename T>
-quaternion<T> sensor_data2quat(const sensor_data<T> data, const vect<T, REF_VEC_SIZE> ref_vec)
-{
- vect<T, REF_VEC_SIZE> axis;
- T angle;
-
- axis = cross(data.m_data, ref_vec);
- angle = acos(dot(data.m_data, ref_vec));
-
- return axis2quat(axis, angle);
-}
-
-template<typename T>
-void pre_process_data(sensor_data<T> &data_out, const T *data_in, int sign, int scale)
-{
- data_out.m_data.m_vec[0] = sign * data_in[0] / scale;
- data_out.m_data.m_vec[1] = sign * data_in[1] / scale;
- data_out.m_data.m_vec[2] = sign * data_in[2] / scale;
-}
-
-#endif /* _SENSOR_DATA_H_ */
+++ /dev/null
-/*
- * sensord
- *
- * Copyright (c) 2014 Samsung Electronics Co., Ltd.
- *
- * 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 _SENSOR_DATA_H_
-#define _SENSOR_DATA_H_
-
-#include "vector.h"
-#include "quaternion.h"
-
-#define SENSOR_DATA_SIZE 3
-
-template <typename TYPE>
-class sensor_data {
-public:
- vect<TYPE, SENSOR_DATA_SIZE> m_data;
- unsigned long long m_time_stamp { 0 };
-
- sensor_data();
- sensor_data(const TYPE x, const TYPE y, const TYPE z,
- const unsigned long long time_stamp);
- sensor_data(const vect<TYPE, SENSOR_DATA_SIZE> v,
- const unsigned long long time_stamp);
- sensor_data(const sensor_data<TYPE>& s);
- ~sensor_data();
-
- sensor_data<TYPE> operator =(const sensor_data<TYPE>& s);
-
- template<typename T> friend sensor_data<T> operator +(sensor_data<T> data1,
- sensor_data<T> data2);
-
- template<typename T> friend void normalize(sensor_data<T>& data);
- template<typename T> friend sensor_data<T> scale_data(sensor_data<T> data,
- T scaling_factor);
- template<typename T> friend quaternion<T> sensor_data2quat(const sensor_data<T> data,
- const vect<T, REF_VEC_SIZE> ref_vec);
- template<typename T> friend void pre_process_data(sensor_data<T> &data_out,
- const T *data_in, int sign, int scale);
-};
-
-#include "sensor_data.cpp"
-
-#endif /* _SENSOR_DATA_H_ */
--- /dev/null
+/*
+ * 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.
+ */
+
+// released in android-11.0.0_r9
+
+#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 */
--- /dev/null
+/*
+ * 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.
+ */
+
+// released in android-11.0.0_r9
+
+#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) { } // NOLINT(google-explicit-constructor)
+
+ // -----------------------------------------------------------------------
+ // conversion constructors
+
+ vec(pTYPE rhs) { // NOLINT(google-explicit-constructor)
+ 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 */
+++ /dev/null
-/*
- * sensord
- *
- * Copyright (c) 2014 Samsung Electronics Co., Ltd.
- *
- * 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.
- *
- */
-
-#if defined (_VECTOR_H_) && defined (_MATRIX_H_)
-
-TYPE_SIZE vect<TYPE, SIZE>::vect(void)
-{
- for (int i = 0; i < SIZE; i++)
- m_vec[i] = 0;
-}
-
-TYPE_SIZE vect<TYPE, SIZE>::vect(TYPE vec_data[SIZE])
-{
- for (int j = 0; j < SIZE; j++)
- m_vec[j] = vec_data[j];
-}
-
-TYPE_SIZE vect<TYPE, SIZE>::vect(const vect<TYPE, SIZE>& v)
-{
- for (int q = 0; q < SIZE; q++)
- m_vec[q] = v.m_vec[q];
-}
-
-
-TYPE_SIZE vect<TYPE, SIZE>::~vect()
-{
-}
-
-TYPE_SIZE vect<TYPE, SIZE> vect<TYPE, SIZE>::operator =(const vect<TYPE, SIZE>& v)
-{
- if (this == &v)
- return *this;
-
- for (int q = 0; q < SIZE; q++)
- m_vec[q] = v.m_vec[q];
-
- return *this;
-}
-
-T_S ostream& operator <<(ostream& dout, vect<T, S>& v)
-{
- for (int j = 0; j < S; j++)
- dout << v.m_vec[j] << "\t";
-
- dout << endl;
-
- return dout;
-}
-
-T_S vect<T, S> operator +(const vect<T, S> v1, const vect<T, S> v2)
-{
- vect<T, S> v3;
-
- for (int j = 0; j < S; j++)
- v3.m_vec[j] = v1.m_vec[j] + v2.m_vec[j];
-
- return v3;
-}
-
-T_S vect<T, S> operator +(const vect<T, S> v, const T val)
-{
- vect<T, S> v1;
-
- for (int j = 0; j < S; j++)
- v1.m_vec[j] = v.m_vec[j] + val;
-
- return v1;
-}
-
-T_S vect<T, S> operator -(const vect<T, S> v1, const vect<T, S> v2)
-{
- vect<T, S> v3;
-
- for (int j = 0; j < S; j++)
- v3.m_vec[j] = v1.m_vec[j] - v2.m_vec[j];
-
- return v3;
-}
-
-T_S vect<T, S> operator -(const vect<T, S> v, const T val)
-{
- vect<T, S> v1;
-
- for (int j = 0; j < S; j++)
- v1.m_vec[j] = v.m_vec[j] - val;
-
- return v1;
-}
-
-T_S_R_C matrix<T, R, S> operator *(const matrix<T, R, C> m, const vect<T, S> v)
-{
- assert(R == S);
- assert(C == 1);
-
- matrix<T, R, S> m1;
-
- for (int i = 0; i < R; i++)
- for (int j = 0; j < S; j++)
- m1.m_mat[i][j] = m.m_mat[i][0] * v.m_vec[j];
-
- return m1;
-}
-
-T_S_R_C vect<T, S> operator *(const vect<T, S> v, const matrix<T, R, C> m)
-{
- assert(R == S);
- assert(C != 1);
- vect<T, C> v1;
-
- for (int j = 0; j < C; j++)
- {
- v1.m_vec[j] = 0;
- for (int k = 0; k < R; k++)
- v1.m_vec[j] += v.m_vec[k] * m.m_mat[k][j];
- }
-
- return v1;
-}
-
-T_S vect<T, S> operator *(const vect<T, S> v, const T val)
-{
- vect<T, S> v1;
-
- for (int j = 0; j < S; j++)
- v1.m_vec[j] = v.m_vec[j] * val;
-
- return v1;
-}
-
-T_S vect<T, S> operator /(const vect<T, S> v, const T val)
-{
- vect<T, S> v1;
-
- for (int j = 0; j < S; j++)
- v1.m_vec[j] = v.m_vec[j] / val;
-
- return v1;
-}
-
-T_S1_S2 bool operator ==(const vect<T, S1> v1, const vect<T, S2> v2)
-{
- if (S1 == S2) {
- for (int i = 0; i < S1; i++)
- if (v1.m_vec[i] != v2.m_vec[i])
- return false;
- return true;
- }
-
- return false;
-}
-
-T_S1_S2 bool operator !=(const vect<T, S1> v1, const vect<T, S2> v2)
-{
- return (!(v1 == v2));
-}
-
-T_S matrix<T, S, 1> transpose(const vect<T, S> v)
-{
- matrix<T, S, 1> m;
-
- for (int i = 0; i < S; i++)
- m.m_mat[i][0] = v.m_vec[i];
-
- return m;
-}
-
-T_S vect<T, S> transpose(const matrix<T, S, 1> m)
-{
- vect<T, S> v;
-
- for (int i = 0; i < S; i++)
- v.m_vec[i] = m.m_mat[i][0];
-
- return v;
-}
-
-T_S void insert_end(vect<T, S>& v, T val)
-{
- for (int i = 0; i < (S - 1); i++)
- v.m_vec[i] = v.m_vec[i+1];
-
- v.m_vec[S-1] = val;
-}
-
-T_S vect<T, S> cross(const vect<T, S> v1, const vect<T, S> v2)
-{
- vect<T, S> v3;
-
- v3.m_vec[0] = ((v1.m_vec[1] * v2.m_vec[2]) - (v1.m_vec[2] * v2.m_vec[1]));
- v3.m_vec[1] = ((v1.m_vec[2] * v2.m_vec[0]) - (v1.m_vec[0] * v2.m_vec[2]));
- v3.m_vec[2] = ((v1.m_vec[0] * v2.m_vec[1]) - (v1.m_vec[1] * v2.m_vec[0]));
-
- return v3;
-}
-
-T_S T dot(const vect<T, S> v1, const vect<T, S> v2)
-{
- return (v1.m_vec[0] * v2.m_vec[0] + v1.m_vec[1] * v2.m_vec[1] + v1.m_vec[2] * v2.m_vec[2]);
-}
-
-T_S bool is_initialized(const vect<T, S> v)
-{
- vect<T, S> v1;
- bool retval;
-
- retval = (v == v1) ? false : true;
-
- return retval;
-}
-
-T_S T var(const vect<T, S> v)
-{
- T val = 0;
- T mean, var, diff;
-
- for (int i = 0; i < S; i++)
- val += v.m_vec[i];
-
- mean = val / S;
-
- val = 0;
- for (int i = 0; i < S; i++)
- {
- diff = (v.m_vec[i] - mean);
- val += diff * diff;
- }
-
- var = val / (S - 1);
-
- return var;
-}
-#endif
-
+++ /dev/null
-/*
- * sensord
- *
- * Copyright (c) 2014 Samsung Electronics Co., Ltd.
- *
- * 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 _VECTOR_H_
-#define _VECTOR_H_
-
-#include "matrix.h"
-
-#define TYPE_SIZE template<typename TYPE, int SIZE>
-#define T_S template<typename T, int S>
-#define T_S_R_C template<typename T, int S, int R, int C>
-#define T_S1_S2 template<typename T, int S1, int S2>
-
-TYPE_SIZE class vect {
-public:
- TYPE m_vec[SIZE];
- vect(void);
- vect(TYPE vec_data[SIZE]);
- vect(const vect<TYPE, SIZE>& v);
- ~vect();
-
- vect<TYPE, SIZE> operator =(const vect<TYPE, SIZE>& v);
-
- T_S friend ostream& operator << (ostream& dout, vect<T, S>& v);
- T_S friend vect<T, S> operator +(const vect<T, S> v1, const vect<T, S> v2);
- T_S friend vect<T, S> operator +(const vect<T, S> v, const T val);
- T_S friend vect<T, S> operator -(const vect<T, S> v1, const vect<T, S> v2);
- T_S friend vect<T, S> operator -(const vect<T, S> v, const T val);
- T_S_R_C friend matrix<T, R, S> operator *(const matrix<T, R, C> v1, const vect<T, S> v2);
- T_S_R_C friend vect<T, S> operator *(const vect<T, S> v, const matrix<T, R, C> m);
- T_S friend vect<T, S> operator *(const vect<T, S> v, const T val);
- T_S friend vect<T, S> operator /(const vect<T, S> v1, const T val);
- T_S1_S2 friend bool operator ==(const vect<T, S1> v1, const vect<T, S2> v2);
- T_S1_S2 friend bool operator !=(const vect<T, S1> v1, const vect<T, S2> v2);
-
- T_S friend void insert_end(vect<T, S>& v, T val);
- T_S friend matrix<T, S, 1> transpose(const vect<T, S> v);
- T_S friend vect<T, S> transpose(const matrix<T, S, 1> m);
- T_S friend vect<T, S> cross(const vect<T, S> v1, const vect<T, S> v2);
- T_S friend T dot(const vect<T, S> v1, const vect<T, S> v2);
- T_S friend T var(const vect<T, S> v);
- T_S friend bool is_initialized(const vect<T, S> v);
-};
-
-#include "vector.cpp"
-
-#endif /* _VECTOR_H_ */
-
gyro_fusion::gyro_fusion()
{
+ m_orientation_filter.init(1);
}
gyro_fusion::~gyro_fusion()
{
-}
-
-bool gyro_fusion::get_orientation(void)
-{
- //_I("[fusion_sensor] : enable values are %d %d %d", m_enable_accel, m_enable_gyro, m_enable_magnetic);
- if (!m_enable_accel || !m_enable_gyro)
- return false;
-
- m_orientation_filter.get_device_orientation(&m_accel, &m_gyro, NULL);
- m_timestamp = fmax(m_accel.m_time_stamp, m_gyro.m_time_stamp);
- return true;
-}
+}
\ No newline at end of file
public:
gyro_fusion();
~gyro_fusion();
-private:
- bool get_orientation();
};
#endif /* __GYRO_FUSION_H__ */
gyro_magnetic_fusion::gyro_magnetic_fusion()
{
+ m_orientation_filter.init(0);
}
gyro_magnetic_fusion::~gyro_magnetic_fusion()
{
-}
-
-bool gyro_magnetic_fusion::get_orientation(void)
-{
- //_I("[fusion_sensor] : enable values are %d %d %d", m_enable_accel, m_enable_gyro, m_enable_magnetic);
- if (!m_enable_accel || !m_enable_gyro || !m_enable_magnetic)
- return false;
-
- m_orientation_filter.get_device_orientation(&m_accel, &m_gyro, &m_magnetic);
- m_timestamp = fmax(m_accel.m_time_stamp, m_gyro.m_time_stamp);
- m_timestamp = fmax(m_timestamp, m_magnetic.m_time_stamp);
- return true;
-}
+}
\ No newline at end of file
public:
gyro_magnetic_fusion();
~gyro_magnetic_fusion();
-private:
- bool get_orientation();
};
#endif /* __GYRO_MAGNETIC_FUSION_H__ */
#include <sensor_log.h>
#include <sensor_types.h>
-#include <fusion_util.h>
#define NAME_SENSOR "http://tizen.org/sensor/general/gyroscope_rotation_vector/tizen_default"
#define NAME_VENDOR "tizen.org"
sensor_data->accuracy = m_accuracy;
sensor_data->timestamp = m_time;
sensor_data->value_count = 4;
- sensor_data->values[0] = m_x;
- sensor_data->values[1] = m_y;
- sensor_data->values[2] = m_z;
- sensor_data->values[3] = m_w;
+ sensor_data->values[0] = m_w;
+ sensor_data->values[1] = m_x;
+ sensor_data->values[2] = m_y;
+ sensor_data->values[3] = m_z;
*data = sensor_data;
*length = sizeof(sensor_data_t);
magnetic_fusion::magnetic_fusion()
{
+ m_orientation_filter.init(2);
}
magnetic_fusion::~magnetic_fusion()
{
}
-
-bool magnetic_fusion::get_orientation(void)
-{
- //_I("[fusion_sensor] : enable values are %d %d %d", m_enable_accel, m_enable_magnetic, m_enable_magnetic);
- if (!m_enable_accel || !m_enable_magnetic)
- return false;
-
- m_orientation_filter.get_device_orientation(&m_accel, NULL, &m_magnetic);
- m_timestamp = fmax(m_accel.m_time_stamp, m_magnetic.m_time_stamp);
- return true;
-}
public:
magnetic_fusion();
~magnetic_fusion();
-private:
- bool get_orientation();
};
#endif /* __MAGNETIC_FUSION_H__ */
#include <sensor_log.h>
#include <sensor_types.h>
-#include <fusion_util.h>
#define NAME_SENSOR "http://tizen.org/sensor/general/geomagnetic_rotation_vector/tizen_default"
#define NAME_VENDOR "tizen.org"
#include <sensor_log.h>
#include <sensor_types.h>
-#include <fusion_util.h>
#define NAME_SENSOR "http://tizen.org/sensor/general/rotation_vector/tizen_default"
#define NAME_VENDOR "tizen.org"
projects / platform / core / system / sensord.git / commitdiff