From: Kibak Yoon Date: Mon, 22 Sep 2014 04:37:23 +0000 (-0700) Subject: Merge "Adding AK8975 geo-sensor info in sensors.xml.in required by geo-plugin" into... X-Git-Tag: submit/tizen/20150113.012540~133 X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=f60fe2ea39ca11950924c51d9a0d8c5d66ccd8b6;hp=3c53d1954154d682b8223d6be13d4c67d6eb4fbf;p=platform%2Fcore%2Fsystem%2Fsensord.git Merge "Adding AK8975 geo-sensor info in sensors.xml.in required by geo-plugin" into tizen --- diff --git a/CMakeLists.txt b/CMakeLists.txt index fbcfe9a..b555de8 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,7 +16,6 @@ set(PROJECT_MAJOR_VERSION "0") set(PROJECT_MINOR_VERSION "2") set(PROJECT_RELEASE_VERSION "1") set(CMAKE_VERBOSE_MAKEFILE OFF) - add_definitions(-Wall -O3 -omit-frame-pointer) add_definitions(-DUSE_DLOG_LOG) #add_definitions(-Wall -g -D_DEBUG) @@ -50,4 +49,13 @@ ENDIF("${ARCH}" MATCHES "^arm.*") INSTALL(FILES ${CMAKE_CURRENT_SOURCE_DIR}/LICENSE.APLv2 DESTINATION share/license RENAME sensord) INSTALL(FILES ${CMAKE_CURRENT_SOURCE_DIR}/LICENSE.APLv2 DESTINATION share/license RENAME libsensord) +IF("${TEST_SUITE}" STREQUAL "ON") +INSTALL(FILES ${CMAKE_CURRENT_SOURCE_DIR}/LICENSE.APLv2 DESTINATION share/license RENAME test) +ENDIF() + add_subdirectory(src) + +IF("${TEST_SUITE}" STREQUAL "ON") +add_subdirectory(test) +ENDIF() + diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 4b094de..aff3b9e 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -10,12 +10,13 @@ Source2: sensord.socket %define accel_state ON %define gyro_state ON -%define proxi_state OFF +%define proxi_state ON %define light_state OFF %define geo_state ON %define gravity_state OFF %define linear_accel_state OFF %define motion_state OFF +%define sensor_fusion_state ON BuildRequires: cmake BuildRequires: vconf-keys-devel @@ -63,7 +64,8 @@ Sensord library (devel) cmake . -DCMAKE_INSTALL_PREFIX=%{_prefix} -DACCEL=%{accel_state} \ -DGYRO=%{gyro_state} -DPROXI=%{proxi_state} -DLIGHT=%{light_state} \ -DGEO=%{geo_state} -DGRAVITY=%{gravity_state} \ - -DLINEAR_ACCEL=%{linear_accel_state} -DMOTION=%{motion_state} + -DLINEAR_ACCEL=%{linear_accel_state} -DMOTION=%{motion_state} \ + -DSENSOR_FUSION=%{sensor_fusion_state} make %{?jobs:-j%jobs} diff --git a/sensor_plugins.xml.in b/sensor_plugins.xml.in index 2e844ff..fcaef6a 100755 --- a/sensor_plugins.xml.in +++ b/sensor_plugins.xml.in @@ -14,7 +14,7 @@ - + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index f2872e1..d1bc239 100755 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -35,6 +35,10 @@ ENDIF() IF("${GEO}" STREQUAL "ON") add_subdirectory(geo) ENDIF() +IF("${ORIENTATION}" STREQUAL "ON") +add_subdirectory(sensor_fusion) +add_subdirectory(orientation) +ENDIF() IF("${GRAVITY}" STREQUAL "ON") add_subdirectory(gravity) ENDIF() @@ -44,9 +48,6 @@ ENDIF() IF("${MOTION}" STREQUAL "ON") add_subdirectory(motion) ENDIF() -IF("${SENSOR_FUSION}" STREQUAL "ON") -add_subdirectory(sensor_fusion) -ENDIF() add_subdirectory(server) add_subdirectory(libsensord) diff --git a/src/gravity/gravity_sensor.cpp b/src/gravity/gravity_sensor.cpp index 34eee07..5d574a0 100755 --- a/src/gravity/gravity_sensor.cpp +++ b/src/gravity/gravity_sensor.cpp @@ -28,23 +28,21 @@ #include #include #include -#include #include #include #define INITIAL_VALUE -1 #define INITIAL_TIME 0 -#define TIME_CONSTANT 0.18 #define GRAVITY 9.80665 #define SENSOR_NAME "GRAVITY_SENSOR" gravity_sensor::gravity_sensor() -: m_accel_sensor(NULL) +: m_orientation_sensor(NULL) , m_x(INITIAL_VALUE) , m_y(INITIAL_VALUE) , m_z(INITIAL_VALUE) -, m_time(INITIAL_TIME) +, m_timestamp(INITIAL_TIME) { m_name = string(SENSOR_NAME); @@ -58,10 +56,10 @@ gravity_sensor::~gravity_sensor() bool gravity_sensor::init() { - m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); + m_orientation_sensor = sensor_plugin_loader::get_instance().get_sensor(ORIENTATION_SENSOR); - if (!m_accel_sensor) { - ERR("cannot load accel sensor_hal[%s]", sensor_base::get_name()); + if (!m_orientation_sensor) { + ERR("Failed to load orientation sensor: 0x%x", m_orientation_sensor); return false; } @@ -78,8 +76,8 @@ bool gravity_sensor::on_start(void) { AUTOLOCK(m_mutex); - m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); - m_accel_sensor->start(); + m_orientation_sensor->add_client(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME); + m_orientation_sensor->start(); activate(); return true; @@ -89,118 +87,92 @@ bool gravity_sensor::on_stop(void) { AUTOLOCK(m_mutex); - m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); - m_accel_sensor->stop(); + m_orientation_sensor->delete_client(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME); + m_orientation_sensor->stop(); deactivate(); return true; } -bool gravity_sensor::add_interval(int client_id, unsigned int interval, bool is_processor) +bool gravity_sensor::add_interval(int client_id, unsigned int interval) { - m_accel_sensor->add_interval(client_id, interval, true); + AUTOLOCK(m_mutex); + m_orientation_sensor->add_interval(client_id , interval, true); + return sensor_base::add_interval(client_id, interval, true); } -bool gravity_sensor::delete_interval(int client_id, bool is_processor) +bool gravity_sensor::delete_interval(int client_id) { - m_accel_sensor->delete_interval(client_id, true); + AUTOLOCK(m_mutex); + m_orientation_sensor->delete_interval(client_id , true); + return sensor_base::delete_interval(client_id, true); } void gravity_sensor::synthesize(const sensor_event_t &event, vector &outs) { - if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) { - float x, y, z; - calibrate_gravity(event, x, y, z); - - sensor_event_t event; - event.event_type = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME; - event.data.data_accuracy = SENSOR_ACCURACY_GOOD; - event.data.data_unit_idx = SENSOR_UNIT_METRE_PER_SECOND_SQUARED; - event.data.timestamp = m_time; - event.data.values_num = 3; - event.data.values[0] = x; - event.data.values[1] = y; - event.data.values[2] = z; - outs.push_back(event); - - AUTOLOCK(m_value_mutex); - m_x = x; - m_y = y; - m_z = z; - + sensor_event_t gravity_event; + vector orientation_event; + ((virtual_sensor *)m_orientation_sensor)->synthesize(event, orientation_event); + + if (!orientation_event.empty()) { + AUTOLOCK(m_mutex); + + m_timestamp = orientation_event[0].data.timestamp; + gravity_event.data.values[0] = GRAVITY * sin(orientation_event[0].data.values[1]); + gravity_event.data.values[1] = GRAVITY * sin(orientation_event[0].data.values[0]); + gravity_event.data.values[2] = GRAVITY * cos(orientation_event[0].data.values[1] * + orientation_event[0].data.values[0]); + gravity_event.data.values_num = 3; + gravity_event.data.timestamp = m_timestamp; + gravity_event.data.data_accuracy = SENSOR_ACCURACY_GOOD; + gravity_event.data.data_unit_idx = SENSOR_UNIT_METRE_PER_SECOND_SQUARED; + + outs.push_back(gravity_event); return; } } -int gravity_sensor::get_sensor_data(const unsigned int data_id, sensor_data_t &data) +int gravity_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data) { - if (data_id != GRAVITY_BASE_DATA_SET) + sensor_data_t orientation_data; + + if (event_type != GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME) return -1; - AUTOLOCK(m_value_mutex); + m_orientation_sensor->get_sensor_data(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME, orientation_data); data.data_accuracy = SENSOR_ACCURACY_GOOD; data.data_unit_idx = SENSOR_UNIT_METRE_PER_SECOND_SQUARED; - data.time_stamp = m_time; + data.timestamp = orientation_data.timestamp; + data.values[0] = GRAVITY * sin(orientation_data.values[1]); + data.values[1] = GRAVITY * sin(orientation_data.values[0]); + data.values[2] = GRAVITY * cos(orientation_data.values[1] * orientation_data.values[0]); data.values_num = 3; - data.values[0] = m_x; - data.values[1] = m_y; - data.values[2] = m_z; return 0; } bool gravity_sensor::get_properties(const unsigned int type, sensor_properties_t &properties) { - m_accel_sensor->get_properties(type, properties); - - if (type != GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME) - return true; - - properties.sensor_min_range = properties.sensor_min_range / GRAVITY; - properties.sensor_max_range = properties.sensor_max_range / GRAVITY; - properties.sensor_resolution = properties.sensor_resolution / GRAVITY; - strncpy(properties.sensor_name, "Gravity Sensor", MAX_KEY_LENGTH); + properties.sensor_unit_idx = SENSOR_UNIT_DEGREE; + properties.sensor_min_range = -GRAVITY; + properties.sensor_max_range = GRAVITY; + properties.sensor_resolution = 1; + strncpy(properties.sensor_vendor, "Samsung", MAX_KEY_LENGTH); + strncpy(properties.sensor_name, SENSOR_NAME, MAX_KEY_LENGTH); return true; } -void gravity_sensor::calibrate_gravity(const sensor_event_t &raw, float &x, float &y, float &z) -{ - unsigned long long timestamp; - float dt; - float alpha; - float last_x = 0, last_y = 0, last_z = 0; - - { - AUTOLOCK(m_value_mutex); - last_x = m_x; - last_y = m_y; - last_z = m_z; - } - - timestamp = get_timestamp(); - dt = (timestamp - m_time) / US_TO_SEC; - m_time = timestamp; - alpha = TIME_CONSTANT / (TIME_CONSTANT + dt); - - if (dt > 1.0) - alpha = 0.0; - - x = (alpha * last_x) + ((1 - alpha) * raw.data.values[0] / GRAVITY); - y = (alpha * last_y) + ((1 - alpha) * raw.data.values[1] / GRAVITY); - z = (alpha * last_z) + ((1 - alpha) * raw.data.values[2] / GRAVITY); -} - extern "C" void *create(void) { gravity_sensor *inst; try { inst = new gravity_sensor(); - } catch (int ErrNo) { + } catch (int err) { ERR("Failed to create gravity_sensor class, errno : %d, errstr : %s", err, strerror(err)); return NULL; } diff --git a/src/gravity/gravity_sensor.h b/src/gravity/gravity_sensor.h index 0737707..33e1eaa 100755 --- a/src/gravity/gravity_sensor.h +++ b/src/gravity/gravity_sensor.h @@ -42,21 +42,18 @@ public: void synthesize(const sensor_event_t &event, vector &outs); - virtual bool add_interval(int client_id, unsigned int interval, bool is_processor = false); - virtual bool delete_interval(int client_id, bool is_processor = false); + bool add_interval(int client_id, unsigned int interval); + bool delete_interval(int client_id); - int get_sensor_data(const unsigned int data_id, sensor_data_t &data); + int get_sensor_data(const unsigned int event_type, sensor_data_t &data); bool get_properties(const unsigned int type, sensor_properties_t &properties); private: - sensor_base *m_accel_sensor; - cmutex m_value_mutex; + sensor_base *m_orientation_sensor; float m_x; float m_y; float m_z; - unsigned long long m_time; - - void calibrate_gravity(const sensor_event_t &raw, float &x, float &y, float &z); + unsigned long long m_timestamp; }; #endif /*_GRAVITY_SENSOR_H_*/ diff --git a/src/libsensord/client_common.cpp b/src/libsensord/client_common.cpp index 62aa507..5ed83f3 100755 --- a/src/libsensord/client_common.cpp +++ b/src/libsensord/client_common.cpp @@ -75,9 +75,6 @@ log_element g_log_elements[] = { FILL_LOG_ELEMENT(LOG_ID_DATA, GEOMAGNETIC_ATTITUDE_DATA_SET, 0, 25), FILL_LOG_ELEMENT(LOG_ID_DATA, LIGHT_BASE_DATA_SET, 0, 25), FILL_LOG_ELEMENT(LOG_ID_DATA, LIGHT_LUX_DATA_SET, 0, 25), - FILL_LOG_ELEMENT(LOG_ID_DATA, GRAVITY_BASE_DATA_SET, 0, 25), - FILL_LOG_ELEMENT(LOG_ID_DATA, LINEAR_ACCEL_BASE_DATA_SET, 0, 25), - FILL_LOG_ELEMENT(LOG_ID_DATA, ORIENTATION_BASE_DATA_SET, 0, 25), FILL_LOG_ELEMENT(LOG_ID_ROTATE, ROTATION_EVENT_0, 0, 1), FILL_LOG_ELEMENT(LOG_ID_ROTATE, ROTATION_EVENT_90, 0, 1), @@ -210,8 +207,6 @@ unsigned int get_calibration_event_type(unsigned int event_type) switch (sensor) { case GEOMAGNETIC_SENSOR: return GEOMAGNETIC_EVENT_CALIBRATION_NEEDED; - case ORIENTATION_SENSOR: - return ORIENTATION_EVENT_CALIBRATION_NEEDED; default: return 0; } diff --git a/src/libsensord/sensor_gravity.h b/src/libsensord/sensor_gravity.h index 16a5f82..d80ad41 100755 --- a/src/libsensord/sensor_gravity.h +++ b/src/libsensord/sensor_gravity.h @@ -36,12 +36,8 @@ extern "C" * @{ */ -enum gravity_data_id { - GRAVITY_BASE_DATA_SET = (GRAVITY_SENSOR << 16) | 0x0001, -}; - enum gravity_event_type { - GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME = (GRAVITY_SENSOR << 16) | 0x0002, + GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME = (GRAVITY_SENSOR << 16) | 0x0001, }; /** diff --git a/src/libsensord/sensor_linear_accel.h b/src/libsensord/sensor_linear_accel.h index 4a98503..ba48a83 100755 --- a/src/libsensord/sensor_linear_accel.h +++ b/src/libsensord/sensor_linear_accel.h @@ -36,10 +36,6 @@ extern "C" * @{ */ -enum linear_accel_data_id { - LINEAR_ACCEL_BASE_DATA_SET = (LINEAR_ACCEL_SENSOR << 16) | 0x0001, -}; - enum linear_accel_event_type { LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME = (LINEAR_ACCEL_SENSOR << 16) | 0x0001, }; diff --git a/src/libsensord/sensor_orientation.h b/src/libsensord/sensor_orientation.h index 0444887..3c8eb6e 100755 --- a/src/libsensord/sensor_orientation.h +++ b/src/libsensord/sensor_orientation.h @@ -36,13 +36,8 @@ extern "C" * @{ */ -enum orientation_data_id { - ORIENTATION_BASE_DATA_SET = (ORIENTATION_SENSOR << 16) | 0x0001, -}; - enum orientation_event_type { - ORIENTATION_EVENT_CALIBRATION_NEEDED = (ORIENTATION_SENSOR << 16) | 0x0001, - ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME = (ORIENTATION_SENSOR << 16) | 0x0002, + ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME = (ORIENTATION_SENSOR << 16) | 0x0001, }; /** diff --git a/src/linear_accel/linear_accel_sensor.cpp b/src/linear_accel/linear_accel_sensor.cpp index 7465d61..6967da0 100755 --- a/src/linear_accel/linear_accel_sensor.cpp +++ b/src/linear_accel/linear_accel_sensor.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -56,9 +57,11 @@ linear_accel_sensor::~linear_accel_sensor() bool linear_accel_sensor::init() { m_gravity_sensor = sensor_plugin_loader::get_instance().get_sensor(GRAVITY_SENSOR); + m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); - if (!m_gravity_sensor) { - ERR("cannot load gravity sensor_hal[%s]", sensor_base::get_name()); + if (!m_accel_sensor || !m_gravity_sensor) { + ERR("Failed to load sensors, accel: 0x%x, gravity: 0x%x", + m_accel_sensor, m_gravity_sensor); return false; } @@ -89,52 +92,58 @@ bool linear_accel_sensor::on_stop(void) return true; } -bool linear_accel_sensor::add_interval(int client_id, unsigned int interval, bool is_processor) +bool linear_accel_sensor::add_interval(int client_id, unsigned int interval) { - m_gravity_sensor->add_interval((int)this , interval, true); - return sensor_base::add_interval(client_id, interval, is_processor); + AUTOLOCK(m_mutex); + m_gravity_sensor->add_interval(client_id, interval, true); + return sensor_base::add_interval(client_id, interval, true); } -bool linear_accel_sensor::delete_interval(int client_id, bool is_processor) +bool linear_accel_sensor::delete_interval(int client_id) { - m_gravity_sensor->delete_interval((int)this , true); - return sensor_base::delete_interval(client_id, is_processor); + AUTOLOCK(m_mutex); + m_gravity_sensor->delete_interval(client_id, true); + return sensor_base::delete_interval(client_id, true); } void linear_accel_sensor::synthesize(const sensor_event_t &event, vector &outs) { vector gravity_event; + sensor_event_t lin_accel_event; ((virtual_sensor *)m_gravity_sensor)->synthesize(event, gravity_event); if (!gravity_event.empty() && event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) { AUTOLOCK(m_value_mutex); - m_time = gravity_event[0].data.timestamp; - gravity_event[0].event_type = LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME; - m_x = event.data.values[0] - gravity_event[0].data.values[0] * GRAVITY; - m_y = event.data.values[1] - gravity_event[0].data.values[1] * GRAVITY; - m_z = event.data.values[2] - gravity_event[0].data.values[2] * GRAVITY; - - gravity_event[0].data.values[0] = m_x; - gravity_event[0].data.values[1] = m_y; - gravity_event[0].data.values[2] = m_z; - outs.push_back(gravity_event[0]); - return; + lin_accel_event.data.values_num = 3; + lin_accel_event.data.timestamp = gravity_event[0].data.timestamp; + lin_accel_event.event_type = LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME; + lin_accel_event.data.data_accuracy = SENSOR_ACCURACY_GOOD; + lin_accel_event.data.data_unit_idx = SENSOR_UNIT_METRE_PER_SECOND_SQUARED; + lin_accel_event.data.values[0] = event.data.values[0] - gravity_event[0].data.values[0]; + lin_accel_event.data.values[1] = event.data.values[1] - gravity_event[0].data.values[1]; + lin_accel_event.data.values[2] = event.data.values[2] - gravity_event[0].data.values[2]; + outs.push_back(lin_accel_event); } + + return; } -int linear_accel_sensor::get_sensor_data(const unsigned int data_id, sensor_data_t &data) +int linear_accel_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data) { - if (data_id != LINEAR_ACCEL_BASE_DATA_SET) + sensor_data_t gravity_data, accel_data; + ((virtual_sensor *)m_gravity_sensor)->get_sensor_data(GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME, gravity_data); + m_accel_sensor->get_sensor_data(ACCELEROMETER_BASE_DATA_SET, accel_data); + + if (event_type != LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME) return -1; - AUTOLOCK(m_value_mutex); data.data_accuracy = SENSOR_ACCURACY_GOOD; data.data_unit_idx = SENSOR_UNIT_METRE_PER_SECOND_SQUARED; - data.time_stamp = m_time; - data.values[0] = m_x; - data.values[1] = m_y; - data.values[2] = m_z; + data.timestamp = gravity_data.timestamp; + data.values[0] = accel_data.values[0] - gravity_data.values[0]; + data.values[1] = accel_data.values[1] - gravity_data.values[1]; + data.values[2] = accel_data.values[2] - gravity_data.values[2]; data.values_num = 3; return 0; } @@ -156,7 +165,7 @@ extern "C" void *create(void) try { inst = new linear_accel_sensor(); - } catch (int ErrNo) { + } catch (int err) { ERR("Failed to create linear_accel_sensor class, errno : %d, errstr : %s", err, strerror(err)); return NULL; } diff --git a/src/linear_accel/linear_accel_sensor.h b/src/linear_accel/linear_accel_sensor.h index 2d1e96f..1616392 100755 --- a/src/linear_accel/linear_accel_sensor.h +++ b/src/linear_accel/linear_accel_sensor.h @@ -23,6 +23,7 @@ #include #include #include +#include using std::string; @@ -42,12 +43,13 @@ public: void synthesize(const sensor_event_t &event, vector &outs); - virtual bool add_interval(int client_id, unsigned int interval, bool is_processor = false); - virtual bool delete_interval(int client_id, bool is_processor = false); + bool add_interval(int client_id, unsigned int interval); + bool delete_interval(int client_id); - int get_sensor_data(const unsigned int data_id, sensor_data_t &data); + int get_sensor_data(const unsigned int event_type, sensor_data_t &data); bool get_properties(const unsigned int type, sensor_properties_t &properties); private: + sensor_base *m_accel_sensor; sensor_base *m_gravity_sensor; cmutex m_value_mutex; diff --git a/src/orientation/CMakeLists.txt b/src/orientation/CMakeLists.txt new file mode 100755 index 0000000..2ef7592 --- /dev/null +++ b/src/orientation/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 2.6) +project(orientation CXX) + +# to install pkgconfig setup file. +SET(EXEC_PREFIX "\${prefix}") +SET(LIBDIR "\${prefix}/lib") +SET(VERSION 1.0) + +SET(SENSOR_NAME orientation_sensor) + +include_directories(${CMAKE_CURRENT_SOURCE_DIR}) +include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) +include_directories(${CMAKE_SOURCE_DIR}/src/sensor_fusion) + +include(FindPkgConfig) +pkg_check_modules(rpkgs REQUIRED vconf) + +set(PROJECT_MAJOR_VERSION "0") +set(PROJECT_MINOR_VERSION "0") +set(PROJECT_RELEASE_VERSION "1") +set(CMAKE_VERBOSE_MAKEFILE OFF) + + +FIND_PROGRAM(UNAME NAMES uname) +EXEC_PROGRAM("${UNAME}" ARGS "-m" OUTPUT_VARIABLE "ARCH") +IF("${ARCH}" MATCHES "^arm.*") + ADD_DEFINITIONS("-DTARGET -DHWREV_CHECK") + MESSAGE("add -DTARGET -DHWREV_CHECK") +ELSE("${ARCH}" MATCHES "^arm.*") + ADD_DEFINITIONS("-DSIMULATOR") + MESSAGE("add -DSIMULATOR") +ENDIF("${ARCH}" MATCHES "^arm.*") + +add_definitions(-Wall -O3 -omit-frame-pointer) +#add_definitions(-Wall -g -D_DEBUG) +add_definitions(-DUSE_DLOG_LOG) +add_definitions(-Iinclude) + +add_library(${SENSOR_NAME} SHARED + orientation_sensor.cpp + ) + +target_link_libraries(${SENSOR_NAME} ${rpkgs_LDFLAGS} ${GLES_LDFLAGS} "-lm") + +install(TARGETS ${SENSOR_NAME} DESTINATION lib/sensord) diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp new file mode 100755 index 0000000..0b051ce --- /dev/null +++ b/src/orientation/orientation_sensor.cpp @@ -0,0 +1,308 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define SENSOR_NAME "ORIENTATION_SENSOR" + +#define ACCELEROMETER_ENABLED 0x01 +#define GYROSCOPE_ENABLED 0x02 +#define GEOMAGNETIC_ENABLED 0x04 +#define ORIENTATION_ENABLED 7 + +#define INITIAL_VALUE -1 +#define INITIAL_TIME 0 + +// Below Defines const variables to be input from sensor config files once code is stabilized +#define SAMPLING_TIME 100 +#define MS_TO_US 1000 + +float bias_accel[] = {0.098586, 0.18385, (10.084 - GRAVITY)}; +float bias_gyro[] = {-5.3539, 0.24325, 2.3391}; +float bias_magnetic[] = {0, -37.6, +37.6}; +int sign_accel[] = {+1, +1, +1}; +int sign_gyro[] = {+1, +1, +1}; +int sign_magnetic[] = {+1, -1, +1}; +float scale_accel = 1; +float scale_gyro = 580 * 2; +float scale_magnetic = 1; + +int pitch_phase_compensation = 1; +int roll_phase_compensation = 1; +int yaw_phase_compensation = -1; +int magnetic_alignment_factor = 1; + +void pre_process_data(sensor_data &data_out, sensor_data_t &data_in, float *bias, int *sign, float scale) +{ + data_out.m_data.m_vec[0] = sign[0] * (data_in.values[0] - bias[0]) / scale; + data_out.m_data.m_vec[1] = sign[1] * (data_in.values[1] - bias[1]) / scale; + data_out.m_data.m_vec[2] = sign[2] * (data_in.values[2] - bias[2]) / scale; + + data_out.m_time_stamp = data_in.timestamp; +} + +orientation_sensor::orientation_sensor() +: m_accel_sensor(NULL) +, m_gyro_sensor(NULL) +, m_magnetic_sensor(NULL) +, m_roll(INITIAL_VALUE) +, m_pitch(INITIAL_VALUE) +, m_yaw(INITIAL_VALUE) +{ + m_name = string(SENSOR_NAME); + register_supported_event(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME); + m_interval = SAMPLING_TIME * MS_TO_US; + m_timestamp = get_timestamp(); + m_enable_orientation = 0; +} + +orientation_sensor::~orientation_sensor() +{ + INFO("orientation_sensor is destroyed!\n"); +} + +bool orientation_sensor::init(void) +{ + m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); + m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR); + m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR); + + if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor) { + ERR("Failed to load sensors, accel: 0x%x, gyro: 0x%x, mag: 0x%x", + m_accel_sensor, m_gyro_sensor, m_magnetic_sensor); + return false; + } + + INFO("%s is created!", sensor_base::get_name()); + return true; +} + + +sensor_type_t orientation_sensor::get_type(void) +{ + return ORIENTATION_SENSOR; +} + +bool orientation_sensor::on_start(void) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->start(); + m_gyro_sensor->add_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); + m_gyro_sensor->start(); + m_magnetic_sensor->add_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); + m_magnetic_sensor->start(); + + activate(); + return true; +} + +bool orientation_sensor::on_stop(void) +{ + m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->stop(); + m_gyro_sensor->delete_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); + m_gyro_sensor->stop(); + m_magnetic_sensor->delete_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); + m_magnetic_sensor->stop(); + + deactivate(); + return true; +} + +bool orientation_sensor::add_interval(int client_id, unsigned int interval) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->add_interval(client_id, interval, true); + m_gyro_sensor->add_interval(client_id, interval, true); + m_magnetic_sensor->add_interval(client_id, interval, true); + + return sensor_base::add_interval(client_id, interval, true); +} + +bool orientation_sensor::delete_interval(int client_id) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->delete_interval(client_id, true); + m_gyro_sensor->delete_interval(client_id, true); + m_magnetic_sensor->delete_interval(client_id, true); + + return sensor_base::delete_interval(client_id, true); +} + +void orientation_sensor::synthesize(const sensor_event_t &event, vector &outs) +{ + const float MIN_DELIVERY_DIFF_FACTOR = 0.75f; + unsigned long long diff_time; + + sensor_data accel; + sensor_data gyro; + sensor_data magnetic; + + sensor_data_t accel_data; + sensor_data_t gyro_data; + sensor_data_t mag_data; + + sensor_event_t orientation_event; + euler_angles euler_orientation; + + if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) { + diff_time = event.data.timestamp - m_timestamp; + + if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + pre_process_data(accel, accel_data, bias_accel, sign_accel, scale_accel); + + m_enable_orientation |= ACCELEROMETER_ENABLED; + } + else if (event.event_type == GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME) { + diff_time = event.data.timestamp - m_timestamp; + + if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + pre_process_data(gyro, gyro_data, bias_gyro, sign_gyro, scale_gyro); + + m_enable_orientation |= GYROSCOPE_ENABLED; + } + else if (event.event_type == GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME) { + diff_time = event.data.timestamp - m_timestamp; + + if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + pre_process_data(magnetic, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); + + m_enable_orientation |= GEOMAGNETIC_ENABLED; + } + + if (m_enable_orientation == ORIENTATION_ENABLED) { + m_enable_orientation = 0; + m_timestamp = get_timestamp(); + + m_orientation.m_pitch_phase_compensation = pitch_phase_compensation; + m_orientation.m_roll_phase_compensation = roll_phase_compensation; + m_orientation.m_yaw_phase_compensation = yaw_phase_compensation; + m_orientation.m_magnetic_alignment_factor = magnetic_alignment_factor; + + euler_orientation = m_orientation.get_orientation(accel, gyro, magnetic); + + orientation_event.data.data_accuracy = SENSOR_ACCURACY_GOOD; + orientation_event.data.data_unit_idx = SENSOR_UNIT_DEGREE; + orientation_event.event_type = ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME; + orientation_event.data.timestamp = m_timestamp; + orientation_event.data.values_num = 3; + orientation_event.data.values[0] = euler_orientation.m_ang.m_vec[0]; + orientation_event.data.values[1] = euler_orientation.m_ang.m_vec[1]; + orientation_event.data.values[2] = euler_orientation.m_ang.m_vec[2]; + + outs.push_back(orientation_event); + } + + return; +} + +int orientation_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data) +{ + sensor_data accel; + sensor_data gyro; + sensor_data magnetic; + + sensor_data_t accel_data; + sensor_data_t gyro_data; + sensor_data_t magnetic_data; + + euler_angles euler_orientation; + + if (event_type != ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME) + return -1; + + m_accel_sensor->get_sensor_data(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME, accel_data); + m_gyro_sensor->get_sensor_data(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME, gyro_data); + m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, magnetic_data); + + pre_process_data(accel, accel_data, bias_accel, sign_accel, scale_accel); + pre_process_data(gyro, gyro_data, bias_gyro, sign_gyro, scale_gyro); + pre_process_data(magnetic, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); + + m_orientation.m_pitch_phase_compensation = pitch_phase_compensation; + m_orientation.m_roll_phase_compensation = roll_phase_compensation; + m_orientation.m_yaw_phase_compensation = yaw_phase_compensation; + m_orientation.m_magnetic_alignment_factor = magnetic_alignment_factor; + + euler_orientation = m_orientation.get_orientation(accel, gyro, magnetic); + + data.data_accuracy = SENSOR_ACCURACY_GOOD; + data.data_unit_idx = SENSOR_UNIT_DEGREE; + data.timestamp = get_timestamp(); + data.values[0] = euler_orientation.m_ang.m_vec[0]; + data.values[1] = euler_orientation.m_ang.m_vec[1]; + data.values[2] = euler_orientation.m_ang.m_vec[2]; + data.values_num = 3; + + return 0; +} + +bool orientation_sensor::get_properties(sensor_properties_t &properties) +{ + properties.sensor_unit_idx = SENSOR_UNIT_DEGREE; + properties.sensor_min_range = -180; + properties.sensor_max_range = 180; + properties.sensor_resolution = 1; + + strncpy(properties.sensor_vendor, "Samsung", MAX_KEY_LENGTH); + strncpy(properties.sensor_name, SENSOR_NAME, MAX_KEY_LENGTH); + + return true; +} + +extern "C" void *create(void) +{ + orientation_sensor *inst; + + try { + inst = new orientation_sensor(); + } catch (int err) { + ERR("orientation_sensor class create fail , errno : %d , errstr : %s", err, strerror(err)); + return NULL; + } + + return (void *)inst; +} + +extern "C" void destroy(void *inst) +{ + delete (orientation_sensor *)inst; +} diff --git a/src/sensor_fusion/lib_sensor_fusion.h b/src/orientation/orientation_sensor.h similarity index 53% rename from src/sensor_fusion/lib_sensor_fusion.h rename to src/orientation/orientation_sensor.h index 85ddd3e..9f289f0 100755 --- a/src/sensor_fusion/lib_sensor_fusion.h +++ b/src/orientation/orientation_sensor.h @@ -17,39 +17,45 @@ * */ -#ifndef _LIB_SENSOR_FUSION_H_ -#define _LIB_SENSOR_FUSION_H_ +#ifndef _ORIENTATION_SENSOR_H_ +#define _ORIENTATION_SENSOR_H_ -#include +#include +#include +#include -class lib_sensor_fusion : public sensor_fusion -{ +class orientation_sensor : public virtual_sensor { public: - lib_sensor_fusion(); - ~lib_sensor_fusion(); + orientation_sensor(); + ~orientation_sensor(); bool init(void); bool on_start(void); bool on_stop(void); + void synthesize(const sensor_event_t &event, vector &outs); + bool add_interval(int client_id, unsigned int interval); bool delete_interval(int client_id); bool get_properties(sensor_properties_t &properties); + sensor_type_t get_type(void); + + int get_sensor_data(const unsigned int data_id, sensor_data_t &data); - void fuse(const sensor_event_t &event); - bool get_rotation_matrix(arr33_t &rot); - bool get_attitude(float &x, float &y, float &z, float &w); - bool get_gyro_bias(float &x, float &y, float &z); - bool get_rotation_vector(float &x, float &y, float &z, float &w, float &heading_accuracy); - bool get_linear_acceleration(float &x, float &y, float &z); - bool get_gravity(float &x, float &y, float &z); - bool get_rotation_vector_6axis(float &x, float &y, float &z, float &w, float &heading_accuracy); - bool get_geomagnetic_rotation_vector(float &x, float &y, float &z, float &w); - bool get_orientation(float &azimuth, float &pitch, float &roll); private: sensor_base *m_accel_sensor; sensor_base *m_gyro_sensor; sensor_base *m_magnetic_sensor; + + orientation_filter m_orientation; + + unsigned int m_enable_orientation; + + float m_roll; + float m_pitch; + float m_yaw; + unsigned long long m_timestamp; + unsigned int m_interval; }; -#endif /*_LIB_SENSOR_FUSION_H_*/ +#endif /* _ORIENTATION_SENSOR_H_ */ diff --git a/src/proxi/proxi_sensor_hal.cpp b/src/proxi/proxi_sensor_hal.cpp index eed0c9b..c1fb0d7 100755 --- a/src/proxi/proxi_sensor_hal.cpp +++ b/src/proxi/proxi_sensor_hal.cpp @@ -25,21 +25,13 @@ #include #include #include +#include using std::ifstream; using config::CConfig; -#define NODE_NAME "name" -#define NODE_INPUT "input" -#define NODE_ENABLE "enable" -#define SENSOR_NODE "/sys/class/sensors/" -#define SENSORHUB_NODE "/sys/class/sensors/ssp_sensor/" -#define INPUT_DEVICE_NODE "/sys/class/input/" -#define DEV_INPUT_NODE "/dev/input/event/" - #define INITIAL_VALUE -1 #define INITIAL_TIME 0 -#define PROXI_CODE 0x0019 #define SENSOR_TYPE_PROXI "PROXI" #define ELEMENT_NAME "NAME" @@ -48,42 +40,56 @@ using config::CConfig; #define INPUT_NAME "proximity_sensor" +#define NO_FLAG 0 +#define ENABLE_VAL true +#define DISABLE_VAL false + proxi_sensor_hal::proxi_sensor_hal() : m_state(PROXIMITY_STATE_FAR) , m_node_handle(INITIAL_VALUE) , m_fired_time(INITIAL_TIME) , m_sensorhub_supported(false) { - if (!check_hw_node()) { + int fd, ret; + + if (!check_hw_node()) + { ERR("check_hw_node() fail"); throw ENXIO; } CConfig &config = CConfig::get_instance(); - if (!config.get(SENSOR_TYPE_PROXI, m_model_id, ELEMENT_VENDOR, m_vendor)) { + if (!config.get(SENSOR_TYPE_PROXI, m_model_id, ELEMENT_VENDOR, m_vendor)) + { ERR("[VENDOR] is empty"); throw ENXIO; } INFO("m_vendor = %s", m_vendor.c_str()); - if (!config.get(SENSOR_TYPE_PROXI, m_model_id, ELEMENT_NAME, m_chip_name)) { + if (!config.get(SENSOR_TYPE_PROXI, m_model_id, ELEMENT_NAME, m_chip_name)) + { ERR("[NAME] is empty"); throw ENXIO; } INFO("m_chip_name = %s", m_chip_name.c_str()); - if ((m_node_handle = open(m_resource.c_str(), O_RDWR)) < 0) { - ERR("Failed to open handle(%d)", m_node_handle); + fd = open(m_event_resource.c_str(), NO_FLAG); + if (fd == -1) + { + ERR("Could not open event resource"); throw ENXIO; } - int clockId = CLOCK_MONOTONIC; + ret = ioctl(fd, IOCTL_IIO_EVENT_FD, &m_event_fd); + + close(fd); - if (ioctl(m_node_handle, EVIOCSCLOCKID, &clockId) != 0) { - ERR("Fail to set monotonic timestamp for %s", m_resource.c_str()); + if ((ret == -1) || (m_event_fd == -1)) + { + ERR("Failed to retrieve event fd"); throw ENXIO; } @@ -92,9 +98,7 @@ proxi_sensor_hal::proxi_sensor_hal() proxi_sensor_hal::~proxi_sensor_hal() { - close(m_node_handle); - m_node_handle = INITIAL_VALUE; - + close(m_event_fd); INFO("proxi_sensor_hal is destroyed!"); } @@ -108,53 +112,9 @@ sensor_type_t proxi_sensor_hal::get_type(void) return PROXIMITY_SENSOR; } -bool proxi_sensor_hal::enable_resource(string &resource_node, bool enable) +bool proxi_sensor_hal::enable_resource(bool enable) { - int prev_status, status; - FILE *fp = NULL; - fp = fopen(resource_node.c_str(), "r"); - - if (!fp) { - ERR("Fail to open a resource file: %s", resource_node.c_str()); - return false; - } - - if (fscanf(fp, "%d", &prev_status) < 0) { - ERR("Failed to get data from %s", resource_node.c_str()); - fclose(fp); - return false; - } - - fclose(fp); - - if (enable) { - if (m_sensorhub_supported) - status = prev_status | (1 << SENSORHUB_PROXIMITY_ENABLE_BIT); - else - status = 1; - } else { - if (m_sensorhub_supported) - status = prev_status ^ (1 << SENSORHUB_PROXIMITY_ENABLE_BIT); - else - status = 0; - } - - fp = fopen(resource_node.c_str(), "w"); - - if (!fp) { - ERR("Failed to open a resource file: %s", resource_node.c_str()); - return false; - } - - if (fprintf(fp, "%d", status) < 0) { - ERR("Failed to enable a resource file: %s", resource_node.c_str()); - fclose(fp); - return false; - } - - if (fp) - fclose(fp); - + update_sysfs_num(m_enable_resource.c_str(), enable); return true; } @@ -162,10 +122,10 @@ bool proxi_sensor_hal::enable(void) { AUTOLOCK(m_mutex); - enable_resource(m_enable_resource, true); + enable_resource(ENABLE_VAL); m_fired_time = 0; - INFO("Proxi sensor real starting"); + INFO("Proximity sensor real starting"); return true; } @@ -173,68 +133,81 @@ bool proxi_sensor_hal::disable(void) { AUTOLOCK(m_mutex); - enable_resource(m_enable_resource, false); - INFO("Proxi sensor real stopping"); + enable_resource(DISABLE_VAL); + + INFO("Proximity sensor real stopping"); return true; } bool proxi_sensor_hal::update_value(bool wait) { - struct input_event proxi_event; + iio_event_t proxi_event; fd_set readfds, exceptfds; FD_ZERO(&readfds); FD_ZERO(&exceptfds); - FD_SET(m_node_handle, &readfds); - FD_SET(m_node_handle, &exceptfds); + FD_SET(m_event_fd, &readfds); + FD_SET(m_event_fd, &exceptfds); int ret; - ret = select(m_node_handle + 1, &readfds, NULL, &exceptfds, NULL); + ret = select(m_event_fd + 1, &readfds, NULL, &exceptfds, NULL); - if (ret == -1) { - ERR("select error:%s m_node_handle:d", strerror(errno), m_node_handle); + if (ret == -1) + { + ERR("select error:%s m_event_fd:d", strerror(errno), m_event_fd); return false; - } else if (!ret) { + } + else if (!ret) + { DBG("select timeout"); return false; } - if (FD_ISSET(m_node_handle, &exceptfds)) { + if (FD_ISSET(m_event_fd, &exceptfds)) + { ERR("select exception occurred!"); return false; } - if (FD_ISSET(m_node_handle, &readfds)) { - INFO("proxi event detection!"); - int len = read(m_node_handle, &proxi_event, sizeof(proxi_event)); + if (FD_ISSET(m_event_fd, &readfds)) + { + INFO("proximity event detection!"); + int len = read(m_event_fd, &proxi_event, sizeof(proxi_event)); - if (len == -1) { - DBG("read(m_node_handle) is error:%s.", strerror(errno)); + if (len == -1) + { + DBG("Error in read(m_event_fd):%s.", strerror(errno)); return false; } - DBG("read event, len : %d , type : %x , code : %x , value : %x", len, proxi_event.type, proxi_event.code, proxi_event.value); - - if ((proxi_event.type == EV_ABS) && (proxi_event.code == PROXI_CODE)) { + ull_bytes_t ev_data; + ev_data.num = proxi_event.event_id; + if (ev_data.bytes[CH_TYPE] == PROXIMITY_TYPE) + { AUTOLOCK(m_value_mutex); - - if (proxi_event.value == PROXIMITY_NODE_STATE_FAR) { - INFO("PROXIMITY_STATE_FAR state occured"); + int temp; + temp = GET_DIR_VAL(ev_data.bytes[DIRECTION]); + if (temp == PROXIMITY_NODE_STATE_FAR) + { + INFO("PROXIMITY_STATE_FAR state occurred"); m_state = PROXIMITY_STATE_FAR; - } else if (proxi_event.value == PROXIMITY_NODE_STATE_NEAR) { - INFO("PROXIMITY_STATE_NEAR state occured"); + } + else if (temp == PROXIMITY_NODE_STATE_NEAR) + { + INFO("PROXIMITY_STATE_NEAR state occurred"); m_state = PROXIMITY_STATE_NEAR; - } else { - ERR("PROXIMITY_STATE Unknown: %d", proxi_event.value); + } + else + { + ERR("PROXIMITY_STATE Unknown: %d", proxi_event.event_id); return false; } - - m_fired_time = sensor_hal::get_timestamp(&proxi_event.time); - } else { - return false; } - } else { - ERR("select nothing to read!!!"); + m_fired_time = proxi_event.timestamp; + } + else + { + ERR("No proximity event data available to read"); return false; } @@ -255,7 +228,7 @@ int proxi_sensor_hal::get_sensor_data(sensor_data_t &data) data.data_unit_idx = SENSOR_UNIT_STATE_ON_OFF; data.timestamp = m_fired_time; data.values_num = 1; - data.values[0] = m_state; + data.values[0] = (float)(m_state); return 0; } @@ -272,23 +245,14 @@ bool proxi_sensor_hal::get_properties(sensor_properties_t &properties) bool proxi_sensor_hal::is_sensorhub_supported(void) { - DIR *main_dir = NULL; - main_dir = opendir(SENSORHUB_NODE); - - if (!main_dir) { - INFO("Sensor Hub is not supported"); - return false; - } - - INFO("It supports sensor hub"); - closedir(main_dir); - return true; + return false; } bool proxi_sensor_hal::check_hw_node(void) { string name_node; string hw_name; + string file_name; DIR *main_dir = NULL; struct dirent *dir_entry = NULL; bool find_node = false; @@ -296,83 +260,53 @@ bool proxi_sensor_hal::check_hw_node(void) INFO("======================start check_hw_node============================="); m_sensorhub_supported = is_sensorhub_supported(); - main_dir = opendir(SENSOR_NODE); + main_dir = opendir(IIO_DIR); - if (!main_dir) { - ERR("Directory open failed to collect data"); + if (!main_dir) + { + ERR("Could not open IIO directory\n"); return false; } - while ((!find_node) && (dir_entry = readdir(main_dir))) { - if ((strncasecmp(dir_entry->d_name , ".", 1 ) != 0) && (strncasecmp(dir_entry->d_name , "..", 2 ) != 0) && (dir_entry->d_ino != 0)) { - name_node = string(SENSOR_NODE) + string(dir_entry->d_name) + string("/") + string(NODE_NAME); + while (!find_node) + { + dir_entry = readdir(main_dir); + if(dir_entry == NULL) + break; - ifstream infile(name_node.c_str()); + if ((strncasecmp(dir_entry->d_name , ".", 1 ) != 0) && (strncasecmp(dir_entry->d_name , "..", 2 ) != 0) && (dir_entry->d_ino != 0)) + { + file_name = string(IIO_DIR) + string(dir_entry->d_name) + string(NAME_NODE); + + ifstream infile(file_name.c_str()); if (!infile) continue; infile >> hw_name; - if (CConfig::get_instance().is_supported(SENSOR_TYPE_PROXI, hw_name) == true) { - m_name = m_model_id = hw_name; - INFO("m_model_id = %s", m_model_id.c_str()); - find_node = true; - break; - } - } - } + if (strncmp(dir_entry->d_name, IIO_DEV_BASE_NAME, IIO_DEV_STR_LEN) == 0) + { + if (CConfig::get_instance().is_supported(SENSOR_TYPE_PROXI, hw_name) == true) + { + m_name = m_model_id = hw_name; + m_proxi_dir = string(dir_entry->d_name); + m_enable_resource = string(IIO_DIR) + m_proxi_dir + string(EVENT_DIR) + string(EVENT_EN_NODE); + m_event_resource = string(DEV_DIR) + m_proxi_dir; - closedir(main_dir); - - if (find_node) { - main_dir = opendir(INPUT_DEVICE_NODE); - - if (!main_dir) { - ERR("Directory open failed to collect data"); - return false; - } - - find_node = false; - - while ((!find_node) && (dir_entry = readdir(main_dir))) { - if (strncasecmp(dir_entry->d_name, NODE_INPUT, 5) == 0) { - name_node = string(INPUT_DEVICE_NODE) + string(dir_entry->d_name) + string("/") + string(NODE_NAME); - ifstream infile(name_node.c_str()); - - if (!infile) - continue; - - infile >> hw_name; - - if (hw_name == string(INPUT_NAME)) { - INFO("name_node = %s", name_node.c_str()); - DBG("Find H/W for proxi_sensor"); + INFO("m_enable_resource = %s", m_enable_resource.c_str()); + INFO("m_model_id = %s", m_model_id.c_str()); + INFO("m_proxi_dir = %s", m_proxi_dir.c_str()); + INFO("m_event_resource = %s", m_event_resource.c_str()); find_node = true; - string dir_name; - dir_name = string(dir_entry->d_name); - unsigned found = dir_name.find_first_not_of(NODE_INPUT); - m_resource = string(DEV_INPUT_NODE) + dir_name.substr(found); - - if (m_sensorhub_supported) - m_enable_resource = string(SENSORHUB_NODE) + string(NODE_ENABLE); - else - m_enable_resource = string(INPUT_DEVICE_NODE) + string(dir_entry->d_name) + string("/") + string(NODE_ENABLE); - break; } } } - - closedir(main_dir); - } - - if (find_node) { - INFO("m_resource = %s", m_resource.c_str()); - INFO("m_enable_resource = %s", m_enable_resource.c_str()); } + closedir(main_dir); return find_node; } @@ -380,9 +314,12 @@ extern "C" void *create(void) { proxi_sensor_hal *inst; - try { + try + { inst = new proxi_sensor_hal(); - } catch (int err) { + } + catch (int err) + { ERR("Failed to create proxi_sensor_hal class, errno : %d, errstr : %s", err, strerror(err)); return NULL; } diff --git a/src/proxi/proxi_sensor_hal.h b/src/proxi/proxi_sensor_hal.h index d483814..bd273fa 100755 --- a/src/proxi/proxi_sensor_hal.h +++ b/src/proxi/proxi_sensor_hal.h @@ -23,17 +23,24 @@ #include #include +#define IIO_DIR "/sys/bus/iio/devices/" +#define NAME_NODE "/name" +#define EVENT_DIR "/events" +#define EVENT_EN_NODE "/in_proximity_thresh_either_en" +#define DEV_DIR "/dev/" + +#define IIO_DEV_BASE_NAME "iio:device" +#define IIO_DEV_STR_LEN 10 + +#define PROXIMITY_NODE_STATE_NEAR 1 +#define PROXIMITY_NODE_STATE_FAR 2 +#define PROXIMITY_TYPE 8 + using std::string; class proxi_sensor_hal : public sensor_hal { public: - enum proxi_node_state_event_t { - PROXIMITY_NODE_STATE_NEAR = 0, - PROXIMITY_NODE_STATE_FAR = 1, - PROXIMITY_NODE_STATE_UNKNOWN = 2, - }; - proxi_sensor_hal(); virtual ~proxi_sensor_hal(); string get_model_id(void); @@ -56,13 +63,17 @@ private: string m_vendor; string m_chip_name; - string m_resource; + string m_proxi_dir; + string m_enable_resource; + string m_event_resource; cmutex m_value_mutex; - bool enable_resource(string &resource_node, bool enable); + int m_event_fd; + + bool enable_resource(bool enable); bool update_value(bool wait); bool is_sensorhub_supported(void); }; -#endif /*_PROXI_SENSOR_HAL_H_*/ \ No newline at end of file +#endif /*_PROXI_SENSOR_HAL_H_*/ diff --git a/src/sensor_fusion/CMakeLists.txt b/src/sensor_fusion/CMakeLists.txt index d03dbde..da05d86 100755 --- a/src/sensor_fusion/CMakeLists.txt +++ b/src/sensor_fusion/CMakeLists.txt @@ -35,7 +35,13 @@ add_definitions(-Iinclude) SET(SENSOR_FUSION_NAME sensor_fusion) add_library(${SENSOR_FUSION_NAME} SHARED - lib_sensor_fusion.cpp + euler_angles.cpp + matrix.cpp + orientation_filter.cpp + quaternion.cpp + rotation_matrix.cpp + sensor_data.cpp + vector.cpp ) target_link_libraries(${SENSOR_FUSION_NAME} ${rpkgs_LDFLAGS} ${GLES_LDFLAGS} "-lm") diff --git a/src/sensor_fusion/design/lib/estimate_orientation.m b/src/sensor_fusion/design/lib/estimate_orientation.m index dfbbc18..6e96c89 100755 --- a/src/sensor_fusion/design/lib/estimate_orientation.m +++ b/src/sensor_fusion/design/lib/estimate_orientation.m @@ -25,6 +25,10 @@ function [OR_driv, OR_aid, OR_err] = estimate_orientation(Accel_data, Gyro_data LOW_PASS_FILTERING_ON = 0; PLOT_SCALED_SENSOR_COMPARISON_DATA = 0; PLOT_INDIVIDUAL_SENSOR_INPUT_DATA = 0; + MAGNETIC_ALIGNMENT_FACTOR = -1; + PITCH_PHASE_CORRECTION = -1; + ROLL_PHASE_CORRECTION = -1; + YAW_PHASE_CORRECTION = -1; GRAVITY = 9.80665; PI = 3.141593; @@ -124,7 +128,7 @@ function [OR_driv, OR_aid, OR_err] = estimate_orientation(Accel_data, Gyro_data M_T(1) = 100000; acc_e = [0.0;0.0;1.0]; % gravity vector in earth frame - mag_e = [0.0;1.0;0.0]; % magnetic field vector in earth frame + mag_e = [0.0;MAGNETIC_ALIGNMENT_FACTOR;0.0]; % magnetic field vector in earth frame H = [eye(3) zeros(3,3); zeros(3,6)]; x = zeros(6,BUFFER_SIZE); @@ -303,9 +307,9 @@ function [OR_driv, OR_aid, OR_err] = estimate_orientation(Accel_data, Gyro_data OR_aid(3,:) = yaw * RAD2DEG; euler = quat2euler(quat_driv); - OR_driv(1,:) = euler(:,2)' * RAD2DEG; - OR_driv(2,:) = euler(:,1)' * RAD2DEG; - OR_driv(3,:) = -euler(:,3)' * RAD2DEG; + OR_driv(1,:) = ROLL_PHASE_CORRECTION * euler(:,2)' * RAD2DEG; + OR_driv(2,:) = PITCH_PHASE_CORRECTION * euler(:,1)' * RAD2DEG; + OR_driv(3,:) = YAW_PHASE_CORRECTION * euler(:,3)' * RAD2DEG; euler = quat2euler(quat_error); OR_err(1,:) = euler(:,2)' * RAD2DEG; diff --git a/src/sensor_fusion/design/sf_gravity.m b/src/sensor_fusion/design/sf_gravity.m index 19f249c..07e47d6 100755 --- a/src/sensor_fusion/design/sf_gravity.m +++ b/src/sensor_fusion/design/sf_gravity.m @@ -39,6 +39,14 @@ Bias_Gx = -5.3539; Bias_Gy = 0.24325; Bias_Gz = 2.3391; +Bias_Mx = 0; +Bias_My = 0; +Bias_Mz = 0; + +Sign_Mx = 1; +Sign_My = 1; +Sign_Mz = 1; + BUFFER_SIZE = 100; Accel_data = zeros(4,BUFFER_SIZE); @@ -69,9 +77,9 @@ Gyro_data(2,:) = Gyro_data(2,:)/scale_Gyro; Gyro_data(3,:) = Gyro_data(3,:)/scale_Gyro; % get magnetometer x,y,z axis data from stored file -Mag_data(1,:) = (((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,1))'))(1:BUFFER_SIZE); -Mag_data(2,:) = (((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,2))'))(1:BUFFER_SIZE); -Mag_data(3,:) = (((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,3))'))(1:BUFFER_SIZE); +Mag_data(1,:) = (((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,1))') + Bias_Mx)(1:BUFFER_SIZE) * Sign_Mx; +Mag_data(2,:) = (((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,2))') + Bias_My)(1:BUFFER_SIZE) * Sign_My; +Mag_data(3,:) = (((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,3))') + Bias_Mz)(1:BUFFER_SIZE) * Sign_Mz; Mag_data(4,:) = ((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,4))')(1:BUFFER_SIZE); % estimate orientation @@ -92,7 +100,7 @@ hold on; grid on; UA = Accel_data(3,:); p3 = plot(1:length(UA),UA(1,1:length(UA)),'r'); -title(['Accelerometer Raw Data']); +title(['Raw Accelerometer Data']); legend([p1 p2 p3],'x-axis', 'y-axis', 'z-axis'); % Gravity Plot Results diff --git a/src/sensor_fusion/design/sf_linear_acceleration.m b/src/sensor_fusion/design/sf_linear_acceleration.m index edb78e0..21555b4 100755 --- a/src/sensor_fusion/design/sf_linear_acceleration.m +++ b/src/sensor_fusion/design/sf_linear_acceleration.m @@ -39,6 +39,14 @@ Bias_Gx = -5.3539; Bias_Gy = 0.24325; Bias_Gz = 2.3391; +Bias_Mx = 0; +Bias_My = 0; +Bias_Mz = 0; + +Sign_Mx = 1; +Sign_My = 1; +Sign_Mz = 1; + BUFFER_SIZE = 125; Accel_data = zeros(4,BUFFER_SIZE); @@ -69,9 +77,9 @@ Gyro_data(2,:) = Gyro_data(2,:)/scale_Gyro; Gyro_data(3,:) = Gyro_data(3,:)/scale_Gyro; % get magnetometer x,y,z axis data from stored file -Mag_data(1,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,1))'))(1:BUFFER_SIZE); -Mag_data(2,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,2))'))(1:BUFFER_SIZE); -Mag_data(3,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,3))'))(1:BUFFER_SIZE); +Mag_data(1,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,1))') + Bias_Mx)(1:BUFFER_SIZE) * Sign_Mx; +Mag_data(2,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,2))') + Bias_My)(1:BUFFER_SIZE) * Sign_My; +Mag_data(3,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,3))') + Bias_Mz)(1:BUFFER_SIZE) * Sign_Mz; Mag_data(4,:) = ((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,4))')(1:BUFFER_SIZE); % estimate orientation diff --git a/src/sensor_fusion/design/sf_orientation.m b/src/sensor_fusion/design/sf_orientation.m index d1c927c..be1d107 100755 --- a/src/sensor_fusion/design/sf_orientation.m +++ b/src/sensor_fusion/design/sf_orientation.m @@ -39,6 +39,14 @@ Bias_Gx = -5.3539; Bias_Gy = 0.24325; Bias_Gz = 2.3391; +Bias_Mx = 0; +Bias_My = 0; +Bias_Mz = 0; + +Sign_Mx = 1; +Sign_My = 1; +Sign_Mz = 1; + BUFFER_SIZE = 1095; Accel_data = zeros(4,BUFFER_SIZE); @@ -69,9 +77,9 @@ Gyro_data(2,:) = Gyro_data(2,:)/scale_Gyro; Gyro_data(3,:) = Gyro_data(3,:)/scale_Gyro; % get magnetometer x,y,z axis data from stored file -Mag_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,1))'))(1:BUFFER_SIZE); -Mag_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,2))'))(1:BUFFER_SIZE); -Mag_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,3))'))(1:BUFFER_SIZE); +Mag_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,1))') + Bias_Mx)(1:BUFFER_SIZE) * Sign_Mx; +Mag_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,2))') + Bias_My)(1:BUFFER_SIZE) * Sign_My; +Mag_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,3))') + Bias_Mz)(1:BUFFER_SIZE) * Sign_Mz; Mag_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,4))')(1:BUFFER_SIZE); % estimate orientation diff --git a/src/sensor_fusion/standalone/util/euler_angles.cpp b/src/sensor_fusion/euler_angles.cpp similarity index 95% rename from src/sensor_fusion/standalone/util/euler_angles.cpp rename to src/sensor_fusion/euler_angles.cpp index 4241788..c668318 100644 --- a/src/sensor_fusion/standalone/util/euler_angles.cpp +++ b/src/sensor_fusion/euler_angles.cpp @@ -34,12 +34,12 @@ euler_angles::euler_angles(const TYPE roll, const TYPE pitch, const TYPE y { TYPE euler_data[EULER_SIZE] = {roll, pitch, yaw}; - vector v(EULER_SIZE, euler_data); + vect v(EULER_SIZE, euler_data); m_ang = v; } template -euler_angles::euler_angles(const vector v) +euler_angles::euler_angles(const vect v) { m_ang = v; } diff --git a/src/sensor_fusion/standalone/util/euler_angles.h b/src/sensor_fusion/euler_angles.h similarity index 95% rename from src/sensor_fusion/standalone/util/euler_angles.h rename to src/sensor_fusion/euler_angles.h index bafe8df..69fe0dd 100644 --- a/src/sensor_fusion/standalone/util/euler_angles.h +++ b/src/sensor_fusion/euler_angles.h @@ -26,11 +26,11 @@ template class euler_angles { public: - vector m_ang; + vect m_ang; euler_angles(); euler_angles(const TYPE roll, const TYPE pitch, const TYPE yaw); - euler_angles(const vector v); + euler_angles(const vect v); euler_angles(const euler_angles& e); ~euler_angles(); diff --git a/src/sensor_fusion/lib_sensor_fusion.cpp b/src/sensor_fusion/lib_sensor_fusion.cpp deleted file mode 100755 index 0217c97..0000000 --- a/src/sensor_fusion/lib_sensor_fusion.cpp +++ /dev/null @@ -1,183 +0,0 @@ -/* - * 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define SENSOR_NAME "Sensor Fusion" - -lib_sensor_fusion::lib_sensor_fusion() -{ - m_name = string(SENSOR_NAME); -} - -lib_sensor_fusion::~lib_sensor_fusion() -{ -} - -bool lib_sensor_fusion::init(void) -{ - m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); - m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR); - m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR); - - if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor) { - ERR("Fail to load sensors, accel: 0x%x, gyro: 0x%x, mag: 0x%x", - m_accel_sensor, m_gyro_sensor, m_magnetic_sensor); - return false; - } - - INFO("%s is created!", sensor_base::get_name()); - return true; -} - -bool lib_sensor_fusion::on_start(void) -{ - AUTOLOCK(m_mutex); - - m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); - m_accel_sensor->start(); - m_gyro_sensor->add_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); - m_gyro_sensor->start(); - m_magnetic_sensor->add_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); - m_magnetic_sensor->start(); - return true; -} - -bool lib_sensor_fusion::on_stop(void) -{ - m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); - m_accel_sensor->stop(); - m_gyro_sensor->delete_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); - m_gyro_sensor->stop(); - m_magnetic_sensor->delete_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); - m_magnetic_sensor->stop(); - return true; -} - -bool lib_sensor_fusion::add_interval(int client_id, unsigned int interval) -{ - AUTOLOCK(m_mutex); - - m_accel_sensor->add_interval(client_id, interval, true); - m_gyro_sensor->add_interval(client_id, interval, true); - m_magnetic_sensor->add_interval(client_id, interval, true); - return true; -} - -bool lib_sensor_fusion::delete_interval(int client_id) -{ - AUTOLOCK(m_mutex); - - m_accel_sensor->delete_interval(client_id, true); - m_gyro_sensor->delete_interval(client_id, true); - m_magnetic_sensor->delete_interval(client_id, true); - return true; -} - -bool lib_sensor_fusion::get_properties(sensor_properties_t &properties) -{ - properties.sensor_unit_idx = SENSOR_UNDEFINED_UNIT; - properties.sensor_min_range = 0; - properties.sensor_max_range = 1; - properties.sensor_resolution = 1; - strncpy(properties.sensor_vendor, "Samsung", MAX_KEY_LENGTH); - strncpy(properties.sensor_name, SENSOR_NAME, MAX_KEY_LENGTH); - return true; -} - -void lib_sensor_fusion::fuse(const sensor_event_t &event) -{ - return; -} - -bool lib_sensor_fusion::get_rotation_matrix(arr33_t &rot) -{ - return true; -} - -bool lib_sensor_fusion::get_attitude(float &x, float &y, float &z, float &w) -{ - return true; -} - -bool lib_sensor_fusion::get_gyro_bias(float &x, float &y, float &z) -{ - return true; -} - -bool lib_sensor_fusion::get_rotation_vector(float &x, float &y, float &z, float &w, float &heading_accuracy) -{ - return true; -} - -bool lib_sensor_fusion::get_linear_acceleration(float &x, float &y, float &z) -{ - return true; -} - -bool lib_sensor_fusion::get_gravity(float &x, float &y, float &z) -{ - return true; -} - -bool lib_sensor_fusion::get_rotation_vector_6axis(float &x, float &y, float &z, float &w, float &heading_accuracy) -{ - return true; -} - -bool lib_sensor_fusion::get_geomagnetic_rotation_vector(float &x, float &y, float &z, float &w) -{ - return true; -} - -bool lib_sensor_fusion::get_orientation(float &azimuth, float &pitch, float &roll) -{ - return true; -} - -extern "C" void *create(void) -{ - lib_sensor_fusion *inst; - - try { - inst = new lib_sensor_fusion(); - } catch (int err) { - ERR("lib_sensor_fusion class create fail , errno : %d , errstr : %s", err, strerror(err)); - return NULL; - } - - return (void *)inst; -} - -extern "C" void destroy(void *inst) -{ - delete (lib_sensor_fusion *)inst; -} diff --git a/src/sensor_fusion/standalone/util/matrix.cpp b/src/sensor_fusion/matrix.cpp similarity index 100% rename from src/sensor_fusion/standalone/util/matrix.cpp rename to src/sensor_fusion/matrix.cpp diff --git a/src/sensor_fusion/standalone/util/matrix.h b/src/sensor_fusion/matrix.h similarity index 100% rename from src/sensor_fusion/standalone/util/matrix.h rename to src/sensor_fusion/matrix.h diff --git a/src/sensor_fusion/standalone/util/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp similarity index 85% rename from src/sensor_fusion/standalone/util/orientation_filter.cpp rename to src/sensor_fusion/orientation_filter.cpp index 0cdb7fd..2226cbe 100644 --- a/src/sensor_fusion/standalone/util/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -42,18 +42,6 @@ #define QWB_CONST ((2 * (ZIGMA_W * ZIGMA_W)) / TAU_W) #define F_CONST (-1 / TAU_W) -#define BIAS_AX 0.098586 -#define BIAS_AY 0.18385 -#define BIAS_AZ (10.084 - GRAVITY) - -#define BIAS_GX -5.3539 -#define BIAS_GY 0.24325 -#define BIAS_GZ 2.3391 - -#define DRIVING_SYSTEM_PHASE_COMPENSATION -1 - -#define SCALE_GYRO 575 - #define ENABLE_LPF false #define M3X3R 3 @@ -73,9 +61,9 @@ orientation_filter::orientation_filter() std::fill_n(arr, MOVING_AVERAGE_WINDOW_LENGTH, NON_ZERO_VAL); - vector vec(MOVING_AVERAGE_WINDOW_LENGTH, arr); - vector vec1x3(V1x3S); - vector vec1x6(V1x6S); + vect vec(MOVING_AVERAGE_WINDOW_LENGTH, arr); + vect vec1x3(V1x3S); + vect vec1x6(V1x6S); matrix mat6x6(M6X6R, M6X6C); m_var_gyr_x = vec; @@ -95,6 +83,11 @@ orientation_filter::orientation_filter() m_state_new = vec1x6; m_state_old = vec1x6; m_state_error = vec1x6; + + m_pitch_phase_compensation = 1; + m_roll_phase_compensation = 1; + m_yaw_phase_compensation = 1; + m_magnetic_alignment_factor = 1; } template @@ -110,16 +103,8 @@ inline void orientation_filter::filter_sensor_data(const sensor_data const TYPE iir_b[] = {0.98, 0}; const TYPE iir_a[] = {1.0000000, 0.02}; - TYPE a_bias[] = {BIAS_AX, BIAS_AY, BIAS_AZ}; - TYPE g_bias[] = {BIAS_GX, BIAS_GY, BIAS_GZ}; - - vector acc_data(V1x3S); - vector gyr_data(V1x3S); - vector acc_bias(V1x3S, a_bias); - vector gyr_bias(V1x3S, g_bias); - - acc_data = accel.m_data - acc_bias; - gyr_data = (gyro.m_data - gyr_bias) / (TYPE) SCALE_GYRO; + vect acc_data(V1x3S); + vect gyr_data(V1x3S); m_filt_accel[0] = m_filt_accel[1]; m_filt_gyro[0] = m_filt_gyro[1]; @@ -127,14 +112,14 @@ inline void orientation_filter::filter_sensor_data(const sensor_data if (ENABLE_LPF) { - m_filt_accel[1].m_data = acc_data * iir_b[0] - m_filt_accel[0].m_data * iir_a[1]; - m_filt_gyro[1].m_data = gyr_data * iir_b[0] - m_filt_gyro[0].m_data * iir_a[1]; + m_filt_accel[1].m_data = accel.m_data * iir_b[0] - m_filt_accel[0].m_data * iir_a[1]; + m_filt_gyro[1].m_data = gyro.m_data * iir_b[0] - m_filt_gyro[0].m_data * iir_a[1]; m_filt_magnetic[1].m_data = magnetic.m_data * iir_b[0] - m_filt_magnetic[0].m_data * iir_a[1]; } else { - m_filt_accel[1].m_data = acc_data; - m_filt_gyro[1].m_data = gyr_data; + m_filt_accel[1].m_data = accel.m_data; + m_filt_gyro[1].m_data = gyro.m_data; m_filt_magnetic[1].m_data = magnetic.m_data; } @@ -142,9 +127,6 @@ inline void orientation_filter::filter_sensor_data(const sensor_data m_filt_gyro[1].m_time_stamp = accel.m_time_stamp; m_filt_magnetic[1].m_time_stamp = accel.m_time_stamp; - normalize(m_filt_accel[1]); - normalize(m_filt_magnetic[1]); - m_filt_gyro[1].m_data = m_filt_gyro[1].m_data - m_bias_correction; } @@ -152,16 +134,16 @@ template inline void orientation_filter::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}; + TYPE arr_mag_e[V1x3S] = {0.0, (TYPE) m_magnetic_alignment_factor, 0.0}; - vector acc_e(V1x3S, arr_acc_e); - vector mag_e(V1x3S, arr_mag_e); + vect acc_e(V1x3S, arr_acc_e); + vect mag_e(V1x3S, arr_mag_e); - vector acc_b_x_mag_b = cross(m_filt_accel[1].m_data, m_filt_magnetic[1].m_data); - vector acc_e_x_mag_e = cross(acc_e, mag_e); + vect acc_b_x_mag_b = cross(m_filt_accel[1].m_data, m_filt_magnetic[1].m_data); + vect acc_e_x_mag_e = cross(acc_e, mag_e); - vector cross1 = cross(acc_b_x_mag_b, m_filt_accel[1].m_data); - vector cross2 = cross(acc_e_x_mag_e, acc_e); + vect cross1 = cross(acc_b_x_mag_b, m_filt_accel[1].m_data); + vect cross2 = cross(acc_e_x_mag_e, acc_e); matrix mat_b(M3X3R, M3X3C); matrix mat_e(M3X3R, M3X3C); @@ -261,7 +243,9 @@ inline void orientation_filter::time_update() orientation = quat2euler(m_quat_driv); - m_orientation = orientation.m_ang * (TYPE) DRIVING_SYSTEM_PHASE_COMPENSATION; + m_orientation.m_ang.m_vec[0] = orientation.m_ang.m_vec[0] * m_roll_phase_compensation; + m_orientation.m_ang.m_vec[1] = orientation.m_ang.m_vec[1] * m_pitch_phase_compensation; + m_orientation.m_ang.m_vec[2] = orientation.m_ang.m_vec[2] * m_yaw_phase_compensation; m_rot_matrix = quat2rot_mat(m_quat_driv); @@ -312,7 +296,7 @@ inline void orientation_filter::measurement_update() m_state_old = m_state_new; TYPE arr_bias[V1x3S] = {m_state_new.m_vec[3], m_state_new.m_vec[4], m_state_new.m_vec[5]}; - vector vec(V1x3S, arr_bias); + vect vec(V1x3S, arr_bias); m_bias_correction = vec; } diff --git a/src/sensor_fusion/standalone/util/orientation_filter.h b/src/sensor_fusion/orientation_filter.h similarity index 83% rename from src/sensor_fusion/standalone/util/orientation_filter.h rename to src/sensor_fusion/orientation_filter.h index 6f9ce37..3f97569 100644 --- a/src/sensor_fusion/standalone/util/orientation_filter.h +++ b/src/sensor_fusion/orientation_filter.h @@ -33,26 +33,31 @@ public: sensor_data m_filt_accel[2]; sensor_data m_filt_gyro[2]; sensor_data m_filt_magnetic[2]; - vector m_var_gyr_x; - vector m_var_gyr_y; - vector m_var_gyr_z; - vector m_var_roll; - vector m_var_pitch; - vector m_var_yaw; + vect m_var_gyr_x; + vect m_var_gyr_y; + vect m_var_gyr_z; + vect m_var_roll; + vect m_var_pitch; + vect m_var_yaw; matrix m_driv_cov; matrix m_aid_cov; matrix m_tran_mat; matrix m_measure_mat; matrix m_pred_cov; - vector m_state_new; - vector m_state_old; - vector m_state_error; - vector m_bias_correction; + vect m_state_new; + vect m_state_old; + vect m_state_error; + vect m_bias_correction; quaternion m_quat_aid; quaternion m_quat_driv; rotation_matrix m_rot_matrix; euler_angles m_orientation; + int m_pitch_phase_compensation; + int m_roll_phase_compensation; + int m_yaw_phase_compensation; + int m_magnetic_alignment_factor; + orientation_filter(); ~orientation_filter(); diff --git a/src/sensor_fusion/standalone/util/quaternion.cpp b/src/sensor_fusion/quaternion.cpp similarity index 96% rename from src/sensor_fusion/standalone/util/quaternion.cpp rename to src/sensor_fusion/quaternion.cpp index 2649f73..5a9148a 100755 --- a/src/sensor_fusion/standalone/util/quaternion.cpp +++ b/src/sensor_fusion/quaternion.cpp @@ -33,12 +33,12 @@ quaternion::quaternion(const TYPE w, const TYPE x, const TYPE y, const TYP { TYPE vec_data[QUAT_SIZE] = {w, x, y, z}; - vector v(QUAT_SIZE, vec_data); + vect v(QUAT_SIZE, vec_data); m_quat = v; } template -quaternion::quaternion(const vector v) +quaternion::quaternion(const vect v) { m_quat = v; } diff --git a/src/sensor_fusion/standalone/util/quaternion.h b/src/sensor_fusion/quaternion.h similarity index 96% rename from src/sensor_fusion/standalone/util/quaternion.h rename to src/sensor_fusion/quaternion.h index 6be945c..90ce4e8 100755 --- a/src/sensor_fusion/standalone/util/quaternion.h +++ b/src/sensor_fusion/quaternion.h @@ -25,11 +25,11 @@ template class quaternion { public: - vector m_quat; + vect m_quat; quaternion(); quaternion(const TYPE w, const TYPE x, const TYPE y, const TYPE z); - quaternion(const vector v); + quaternion(const vect v); quaternion(const quaternion& q); ~quaternion(); diff --git a/src/sensor_fusion/standalone/util/rotation_matrix.cpp b/src/sensor_fusion/rotation_matrix.cpp similarity index 100% rename from src/sensor_fusion/standalone/util/rotation_matrix.cpp rename to src/sensor_fusion/rotation_matrix.cpp diff --git a/src/sensor_fusion/standalone/util/rotation_matrix.h b/src/sensor_fusion/rotation_matrix.h similarity index 100% rename from src/sensor_fusion/standalone/util/rotation_matrix.h rename to src/sensor_fusion/rotation_matrix.h diff --git a/src/sensor_fusion/standalone/util/sensor_data.cpp b/src/sensor_fusion/sensor_data.cpp similarity index 95% rename from src/sensor_fusion/standalone/util/sensor_data.cpp rename to src/sensor_fusion/sensor_data.cpp index 2671be9..72b9395 100644 --- a/src/sensor_fusion/standalone/util/sensor_data.cpp +++ b/src/sensor_fusion/sensor_data.cpp @@ -34,13 +34,13 @@ sensor_data::sensor_data(const TYPE x, const TYPE y, { TYPE vec_data[SENSOR_DATA_SIZE] = {x, y, z}; - vector v(SENSOR_DATA_SIZE, vec_data); + vect 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 vect v, const unsigned long long time_stamp = 0) { m_data = v; diff --git a/src/sensor_fusion/standalone/util/sensor_data.h b/src/sensor_fusion/sensor_data.h similarity index 96% rename from src/sensor_fusion/standalone/util/sensor_data.h rename to src/sensor_fusion/sensor_data.h index c24c564..4c84bd9 100644 --- a/src/sensor_fusion/standalone/util/sensor_data.h +++ b/src/sensor_fusion/sensor_data.h @@ -25,13 +25,13 @@ template class sensor_data { public: - vector m_data; + vect m_data; unsigned long long m_time_stamp; sensor_data(); sensor_data(const TYPE x, const TYPE y, const TYPE z, const unsigned long long time_stamp); - sensor_data(const vector v, + sensor_data(const vect v, const unsigned long long time_stamp); sensor_data(const sensor_data& s); ~sensor_data(); diff --git a/src/sensor_fusion/standalone/gravity_sensor.cpp b/src/sensor_fusion/standalone/gravity_sensor.cpp index abc6ff9..8470e36 100644 --- a/src/sensor_fusion/standalone/gravity_sensor.cpp +++ b/src/sensor_fusion/standalone/gravity_sensor.cpp @@ -19,10 +19,22 @@ #ifdef _GRAVITY_SENSOR_H +#define GRAVITY 9.80665 + sensor_data gravity_sensor::get_gravity(const sensor_data accel, const sensor_data gyro, const sensor_data magnetic) { - return comp_grav.orientation2gravity(accel, gyro, magnetic); + euler_angles orientation; + sensor_data gravity; + + orientation = orien_sensor.get_orientation(accel, gyro, magnetic); + + gravity.m_data.m_vec[0] = GRAVITY * sin(orientation.m_ang.m_vec[0]); + gravity.m_data.m_vec[1] = GRAVITY * sin(orientation.m_ang.m_vec[1]); + gravity.m_data.m_vec[2] = GRAVITY * cos(orientation.m_ang.m_vec[0]) * + cos(orientation.m_ang.m_vec[1]); + + return gravity; } #endif diff --git a/src/sensor_fusion/standalone/gravity_sensor.h b/src/sensor_fusion/standalone/gravity_sensor.h index 40baf9e..780d280 100644 --- a/src/sensor_fusion/standalone/gravity_sensor.h +++ b/src/sensor_fusion/standalone/gravity_sensor.h @@ -20,12 +20,12 @@ #ifndef _GRAVITY_SENSOR_H #define _GRAVITY_SENSOR_H -#include "./util/compute_gravity.h" +#include "orientation_sensor.h" class gravity_sensor { public: - compute_gravity comp_grav; + orientation_sensor orien_sensor; sensor_data get_gravity(const sensor_data accel, const sensor_data gyro, const sensor_data magnetic); diff --git a/src/sensor_fusion/standalone/linear_acceleration_sensor.cpp b/src/sensor_fusion/standalone/linear_acceleration_sensor.cpp index ea219cc..a29a9ca 100644 --- a/src/sensor_fusion/standalone/linear_acceleration_sensor.cpp +++ b/src/sensor_fusion/standalone/linear_acceleration_sensor.cpp @@ -25,7 +25,7 @@ sensor_data linear_acceleration_sensor::get_linear_acceleration(const sen sensor_data gravity_data; float la_x, la_y, la_z; - gravity_data = comp_grav.orientation2gravity(accel, gyro, magnetic); + gravity_data = grav_sensor.get_gravity(accel, gyro, magnetic); la_x = accel.m_data.m_vec[0] - gravity_data.m_data.m_vec[1]; la_y = accel.m_data.m_vec[1] - gravity_data.m_data.m_vec[0]; diff --git a/src/sensor_fusion/standalone/linear_acceleration_sensor.h b/src/sensor_fusion/standalone/linear_acceleration_sensor.h index 81e8d91..df8a811 100644 --- a/src/sensor_fusion/standalone/linear_acceleration_sensor.h +++ b/src/sensor_fusion/standalone/linear_acceleration_sensor.h @@ -20,12 +20,12 @@ #ifndef _LINEAR_ACCELERATION_SENSOR_H #define _LINEAR_ACCELERATION_SENSOR_H -#include "./util/compute_gravity.h" +#include "gravity_sensor.h" class linear_acceleration_sensor { public: - compute_gravity comp_grav; + gravity_sensor grav_sensor; sensor_data get_linear_acceleration(const sensor_data accel, const sensor_data gyro, const sensor_data magnetic); diff --git a/src/sensor_fusion/standalone/orientation_sensor.cpp b/src/sensor_fusion/standalone/orientation_sensor.cpp index b86919d..26412c2 100644 --- a/src/sensor_fusion/standalone/orientation_sensor.cpp +++ b/src/sensor_fusion/standalone/orientation_sensor.cpp @@ -19,16 +19,64 @@ #ifdef _ORIENTATION_SENSOR_H -euler_angles orientation_sensor::get_orientation(const sensor_data accel, - const sensor_data gyro, const sensor_data magnetic) +float bias_accel[] = {0.098586, 0.18385, (10.084 - GRAVITY)}; +float bias_gyro[] = {-5.3539, 0.24325, 2.3391}; +float bias_magnetic[] = {0, 0, 0}; +int sign_accel[] = {+1, +1, +1}; +int sign_gyro[] = {+1, +1, +1}; +int sign_magnetic[] = {+1, +1, +1}; +float scale_accel = 1; +float scale_gyro = 575; +float scale_magnetic = 1; + +int pitch_phase_compensation = -1; +int roll_phase_compensation = -1; +int yaw_phase_compensation = -1; +int magnetic_alignment_factor = -1; + +void pre_process_data(sensor_data &data_out, sensor_data &data_in, float *bias, int *sign, float scale) +{ + data_out.m_data.m_vec[0] = sign[0] * (data_in.m_data.m_vec[0] - bias[0]) / scale; + data_out.m_data.m_vec[1] = sign[1] * (data_in.m_data.m_vec[1] - bias[1]) / scale; + data_out.m_data.m_vec[2] = sign[2] * (data_in.m_data.m_vec[2] - bias[2]) / scale; + + data_out.m_time_stamp = data_in.m_time_stamp; +} + +euler_angles orientation_sensor::get_orientation(sensor_data accel_data, + sensor_data gyro_data, sensor_data magnetic_data) { - return orien_filter.get_orientation(accel, gyro, magnetic); + + pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); + normalize(accel_data); + pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro); + pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); + normalize(magnetic_data); + + orien_filter.m_pitch_phase_compensation = pitch_phase_compensation; + orien_filter.m_roll_phase_compensation = roll_phase_compensation; + orien_filter.m_yaw_phase_compensation = yaw_phase_compensation; + orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor; + + return orien_filter.get_orientation(accel_data, gyro_data, magnetic_data); } -rotation_matrix orientation_sensor::get_rotation_matrix(const sensor_data accel, - const sensor_data gyro, const sensor_data magnetic) +rotation_matrix orientation_sensor::get_rotation_matrix(sensor_data accel_data, + sensor_data gyro_data, sensor_data magnetic_data) { - return orien_filter.get_rotation_matrix(accel, gyro, magnetic); + + pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); + normalize(accel_data); + pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro); + pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); + normalize(magnetic_data); + + orien_filter.m_pitch_phase_compensation = pitch_phase_compensation; + orien_filter.m_roll_phase_compensation = roll_phase_compensation; + orien_filter.m_yaw_phase_compensation = yaw_phase_compensation; + orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor; + + return orien_filter.get_rotation_matrix(accel_data, gyro_data, magnetic_data); } #endif diff --git a/src/sensor_fusion/standalone/orientation_sensor.h b/src/sensor_fusion/standalone/orientation_sensor.h index 89312c3..e670309 100644 --- a/src/sensor_fusion/standalone/orientation_sensor.h +++ b/src/sensor_fusion/standalone/orientation_sensor.h @@ -20,17 +20,17 @@ #ifndef _ORIENTATION_SENSOR_H #define _ORIENTATION_SENSOR_H -#include "./util/orientation_filter.h" +#include "../orientation_filter.h" class orientation_sensor { public: orientation_filter orien_filter; - euler_angles get_orientation(const sensor_data accel, - const sensor_data gyro, const sensor_data magnetic); - rotation_matrix get_rotation_matrix(const sensor_data accel, - const sensor_data gyro, const sensor_data magnetic); + euler_angles get_orientation(sensor_data accel, + sensor_data gyro, sensor_data magnetic); + rotation_matrix get_rotation_matrix(sensor_data accel, + sensor_data gyro, sensor_data magnetic); }; #include "orientation_sensor.cpp" diff --git a/src/sensor_fusion/standalone/util/test/euler_angles_test/.cproject b/src/sensor_fusion/standalone/test/euler_angles_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/euler_angles_test/.cproject rename to src/sensor_fusion/standalone/test/euler_angles_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/euler_angles_test/.project b/src/sensor_fusion/standalone/test/euler_angles_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/euler_angles_test/.project rename to src/sensor_fusion/standalone/test/euler_angles_test/.project diff --git a/src/sensor_fusion/standalone/util/test/euler_angles_test/euler_angles_main.cpp b/src/sensor_fusion/standalone/test/euler_angles_test/euler_angles_main.cpp similarity index 93% rename from src/sensor_fusion/standalone/util/test/euler_angles_test/euler_angles_main.cpp rename to src/sensor_fusion/standalone/test/euler_angles_test/euler_angles_main.cpp index 592e782..06e449d 100644 --- a/src/sensor_fusion/standalone/util/test/euler_angles_test/euler_angles_main.cpp +++ b/src/sensor_fusion/standalone/test/euler_angles_test/euler_angles_main.cpp @@ -17,7 +17,7 @@ * */ -#include "../../euler_angles.h" +#include "../../../euler_angles.h" int main() { @@ -26,10 +26,10 @@ int main() 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); + vect v0(3, arr0); + vect v1(3, arr1); + vect v2(4, arr2); + vect v3(4, arr3); quaternion q1(v2); quaternion q2(v3); diff --git a/src/sensor_fusion/standalone/util/test/gravity_sensor_test/.cproject b/src/sensor_fusion/standalone/test/gravity_sensor_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/gravity_sensor_test/.cproject rename to src/sensor_fusion/standalone/test/gravity_sensor_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/gravity_sensor_test/.project b/src/sensor_fusion/standalone/test/gravity_sensor_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/gravity_sensor_test/.project rename to src/sensor_fusion/standalone/test/gravity_sensor_test/.project diff --git a/src/sensor_fusion/standalone/util/test/gravity_sensor_test/gravity_sensor_main.cpp b/src/sensor_fusion/standalone/test/gravity_sensor_test/gravity_sensor_main.cpp similarity index 96% rename from src/sensor_fusion/standalone/util/test/gravity_sensor_test/gravity_sensor_main.cpp rename to src/sensor_fusion/standalone/test/gravity_sensor_test/gravity_sensor_main.cpp index 0dfeb11..a9f801c 100644 --- a/src/sensor_fusion/standalone/util/test/gravity_sensor_test/gravity_sensor_main.cpp +++ b/src/sensor_fusion/standalone/test/gravity_sensor_test/gravity_sensor_main.cpp @@ -17,14 +17,14 @@ * */ -#include "../../../gravity_sensor.h" +#include "../../gravity_sensor.h" #include #include #include #include using namespace std; -#define GRAVITY_DATA_PATH "../../../../design/data/100ms/gravity/throw/" +#define GRAVITY_DATA_PATH "../../../design/data/100ms/gravity/throw/" #define GRAVITY_DATA_SIZE 135 int main() diff --git a/src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/.cproject b/src/sensor_fusion/standalone/test/linear_acceleration_sensor_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/.cproject rename to src/sensor_fusion/standalone/test/linear_acceleration_sensor_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/.project b/src/sensor_fusion/standalone/test/linear_acceleration_sensor_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/.project rename to src/sensor_fusion/standalone/test/linear_acceleration_sensor_test/.project diff --git a/src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/linear_acceleration_sensor_main.cpp b/src/sensor_fusion/standalone/test/linear_acceleration_sensor_test/linear_acceleration_sensor_main.cpp similarity index 95% rename from src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/linear_acceleration_sensor_main.cpp rename to src/sensor_fusion/standalone/test/linear_acceleration_sensor_test/linear_acceleration_sensor_main.cpp index b5686ba..302affa 100644 --- a/src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/linear_acceleration_sensor_main.cpp +++ b/src/sensor_fusion/standalone/test/linear_acceleration_sensor_test/linear_acceleration_sensor_main.cpp @@ -17,14 +17,14 @@ * */ -#include "../../../linear_acceleration_sensor.h" +#include "../../linear_acceleration_sensor.h" #include #include #include #include using namespace std; -#define LA_DATA_PATH "../../../../design/data/100ms/linear_acceleration/move_x_y_z/" +#define LA_DATA_PATH "../../../design/data/100ms/linear_acceleration/move_x_y_z/" #define LA_DATA_SIZE 170 int main() diff --git a/src/sensor_fusion/standalone/util/test/matrix_test/.cproject b/src/sensor_fusion/standalone/test/matrix_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/matrix_test/.cproject rename to src/sensor_fusion/standalone/test/matrix_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/matrix_test/.project b/src/sensor_fusion/standalone/test/matrix_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/matrix_test/.project rename to src/sensor_fusion/standalone/test/matrix_test/.project diff --git a/src/sensor_fusion/standalone/util/test/matrix_test/matrix_main.cpp b/src/sensor_fusion/standalone/test/matrix_test/matrix_main.cpp similarity index 99% rename from src/sensor_fusion/standalone/util/test/matrix_test/matrix_main.cpp rename to src/sensor_fusion/standalone/test/matrix_test/matrix_main.cpp index ed00949..d1a2153 100644 --- a/src/sensor_fusion/standalone/util/test/matrix_test/matrix_main.cpp +++ b/src/sensor_fusion/standalone/test/matrix_test/matrix_main.cpp @@ -17,7 +17,7 @@ * */ -#include "../../matrix.h" +#include "../../../matrix.h" int main() { diff --git a/src/sensor_fusion/standalone/util/test/orientation_filter_test/.cproject b/src/sensor_fusion/standalone/test/orientation_filter_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/orientation_filter_test/.cproject rename to src/sensor_fusion/standalone/test/orientation_filter_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/orientation_filter_test/.project b/src/sensor_fusion/standalone/test/orientation_filter_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/orientation_filter_test/.project rename to src/sensor_fusion/standalone/test/orientation_filter_test/.project diff --git a/src/sensor_fusion/standalone/util/test/orientation_filter_test/orientation_filter_main.cpp b/src/sensor_fusion/standalone/test/orientation_filter_test/orientation_filter_main.cpp similarity index 64% rename from src/sensor_fusion/standalone/util/test/orientation_filter_test/orientation_filter_main.cpp rename to src/sensor_fusion/standalone/test/orientation_filter_test/orientation_filter_main.cpp index 2743f97..99bb3d9 100644 --- a/src/sensor_fusion/standalone/util/test/orientation_filter_test/orientation_filter_main.cpp +++ b/src/sensor_fusion/standalone/test/orientation_filter_test/orientation_filter_main.cpp @@ -17,16 +17,40 @@ * */ -#include "../../orientation_filter.h" +#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_PATH "../../../design/data/100ms/orientation/roll_pitch_yaw/" #define ORIENTATION_DATA_SIZE 1095 +float bias_accel[] = {0.098586, 0.18385, (10.084 - GRAVITY)}; +float bias_gyro[] = {-5.3539, 0.24325, 2.3391}; +float bias_magnetic[] = {0, 0, 0}; +int sign_accel[] = {+1, +1, +1}; +int sign_gyro[] = {+1, +1, +1}; +int sign_magnetic[] = {+1, +1, +1}; +float scale_accel = 1; +float scale_gyro = 575; +float scale_magnetic = 1; + +int pitch_phase_compensation = -1; +int roll_phase_compensation = -1; +int yaw_phase_compensation = -1; +int magnetic_alignment_factor = -1; + +void pre_process_data(sensor_data &data_out, sensor_data &data_in, float *bias, int *sign, float scale) +{ + data_out.m_data.m_vec[0] = sign[0] * (data_in.m_data.m_vec[0] - bias[0]) / scale; + data_out.m_data.m_vec[1] = sign[1] * (data_in.m_data.m_vec[1] - bias[1]) / scale; + data_out.m_data.m_vec[2] = sign[2] * (data_in.m_data.m_vec[2] - bias[2]) / scale; + + data_out.m_time_stamp = data_in.m_time_stamp; +} + int main() { int data_available = ORIENTATION_DATA_SIZE; @@ -55,8 +79,6 @@ int main() 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); @@ -64,8 +86,6 @@ int main() 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); @@ -73,8 +93,21 @@ int main() time_stamp = strtoull (token, NULL, 10); sensor_data magnetic_data(sdata[0], sdata[1], sdata[2], time_stamp); + pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); + normalize(accel_data); + pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro); + pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); + normalize(magnetic_data); + + cout << "Accel Data\t" << accel_data.m_data << "\t Time Stamp\t" << accel_data.m_time_stamp << "\n\n"; + cout << "Gyro Data\t" << gyro_data.m_data << "\t Time Stamp\t" << gyro_data.m_time_stamp << "\n\n"; cout << "Magnetic Data\t" << magnetic_data.m_data << "\t Time Stamp\t" << magnetic_data.m_time_stamp << "\n\n"; + orien_filter.m_pitch_phase_compensation = pitch_phase_compensation; + orien_filter.m_roll_phase_compensation = roll_phase_compensation; + orien_filter.m_yaw_phase_compensation = yaw_phase_compensation; + orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor; + orientation = orien_filter.get_orientation(accel_data, gyro_data, magnetic_data); orien_file << orientation.m_ang; diff --git a/src/sensor_fusion/standalone/util/test/orientation_sensor_test/.cproject b/src/sensor_fusion/standalone/test/orientation_sensor_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/orientation_sensor_test/.cproject rename to src/sensor_fusion/standalone/test/orientation_sensor_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/orientation_sensor_test/.project b/src/sensor_fusion/standalone/test/orientation_sensor_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/orientation_sensor_test/.project rename to src/sensor_fusion/standalone/test/orientation_sensor_test/.project diff --git a/src/sensor_fusion/standalone/util/test/orientation_sensor_test/orientation_sensor_main.cpp b/src/sensor_fusion/standalone/test/orientation_sensor_test/orientation_sensor_main.cpp similarity index 95% rename from src/sensor_fusion/standalone/util/test/orientation_sensor_test/orientation_sensor_main.cpp rename to src/sensor_fusion/standalone/test/orientation_sensor_test/orientation_sensor_main.cpp index c755cbe..b3ef1f5 100644 --- a/src/sensor_fusion/standalone/util/test/orientation_sensor_test/orientation_sensor_main.cpp +++ b/src/sensor_fusion/standalone/test/orientation_sensor_test/orientation_sensor_main.cpp @@ -17,14 +17,14 @@ * */ -#include "../../../orientation_sensor.h" +#include "../../orientation_sensor.h" #include #include #include #include using namespace std; -#define ORIENTATION_DATA_PATH "../../../../design/data/100ms/orientation/roll_pitch_yaw/" +#define ORIENTATION_DATA_PATH "../../../design/data/100ms/orientation/roll_pitch_yaw/" #define ORIENTATION_DATA_SIZE 1095 int main() diff --git a/src/sensor_fusion/standalone/util/test/quaternion_test/.cproject b/src/sensor_fusion/standalone/test/quaternion_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/quaternion_test/.cproject rename to src/sensor_fusion/standalone/test/quaternion_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/quaternion_test/.project b/src/sensor_fusion/standalone/test/quaternion_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/quaternion_test/.project rename to src/sensor_fusion/standalone/test/quaternion_test/.project diff --git a/src/sensor_fusion/standalone/util/test/quaternion_test/quaternion_main.cpp b/src/sensor_fusion/standalone/test/quaternion_test/quaternion_main.cpp similarity index 96% rename from src/sensor_fusion/standalone/util/test/quaternion_test/quaternion_main.cpp rename to src/sensor_fusion/standalone/test/quaternion_test/quaternion_main.cpp index e2e371f..d2ec86b 100644 --- a/src/sensor_fusion/standalone/util/test/quaternion_test/quaternion_main.cpp +++ b/src/sensor_fusion/standalone/test/quaternion_test/quaternion_main.cpp @@ -17,15 +17,15 @@ * */ -#include "../../quaternion.h" +#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); + vect v0(4, arr0); + vect v1(4, arr1); quaternion q0(v0); quaternion q1(v1); diff --git a/src/sensor_fusion/standalone/util/test/rotation_matrix_test/.cproject b/src/sensor_fusion/standalone/test/rotation_matrix_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/rotation_matrix_test/.cproject rename to src/sensor_fusion/standalone/test/rotation_matrix_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/rotation_matrix_test/.project b/src/sensor_fusion/standalone/test/rotation_matrix_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/rotation_matrix_test/.project rename to src/sensor_fusion/standalone/test/rotation_matrix_test/.project diff --git a/src/sensor_fusion/standalone/util/test/rotation_matrix_test/rotation_matrix_main.cpp b/src/sensor_fusion/standalone/test/rotation_matrix_test/rotation_matrix_main.cpp similarity index 98% rename from src/sensor_fusion/standalone/util/test/rotation_matrix_test/rotation_matrix_main.cpp rename to src/sensor_fusion/standalone/test/rotation_matrix_test/rotation_matrix_main.cpp index fa1b639..21ecadf 100644 --- a/src/sensor_fusion/standalone/util/test/rotation_matrix_test/rotation_matrix_main.cpp +++ b/src/sensor_fusion/standalone/test/rotation_matrix_test/rotation_matrix_main.cpp @@ -17,7 +17,7 @@ * */ -#include "../../rotation_matrix.h" +#include "../../../rotation_matrix.h" int main() { diff --git a/src/sensor_fusion/standalone/util/test/sensor_data_test/.cproject b/src/sensor_fusion/standalone/test/sensor_data_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/sensor_data_test/.cproject rename to src/sensor_fusion/standalone/test/sensor_data_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/sensor_data_test/.project b/src/sensor_fusion/standalone/test/sensor_data_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/sensor_data_test/.project rename to src/sensor_fusion/standalone/test/sensor_data_test/.project diff --git a/src/sensor_fusion/standalone/util/test/sensor_data_test/sensor_data_main.cpp b/src/sensor_fusion/standalone/test/sensor_data_test/sensor_data_main.cpp similarity index 88% rename from src/sensor_fusion/standalone/util/test/sensor_data_test/sensor_data_main.cpp rename to src/sensor_fusion/standalone/test/sensor_data_test/sensor_data_main.cpp index 511e949..ef7e5e7 100644 --- a/src/sensor_fusion/standalone/util/test/sensor_data_test/sensor_data_main.cpp +++ b/src/sensor_fusion/standalone/test/sensor_data_test/sensor_data_main.cpp @@ -17,13 +17,13 @@ * */ -#include "../../sensor_data.h" +#include "../../../sensor_data.h" int main() { float arr1[3] = {1.04, -4.678, -2.34}; - vector v1(3, arr1); + vect v1(3, arr1); sensor_data sd1(2.0, 3.0, 4.0, 140737488355328); sensor_data sd2(1.04, -4.678, -2.34); @@ -45,12 +45,12 @@ int main() 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); + normalize(sd3); + cout<< "\nResult:\n" << sd3.m_data << endl; cout<< "\n" << sd2.m_data; - cout<< "\nResult:\n" << sd7.m_data << endl; + normalize(sd2); + cout<< "\nResult:\n" << sd2.m_data << endl; float xx = 2.5; cout<<"\n\n\nScale data:\n"; diff --git a/src/sensor_fusion/standalone/util/test/vector_test/.cproject b/src/sensor_fusion/standalone/test/vector_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/vector_test/.cproject rename to src/sensor_fusion/standalone/test/vector_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/vector_test/.project b/src/sensor_fusion/standalone/test/vector_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/vector_test/.project rename to src/sensor_fusion/standalone/test/vector_test/.project diff --git a/src/sensor_fusion/standalone/util/test/vector_test/vector_main.cpp b/src/sensor_fusion/standalone/test/vector_test/vector_main.cpp similarity index 90% rename from src/sensor_fusion/standalone/util/test/vector_test/vector_main.cpp rename to src/sensor_fusion/standalone/test/vector_test/vector_main.cpp index 725934f..befdeee 100644 --- a/src/sensor_fusion/standalone/util/test/vector_test/vector_main.cpp +++ b/src/sensor_fusion/standalone/test/vector_test/vector_main.cpp @@ -17,7 +17,7 @@ * */ -#include "../../vector.h" +#include "../../../vector.h" int main() { @@ -31,19 +31,19 @@ int main() float arr43[6] = {2.3454,-0.0384,-8.90,3.455,6.785,21.345}; float arr5[5] = {0.2,-0.4,0.6,-0.8,1.0}; - vector v1(5, arr0); - vector v2(5, arr8); - vector v10(4, arr3); - vector v12(4, arr4); - vector v15(6, arr1); - vector v20(6, arr43); - vector v21(3, arr2); - vector v22(3, arr15); - vector v3(4); - vector v6(3); - vector v13(5); - vector v95(6); - vector v35(5, arr5); + vect v1(5, arr0); + vect v2(5, arr8); + vect v10(4, arr3); + vect v12(4, arr4); + vect v15(6, arr1); + vect v20(6, arr43); + vect v21(3, arr2); + vect v22(3, arr15); + vect v3(4); + vect v6(3); + vect v13(5); + vect v95(6); + vect v35(5, arr5); float arr57[3][3] = {{2.24, 0.5, 0.023}, {3.675, 5.32, 0.556}, {1.023, 45.75, 621.6}}; matrix m12(3, 3, (float *) arr57); @@ -102,7 +102,7 @@ int main() cout<< "\nProduct:\n"<< m102 << endl; cout<< "\n\n\nVector x Multiplication matrix:\n"; - vector v102 = (v22 * m12); + vect v102 = (v22 * m12); cout<< "\n" << v22 << "\n" << m12; cout<< "\nProduct:\n" << v102 << endl; float val = mul(v22, m32); @@ -163,7 +163,7 @@ int main() cout<< "\nOutput:\n" << v3 << endl; - vector v111 = cross(v21, v22); + vect v111 = cross(v21, v22); cout<< "\n\n\nCross Product:"; cout << "\n\n" << v21 << "\n\n" << v22; cout << "\nResult:\n\n" << v111; diff --git a/src/sensor_fusion/standalone/util/compute_gravity.cpp b/src/sensor_fusion/standalone/util/compute_gravity.cpp deleted file mode 100644 index 3cf6e19..0000000 --- a/src/sensor_fusion/standalone/util/compute_gravity.cpp +++ /dev/null @@ -1,57 +0,0 @@ -/* - * 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 deleted file mode 100644 index 745eb5c..0000000 --- a/src/sensor_fusion/standalone/util/compute_gravity.h +++ /dev/null @@ -1,39 +0,0 @@ -/* - * 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 deleted file mode 100644 index 88e1d8a..0000000 --- a/src/sensor_fusion/standalone/util/test/compute_gravity_test/.cproject +++ /dev/null @@ -1,112 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/sensor_fusion/standalone/util/test/compute_gravity_test/.project b/src/sensor_fusion/standalone/util/test/compute_gravity_test/.project deleted file mode 100644 index 79f09e9..0000000 --- a/src/sensor_fusion/standalone/util/test/compute_gravity_test/.project +++ /dev/null @@ -1,89 +0,0 @@ - - - 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 deleted file mode 100644 index bd56100..0000000 --- a/src/sensor_fusion/standalone/util/test/compute_gravity_test/compute_gravity_main.cpp +++ /dev/null @@ -1,92 +0,0 @@ -/* - * 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; -} - diff --git a/src/sensor_fusion/standalone/util/vector.h b/src/sensor_fusion/standalone/util/vector.h deleted file mode 100644 index 21566ea..0000000 --- a/src/sensor_fusion/standalone/util/vector.h +++ /dev/null @@ -1,74 +0,0 @@ -/* - * 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" - -template -class vector { -public: - int m_size; - TYPE *m_vec; - - vector(void); - vector(const int size); - vector(const int size, TYPE *vec_data); - vector(const vector& v); - ~vector(); - - vector operator =(const vector& v); - - template friend ostream& operator << (ostream& dout, - vector& v); - template friend vector operator +(const vector v1, - const vector v2); - template friend vector operator +(const vector v, - const T val); - template friend vector operator -(const vector v1, - const vector v2); - template friend vector operator -(const vector v, - const T val); - template friend matrix operator *(const matrix v1, - const vector v2); - template friend vector operator *(const vector v, - const matrix m); - template friend vector operator *(const vector v, - const T val); - template friend vector operator /(const vector v1, - const T val); - template friend bool operator ==(const vector v1, - const vector v2); - template friend bool operator !=(const vector v1, - const vector v2); - - template friend T mul(const vector v, const matrix m); - template friend void insert_end(vector& v, T val); - template friend matrix transpose(const vector v); - template friend vector transpose(const matrix m); - template friend vector cross(const vector v1, - const vector v2); - template friend T var(const vector v); - template friend bool is_initialized(const vector v); -}; - -#include "vector.cpp" - -#endif /* _VECTOR_H */ diff --git a/src/sensor_fusion/standalone/util/vector.cpp b/src/sensor_fusion/vector.cpp similarity index 75% rename from src/sensor_fusion/standalone/util/vector.cpp rename to src/sensor_fusion/vector.cpp index e063d6e..bf2ee65 100644 --- a/src/sensor_fusion/standalone/util/vector.cpp +++ b/src/sensor_fusion/vector.cpp @@ -20,13 +20,13 @@ #if defined (_VECTOR_H) && defined (_MATRIX_H) template -vector::vector(void) +vect::vect(void) { m_vec = NULL; } template -vector::vector(const int size) +vect::vect(const int size) { m_size = size; m_vec = NULL; @@ -34,7 +34,7 @@ vector::vector(const int size) } template -vector::vector(const int size, TYPE *vec_data) +vect::vect(const int size, TYPE *vec_data) { m_size = size; m_vec = NULL; @@ -45,7 +45,7 @@ vector::vector(const int size, TYPE *vec_data) } template -vector::vector(const vector& v) +vect::vect(const vect& v) { m_size = v.m_size; m_vec = NULL; @@ -56,14 +56,14 @@ vector::vector(const vector& v) } template -vector::~vector() +vect::~vect() { if (m_vec != NULL) delete[] m_vec; } template -vector vector::operator =(const vector& v) +vect vect::operator =(const vect& v) { if (this == &v) { @@ -95,7 +95,7 @@ vector vector::operator =(const vector& v) } template -ostream& operator <<(ostream& dout, vector& v) +ostream& operator <<(ostream& dout, vect& v) { for (int j = 0; j < v.m_size; j++) { @@ -108,11 +108,11 @@ ostream& operator <<(ostream& dout, vector& v) } template -vector operator +(const vector v1, const vector v2) +vect operator +(const vect v1, const vect v2) { assert(v1.m_size == v2.m_size); - vector v3(v1.m_size); + vect v3(v1.m_size); for (int j = 0; j < v1.m_size; j++) v3.m_vec[j] = v1.m_vec[j] + v2.m_vec[j]; @@ -121,9 +121,9 @@ vector operator +(const vector v1, const vector v2) } template -vector operator +(const vector v, const T val) +vect operator +(const vect v, const T val) { - vector v1(v.m_size); + vect v1(v.m_size); for (int j = 0; j < v.m_size; j++) v1.m_vec[j] = v.m_vec[j] + val; @@ -132,11 +132,11 @@ vector operator +(const vector v, const T val) } template -vector operator -(const vector v1, const vector v2) +vect operator -(const vect v1, const vect v2) { assert(v1.m_size == v2.m_size); - vector v3(v1.m_size); + vect v3(v1.m_size); for (int j = 0; j < v1.m_size; j++) v3.m_vec[j] = v1.m_vec[j] - v2.m_vec[j]; @@ -145,9 +145,9 @@ vector operator -(const vector v1, const vector v2) } template -vector operator -(const vector v, const T val) +vect operator -(const vect v, const T val) { - vector v1(v.m_size); + vect v1(v.m_size); for (int j = 0; j < v.m_size; j++) v1.m_vec[j] = v.m_vec[j] - val; @@ -156,7 +156,7 @@ vector operator -(const vector v, const T val) } template -matrix operator *(const matrix m, const vector v) +matrix operator *(const matrix m, const vect v) { assert(m.m_rows == v.m_size); assert(m.m_cols == 1); @@ -175,12 +175,12 @@ matrix operator *(const matrix m, const vector v) } template -vector operator *(const vector v, const matrix m) +vect operator *(const vect v, const matrix m) { assert(m.m_rows == v.m_size); assert(m.m_cols != 1); - vector v1(m.m_cols); + vect v1(m.m_cols); for (int j = 0; j < m.m_cols; j++) { @@ -193,9 +193,9 @@ vector operator *(const vector v, const matrix m) } template -vector operator *(const vector v, const T val) +vect operator *(const vect v, const T val) { - vector v1(v.m_size); + vect v1(v.m_size); for (int j = 0; j < v.m_size; j++) v1.m_vec[j] = v.m_vec[j] * val; @@ -204,9 +204,9 @@ vector operator *(const vector v, const T val) } template -vector operator /(const vector v, const T val) +vect operator /(const vect v, const T val) { - vector v1(v.m_size); + vect v1(v.m_size); for (int j = 0; j < v.m_size; j++) v1.m_vec[j] = v.m_vec[j] / val; @@ -215,7 +215,7 @@ vector operator /(const vector v, const T val) } template -bool operator ==(const vector v1, const vector v2) +bool operator ==(const vect v1, const vect v2) { if (v1.m_size == v2.m_size) { @@ -230,13 +230,13 @@ bool operator ==(const vector v1, const vector v2) } template -bool operator !=(const vector v1, const vector v2) +bool operator !=(const vect v1, const vect v2) { return (!(v1 == v2)); } template -matrix transpose(const vector v) +matrix transpose(const vect v) { matrix m(v.m_size, 1); @@ -247,9 +247,9 @@ matrix transpose(const vector v) } template -vector transpose(const matrix m) +vect transpose(const matrix m) { - vector v(m.m_rows); + vect v(m.m_rows); for (int i = 0; i < m.m_rows; i++) v.m_vec[i] = m.m_mat[i][0]; @@ -258,7 +258,7 @@ vector transpose(const matrix m) } template -T mul(const vector v, const matrix m) +T mul(const vect v, const matrix m) { assert(m.m_rows == v.m_size); assert(m.m_cols == 1); @@ -273,7 +273,7 @@ T mul(const vector v, const matrix m) template -void insert_end(vector& v, T val) +void insert_end(vect& v, T val) { for (int i = 0; i < (v.m_size - 1); i++) v.m_vec[i] = v.m_vec[i+1]; @@ -282,9 +282,9 @@ void insert_end(vector& v, T val) } template -vector cross(const vector v1, const vector v2) +vect cross(const vect v1, const vect v2) { - vector v3(v1.m_size); + vect v3(v1.m_size); 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])); @@ -294,9 +294,9 @@ vector cross(const vector v1, const vector v2) } template -bool is_initialized(const vector v) +bool is_initialized(const vect v) { - vector v1(v.m_size); + vect v1(v.m_size); bool retval; retval = (v == v1) ? false : true; @@ -305,7 +305,7 @@ bool is_initialized(const vector v) } template -T var(const vector v) +T var(const vect v) { T val = 0; T mean, var, diff; diff --git a/src/sensor_fusion/vector.h b/src/sensor_fusion/vector.h new file mode 100644 index 0000000..4c3eaf1 --- /dev/null +++ b/src/sensor_fusion/vector.h @@ -0,0 +1,74 @@ +/* + * 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" + +template +class vect { +public: + int m_size; + TYPE *m_vec; + + vect(void); + vect(const int size); + vect(const int size, TYPE *vec_data); + vect(const vect& v); + ~vect(); + + vect operator =(const vect& v); + + template friend ostream& operator << (ostream& dout, + vect& v); + template friend vect operator +(const vect v1, + const vect v2); + template friend vect operator +(const vect v, + const T val); + template friend vect operator -(const vect v1, + const vect v2); + template friend vect operator -(const vect v, + const T val); + template friend matrix operator *(const matrix v1, + const vect v2); + template friend vect operator *(const vect v, + const matrix m); + template friend vect operator *(const vect v, + const T val); + template friend vect operator /(const vect v1, + const T val); + template friend bool operator ==(const vect v1, + const vect v2); + template friend bool operator !=(const vect v1, + const vect v2); + + template friend T mul(const vect v, const matrix m); + template friend void insert_end(vect& v, T val); + template friend matrix transpose(const vect v); + template friend vect transpose(const matrix m); + template friend vect cross(const vect v1, + const vect v2); + template friend T var(const vect v); + template friend bool is_initialized(const vect v); +}; + +#include "vector.cpp" + +#endif /* _VECTOR_H */ diff --git a/src/shared/CMakeLists.txt b/src/shared/CMakeLists.txt index 34867b3..5ec473e 100755 --- a/src/shared/CMakeLists.txt +++ b/src/shared/CMakeLists.txt @@ -50,7 +50,6 @@ add_library(sensord-server SHARED sensor_base.cpp physical_sensor.cpp virtual_sensor.cpp - sensor_fusion.cpp iio_common.cpp ) @@ -72,7 +71,6 @@ install(TARGETS sensord-share DESTINATION lib) install(FILES sensord-server.pc DESTINATION lib/pkgconfig) install(FILES ${PROJECT_NAME}.pc DESTINATION lib/pkgconfig) install(FILES - sensor_fusion.h crw_lock.h worker_thread.h cconfig.h @@ -83,7 +81,6 @@ install(FILES sensor_base.h physical_sensor.h virtual_sensor.h - sensor_fusion.h sf_common.h cpacket.h csocket.h diff --git a/src/shared/csensor_event_dispatcher.cpp b/src/shared/csensor_event_dispatcher.cpp index db5d8c0..8df89fa 100755 --- a/src/shared/csensor_event_dispatcher.cpp +++ b/src/shared/csensor_event_dispatcher.cpp @@ -31,7 +31,6 @@ using std::thread; csensor_event_dispatcher::csensor_event_dispatcher() : m_lcd_on(true) { - m_sensor_fusion = sensor_plugin_loader::get_instance().get_fusion(); } csensor_event_dispatcher::~csensor_event_dispatcher() { } @@ -152,11 +151,6 @@ void csensor_event_dispatcher::dispatch_event(void) unsigned int event_cnt = 0; sensor_events[event_cnt++] = *((sensor_event_t *)seed_event); - if (m_sensor_fusion) { - if (m_sensor_fusion->is_started()) - m_sensor_fusion->fuse(*((sensor_event_t *)seed_event)); - } - virtual_sensors v_sensors = get_active_virtual_sensors(); virtual_sensors::iterator it_v_sensor; it_v_sensor = v_sensors.begin(); diff --git a/src/shared/csensor_event_dispatcher.h b/src/shared/csensor_event_dispatcher.h index 569d134..1e21037 100755 --- a/src/shared/csensor_event_dispatcher.h +++ b/src/shared/csensor_event_dispatcher.h @@ -23,7 +23,6 @@ #include #include #include -#include #include #include #include @@ -41,7 +40,6 @@ private: event_type_last_event_map m_last_events; virtual_sensors m_active_virtual_sensors; cmutex m_active_virtual_sensors_mutex; - sensor_fusion *m_sensor_fusion; csensor_event_dispatcher(); ~csensor_event_dispatcher(); diff --git a/src/shared/iio_common.h b/src/shared/iio_common.h index 0f456dc..9f00c13 100644 --- a/src/shared/iio_common.h +++ b/src/shared/iio_common.h @@ -20,7 +20,20 @@ #ifndef IIO_COMMON_H_ #define IIO_COMMON_H_ -struct channel_parameters { +#include + +#define NO_OF_ULL_BYTES 8 +#define NO_OF_SHORT_VAL 4 +#define CH0_INDEX 0 +#define CH1_INDEX 1 + +#define GET_DIFF_BIT(val) (((unsigned short)(val) >> 7) & 0x01) +#define GET_DIR_VAL(val) ((val) & 0x0F) + +#define IOCTL_IIO_EVENT_FD _IOR('i', 0x90, int) + +struct channel_parameters +{ char *prefix_str; float scale; float offset; @@ -35,6 +48,27 @@ struct channel_parameters { unsigned int buf_index; }; +typedef struct iio_event_struct +{ + unsigned long long int event_id; + long long int timestamp; +} iio_event_t; + +typedef enum event_id_field +{ + CH_TYPE = 4, + MODIFIER, + DIRECTION, + EVENT_TYPE, +} event_id_field_t; + +typedef union ull_bytes +{ + unsigned long long num; + short int channels[NO_OF_SHORT_VAL]; + unsigned char bytes[NO_OF_ULL_BYTES]; +} ull_bytes_t; + void sort_channels_by_index(struct channel_parameters *channels, int count); int decode_channel_data_type(const char *device_dir, const char *ch_name, struct channel_parameters *ch_info); int add_channel_to_array(const char *device_dir, const char *ch_name, struct channel_parameters *channel); diff --git a/src/shared/sensor_fusion.cpp b/src/shared/sensor_fusion.cpp deleted file mode 100755 index 1189575..0000000 --- a/src/shared/sensor_fusion.cpp +++ /dev/null @@ -1,98 +0,0 @@ -/* - * libsensord-share - * - * 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_fusion::sensor_fusion() -{ -} - -sensor_fusion::~sensor_fusion() -{ -} - -bool sensor_fusion::is_data_ready(void) -{ - return true; -} - -bool sensor_fusion::get_properties(sensor_properties_t &properties) -{ - return false; -} - -bool sensor_fusion::add_interval(int client_id, unsigned int interval) -{ - return true; -} - -bool sensor_fusion::delete_interval(int client_id) -{ - return true; -} - -bool sensor_fusion::get_rotation_matrix(arr33_t &rot) -{ - return false; -} - -bool sensor_fusion::get_attitude(float &x, float &y, float &z, float &w) -{ - return false; -} - -bool sensor_fusion::get_gyro_bias(float &x, float &y, float &z) -{ - return false; -} - -bool sensor_fusion::get_rotation_vector(float &x, float &y, float &z, float &w, float &heading_accuracy) -{ - return false; -} - -bool sensor_fusion::get_linear_acceleration(float &x, float &y, float &z) -{ - return false; -} - -bool sensor_fusion::get_gravity(float &x, float &y, float &z) -{ - return false; -} - -bool sensor_fusion::get_rotation_vector_6axis(float &x, float &y, float &z, float &w, float &heading_accuracy) -{ - return false; -} - -bool sensor_fusion::get_geomagnetic_rotation_vector(float &x, float &y, float &z, float &w) -{ - return false; -} - -bool sensor_fusion::get_orientation(float &azimuth, float &pitch, float &roll) -{ - return false; -} - -bool sensor_fusion::is_fusion(void) -{ - return true; -} diff --git a/src/shared/sensor_fusion.h b/src/shared/sensor_fusion.h deleted file mode 100755 index 3375dcb..0000000 --- a/src/shared/sensor_fusion.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * libsensord-share - * - * 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_FUSION_H_ -#define _SENSOR_FUSION_H_ - -#include -#include - -typedef std::array , 3> arr33_t; - -class sensor_fusion : public sensor_base -{ -public: - sensor_fusion(); - virtual ~sensor_fusion(); - - virtual void fuse(const sensor_event_t &event) = 0; - virtual bool is_data_ready(void); - virtual bool add_interval(int client_id, unsigned int interval); - virtual bool delete_interval(int client_id); - virtual bool get_properties(sensor_properties_t &properties); - - virtual bool get_rotation_matrix(arr33_t &rot); - virtual bool get_attitude(float &x, float &y, float &z, float &w); - virtual bool get_gyro_bias(float &x, float &y, float &z); - virtual bool get_rotation_vector(float &x, float &y, float &z, float &w, float &heading_accuracy); - virtual bool get_linear_acceleration(float &x, float &y, float &z); - virtual bool get_gravity(float &x, float &y, float &z); - virtual bool get_rotation_vector_6axis(float &x, float &y, float &z, float &w, float &heading_accuracy); - virtual bool get_geomagnetic_rotation_vector(float &x, float &y, float &z, float &w); - virtual bool get_orientation(float &azimuth, float &pitch, float &roll); - - bool is_fusion(void); -}; - -#endif /*_SENSOR_FUSION_H_*/ diff --git a/src/shared/sensor_plugin_loader.cpp b/src/shared/sensor_plugin_loader.cpp index 368278a..23f8d7b 100755 --- a/src/shared/sensor_plugin_loader.cpp +++ b/src/shared/sensor_plugin_loader.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -111,10 +110,7 @@ bool sensor_plugin_loader::insert_module(const char *node_name, const char *path DBG("init [%s] module", module->get_name()); sensor_type_t sensor_type = module->get_type(); - if (module->is_fusion()) - m_fusions.push_back((sensor_fusion *) module); - else - m_sensors.insert(make_pair(sensor_type, module)); + m_sensors.insert(make_pair(sensor_type, module)); } return true; @@ -288,19 +284,6 @@ vector sensor_plugin_loader::get_virtual_sensors(void) return virtual_list; } -sensor_fusion *sensor_plugin_loader::get_fusion(void) -{ - if (m_fusions.empty()) - return NULL; - - return m_fusions.front(); -} - -vector sensor_plugin_loader::get_fusions(void) -{ - return m_fusions; -} - bool sensor_plugin_loader::destroy() { sensor_base *sensor; diff --git a/src/shared/sensor_plugin_loader.h b/src/shared/sensor_plugin_loader.h index fd380df..041aa62 100755 --- a/src/shared/sensor_plugin_loader.h +++ b/src/shared/sensor_plugin_loader.h @@ -29,7 +29,6 @@ class sensor_hal; class sensor_base; -class sensor_fusion; using std::pair; using std::vector; @@ -57,15 +56,6 @@ typedef multimap sensor_plugins; * */ -typedef vector fusion_plugins; -/* -* a fusion_plugins is a group of fusion plugin -* -* ... -* -* -*/ - class sensor_plugin_loader { private: @@ -77,7 +67,6 @@ private: sensor_hal_plugins m_sensor_hals; sensor_plugins m_sensors; - fusion_plugins m_fusions; public: static sensor_plugin_loader &get_instance() { @@ -94,9 +83,6 @@ public: vector get_virtual_sensors(void); - sensor_fusion *get_fusion(void); - vector get_fusions(void); - bool destroy(); }; #endif /*_SENSOR_PLUGIN_LOADER_H_*/ diff --git a/src/shared/virtual_sensor.h b/src/shared/virtual_sensor.h index 2fd1bf2..34112aa 100755 --- a/src/shared/virtual_sensor.h +++ b/src/shared/virtual_sensor.h @@ -29,6 +29,7 @@ public: virtual ~virtual_sensor(); virtual void synthesize(const sensor_event_t &event, vector &outs) = 0; + virtual int get_sensor_data(const unsigned int event_type, sensor_data_t &data) = 0; bool is_virtual(void); protected: diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..c9d75d4 --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,40 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 2.6) +project(sensor-tc C) + +SET(PREFIX ${CMAKE_INSTALL_PREFIX}) +SET(EXEC_PREFIX "\${prefix}") +SET(LIBDIR "\${prefix}/lib") +SET(INCLUDEDIR "\${prefix}/include") +SET(VERSION 1.0) + +INCLUDE(FindPkgConfig) +pkg_check_modules(pkgs REQUIRED glib-2.0 dlog) + +add_definitions(${rpkgs_CFLAGS}) +add_definitions(-DPREFIX="${CMAKE_INSTALL_PREFIX}") + +configure_file(${PROJECT_NAME}.pc.in ${CMAKE_CURRENT_SOURCE_DIR}/${PROJECT_NAME}.pc @ONLY) + +include_directories(${CMAKE_CURRENT_SOURCE_DIR}) +include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) +include_directories(${CMAKE_SOURCE_DIR}/src/shared) + +FOREACH(flag ${pkgs_CFLAGS}) + SET(EXTRA_CFLAGS "${EXTRA_CFLAGS} ${flag}") +ENDFOREACH(flag) + +SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${EXTRA_CFLAGS}") + +link_directories(../src/libsensord/) + +add_executable(accelerometer src/accelerometer.c) + +SET_TARGET_PROPERTIES(accelerometer PROPERTIES LINKER_LANGUAGE C) + +target_link_libraries(accelerometer glib-2.0 dlog sensor) + +INSTALL(TARGETS accelerometer DESTINATION /usr/bin/) + + + + diff --git a/test/sensor-tc.pc b/test/sensor-tc.pc new file mode 100644 index 0000000..33b2994 --- /dev/null +++ b/test/sensor-tc.pc @@ -0,0 +1,13 @@ +# Package Information for pkg-config + +prefix=/usr/bin +exec_prefix=${prefix} +libdir=${prefix}/lib +includedir=${prefix}/include/sensor-tc + +Name: sensor-tc +Description: Sensor functional testing library +Version: 1.0 +Requires: +Libs: -L${libdir} -lsensor-tc +Cflags: -I${includedir} diff --git a/test/sensor-tc.pc.in b/test/sensor-tc.pc.in new file mode 100644 index 0000000..0f821b6 --- /dev/null +++ b/test/sensor-tc.pc.in @@ -0,0 +1,13 @@ +# Package Information for pkg-config + +prefix=@PREFIX@ +exec_prefix=@EXEC_PREFIX@ +libdir=@LIBDIR@ +includedir=@INCLUDEDIR@/sensor-tc + +Name: sensor-tc +Description: Sensor functional testing library +Version: @VERSION@ +Requires: +Libs: -L${libdir} -lsensor-tc +Cflags: -I${includedir} diff --git a/test/src/accelerometer.c b/test/src/accelerometer.c new file mode 100644 index 0000000..fd2bc5e --- /dev/null +++ b/test/src/accelerometer.c @@ -0,0 +1,105 @@ +#include +#include +#include +#include +#include +#include + +static GMainLoop *mainloop; + +void callback(unsigned int event_type, sensor_event_data_t *event, void *user_data) +{ + sensor_data_t *data = (sensor_data_t *)event->event_data; + printf("Accelerometer [%6.6f] [%6.6f] [%6.6f] [%lld]\n\n", data->values[0], data->values[1], data->values[2], data->timestamp); +} + +void printformat() +{ + printf("Usage : ./accelerometer (optional)\n\n"); + printf("event:\n"); + printf("ROTATION_CHECK\n"); + printf("RAW_DATA_REPORT_ON_TIME\n"); + printf("CALIBRATION_NEEDED\n"); + printf("SET_HORIZON\n"); + printf("SET_WAKEUP\n"); + printf("ORIENTATION_DATA_REPORT_ON_TIME\n"); + printf("LINEAR_ACCELERATION_DATA_REPORT_ON_TIME\n"); + printf("GRAVITY_DATA_REPORT_ON_TIME\n\n"); + printf("interval:\n"); + printf("The time interval should be entered based on the sampling frequency supported by accelerometer driver on the device in ms.If no value for sensor is entered default value by the driver will be used.\n"); +} + +int main(int argc,char **argv) +{ + int result, handle; + unsigned int event; + bool error_state = FALSE; + + mainloop = g_main_loop_new(NULL, FALSE); + sensor_type_t type = ACCELEROMETER_SENSOR; + event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); + event_condition->cond_op = CONDITION_EQUAL; + event_condition->cond_value1=100; + + if (argc != 2 && argc != 3) { + printformat(); + error_state = TRUE; + } + else { + if (strcmp(argv[1], "ROTATION_CHECK") == 0) + event = ACCELEROMETER_EVENT_ROTATION_CHECK; + else if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) + event = ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME; + else if (strcmp(argv[1], "CALIBRATION_NEEDED") == 0) + event = ACCELEROMETER_EVENT_CALIBRATION_NEEDED; + else if (strcmp(argv[1], "SET_HORIZON") == 0) + event = ACCELEROMETER_EVENT_SET_HORIZON; + else if (strcmp(argv[1], "SET_WAKEUP") == 0) + event = ACCELEROMETER_EVENT_SET_WAKEUP; + else if (strcmp(argv[1], "ORIENTATION_DATA_REPORT_ON_TIME") == 0) + event = ACCELEROMETER_EVENT_ORIENTATION_DATA_REPORT_ON_TIME; + else if (strcmp(argv[1], "LINEAR_ACCELERATION_DATA_REPORT_ON_TIME") == 0) + event = ACCELEROMETER_EVENT_LINEAR_ACCELERATION_DATA_REPORT_ON_TIME; + else if (strcmp(argv[1], "GRAVITY_DATA_REPORT_ON_TIME") == 0) + event = ACCELEROMETER_EVENT_GRAVITY_DATA_REPORT_ON_TIME; + else { + printformat(); + error_state = TRUE; + } + if (argc == 3) + event_condition->cond_value1 = atof(argv[2]); + } + + if (!error_state) { + handle = sf_connect(type); + result = sf_register_event(handle, event, event_condition, callback, NULL); + + if (result < 0) + printf("Can't register accelerometer\n"); + + if (!(sf_start(handle,0) < 0)) { + printf("Success start \n"); + } + else { + printf("Error\n\n\n\n"); + sf_unregister_event(handle, event); + sf_disconnect(handle); + return -1; + } + + g_main_loop_run(mainloop); + g_main_loop_unref(mainloop); + + sf_unregister_event(handle, event); + + if (!(sf_stop(handle) < 0)) + printf("Success stop \n"); + + sf_disconnect(handle); + } + + free(event_condition); + + return 0; +} +