From ec68ba08acfaadab627782f7548e488d82264a0f Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Tue, 15 Jul 2014 10:27:45 +0530 Subject: [PATCH] Adding compute_gravity class and implementation - Adding class definition and implementation of compute_gravity class - Adding testing code for compute_gravity class signed-off-by: Ramasamy Change-Id: I4150961ddef0f6a7a9c10a369e0355b9615a9e58 --- .../standalone/util/compute_gravity.cpp | 57 +++++++++++ .../standalone/util/compute_gravity.h | 39 +++++++ .../util/test/compute_gravity_test/.cproject | 112 +++++++++++++++++++++ .../util/test/compute_gravity_test/.project | 89 ++++++++++++++++ .../compute_gravity_test/compute_gravity_main.cpp | 92 +++++++++++++++++ 5 files changed, 389 insertions(+) create mode 100644 src/sensor_fusion/standalone/util/compute_gravity.cpp create mode 100644 src/sensor_fusion/standalone/util/compute_gravity.h create mode 100644 src/sensor_fusion/standalone/util/test/compute_gravity_test/.cproject create mode 100644 src/sensor_fusion/standalone/util/test/compute_gravity_test/.project create mode 100644 src/sensor_fusion/standalone/util/test/compute_gravity_test/compute_gravity_main.cpp diff --git a/src/sensor_fusion/standalone/util/compute_gravity.cpp b/src/sensor_fusion/standalone/util/compute_gravity.cpp new file mode 100644 index 0000000..3cf6e19 --- /dev/null +++ b/src/sensor_fusion/standalone/util/compute_gravity.cpp @@ -0,0 +1,57 @@ +/* + * 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 _COMPUTE_GRAVITY_H + +#include "compute_gravity.h" + +#define GRAVITY 9.80665 + +template +compute_gravity::compute_gravity() +{ + +} + +template +compute_gravity::~compute_gravity() +{ + +} + +template +sensor_data compute_gravity::orientation2gravity(const sensor_data accel, + const sensor_data gyro, const sensor_data magnetic) +{ + orientation_filter orien_filter; + euler_angles orientation; + sensor_data gravity; + + orientation = orien_filter.get_orientation(accel, gyro, magnetic); + + gravity.m_data.m_vec[0] = GRAVITY * sin(orien_filter.m_orientation.m_ang.m_vec[0]); + gravity.m_data.m_vec[1] = GRAVITY * sin(orien_filter.m_orientation.m_ang.m_vec[1]); + gravity.m_data.m_vec[2] = GRAVITY * cos(orien_filter.m_orientation.m_ang.m_vec[0]) * + cos(orien_filter.m_orientation.m_ang.m_vec[1]); + + return gravity; +} + +#endif diff --git a/src/sensor_fusion/standalone/util/compute_gravity.h b/src/sensor_fusion/standalone/util/compute_gravity.h new file mode 100644 index 0000000..745eb5c --- /dev/null +++ b/src/sensor_fusion/standalone/util/compute_gravity.h @@ -0,0 +1,39 @@ +/* + * 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 _COMPUTE_GRAVITY_H +#define _COMPUTE_GRAVITY_H + +#include "orientation_filter.h" + +template +class compute_gravity { +public: + orientation_filter estimate_orientation; + + compute_gravity(); + ~compute_gravity(); + + sensor_data orientation2gravity(const sensor_data accel, + const sensor_data gyro, const sensor_data magnetic); +}; + +#include "compute_gravity.cpp" + +#endif /* _COMPUTE_GRAVITY_H */ diff --git a/src/sensor_fusion/standalone/util/test/compute_gravity_test/.cproject b/src/sensor_fusion/standalone/util/test/compute_gravity_test/.cproject new file mode 100644 index 0000000..88e1d8a --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/compute_gravity_test/.cproject @@ -0,0 +1,112 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/sensor_fusion/standalone/util/test/compute_gravity_test/.project b/src/sensor_fusion/standalone/util/test/compute_gravity_test/.project new file mode 100644 index 0000000..79f09e9 --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/compute_gravity_test/.project @@ -0,0 +1,89 @@ + + + compute_gravity_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:/compute_gravity_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/compute_gravity_test/compute_gravity_main.cpp b/src/sensor_fusion/standalone/util/test/compute_gravity_test/compute_gravity_main.cpp new file mode 100644 index 0000000..bd56100 --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/compute_gravity_test/compute_gravity_main.cpp @@ -0,0 +1,92 @@ +/* + * 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 "../../compute_gravity.h" +#include +#include +#include +#include +using namespace std; + +#define GRAVITY_DATA_PATH "../../../../design/data/100ms/gravity/throw/" +#define GRAVITY_DATA_SIZE 135 + +int main() +{ + int data_available = GRAVITY_DATA_SIZE; + ifstream accel_in, gyro_in, mag_in; + ofstream gravity_file; + string line_accel, line_gyro, line_magnetic; + float sdata[3]; + unsigned long long time_stamp; + sensor_data gravity; + compute_gravity comp_grav; + + accel_in.open(((string)GRAVITY_DATA_PATH + (string)"accel.txt").c_str()); + gyro_in.open(((string)GRAVITY_DATA_PATH + (string)"gyro.txt").c_str()); + mag_in.open(((string)GRAVITY_DATA_PATH + (string)"magnetic.txt").c_str()); + + gravity_file.open(((string)"gravity.txt").c_str()); + + char *token = NULL; + + while (data_available-- > 0) + { + getline(accel_in, 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_in, 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(mag_in, 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"; + + gravity = comp_grav.orientation2gravity(accel_data, gyro_data, magnetic_data); + + gravity_file << gravity.m_data; + + cout << "Gravity Data\t" << gravity.m_data << "\n\n"; + } + + accel_in.close(); + gyro_in.close(); + mag_in.close(); + gravity_file.close(); + + return 0; +} + -- 2.7.4