From 51937d62180a35c024ff3a145320bf38ff860fbe Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 19 Jun 2014 13:39:00 +0530 Subject: [PATCH 01/16] Adding quaternion class implementation - Adding quaternion template class - Adding implementation for the quaternion template class - Adding quaternion class testing code signed-off-by: Ramasamy Change-Id: Ia200184bf5ac5ada93e829d422a32bae8314fd63 --- src/sensor_fusion/standalone/util/quaternion.cpp | 135 +++++++++++++++++++++ src/sensor_fusion/standalone/util/quaternion.h | 51 ++++++++ .../standalone/util/test/quaternion_test/.cproject | 112 +++++++++++++++++ .../standalone/util/test/quaternion_test/.project | 89 ++++++++++++++ .../util/test/quaternion_test/quaternion_main.cpp | 72 +++++++++++ 5 files changed, 459 insertions(+) create mode 100755 src/sensor_fusion/standalone/util/quaternion.cpp create mode 100755 src/sensor_fusion/standalone/util/quaternion.h create mode 100644 src/sensor_fusion/standalone/util/test/quaternion_test/.cproject create mode 100644 src/sensor_fusion/standalone/util/test/quaternion_test/.project create mode 100644 src/sensor_fusion/standalone/util/test/quaternion_test/quaternion_main.cpp diff --git a/src/sensor_fusion/standalone/util/quaternion.cpp b/src/sensor_fusion/standalone/util/quaternion.cpp new file mode 100755 index 0000000..d226828 --- /dev/null +++ b/src/sensor_fusion/standalone/util/quaternion.cpp @@ -0,0 +1,135 @@ +/* + * 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 + +#define QUAT_SIZE 4 + +template +quaternion::quaternion() : m_quat(QUAT_SIZE) +{ +} + +template +quaternion::quaternion(const TYPE w, const TYPE x, const TYPE y, const TYPE z) +{ + TYPE vec_data[QUAT_SIZE] = {w, x, y, z}; + + vector v(QUAT_SIZE, vec_data); + m_quat = v; +} + +template +quaternion::quaternion(const vector v) +{ + m_quat = v; +} + +template +quaternion::quaternion(const quaternion& q) +{ + m_quat = q.m_quat; +} + +template +quaternion::~quaternion() +{ +} + +template +quaternion quaternion::operator =(const quaternion& q) +{ + m_quat = q.m_quat; +} + +template +quaternion operator *(const quaternion q, const T val) +{ + return (q.m_quat * val); +} + +template +quaternion operator *(const quaternion q1, const quaternion 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 q(w, x, y, z); + + return q; +} + +template +quaternion operator +(const quaternion q1, const quaternion q2) +{ + return (q1.m_quat + q2.m_quat); +} + +template +quaternion quat_normalize(const quaternion q) +{ + T w, x, y, z; + T val; + + w = q.m_quat.m_vec[0] * q.m_quat.m_vec[0]; + x = q.m_quat.m_vec[1] * q.m_quat.m_vec[1]; + y = q.m_quat.m_vec[2] * q.m_quat.m_vec[2]; + z = q.m_quat.m_vec[3] * q.m_quat.m_vec[3]; + + val = sqrt(w + x + y + z); + + quaternion q1(q.m_quat / val); + + return (q1); +} + +template +quaternion quat_conj(const quaternion q) +{ + T w, x, y, z; + + 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]; + + quaternion q1(w, -x, -y, -z); + + return q1; +} + +#endif //_QUATERNION_H diff --git a/src/sensor_fusion/standalone/util/quaternion.h b/src/sensor_fusion/standalone/util/quaternion.h new file mode 100755 index 0000000..0313797 --- /dev/null +++ b/src/sensor_fusion/standalone/util/quaternion.h @@ -0,0 +1,51 @@ +/* + * 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" + +template +class quaternion { +public: + vector m_quat; + + quaternion(); + quaternion(const TYPE w, const TYPE x, const TYPE y, const TYPE z); + quaternion(const vector v); + quaternion(const quaternion& q); + ~quaternion(); + + quaternion operator =(const quaternion& q); + + template friend quaternion operator *(const quaternion q, + const T val); + template friend quaternion operator *(const quaternion q1, + const quaternion q2); + template friend quaternion operator +(const quaternion q1, + const quaternion q2); + + template friend quaternion quat_normalize(const quaternion q); + template friend quaternion quat_conj(const quaternion q); +}; + +#include "quaternion.cpp" + +#endif //_QUATERNION_H diff --git a/src/sensor_fusion/standalone/util/test/quaternion_test/.cproject b/src/sensor_fusion/standalone/util/test/quaternion_test/.cproject new file mode 100644 index 0000000..de46268 --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/quaternion_test/.cproject @@ -0,0 +1,112 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/sensor_fusion/standalone/util/test/quaternion_test/.project b/src/sensor_fusion/standalone/util/test/quaternion_test/.project new file mode 100644 index 0000000..74b100d --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/quaternion_test/.project @@ -0,0 +1,89 @@ + + + quaternion_test + + + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + ?name? + + + + org.eclipse.cdt.make.core.append_environment + true + + + org.eclipse.cdt.make.core.autoBuildTarget + all + + + org.eclipse.cdt.make.core.buildArguments + + + + org.eclipse.cdt.make.core.buildCommand + make + + + org.eclipse.cdt.make.core.buildLocation + ${workspace_loc:/quaternion_test/Debug} + + + org.eclipse.cdt.make.core.cleanBuildTarget + clean + + + org.eclipse.cdt.make.core.contents + org.eclipse.cdt.make.core.activeConfigSettings + + + org.eclipse.cdt.make.core.enableAutoBuild + false + + + org.eclipse.cdt.make.core.enableCleanBuild + true + + + org.eclipse.cdt.make.core.enableFullBuild + true + + + org.eclipse.cdt.make.core.fullBuildTarget + all + + + org.eclipse.cdt.make.core.stopOnError + true + + + org.eclipse.cdt.make.core.useDefaultBuildCmd + true + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + org.tizen.nativecpp.apichecker.core.builder + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + org.tizen.nativecpp.apichecker.core.tizenCppNature + + diff --git a/src/sensor_fusion/standalone/util/test/quaternion_test/quaternion_main.cpp b/src/sensor_fusion/standalone/util/test/quaternion_test/quaternion_main.cpp new file mode 100644 index 0000000..ca43b8d --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/quaternion_test/quaternion_main.cpp @@ -0,0 +1,72 @@ +/* + * 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. + * + */ + +#include "../../quaternion.h" + +int main() +{ + float arr0[4] = {2344.98, 345.24, 456.12, 98.33}; + float arr1[4] = {0.056, 0.34, -0.0076, 0.001}; + + vector v0(4, arr0); + vector v1(4, arr1); + + quaternion q0(v0); + quaternion q1(v1); + quaternion q2((float)2344.98, (float)345.24, (float)456.12, (float)98.33); + quaternion q3(q1); + quaternion q4; + + cout << "Constructor tests\n"; + cout << "input\t" << v0 << "\n"; + cout << "output\t" << q0.m_quat << "\n\n"; + cout << "input\t" << v1 << "\n"; + cout << "output\t" << q1.m_quat << "\n\n"; + cout << "input\t" << v0 << "\n"; + cout << "output\t" << q2.m_quat << "\n\n"; + cout << "input\t" << v1 << "\n"; + cout << "output\t" << q3.m_quat << "\n\n"; + cout << "default constructor\n"; + cout << "output\t" << q4.m_quat << "\n\n"; + + cout << "Multiplication\n"; + float val = 0.1; + quaternion q5 = q0 * val; + cout << "input\t" << q0.m_quat << "\n" << 0.1 << "\n"; + cout << "output\t" << q5.m_quat << "\n\n"; + quaternion q6 = q0 * q1; + cout << "input\t" << q0.m_quat << "\n" << q1.m_quat << "\n"; + cout << "output\t" << q6.m_quat << "\n\n"; + + cout << "Addition\n"; + quaternion q7 = q0 + q1; + cout << "input\t" << q0.m_quat << "\n" << q1.m_quat << "\n"; + cout << "output\t" << q7.m_quat << "\n\n"; + + cout << "Quaternion Normalization\n"; + quaternion q8 = quat_normalize(q1); + cout << "input\t" << q1.m_quat << "\n"; + cout << "output\t" << q8.m_quat << "\n\n"; + + cout << "Quaternion Conjugate\n"; + quaternion q9 = quat_conj(q1); + cout << "input\t" << q1.m_quat << "\n"; + cout << "output\t" << q9.m_quat << "\n\n"; +} + -- 2.7.4 From 896bab1d5dac7404cc0a3072f2cc50d9696e7818 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 19 Jun 2014 15:31:58 +0530 Subject: [PATCH 02/16] Adding euler_angles class implementation - Adding euler_angles template class - Adding implementation for the euler_angles template class - Adding euler_angles class testing code signed-off-by: Ramasamy Change-Id: Idd8498ce0decf89d0ae53cf2efa424013d692c86 --- src/sensor_fusion/standalone/util/euler_angles.cpp | 104 +++++++++++++++++++ src/sensor_fusion/standalone/util/euler_angles.h | 46 +++++++++ .../util/test/euler_angles_test/.cproject | 112 +++++++++++++++++++++ .../util/test/euler_angles_test/.project | 89 ++++++++++++++++ .../test/euler_angles_test/euler_angles_main.cpp | 73 ++++++++++++++ 5 files changed, 424 insertions(+) create mode 100644 src/sensor_fusion/standalone/util/euler_angles.cpp create mode 100644 src/sensor_fusion/standalone/util/euler_angles.h create mode 100644 src/sensor_fusion/standalone/util/test/euler_angles_test/.cproject create mode 100644 src/sensor_fusion/standalone/util/test/euler_angles_test/.project create mode 100644 src/sensor_fusion/standalone/util/test/euler_angles_test/euler_angles_main.cpp diff --git a/src/sensor_fusion/standalone/util/euler_angles.cpp b/src/sensor_fusion/standalone/util/euler_angles.cpp new file mode 100644 index 0000000..00f9439 --- /dev/null +++ b/src/sensor_fusion/standalone/util/euler_angles.cpp @@ -0,0 +1,104 @@ +/* + * 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 + +#define EULER_SIZE 3 +#define RAD2DEG 57.2957795 +#define DEG2RAD 0.0174532925 + +template +euler_angles::euler_angles() : m_ang(EULER_SIZE) +{ +} + +template +euler_angles::euler_angles(const TYPE roll, const TYPE pitch, const TYPE yaw) +{ + TYPE euler_data[EULER_SIZE] = {roll, pitch, yaw}; + + vector v(EULER_SIZE, euler_data); + m_ang = v; +} + +template +euler_angles::euler_angles(const vector v) +{ + m_ang = v; +} + +template +euler_angles::euler_angles(const euler_angles& e) +{ + m_ang = e.m_ang; +} + +template +euler_angles::~euler_angles() +{ + +} + +template +euler_angles euler_angles::operator =(const euler_angles& e) +{ + m_ang = e.m_ang; +} + +template +euler_angles quat2euler(const quaternion 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 = -atan(R31 / sqrt(1 - (R31 * R31))); + psi = atan2(R21, R11); + + euler_angles e(phi, theta, psi); + + return e; +} + +template +euler_angles rad2deg(const euler_angles e) +{ + return (e.m_ang * (T) RAD2DEG); +} + +template +euler_angles deg2rad(const euler_angles e) +{ + return (e.m_ang * (T) DEG2RAD); +} + +#endif //_EULER_ANGLES_H diff --git a/src/sensor_fusion/standalone/util/euler_angles.h b/src/sensor_fusion/standalone/util/euler_angles.h new file mode 100644 index 0000000..bafe8df --- /dev/null +++ b/src/sensor_fusion/standalone/util/euler_angles.h @@ -0,0 +1,46 @@ +/* + * 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" + +template +class euler_angles { +public: + vector m_ang; + + euler_angles(); + euler_angles(const TYPE roll, const TYPE pitch, const TYPE yaw); + euler_angles(const vector v); + euler_angles(const euler_angles& e); + ~euler_angles(); + + euler_angles operator =(const euler_angles& e); + + template friend euler_angles quat2euler(const quaternion q); + template friend euler_angles rad2deg(const euler_angles e); + template friend euler_angles deg2rad(const euler_angles e); +}; + +#include "euler_angles.cpp" + +#endif //_EULER_ANGLES_H diff --git a/src/sensor_fusion/standalone/util/test/euler_angles_test/.cproject b/src/sensor_fusion/standalone/util/test/euler_angles_test/.cproject new file mode 100644 index 0000000..7f29995 --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/euler_angles_test/.cproject @@ -0,0 +1,112 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/sensor_fusion/standalone/util/test/euler_angles_test/.project b/src/sensor_fusion/standalone/util/test/euler_angles_test/.project new file mode 100644 index 0000000..eca2f5e --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/euler_angles_test/.project @@ -0,0 +1,89 @@ + + + euler_angles_test + + + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + ?name? + + + + org.eclipse.cdt.make.core.append_environment + true + + + org.eclipse.cdt.make.core.autoBuildTarget + all + + + org.eclipse.cdt.make.core.buildArguments + + + + org.eclipse.cdt.make.core.buildCommand + make + + + org.eclipse.cdt.make.core.buildLocation + ${workspace_loc:/euler_angles_test/Debug} + + + org.eclipse.cdt.make.core.cleanBuildTarget + clean + + + org.eclipse.cdt.make.core.contents + org.eclipse.cdt.make.core.activeConfigSettings + + + org.eclipse.cdt.make.core.enableAutoBuild + false + + + org.eclipse.cdt.make.core.enableCleanBuild + true + + + org.eclipse.cdt.make.core.enableFullBuild + true + + + org.eclipse.cdt.make.core.fullBuildTarget + all + + + org.eclipse.cdt.make.core.stopOnError + true + + + org.eclipse.cdt.make.core.useDefaultBuildCmd + true + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + org.tizen.nativecpp.apichecker.core.builder + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + org.tizen.nativecpp.apichecker.core.tizenCppNature + + diff --git a/src/sensor_fusion/standalone/util/test/euler_angles_test/euler_angles_main.cpp b/src/sensor_fusion/standalone/util/test/euler_angles_test/euler_angles_main.cpp new file mode 100644 index 0000000..592e782 --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/euler_angles_test/euler_angles_main.cpp @@ -0,0 +1,73 @@ +/* + * 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. + * + */ + +#include "../../euler_angles.h" + +int main() +{ + float arr0[3] = {234.98, 345.24, -56.12}; + float arr1[3] = {56, -34, 76}; + float arr2[4] = {0.6, 0.6, -.18, -.44}; + float arr3[4] = {-0.5, -0.36, .43, .03}; + + vector v0(3, arr0); + vector v1(3, arr1); + vector v2(4, arr2); + vector v3(4, arr3); + + quaternion q1(v2); + quaternion q2(v3); + + euler_angles e0(v0); + euler_angles e1(v1); + euler_angles e2((float)234.98, (float)345.24, (float)-56.12); + euler_angles e3(e1); + euler_angles e4; + + cout << "Constructor tests\n"; + cout << "input\t" << v0 << "\n"; + cout << "output\t" << e0.m_ang << "\n\n"; + cout << "input\t" << v1 << "\n"; + cout << "output\t" << e1.m_ang << "\n\n"; + cout << "input\t" << v0 << "\n"; + cout << "output\t" << e2.m_ang << "\n\n"; + cout << "input\t" << v1 << "\n"; + cout << "output\t" << e3.m_ang << "\n\n"; + cout << "default constructor\n"; + cout << "output\t" << e4.m_ang << "\n\n"; + + cout << "Quaternion to Euler\n"; + euler_angles e5 = quat2euler(q1); + cout << "input\t" << q1.m_quat << "\n"; + cout << "output\t" << e5.m_ang << "\n\n"; + euler_angles e8 = quat2euler(q2); + cout << "input\t" << q2.m_quat << "\n"; + cout << "output\t" << e8.m_ang << "\n\n"; + + cout << "Radians to Degrees\n"; + euler_angles e6 = deg2rad(e0); + cout << "input\t" << e0.m_ang << "\n"; + cout << "output\t" << e6.m_ang << "\n\n"; + + cout << "Degrees to Radians\n"; + euler_angles e7 = rad2deg(e6); + cout << "input\t" << e6.m_ang << "\n"; + cout << "output\t" << e7.m_ang << "\n\n"; +} + -- 2.7.4 From 7a7f06c1ebfbf1d692a0e0b29c9a18957cc41ddb Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Mon, 23 Jun 2014 11:15:55 +0530 Subject: [PATCH 03/16] Adding sensor_data class implementation - Adding sensor_data template class - Adding implementation for the sensor_data template class - Adding sensor_data class testing code - Testing application contributed by Gurleen Kaur signed-off-by: Ramasamy Change-Id: I96ff03a716b562d0c006bbe95b054936f5b90775 --- src/sensor_fusion/standalone/util/sensor_data.cpp | 104 +++++++++++++++++++ src/sensor_fusion/standalone/util/sensor_data.h | 48 +++++++++ .../util/test/sensor_data_test/.cproject | 112 +++++++++++++++++++++ .../standalone/util/test/sensor_data_test/.project | 89 ++++++++++++++++ .../test/sensor_data_test/sensor_data_main.cpp | 61 +++++++++++ 5 files changed, 414 insertions(+) create mode 100644 src/sensor_fusion/standalone/util/sensor_data.cpp create mode 100644 src/sensor_fusion/standalone/util/sensor_data.h create mode 100644 src/sensor_fusion/standalone/util/test/sensor_data_test/.cproject create mode 100644 src/sensor_fusion/standalone/util/test/sensor_data_test/.project create mode 100644 src/sensor_fusion/standalone/util/test/sensor_data_test/sensor_data_main.cpp diff --git a/src/sensor_fusion/standalone/util/sensor_data.cpp b/src/sensor_fusion/standalone/util/sensor_data.cpp new file mode 100644 index 0000000..ed00cee --- /dev/null +++ b/src/sensor_fusion/standalone/util/sensor_data.cpp @@ -0,0 +1,104 @@ +/* + * 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" + +#define SENSOR_DATA_SIZE 3 + +template +sensor_data::sensor_data() : m_data(SENSOR_DATA_SIZE) +{ +} + +template +sensor_data::sensor_data(const TYPE x, const TYPE y, const TYPE z) +{ + TYPE vec_data[SENSOR_DATA_SIZE] = {x, y, z}; + + vector v(SENSOR_DATA_SIZE, vec_data); + m_data = v; +} + +template +sensor_data::sensor_data(const vector v) +{ + m_data = v; +} + +template +sensor_data::sensor_data(const sensor_data& s) +{ + m_data = s.m_data; +} + +template +sensor_data::~sensor_data() +{ +} + +template +sensor_data sensor_data::operator =(const sensor_data& s) +{ + m_data = s.m_data; +} + +template +sensor_data operator +(sensor_data data1, sensor_data data2) +{ + return (data1.m_data + data2.m_data); +} + +template +sensor_data normalize(sensor_data 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); + + x /= val; + y /= val; + z /= val; + + sensor_data s(x, y, z); + + return s; +} + +template +sensor_data scale_data(sensor_data 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 s(x, y, z); + + return s; +} + +#endif /* _SENSOR_DATA_H */ + diff --git a/src/sensor_fusion/standalone/util/sensor_data.h b/src/sensor_fusion/standalone/util/sensor_data.h new file mode 100644 index 0000000..18a5ee1 --- /dev/null +++ b/src/sensor_fusion/standalone/util/sensor_data.h @@ -0,0 +1,48 @@ +/* + * 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" + +template +class sensor_data { +public: + vector m_data; + + sensor_data(); + sensor_data(const TYPE x, const TYPE y, const TYPE z); + sensor_data(const vector v); + sensor_data(const sensor_data& s); + ~sensor_data(); + + sensor_data operator =(const sensor_data& s); + + template friend sensor_data operator +(sensor_data data1, + sensor_data data2); + + template friend sensor_data normalize(sensor_data data); + template friend sensor_data scale_data(sensor_data data, + T scaling_factor); +}; + +#include "sensor_data.cpp" + +#endif /* _SENSOR_DATA_H */ diff --git a/src/sensor_fusion/standalone/util/test/sensor_data_test/.cproject b/src/sensor_fusion/standalone/util/test/sensor_data_test/.cproject new file mode 100644 index 0000000..031195b --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/sensor_data_test/.cproject @@ -0,0 +1,112 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/sensor_fusion/standalone/util/test/sensor_data_test/.project b/src/sensor_fusion/standalone/util/test/sensor_data_test/.project new file mode 100644 index 0000000..2e0296a --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/sensor_data_test/.project @@ -0,0 +1,89 @@ + + + sensor_data_test + + + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + ?name? + + + + org.eclipse.cdt.make.core.append_environment + true + + + org.eclipse.cdt.make.core.autoBuildTarget + all + + + org.eclipse.cdt.make.core.buildArguments + + + + org.eclipse.cdt.make.core.buildCommand + make + + + org.eclipse.cdt.make.core.buildLocation + ${workspace_loc:/sensor_data_test/Debug} + + + org.eclipse.cdt.make.core.cleanBuildTarget + clean + + + org.eclipse.cdt.make.core.contents + org.eclipse.cdt.make.core.activeConfigSettings + + + org.eclipse.cdt.make.core.enableAutoBuild + false + + + org.eclipse.cdt.make.core.enableCleanBuild + true + + + org.eclipse.cdt.make.core.enableFullBuild + true + + + org.eclipse.cdt.make.core.fullBuildTarget + all + + + org.eclipse.cdt.make.core.stopOnError + true + + + org.eclipse.cdt.make.core.useDefaultBuildCmd + true + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + org.tizen.nativecpp.apichecker.core.builder + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + org.tizen.nativecpp.apichecker.core.tizenCppNature + + diff --git a/src/sensor_fusion/standalone/util/test/sensor_data_test/sensor_data_main.cpp b/src/sensor_fusion/standalone/util/test/sensor_data_test/sensor_data_main.cpp new file mode 100644 index 0000000..8040ad6 --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/sensor_data_test/sensor_data_main.cpp @@ -0,0 +1,61 @@ +/* + * 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. + * + */ + +#include "../../sensor_data.h" + +int main() +{ + float arr1[3] = {1.04, -4.678, -2.34}; + + vector v1(3, arr1); + + sensor_data sd1(2.0, 3.0, 4.0); + sensor_data sd2(1.04, -4.678, -2.34); + sensor_data sd3(0.054, 1.097, 4.456); + sensor_data sd10(v1); + + cout << "Constructor tests\n"; + cout << "input\t" << v1 << "\n"; + cout << "output\t" << sd10.m_data << "\n\n"; + cout << "input\t" << v1 << "\n"; + cout << "output\t" << sd2.m_data<< "\n\n"; + + cout<< "Addition:\n"; + sensor_data sd4 = sd1 + sd2; + cout<< "\n" << sd1.m_data << "\n" << sd2.m_data; + cout<< "\nSum:\n" << sd4.m_data << endl; + sensor_data sd9 = sd1 + sd10; + cout<< "\n" << sd1.m_data << "\n" << sd10.m_data; + cout<< "\nSum:\n" << sd9.m_data << endl; + + cout<< "\n\n\nNormalization:\n"; + sensor_data sd6 = normalize(sd3); + cout<< "\n" << sd3.m_data; + cout<< "\nResult:\n" << sd6.m_data << endl; + sensor_data sd7 = normalize(sd2); + cout<< "\n" << sd2.m_data; + cout<< "\nResult:\n" << sd7.m_data << endl; + + float xx = 2.5; + cout<<"\n\n\nScale data:\n"; + sensor_data sd8 = scale_data(sd2, xx); + cout<< "\n" << sd2.m_data << "\n" << xx; + cout<< "\nResult:\n" << sd8.m_data << endl; +} + -- 2.7.4 From eb6ead93469da6a0c2de7fcbe9a508420b62b195 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Tue, 24 Jun 2014 09:03:01 +0530 Subject: [PATCH 04/16] Fix related return values of assignment operators - Added return value for assignment operators that was missing - Cleanup of code signed-off-by: Ramasamy Change-Id: I584a56146c371ec2d398899f3a96f3101153e756 --- src/sensor_fusion/standalone/util/euler_angles.cpp | 2 ++ src/sensor_fusion/standalone/util/matrix.cpp | 40 ++++++++++++++++------ src/sensor_fusion/standalone/util/quaternion.cpp | 2 ++ src/sensor_fusion/standalone/util/sensor_data.cpp | 2 ++ .../util/test/matrix_test/matrix_main.cpp | 10 ++++-- .../util/test/vector_test/vector_main.cpp | 5 +++ src/sensor_fusion/standalone/util/vector.cpp | 23 ++++++++++--- 7 files changed, 67 insertions(+), 17 deletions(-) diff --git a/src/sensor_fusion/standalone/util/euler_angles.cpp b/src/sensor_fusion/standalone/util/euler_angles.cpp index 00f9439..4241788 100644 --- a/src/sensor_fusion/standalone/util/euler_angles.cpp +++ b/src/sensor_fusion/standalone/util/euler_angles.cpp @@ -60,6 +60,8 @@ template euler_angles euler_angles::operator =(const euler_angles& e) { m_ang = e.m_ang; + + return *this; } template diff --git a/src/sensor_fusion/standalone/util/matrix.cpp b/src/sensor_fusion/standalone/util/matrix.cpp index b6841ac..4c30d84 100755 --- a/src/sensor_fusion/standalone/util/matrix.cpp +++ b/src/sensor_fusion/standalone/util/matrix.cpp @@ -72,9 +72,12 @@ matrix::matrix(const int rows, const int cols, TYPE *mat_data) template matrix::~matrix() { - for (int i = 0; i < m_rows; i++) - delete[] m_mat[i]; - delete[] m_mat; + if (m_mat != NULL) + { + for (int i = 0; i < m_rows; i++) + delete[] m_mat[i]; + delete[] m_mat; + } } template @@ -85,16 +88,31 @@ matrix matrix::operator =(const matrix& m) return *this; } - for (int i = 0; i < m_rows; i++) - delete[] m_mat[i]; - delete[] m_mat; + if (m_mat == NULL) + { + m_rows = m.m_rows; + m_cols = m.m_cols; + m_mat = new TYPE *[m_rows]; - m_rows = m.m_rows; - m_cols = m.m_cols; - m_mat = new TYPE *[m_rows]; + for (int i = 0; i < m_rows; i++) + m_mat[i] = new TYPE [m_cols]; + } + else + { + if ((m_rows != m.m_rows) || (m_cols != m.m_cols)) + { + for (int i = 0; i < m_rows; i++) + delete[] m_mat[i]; + delete[] m_mat; - for (int i = 0; i < m_rows; i++) - m_mat[i] = new TYPE [m_cols]; + m_rows = m.m_rows; + m_cols = m.m_cols; + m_mat = new TYPE *[m_rows]; + + for (int i = 0; i < m_rows; i++) + m_mat[i] = new TYPE [m_cols]; + } + } for (int p = 0; p < m_rows; p++) for (int q = 0; q < m_cols; q++) diff --git a/src/sensor_fusion/standalone/util/quaternion.cpp b/src/sensor_fusion/standalone/util/quaternion.cpp index d226828..37e8ac6 100755 --- a/src/sensor_fusion/standalone/util/quaternion.cpp +++ b/src/sensor_fusion/standalone/util/quaternion.cpp @@ -58,6 +58,8 @@ template quaternion quaternion::operator =(const quaternion& q) { m_quat = q.m_quat; + + return *this; } template diff --git a/src/sensor_fusion/standalone/util/sensor_data.cpp b/src/sensor_fusion/standalone/util/sensor_data.cpp index ed00cee..4276631 100644 --- a/src/sensor_fusion/standalone/util/sensor_data.cpp +++ b/src/sensor_fusion/standalone/util/sensor_data.cpp @@ -58,6 +58,8 @@ template sensor_data sensor_data::operator =(const sensor_data& s) { m_data = s.m_data; + + return *this; } template diff --git a/src/sensor_fusion/standalone/util/test/matrix_test/matrix_main.cpp b/src/sensor_fusion/standalone/util/test/matrix_test/matrix_main.cpp index f7cbf80..3fc2fd6 100644 --- a/src/sensor_fusion/standalone/util/test/matrix_test/matrix_main.cpp +++ b/src/sensor_fusion/standalone/util/test/matrix_test/matrix_main.cpp @@ -92,11 +92,11 @@ int main() float x = 4.0; float x1 = -2.5; cout<< "\n\n\nScalar subtraction\n"; - m3 = m11 - x; + m13 = m11 - x; m6 = m10 - x1; cout<< "\nNumber Subtracted:" << x; cout<< "\n\n" << m11; - cout<< "\nResult:\n\n" << m3; + cout<< "\nResult:\n\n" << m13; cout<< "\nNumber Subtracted:" << x1; cout<< "\n\n" << m10; cout<< "\nResult:\n\n" << m6; @@ -137,4 +137,10 @@ int main() cout << "\n\nm1:\n\n" << m1; cout << "\n\n\nm6 != m1 :"; cout << (m6 != m1); + + + cout<< "\n\nAssignment\n"; + m3 = m12; + cout<< "Input \n" << m12; + cout<< "\nOutput:\n" << m3 << endl; } diff --git a/src/sensor_fusion/standalone/util/test/vector_test/vector_main.cpp b/src/sensor_fusion/standalone/util/test/vector_test/vector_main.cpp index 6ec31b8..8e46ab2 100644 --- a/src/sensor_fusion/standalone/util/test/vector_test/vector_main.cpp +++ b/src/sensor_fusion/standalone/util/test/vector_test/vector_main.cpp @@ -147,5 +147,10 @@ int main() cout << "\n\nv15:\n\n" << v15; cout << "\n\n\nv15 != v15 :"; cout << (v15 != v15); + + cout<< "\n\nAssignment\n"; + v3 = v1; + cout<< "Input \n" << v1; + cout<< "\nOutput:\n" << v3 << endl; } diff --git a/src/sensor_fusion/standalone/util/vector.cpp b/src/sensor_fusion/standalone/util/vector.cpp index e234319..3a572fc 100644 --- a/src/sensor_fusion/standalone/util/vector.cpp +++ b/src/sensor_fusion/standalone/util/vector.cpp @@ -58,7 +58,8 @@ vector::vector(const vector& v) template vector::~vector() { - delete[] m_vec; + if (m_vec != NULL) + delete[] m_vec; } template @@ -69,13 +70,27 @@ vector vector::operator =(const vector& v) return *this; } - delete[] m_vec; - m_size = v.m_size; - m_vec = new TYPE [m_size]; + if (m_vec == NULL) + { + m_size = v.m_size; + m_vec = new TYPE [m_size]; + } + else + { + if (m_size != v.m_size) + { + delete[] m_vec; + + m_size = v.m_size; + m_vec = new TYPE [m_size]; + } + } + for (int q = 0; q < m_size; q++) m_vec[q] = v.m_vec[q]; + return *this; } -- 2.7.4 From f0db69ba2e0b34f77797e96bb44f8762c1081ec6 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Tue, 24 Jun 2014 17:19:47 +0530 Subject: [PATCH 05/16] Adding rotation matrix class implementation - Adding rotation_matrix template class - Adding implementation for the rotation_matrix template class - Adding rotation_matrix class testing code signed-off-by: Ramasamy Change-Id: Ife6b6cefd6fa2801d037d6607c7518cc31dfb6b7 --- .../standalone/util/rotation_matrix.cpp | 158 +++++++++++++++++++++ .../standalone/util/rotation_matrix.h | 45 ++++++ .../util/test/rotation_matrix_test/.cproject | 112 +++++++++++++++ .../util/test/rotation_matrix_test/.project | 89 ++++++++++++ .../rotation_matrix_test/rotation_matrix_main.cpp | 62 ++++++++ 5 files changed, 466 insertions(+) create mode 100644 src/sensor_fusion/standalone/util/rotation_matrix.cpp create mode 100644 src/sensor_fusion/standalone/util/rotation_matrix.h create mode 100644 src/sensor_fusion/standalone/util/test/rotation_matrix_test/.cproject create mode 100644 src/sensor_fusion/standalone/util/test/rotation_matrix_test/.project create mode 100644 src/sensor_fusion/standalone/util/test/rotation_matrix_test/rotation_matrix_main.cpp diff --git a/src/sensor_fusion/standalone/util/rotation_matrix.cpp b/src/sensor_fusion/standalone/util/rotation_matrix.cpp new file mode 100644 index 0000000..aecce4f --- /dev/null +++ b/src/sensor_fusion/standalone/util/rotation_matrix.cpp @@ -0,0 +1,158 @@ +/* + * 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 +#define ROT_MAT_ROWS 3 +#define ROT_MAT_COLS 3 + +template T get_sign(T val) +{ + return (val >= (T) 0) ? (T) 1 : (T) -1; +} + +template +rotation_matrix::rotation_matrix() : m_rot_mat(ROT_MAT_ROWS, ROT_MAT_COLS) +{ + +} + +template +rotation_matrix::rotation_matrix(const matrix m) +{ + m_rot_mat = m; +} + +template +rotation_matrix::rotation_matrix(const int rows, const int cols, TYPE *mat_data) +{ + matrix m(rows, cols, mat_data); + m_rot_mat = m; +} + +template +rotation_matrix::rotation_matrix(const rotation_matrix& rm) +{ + m_rot_mat = rm.m_rot_mat; +} + +template +rotation_matrix::~rotation_matrix() +{ + +} + +template +rotation_matrix rotation_matrix::operator =(const rotation_matrix& rm) +{ + m_rot_mat = rm.m_rot_mat; + + return *this; +} + +template +rotation_matrix quat2rot_mat(quaternion 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 rm(ROT_MAT_ROWS, ROT_MAT_COLS, &R[0][0]); + + return rm; +} + +template +quaternion rot_mat2quat(rotation_matrix rm) +{ + T q0, q1, q2, q3; + + q0 = (rm.m_rot_mat.m_mat[0][0] + rm.m_rot_mat.m_mat[1][1] + + rm.m_rot_mat.m_mat[2][2] + (T) 1) / (T) QUAT_LEN; + q1 = (rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] - + rm.m_rot_mat.m_mat[2][2] + (T) 1) / (T) QUAT_LEN; + q2 = (-rm.m_rot_mat.m_mat[0][0] + rm.m_rot_mat.m_mat[1][1] - + rm.m_rot_mat.m_mat[2][2] + (T) 1) / (T) QUAT_LEN; + q3 = (-rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] + + rm.m_rot_mat.m_mat[2][2] + (T) 1) / (T) QUAT_LEN; + + if(q0 < (T) 0) + q0 = (T) 0; + if(q1 < (T) 0) + q1 = (T) 0; + if(q2 < (T) 0) + q2 = (T) 0; + if(q3 < (T) 0) + q3 = (T) 0; + + q0 = sqrt(q0); + q1 = sqrt(q1); + q2 = sqrt(q2); + q3 = sqrt(q3); + + if (q0 >= q1 && q0 >= q2 && q0 >= q3) + { + q0 *= (T) 1; + q1 *= get_sign(rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2]); + q2 *= get_sign(rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0]); + q3 *= get_sign(rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1]); + } + else if (q1 >= q0 && q1 >= q2 && q1 >= q3) + { + q0 *= get_sign(rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2]); + q1 *= (T) 1; + q2 *= get_sign(rm.m_rot_mat.m_mat[1][0] + rm.m_rot_mat.m_mat[0][1]); + q3 *= get_sign(rm.m_rot_mat.m_mat[0][2] + rm.m_rot_mat.m_mat[2][0]); + } + else if (q2 >= q0 && q2 >= q1 && q2 >= q3) + { + q0 *= get_sign(rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0]); + q1 *= get_sign(rm.m_rot_mat.m_mat[1][0] + rm.m_rot_mat.m_mat[0][1]); + q2 *= (T) 1; + q3 *= get_sign(rm.m_rot_mat.m_mat[2][1] + rm.m_rot_mat.m_mat[1][2]); + } + else if(q3 >= q0 && q3 >= q1 && q3 >= q2) + { + q0 *= get_sign(rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1]); + q1 *= get_sign(rm.m_rot_mat.m_mat[2][0] + rm.m_rot_mat.m_mat[0][2]); + q2 *= get_sign(rm.m_rot_mat.m_mat[2][1] + rm.m_rot_mat.m_mat[1][2]); + q3 *= (T) 1; + } + + quaternion q(-q0, q1, q2, q3); + + return quat_normalize(q); +} + +#endif /* _ROTATION_MATRIX_H */ diff --git a/src/sensor_fusion/standalone/util/rotation_matrix.h b/src/sensor_fusion/standalone/util/rotation_matrix.h new file mode 100644 index 0000000..7ded3e2 --- /dev/null +++ b/src/sensor_fusion/standalone/util/rotation_matrix.h @@ -0,0 +1,45 @@ +/* + * 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" + +template +class rotation_matrix { +public: + matrix m_rot_mat; + + rotation_matrix(); + rotation_matrix(const matrix m); + rotation_matrix(const int rows, const int cols, TYPE *mat_data); + rotation_matrix(const rotation_matrix& rm); + ~rotation_matrix(); + + rotation_matrix operator =(const rotation_matrix& rm); + + template friend rotation_matrix quat2rot_mat(quaternion q); + template friend quaternion rot_mat2quat(rotation_matrix rm); +}; + +#include "rotation_matrix.cpp" + +#endif /* _ROTATION_MATRIX_H */ diff --git a/src/sensor_fusion/standalone/util/test/rotation_matrix_test/.cproject b/src/sensor_fusion/standalone/util/test/rotation_matrix_test/.cproject new file mode 100644 index 0000000..dd975a2 --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/rotation_matrix_test/.cproject @@ -0,0 +1,112 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/sensor_fusion/standalone/util/test/rotation_matrix_test/.project b/src/sensor_fusion/standalone/util/test/rotation_matrix_test/.project new file mode 100644 index 0000000..6e760b0 --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/rotation_matrix_test/.project @@ -0,0 +1,89 @@ + + + rotation_matrix_test + + + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + ?name? + + + + org.eclipse.cdt.make.core.append_environment + true + + + org.eclipse.cdt.make.core.autoBuildTarget + all + + + org.eclipse.cdt.make.core.buildArguments + + + + org.eclipse.cdt.make.core.buildCommand + make + + + org.eclipse.cdt.make.core.buildLocation + ${workspace_loc:/rotation_matrix_test/Debug} + + + org.eclipse.cdt.make.core.cleanBuildTarget + clean + + + org.eclipse.cdt.make.core.contents + org.eclipse.cdt.make.core.activeConfigSettings + + + org.eclipse.cdt.make.core.enableAutoBuild + false + + + org.eclipse.cdt.make.core.enableCleanBuild + true + + + org.eclipse.cdt.make.core.enableFullBuild + true + + + org.eclipse.cdt.make.core.fullBuildTarget + all + + + org.eclipse.cdt.make.core.stopOnError + true + + + org.eclipse.cdt.make.core.useDefaultBuildCmd + true + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + org.tizen.nativecpp.apichecker.core.builder + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + org.tizen.nativecpp.apichecker.core.tizenCppNature + + diff --git a/src/sensor_fusion/standalone/util/test/rotation_matrix_test/rotation_matrix_main.cpp b/src/sensor_fusion/standalone/util/test/rotation_matrix_test/rotation_matrix_main.cpp new file mode 100644 index 0000000..fa1b639 --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/rotation_matrix_test/rotation_matrix_main.cpp @@ -0,0 +1,62 @@ +/* + * 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. + * + */ + +#include "../../rotation_matrix.h" + +int main() +{ + float arr1[3][3] = {{20.2, 40.5, 10.0}, {3.6, 52.0, 5.5}, {1.0, 45.5, 66.6}}; + float arr2[3][3] = {{2.24, 0.5, 0.023}, {3.675, 5.32, 0.556}, {1.023, 45.75, 621.6}}; + float arr3[3][3] = {{4.75, 0.65, 0.123}, {0.075, 5.302, 0.56}, {1.113, 0.475, 2.362}}; + + matrix m1(3, 3, (float *) arr1); + matrix m2(3, 3, (float *) arr2); + matrix m3(3, 3, (float *) arr3); + + rotation_matrix rm0, rm5; + rotation_matrix rm1(m1); + rotation_matrix rm2(m2); + rotation_matrix rm3(m3); + rotation_matrix rm4(3, 3, (float *) arr1); + + quaternion q0(-0.612372, 0.456436, 0.456436, 0.456436); + quaternion q1; + + cout << "Constructor tests\n"; + cout << "input\n" << m1 << "\n"; + cout << "output\n" << rm1.m_rot_mat << "\n\n"; + cout << "input\n" << m2 << "\n"; + cout << "output\n" << rm2.m_rot_mat << "\n\n"; + cout << "input\n" << m3 << "\n"; + cout << "output\n" << rm3.m_rot_mat << "\n\n"; + cout << "input\n" << m1 << "\n"; + cout << "output\n" << rm4.m_rot_mat << "\n\n"; + cout << "default constructor\n"; + cout << "output\n" << rm0.m_rot_mat << "\n\n"; + + cout << "Quaternion to Rotation Matrix\n"; + cout << "input\n" << q0.m_quat << "\n"; + rm0 = quat2rot_mat(q0); + cout << "output\n" << rm0.m_rot_mat << "\n\n"; + + cout << "Rotation Matrix to Quaternion\n"; + cout << "input\n" << rm0.m_rot_mat << "\n"; + q1 = rot_mat2quat(rm0); + cout << "output\n" << q1.m_quat << "\n\n"; +} -- 2.7.4 From ca9116c411d5b3ddf0e8aca7ad81c9b5c54be5f5 Mon Sep 17 00:00:00 2001 From: Amit Dharmapurikar Date: Wed, 2 Jul 2014 10:29:17 +0530 Subject: [PATCH 06/16] Fixing a typo error in gyro_sensor.cpp The typo error causes build failure when gyroscope plugins are enabled. Change-Id: I3107bd7985ee4b022a08b0d1e461052dc7be5310 Signed-off-by: Amit Dharmapurikar --- src/gyro/gyro_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gyro/gyro_sensor.cpp b/src/gyro/gyro_sensor.cpp index 26b9830..78b05be 100755 --- a/src/gyro/gyro_sensor.cpp +++ b/src/gyro/gyro_sensor.cpp @@ -22,7 +22,7 @@ #include #include -#define INTIAL_VALUE -1 +#define INITIAL_VALUE -1 #define MS_TO_US 1000 #define DPS_TO_MDPS 1000 #define RAW_DATA_TO_DPS_UNIT(X) ((float)(X)/((float)DPS_TO_MDPS)) -- 2.7.4 From b1679a7488c3cdfb7c86009e47d824a3a32272ab Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 26 Jun 2014 11:04:42 +0530 Subject: [PATCH 07/16] Adding timestamp info to sensor_data class - Adding new variable for time stamp info for received sensor data signed-off-by: Ramasamy Change-Id: I19e28b0021eb836cea28fcb9c56e44fa31589d28 --- src/sensor_fusion/standalone/util/sensor_data.cpp | 16 +++++++++++----- src/sensor_fusion/standalone/util/sensor_data.h | 7 +++++-- .../util/test/sensor_data_test/sensor_data_main.cpp | 10 +++++----- 3 files changed, 21 insertions(+), 12 deletions(-) diff --git a/src/sensor_fusion/standalone/util/sensor_data.cpp b/src/sensor_fusion/standalone/util/sensor_data.cpp index 4276631..c8960b8 100644 --- a/src/sensor_fusion/standalone/util/sensor_data.cpp +++ b/src/sensor_fusion/standalone/util/sensor_data.cpp @@ -24,29 +24,34 @@ #define SENSOR_DATA_SIZE 3 template -sensor_data::sensor_data() : m_data(SENSOR_DATA_SIZE) +sensor_data::sensor_data() : m_data(SENSOR_DATA_SIZE), m_time_stamp(0) { } template -sensor_data::sensor_data(const TYPE x, const TYPE y, const TYPE z) +sensor_data::sensor_data(const TYPE x, const TYPE y, + const TYPE z, const unsigned long long time_stamp = 0) { TYPE vec_data[SENSOR_DATA_SIZE] = {x, y, z}; vector v(SENSOR_DATA_SIZE, vec_data); m_data = v; + m_time_stamp = time_stamp; } template -sensor_data::sensor_data(const vector v) +sensor_data::sensor_data(const vector v, + const unsigned long long time_stamp = 0) { m_data = v; + m_time_stamp = time_stamp; } template sensor_data::sensor_data(const sensor_data& s) { m_data = s.m_data; + m_time_stamp = s.m_time_stamp; } template @@ -58,6 +63,7 @@ template sensor_data sensor_data::operator =(const sensor_data& s) { m_data = s.m_data; + m_time_stamp = s.m_time_stamp; return *this; } @@ -83,7 +89,7 @@ sensor_data normalize(sensor_data data) y /= val; z /= val; - sensor_data s(x, y, z); + sensor_data s(x, y, z, data.m_time_stamp); return s; } @@ -97,7 +103,7 @@ sensor_data scale_data(sensor_data data, T scaling_factor) y = data.m_data.m_vec[1] / scaling_factor; z = data.m_data.m_vec[2] / scaling_factor; - sensor_data s(x, y, z); + sensor_data s(x, y, z, data.m_time_stamp); return s; } diff --git a/src/sensor_fusion/standalone/util/sensor_data.h b/src/sensor_fusion/standalone/util/sensor_data.h index 18a5ee1..28bdcf7 100644 --- a/src/sensor_fusion/standalone/util/sensor_data.h +++ b/src/sensor_fusion/standalone/util/sensor_data.h @@ -26,10 +26,13 @@ template class sensor_data { public: vector m_data; + unsigned long long m_time_stamp; sensor_data(); - sensor_data(const TYPE x, const TYPE y, const TYPE z); - sensor_data(const vector v); + sensor_data(const TYPE x, const TYPE y, const TYPE z, + const unsigned long long time_stamp); + sensor_data(const vector v, + const unsigned long long time_stamp); sensor_data(const sensor_data& s); ~sensor_data(); diff --git a/src/sensor_fusion/standalone/util/test/sensor_data_test/sensor_data_main.cpp b/src/sensor_fusion/standalone/util/test/sensor_data_test/sensor_data_main.cpp index 8040ad6..511e949 100644 --- a/src/sensor_fusion/standalone/util/test/sensor_data_test/sensor_data_main.cpp +++ b/src/sensor_fusion/standalone/util/test/sensor_data_test/sensor_data_main.cpp @@ -25,16 +25,16 @@ int main() vector v1(3, arr1); - sensor_data sd1(2.0, 3.0, 4.0); + sensor_data sd1(2.0, 3.0, 4.0, 140737488355328); sensor_data sd2(1.04, -4.678, -2.34); - sensor_data sd3(0.054, 1.097, 4.456); - sensor_data sd10(v1); + sensor_data sd3(0.054, 1.097, 4.456, 140737488355328); + sensor_data sd10(v1, 140737488355328); cout << "Constructor tests\n"; cout << "input\t" << v1 << "\n"; - cout << "output\t" << sd10.m_data << "\n\n"; + cout << "output\t" << sd10.m_data << "\t" << sd10.m_time_stamp << "\n\n"; cout << "input\t" << v1 << "\n"; - cout << "output\t" << sd2.m_data<< "\n\n"; + cout << "output\t" << sd2.m_data << "\t" << sd2.m_time_stamp << "\n\n"; cout<< "Addition:\n"; sensor_data sd4 = sd1 + sd2; -- 2.7.4 From 94f6080ddcd1f8db5ebb120ba56f2ec1257910aa Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 26 Jun 2014 12:03:11 +0530 Subject: [PATCH 08/16] Adding orientation_filter class definition - adding orientation_filter source files with definitions - adding test application to read sensor data from sample data files Gurleen Kaur will help with implementation and testing of the class signed-off-by: Ramasamy Change-Id: I9417c85f7e5b656e592772afee7b1e6f4fc1aba9 --- .../standalone/util/orientation_filter.cpp | 163 +++++++++++++++++++++ .../standalone/util/orientation_filter.h | 75 ++++++++++ .../util/test/orientation_filter_test/.cproject | 112 ++++++++++++++ .../util/test/orientation_filter_test/.project | 89 +++++++++++ .../orientation_filter_main.cpp | 80 ++++++++++ 5 files changed, 519 insertions(+) create mode 100644 src/sensor_fusion/standalone/util/orientation_filter.cpp create mode 100644 src/sensor_fusion/standalone/util/orientation_filter.h create mode 100644 src/sensor_fusion/standalone/util/test/orientation_filter_test/.cproject create mode 100644 src/sensor_fusion/standalone/util/test/orientation_filter_test/.project create mode 100644 src/sensor_fusion/standalone/util/test/orientation_filter_test/orientation_filter_main.cpp diff --git a/src/sensor_fusion/standalone/util/orientation_filter.cpp b/src/sensor_fusion/standalone/util/orientation_filter.cpp new file mode 100644 index 0000000..83d6a3a --- /dev/null +++ b/src/sensor_fusion/standalone/util/orientation_filter.cpp @@ -0,0 +1,163 @@ +/* + * 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. + * + */ + +#include "orientation_filter.h" + +#define MOVING_AVERAGE_WINDOW_LENGTH 20 +#define RAD2DEG 57.2957795 +#define DEG2RAD 0.0174532925 +#define US2S (1.0 / 1000000.0) +#define GRAVITY 9.80665 +#define PI 3.141593 + +// Gyro Types +// Systron-donner "Horizon" +#define ZIGMA_W (0.05 * DEG2RAD) //deg/s +#define TAU_W 1000 //secs +// Crossbow DMU-6X +//#define ZIGMA_W 0.05 * DEG2RAD //deg/s +//#define TAU_W 300 //secs +// FOGs (KVH Autogyro and Crossbow DMU-FOG) +//#define ZIGMA_W 0 //deg/s + +template +orientation_filter::orientation_filter() +{ + +} + +template +orientation_filter::~orientation_filter() +{ + +} + +template +inline void orientation_filter::filter_sensor_data(sensor_data accel, + sensor_data gyro, sensor_data magnetic) +{ + +} + +template +inline void orientation_filter::orientation_triad_algorithm() +{ + +} + +template +inline void orientation_filter::compute_aiding_var() +{ + +} + +template +inline void orientation_filter::compute_driving_var() +{ + +} + +template +inline void orientation_filter::compute_process_covar() +{ + +} + +template +inline void orientation_filter::compute_measurement_covar() +{ + +} + +template +inline void orientation_filter::compute_prediction_covar() +{ + +} + +template +inline void orientation_filter::compute_quat_diff() +{ + +} + +template +inline void orientation_filter::compute_quat_sum() +{ + +} + +template +inline void orientation_filter::update_quat_sum() +{ + +} + +template +inline void orientation_filter::time_update() +{ + +} + +template +inline void orientation_filter::measurement_update() +{ + +} + +template +inline euler_angles orientation_filter::get_corrected_orientation() +{ + euler_angles euler_ang; + + return euler_ang; +} + +template +euler_angles orientation_filter::get_orientation(sensor_data accel, + sensor_data gyro, sensor_data magnetic) +{ + euler_angles cor_euler_ang; + + filter_sensor_data(accel, gyro, magnetic); + + orientation_triad_algorithm(); + + compute_aiding_var(); + + compute_driving_var(); + + compute_measurement_covar(); + + compute_prediction_covar(); + + compute_quat_diff(); + + compute_quat_sum(); + + update_quat_sum(); + + time_update(); + + measurement_update(); + + cor_euler_ang = get_corrected_orientation(); + + return cor_euler_ang; +} diff --git a/src/sensor_fusion/standalone/util/orientation_filter.h b/src/sensor_fusion/standalone/util/orientation_filter.h new file mode 100644 index 0000000..a08d3f1 --- /dev/null +++ b/src/sensor_fusion/standalone/util/orientation_filter.h @@ -0,0 +1,75 @@ +/* + * 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 _ORIENTATION_FILTER_H +#define _ORIENTATION_FILTER_H + +#include "matrix.h" +#include "vector.h" +#include "sensor_data.h" +#include "quaternion.h" +#include "euler_angles.h" +#include "rotation_matrix.h" + +template +class orientation_filter { +public: + sensor_data filt_accel[2]; + sensor_data filt_gyro[2]; + sensor_data filt_magnetic[2]; + matrix transition_matrix; + matrix prediction_covariance; + vector state_new; + vector state_old; + vector state_error; + vector bias_correction; + quaternion quat_diff; + quaternion quat_sum; + quaternion quat_aid; + quaternion quat_driv; + quaternion quat_error; + euler_angles euler_error; + rotation_matrix rot_matrix; + euler_angles orientation; + + orientation_filter(); + ~orientation_filter(); + + inline void filter_sensor_data(sensor_data accel, + sensor_data gyro, sensor_data magnetic); + inline void orientation_triad_algorithm(); + inline void compute_aiding_var(); + inline void compute_driving_var(); + inline void compute_process_covar(); + inline void compute_measurement_covar(); + inline void compute_prediction_covar(); + inline void compute_quat_diff(); + inline void compute_quat_sum(); + inline void update_quat_sum(); + inline void time_update(); + inline void measurement_update(); + inline euler_angles get_corrected_orientation(); + + euler_angles get_orientation(sensor_data accel, + sensor_data gyro, sensor_data magnetic); +}; + +#include "orientation_filter.cpp" + +#endif /* _ORIENTATION_FILTER_H */ diff --git a/src/sensor_fusion/standalone/util/test/orientation_filter_test/.cproject b/src/sensor_fusion/standalone/util/test/orientation_filter_test/.cproject new file mode 100644 index 0000000..c9958ee --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/orientation_filter_test/.cproject @@ -0,0 +1,112 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/sensor_fusion/standalone/util/test/orientation_filter_test/.project b/src/sensor_fusion/standalone/util/test/orientation_filter_test/.project new file mode 100644 index 0000000..7e048f5 --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/orientation_filter_test/.project @@ -0,0 +1,89 @@ + + + orientation_filter_main + + + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + ?name? + + + + org.eclipse.cdt.make.core.append_environment + true + + + org.eclipse.cdt.make.core.autoBuildTarget + all + + + org.eclipse.cdt.make.core.buildArguments + + + + org.eclipse.cdt.make.core.buildCommand + make + + + org.eclipse.cdt.make.core.buildLocation + ${workspace_loc:/orientation_filter_main/Debug} + + + org.eclipse.cdt.make.core.cleanBuildTarget + clean + + + org.eclipse.cdt.make.core.contents + org.eclipse.cdt.make.core.activeConfigSettings + + + org.eclipse.cdt.make.core.enableAutoBuild + false + + + org.eclipse.cdt.make.core.enableCleanBuild + true + + + org.eclipse.cdt.make.core.enableFullBuild + true + + + org.eclipse.cdt.make.core.fullBuildTarget + all + + + org.eclipse.cdt.make.core.stopOnError + true + + + org.eclipse.cdt.make.core.useDefaultBuildCmd + true + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + org.tizen.nativecpp.apichecker.core.builder + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + org.tizen.nativecpp.apichecker.core.tizenCppNature + + diff --git a/src/sensor_fusion/standalone/util/test/orientation_filter_test/orientation_filter_main.cpp b/src/sensor_fusion/standalone/util/test/orientation_filter_test/orientation_filter_main.cpp new file mode 100644 index 0000000..862428d --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/orientation_filter_test/orientation_filter_main.cpp @@ -0,0 +1,80 @@ +/* + * 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. + * + */ + +#include "../../orientation_filter.h" +#include +#include +#include +#include +using namespace std; + +#define ORIENTATION_DATA_PATH "../../../../design/data/100ms/orientation/roll_pitch_yaw/" +#define ORIENTATION_DATA_SIZE 100 + +int main() +{ + int data_available = ORIENTATION_DATA_SIZE; + ifstream accel_file, gyro_file, magnetic_file; + string line_accel, line_gyro, line_magnetic; + float sdata[3]; + unsigned long long time_stamp; + euler_angles orientation; + orientation_filter orien_filter; + + accel_file.open(((string)ORIENTATION_DATA_PATH + (string)"accel.txt").c_str()); + gyro_file.open(((string)ORIENTATION_DATA_PATH + (string)"gyro.txt").c_str()); + magnetic_file.open(((string)ORIENTATION_DATA_PATH + (string)"magnetic.txt").c_str()); + + char *token = NULL; + + while (data_available-- > 0) + { + getline(accel_file, line_accel); + sdata[0] = strtof(line_accel.c_str(), &token); + sdata[1] = strtof(token, &token); + sdata[2] = strtof(token, &token); + time_stamp = strtoull (token, NULL, 10); + sensor_data accel_data(sdata[0], sdata[1], sdata[2], time_stamp); + + cout << "Accel Data\t" << accel_data.m_data << "\t Time Stamp\t" << accel_data.m_time_stamp << "\n\n"; + + getline(gyro_file, line_gyro); + sdata[0] = strtof(line_gyro.c_str(), &token); + sdata[1] = strtof(token, &token); + sdata[2] = strtof(token, &token); + time_stamp = strtoull (token, NULL, 10); + sensor_data gyro_data(sdata[0], sdata[1], sdata[2], time_stamp); + + cout << "Gyro Data\t" << gyro_data.m_data << "\t Time Stamp\t" << gyro_data.m_time_stamp << "\n\n"; + + getline(magnetic_file, line_magnetic); + sdata[0] = strtof(line_magnetic.c_str(), &token); + sdata[1] = strtof(token, &token); + sdata[2] = strtof(token, &token); + time_stamp = strtoull (token, NULL, 10); + sensor_data magnetic_data(sdata[0], sdata[1], sdata[2], time_stamp); + + cout << "Magnetic Data\t" << magnetic_data.m_data << "\t Time Stamp\t" << magnetic_data.m_time_stamp << "\n\n"; + + orientation = orien_filter.get_orientation(accel_data, gyro_data, magnetic_data); + + } + + return 0; +} -- 2.7.4 From ffea76150295b6b0a821ea4ede23b144cd61bb54 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 2 Jul 2014 14:01:31 +0530 Subject: [PATCH 09/16] Updating vector class, cleanup & rebasing - updating the vector class - clean up of project files - rebasing folder to code updates on previous commits method for cross produect provided by Gurleen Kaur signed-off-by: Ramasamy Change-Id: I9417c85f7e5b656e592772afee7b1e6f4fc1abe2 --- src/sensor_fusion/standalone/util/matrix.h | 1 + .../standalone/util/test/matrix_test/matrix_main.cpp | 1 + .../standalone/util/test/orientation_filter_test/.cproject | 6 +++--- .../standalone/util/test/orientation_filter_test/.project | 4 ++-- .../standalone/util/test/vector_test/vector_main.cpp | 6 ++++++ src/sensor_fusion/standalone/util/vector.cpp | 12 ++++++++++++ src/sensor_fusion/standalone/util/vector.h | 2 ++ 7 files changed, 27 insertions(+), 5 deletions(-) diff --git a/src/sensor_fusion/standalone/util/matrix.h b/src/sensor_fusion/standalone/util/matrix.h index 0f01f5e..1a49c2f 100755 --- a/src/sensor_fusion/standalone/util/matrix.h +++ b/src/sensor_fusion/standalone/util/matrix.h @@ -59,6 +59,7 @@ public: const matrix m2); template friend bool operator !=(const matrix m1, const matrix m2); + template friend matrix transpose(const matrix m); }; diff --git a/src/sensor_fusion/standalone/util/test/matrix_test/matrix_main.cpp b/src/sensor_fusion/standalone/util/test/matrix_test/matrix_main.cpp index 3fc2fd6..82a4da0 100644 --- a/src/sensor_fusion/standalone/util/test/matrix_test/matrix_main.cpp +++ b/src/sensor_fusion/standalone/util/test/matrix_test/matrix_main.cpp @@ -143,4 +143,5 @@ int main() m3 = m12; cout<< "Input \n" << m12; cout<< "\nOutput:\n" << m3 << endl; + } diff --git a/src/sensor_fusion/standalone/util/test/orientation_filter_test/.cproject b/src/sensor_fusion/standalone/util/test/orientation_filter_test/.cproject index c9958ee..4ebc742 100644 --- a/src/sensor_fusion/standalone/util/test/orientation_filter_test/.cproject +++ b/src/sensor_fusion/standalone/util/test/orientation_filter_test/.cproject @@ -18,7 +18,7 @@ - +