From c66be7a6d20ece66faadc66de4bfe0ca459582e0 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Tue, 20 Jan 2015 16:29:34 +0530 Subject: [PATCH 01/16] Cleanup of unused virtual sensor class variables Removing all class variables added to store virtual sensor data as this is not used anymore by get_sensor_data method. Change-Id: Ifd878de73ab819f80b7042f02e1bc407a4427bc1 --- src/gravity/gravity_sensor.cpp | 16 +++------------- src/gravity/gravity_sensor.h | 3 --- src/linear_accel/linear_accel_sensor.cpp | 14 ++------------ src/linear_accel/linear_accel_sensor.h | 3 --- src/orientation/orientation_sensor.cpp | 15 +++------------ src/orientation/orientation_sensor.h | 3 --- src/rotation_vector/rv/rv_sensor.cpp | 17 +++-------------- src/rotation_vector/rv/rv_sensor.h | 4 ---- 8 files changed, 11 insertions(+), 64 deletions(-) diff --git a/src/gravity/gravity_sensor.cpp b/src/gravity/gravity_sensor.cpp index c0a767a..69afaa4 100755 --- a/src/gravity/gravity_sensor.cpp +++ b/src/gravity/gravity_sensor.cpp @@ -53,9 +53,6 @@ gravity_sensor::gravity_sensor() : m_orientation_sensor(NULL) -, m_x(INITIAL_VALUE) -, m_y(INITIAL_VALUE) -, m_z(INITIAL_VALUE) , m_time(0) { cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); @@ -187,6 +184,8 @@ void gravity_sensor::synthesize(const sensor_event_t &event, vector= (M_PI/2)-DEVIATION && roll <= (M_PI/2)+DEVIATION) || @@ -205,19 +204,10 @@ void gravity_sensor::synthesize(const sensor_event_t &event, vector m_accel; sensor_data m_gravity; - float m_x; - float m_y; - float m_z; unsigned long long m_time; unsigned int m_interval; diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp index fe8118d..7629132 100755 --- a/src/orientation/orientation_sensor.cpp +++ b/src/orientation/orientation_sensor.cpp @@ -78,9 +78,6 @@ 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_azimuth(INITIAL_VALUE) , m_time(0) { cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); @@ -355,10 +352,12 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vector & quaternion_orientation = m_orientation.get_9axis_quaternion(m_accel, m_gyro, m_magnetic); } + m_time = get_timestamp(); + rv_event.sensor_id = get_id(); rv_event.event_type = ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME; rv_event.data.accuracy = SENSOR_ACCURACY_GOOD; - rv_event.data.timestamp = get_timestamp(); + rv_event.data.timestamp = m_time; rv_event.data.value_count = 4; rv_event.data.values[0] = quaternion_orientation.m_quat.m_vec[1]; rv_event.data.values[1] = quaternion_orientation.m_quat.m_vec[2]; @@ -325,15 +323,6 @@ void rv_sensor::synthesize(const sensor_event_t& event, vector & rv_event.data.values[3] = quaternion_orientation.m_quat.m_vec[0]; push(rv_event); - - { - AUTOLOCK(m_value_mutex); - m_time = rv_event.data.value_count; - m_x = rv_event.data.values[0]; - m_y = rv_event.data.values[1]; - m_z = rv_event.data.values[2]; - m_w = rv_event.data.values[3]; - } } return; diff --git a/src/rotation_vector/rv/rv_sensor.h b/src/rotation_vector/rv/rv_sensor.h index 04b42b6..ee7cfab 100755 --- a/src/rotation_vector/rv/rv_sensor.h +++ b/src/rotation_vector/rv/rv_sensor.h @@ -55,10 +55,6 @@ private: unsigned int m_enable_orientation; - float m_x; - float m_y; - float m_z; - float m_w; int m_accuracy; unsigned long long m_time; unsigned int m_interval; -- 2.7.4 From 882de21481824f13f429b55f5513217197638601 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Tue, 20 Jan 2015 16:32:02 +0530 Subject: [PATCH 02/16] Adding sensor event check for get_sensor_data RV sensor Added code to check event type for RV sensor get_sensor_data method Change-Id: Ifc9ed8f3cab0fe24ab41296267a10f3b0fde560e --- src/rotation_vector/rv/rv_sensor.cpp | 5 ++++- src/rotation_vector/rv/rv_sensor.h | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/rotation_vector/rv/rv_sensor.cpp b/src/rotation_vector/rv/rv_sensor.cpp index 80cfce7..7846940 100755 --- a/src/rotation_vector/rv/rv_sensor.cpp +++ b/src/rotation_vector/rv/rv_sensor.cpp @@ -328,7 +328,7 @@ void rv_sensor::synthesize(const sensor_event_t& event, vector & return; } -int rv_sensor::get_sensor_data(unsigned int data_id, sensor_data_t &data) +int rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &data) { sensor_data accel; sensor_data gyro; @@ -340,6 +340,9 @@ int rv_sensor::get_sensor_data(unsigned int data_id, sensor_data_t &data) quaternion quaternion_orientation; + if (event_type != ROTATION_VECTOR_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); diff --git a/src/rotation_vector/rv/rv_sensor.h b/src/rotation_vector/rv/rv_sensor.h index ee7cfab..003c3fa 100755 --- a/src/rotation_vector/rv/rv_sensor.h +++ b/src/rotation_vector/rv/rv_sensor.h @@ -38,7 +38,7 @@ public: bool get_properties(sensor_properties_s &properties); sensor_type_t get_type(void); - 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); private: sensor_base *m_accel_sensor; -- 2.7.4 From e3646a0aa9f081a96b151144ce7d39985de20d2c Mon Sep 17 00:00:00 2001 From: Stephane Desneux Date: Mon, 19 Jan 2015 16:20:39 +0100 Subject: [PATCH 03/16] fix build for buxton Change-Id: I93eb443bada926aaaf105029bd33cb205d93c419 Signed-off-by: Stephane Desneux --- packaging/sensord.spec | 1 - 1 file changed, 1 deletion(-) diff --git a/packaging/sensord.spec b/packaging/sensord.spec index c097f02..ecec809 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -9,7 +9,6 @@ Source1: sensord.manifest Source2: libsensord.manifest BuildRequires: cmake -BuildRequires: vconf-keys-devel BuildRequires: libattr-devel BuildRequires: pkgconfig(dlog) BuildRequires: pkgconfig(libxml-2.0) -- 2.7.4 From 87a0eff6cb0f36d968578bddd16af901f68b8982 Mon Sep 17 00:00:00 2001 From: Ankur Date: Thu, 22 Jan 2015 17:31:30 +0530 Subject: [PATCH 04/16] Removed Compiler warning - comparison between signed and unsigned integers -Changed int to unsigned int in permission_checker.cpp to remove warning about comparison between signed and unsigned integers. Change-Id: I3b7b4fe6bfbd303d173633a0035115017974776a --- src/server/permission_checker.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/server/permission_checker.cpp b/src/server/permission_checker.cpp index cca3fef..aec5aef 100755 --- a/src/server/permission_checker.cpp +++ b/src/server/permission_checker.cpp @@ -78,7 +78,7 @@ void permission_checker::init() vector sensors; sensors = sensor_plugin_loader::get_instance().get_sensors(ALL_SENSOR); - for (int i = 0; i < sensors.size(); ++i) + for (unsigned int i = 0; i < sensors.size(); ++i) m_permission_set |= sensors[i]->get_permission(); INFO("Permission Set = %d", m_permission_set); @@ -91,7 +91,7 @@ int permission_checker::get_permission(int sock_fd) { int permission = SENSOR_PERMISSION_NONE; - for (int i = 0; i < m_permission_infos.size(); ++i) { + for (unsigned int i = 0; i < m_permission_infos.size(); ++i) { if (!m_permission_infos[i]->need_to_check) { permission |= m_permission_infos[i]->permission; } else if ((m_permission_set & m_permission_infos[i]->permission) && security_server_check_privilege_by_sockfd) { -- 2.7.4 From 33a40709f3c14b366204925f0bddd2b87c73bb70 Mon Sep 17 00:00:00 2001 From: Kibak Yoon Date: Fri, 23 Jan 2015 14:47:54 +0900 Subject: [PATCH 05/16] sensord: modify the macro for avoiding build-break In both of CAPI and Internal API, declaration of sensor_option_e is duplicated and the macro should be changed by proper name Change-Id: I9b19791b44247d9684dba05845131ecbc41f3917 Signed-off-by: Kibak Yoon --- src/shared/sensor_common.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/shared/sensor_common.h b/src/shared/sensor_common.h index 6cd4bf1..113e924 100755 --- a/src/shared/sensor_common.h +++ b/src/shared/sensor_common.h @@ -140,7 +140,7 @@ enum sensor_accuracy_t { /* * To prevent naming confliction as using same enums as sensor CAPI use */ -#ifndef __SENSORS_H__ +#ifndef __SENSOR_H__ enum sensor_option_t { SENSOR_OPTION_DEFAULT = 0, SENSOR_OPTION_ON_IN_SCREEN_OFF = 1, -- 2.7.4 From 49e889cb2d2a244805306dfd23155d88ad9012c6 Mon Sep 17 00:00:00 2001 From: Kibak Yoon Date: Thu, 29 Jan 2015 19:59:47 +0900 Subject: [PATCH 06/16] sensord: fix the bug about terminating null byte when using strncpy when termination was forced, the index was wrong. Change-Id: I7966527a5bece6bb4a09ceec63d4eec3f751b13b Signed-off-by: Kibak Yoon --- src/shared/csocket.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/shared/csocket.cpp b/src/shared/csocket.cpp index e862b63..96ff1d5 100755 --- a/src/shared/csocket.cpp +++ b/src/shared/csocket.cpp @@ -103,9 +103,9 @@ bool csocket::bind (const char *sock_path) } m_addr.sun_family = AF_UNIX; - int path_size = strlen(sock_path); - strncpy(m_addr.sun_path, sock_path, path_size); - m_addr.sun_path[path_size - 1] = '\0'; + + strncpy(m_addr.sun_path, sock_path, sizeof(m_addr.sun_path)); + m_addr.sun_path[sizeof(m_addr.sun_path)-1] = '\0'; length = strlen(m_addr.sun_path) + sizeof(m_addr.sun_family); @@ -303,9 +303,9 @@ bool csocket::connect(const char *sock_path) set_blocking_mode(false); m_addr.sun_family = AF_UNIX; - int path_size = strlen(sock_path); - strncpy(m_addr.sun_path, sock_path, path_size); - m_addr.sun_path[path_size - 1] = '\0'; + + strncpy(m_addr.sun_path, sock_path, sizeof(m_addr.sun_path)); + m_addr.sun_path[sizeof(m_addr.sun_path)-1] = '\0'; addr_len = strlen(m_addr.sun_path) + sizeof(m_addr.sun_family); -- 2.7.4 From c725c7f74328b85164d48231f8e861fd1355aa92 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 12 Feb 2015 09:44:23 +0530 Subject: [PATCH 07/16] Removing vconf dependencies from sensor plugin build files Removing vconf dependencies from various sensor plugins where vconf functionality is not used. This is to reduce hanging during build. Change-Id: I2f7fcc8af349f240bb88c8e578d7c556c0bb89e5 --- src/accel/CMakeLists.txt | 3 --- src/geo/CMakeLists.txt | 3 --- src/gravity/CMakeLists.txt | 3 --- src/gyro/CMakeLists.txt | 3 --- src/light/CMakeLists.txt | 3 --- src/linear_accel/CMakeLists.txt | 3 --- src/orientation/CMakeLists.txt | 3 --- src/pressure/CMakeLists.txt | 3 --- src/proxi/CMakeLists.txt | 3 --- src/rotation_vector/rv/CMakeLists.txt | 3 --- src/ultraviolet/CMakeLists.txt | 3 --- 11 files changed, 33 deletions(-) diff --git a/src/accel/CMakeLists.txt b/src/accel/CMakeLists.txt index a37c89c..2537c1c 100755 --- a/src/accel/CMakeLists.txt +++ b/src/accel/CMakeLists.txt @@ -7,9 +7,6 @@ SET(SENSOR_HAL_NAME accel_sensor_hal) include_directories(${CMAKE_CURRENT_SOURCE_DIR}) include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) -INCLUDE(FindPkgConfig) -PKG_CHECK_MODULES(accel_pkgs REQUIRED vconf) - FOREACH(flag ${accel_pkgs_LDFLAGS}) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") ENDFOREACH(flag) diff --git a/src/geo/CMakeLists.txt b/src/geo/CMakeLists.txt index c6d4aa1..8a04867 100755 --- a/src/geo/CMakeLists.txt +++ b/src/geo/CMakeLists.txt @@ -7,9 +7,6 @@ SET(SENSOR_HAL_NAME geo_sensor_hal) include_directories(${CMAKE_CURRENT_SOURCE_DIR}) include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) -INCLUDE(FindPkgConfig) -PKG_CHECK_MODULES(geo_pkgs REQUIRED vconf) - FOREACH(flag ${geo_pkgs_LDFLAGS}) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") ENDFOREACH(flag) diff --git a/src/gravity/CMakeLists.txt b/src/gravity/CMakeLists.txt index e66964e..910bf2e 100755 --- a/src/gravity/CMakeLists.txt +++ b/src/gravity/CMakeLists.txt @@ -6,9 +6,6 @@ SET(SENSOR_NAME gravity_sensor) include_directories(${CMAKE_CURRENT_SOURCE_DIR}) include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) -INCLUDE(FindPkgConfig) -PKG_CHECK_MODULES(gravity_pkgs REQUIRED vconf) - FOREACH(flag ${gravity_pkgs_LDFLAGS}) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") ENDFOREACH(flag) diff --git a/src/gyro/CMakeLists.txt b/src/gyro/CMakeLists.txt index 228fac9..1cfef0c 100755 --- a/src/gyro/CMakeLists.txt +++ b/src/gyro/CMakeLists.txt @@ -7,9 +7,6 @@ SET(SENSOR_HAL_NAME gyro_sensor_hal) include_directories(${CMAKE_CURRENT_SOURCE_DIR}) include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) -INCLUDE(FindPkgConfig) -PKG_CHECK_MODULES(gyro_pkgs REQUIRED vconf) - FOREACH(flag ${gyro_pkgs_LDFLAGS}) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") ENDFOREACH(flag) diff --git a/src/light/CMakeLists.txt b/src/light/CMakeLists.txt index 099ff82..9945df2 100755 --- a/src/light/CMakeLists.txt +++ b/src/light/CMakeLists.txt @@ -7,9 +7,6 @@ SET(SENSOR_HAL_NAME light_sensor_hal) include_directories(${CMAKE_CURRENT_SOURCE_DIR}) include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) -INCLUDE(FindPkgConfig) -PKG_CHECK_MODULES(light_pkgs REQUIRED vconf) - FOREACH(flag ${light_pkgs_LDFLAGS}) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") ENDFOREACH(flag) diff --git a/src/linear_accel/CMakeLists.txt b/src/linear_accel/CMakeLists.txt index 986fb11..fbc726f 100755 --- a/src/linear_accel/CMakeLists.txt +++ b/src/linear_accel/CMakeLists.txt @@ -7,9 +7,6 @@ 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(linear_accel_pkgs REQUIRED vconf) - FOREACH(flag ${linear_accel_pkgs_LDFLAGS}) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") ENDFOREACH(flag) diff --git a/src/orientation/CMakeLists.txt b/src/orientation/CMakeLists.txt index 6f02e9a..5418674 100755 --- a/src/orientation/CMakeLists.txt +++ b/src/orientation/CMakeLists.txt @@ -7,9 +7,6 @@ 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(orientation_pkgs REQUIRED vconf) - FOREACH(flag ${orientation_pkgs_LDFLAGS}) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") ENDFOREACH(flag) diff --git a/src/pressure/CMakeLists.txt b/src/pressure/CMakeLists.txt index e61aca4..85a8e9b 100755 --- a/src/pressure/CMakeLists.txt +++ b/src/pressure/CMakeLists.txt @@ -7,9 +7,6 @@ SET(SENSOR_HAL_NAME pressure_sensor_hal) include_directories(${CMAKE_CURRENT_SOURCE_DIR}) include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) -INCLUDE(FindPkgConfig) -PKG_CHECK_MODULES(pressure_pkgs REQUIRED vconf) - FOREACH(flag ${pressure_pkgs_LDFLAGS}) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") ENDFOREACH(flag) diff --git a/src/proxi/CMakeLists.txt b/src/proxi/CMakeLists.txt index 7cd4517..bed7b2d 100755 --- a/src/proxi/CMakeLists.txt +++ b/src/proxi/CMakeLists.txt @@ -7,9 +7,6 @@ SET(SENSOR_HAL_NAME proxi_sensor_hal) include_directories(${CMAKE_CURRENT_SOURCE_DIR}) include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) -INCLUDE(FindPkgConfig) -PKG_CHECK_MODULES(proxi_pkgs REQUIRED vconf) - FOREACH(flag ${proxi_pkgs_LDFLAGS}) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") ENDFOREACH(flag) diff --git a/src/rotation_vector/rv/CMakeLists.txt b/src/rotation_vector/rv/CMakeLists.txt index d26d4f9..1549c6f 100755 --- a/src/rotation_vector/rv/CMakeLists.txt +++ b/src/rotation_vector/rv/CMakeLists.txt @@ -7,9 +7,6 @@ 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(rv_pkgs REQUIRED vconf) - FOREACH(flag ${rv_pkgs_LDFLAGS}) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") ENDFOREACH(flag) diff --git a/src/ultraviolet/CMakeLists.txt b/src/ultraviolet/CMakeLists.txt index da4ac07..2ec2925 100644 --- a/src/ultraviolet/CMakeLists.txt +++ b/src/ultraviolet/CMakeLists.txt @@ -7,9 +7,6 @@ SET(SENSOR_HAL_NAME ultraviolet_sensor_hal) include_directories(${CMAKE_CURRENT_SOURCE_DIR}) include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) -INCLUDE(FindPkgConfig) -PKG_CHECK_MODULES(uv_pkgs REQUIRED vconf) - FOREACH(flag ${uv_pkgs_LDFLAGS}) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") ENDFOREACH(flag) -- 2.7.4 From 2486dd24c5d8bc0b249ed480e621e922ba04da8a Mon Sep 17 00:00:00 2001 From: Kibak Yoon Date: Mon, 16 Feb 2015 22:44:32 +0900 Subject: [PATCH 08/16] sensord: add sensor types for HRM sensor Heart Rate Monitor sensor type is added and supported on sensor fw. Change-Id: If0ec9fd72b5658d13f5a61f9bf2902c663812fdc Signed-off-by: Kibak Yoon --- src/shared/sensor_common.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/shared/sensor_common.h b/src/shared/sensor_common.h index 113e924..311ca62 100755 --- a/src/shared/sensor_common.h +++ b/src/shared/sensor_common.h @@ -70,6 +70,9 @@ typedef enum { HUMIDITY_SENSOR, ULTRAVIOLET_SENSOR, DUST_SENSOR, + BIO_LED_GREEN_SENSOR, + BIO_LED_IR_SENSOR, + BIO_LED_RED_SENSOR, RV_RAW_SENSOR, UNCAL_GYROSCOPE_SENSOR, UNCAL_GEOMAGNETIC_SENSOR -- 2.7.4 From 734a5743ae8a434c661a90c72478678a5165a2e8 Mon Sep 17 00:00:00 2001 From: Ankur Date: Thu, 12 Feb 2015 17:57:12 +0530 Subject: [PATCH 09/16] Optimization of template structure - Feature merge from devel/tizen branch This is one of the four patches being submitted to merge the changes made to devel/tizen to tizen branch. This contains the feature where the template structure of the vector and matrix classes was changed. Patch Set 3: Changes related to coding guidelines as per the comments Build has been checked for armv7l and aarch64 for public repo. Change-Id: Ia26d66b1ca351a22de92b858f6b60dbfee242d42 --- src/sensor_fusion/euler_angles.cpp | 35 +++- src/sensor_fusion/euler_angles.h | 8 +- src/sensor_fusion/matrix.cpp | 211 ++++++-------------- src/sensor_fusion/matrix.h | 56 +++--- src/sensor_fusion/orientation_filter.cpp | 123 ++++++------ src/sensor_fusion/orientation_filter.h | 47 +++-- src/sensor_fusion/quaternion.cpp | 26 ++- src/sensor_fusion/quaternion.h | 9 +- src/sensor_fusion/rotation_matrix.cpp | 12 +- src/sensor_fusion/rotation_matrix.h | 9 +- src/sensor_fusion/sensor_data.cpp | 23 ++- src/sensor_fusion/sensor_data.h | 9 +- .../euler_angles_test/euler_angles_main.cpp | 13 +- .../test/test_projects/matrix_test/matrix_main.cpp | 34 ++-- .../quaternion_test/quaternion_main.cpp | 15 +- .../rotation_matrix_test/rotation_matrix_main.cpp | 8 +- .../sensor_data_test/sensor_data_main.cpp | 12 +- .../test/test_projects/vector_test/vector_main.cpp | 87 +++++---- src/sensor_fusion/vector.cpp | 213 +++++++-------------- src/sensor_fusion/vector.h | 68 +++---- 20 files changed, 462 insertions(+), 556 deletions(-) diff --git a/src/sensor_fusion/euler_angles.cpp b/src/sensor_fusion/euler_angles.cpp index 8964dfb..c3bfbe5 100644 --- a/src/sensor_fusion/euler_angles.cpp +++ b/src/sensor_fusion/euler_angles.cpp @@ -20,12 +20,11 @@ #include -#define EULER_SIZE 3 #define RAD2DEG 57.2957795 #define DEG2RAD 0.0174532925 template -euler_angles::euler_angles() : m_ang(EULER_SIZE) +euler_angles::euler_angles() : m_ang() { } @@ -34,12 +33,12 @@ euler_angles::euler_angles(const TYPE roll, const TYPE pitch, const TYPE a { TYPE euler_data[EULER_SIZE] = {roll, pitch, azimuth}; - vect v(EULER_SIZE, euler_data); + vect v(euler_data); m_ang = v; } template -euler_angles::euler_angles(const vect v) +euler_angles::euler_angles(const vect v) { m_ang = v; } @@ -94,15 +93,35 @@ euler_angles quat2euler(const quaternion q) template euler_angles rad2deg(const euler_angles e) { - euler_angles result(e.m_ang * (T) RAD2DEG); - return result; + return (e.m_ang * (T) RAD2DEG); } template euler_angles deg2rad(const euler_angles e) { - euler_angles result(e.m_ang * (T) DEG2RAD); - return result; + return (e.m_ang * (T) DEG2RAD); +} + +template +quaternion euler2quat(euler_angles euler) { + T theta = euler.m_ang.m_vec[0]; + T phi = euler.m_ang.m_vec[1]; + T psi = euler.m_ang.m_vec[2]; + + T R[ROT_MAT_ROWS][ROT_MAT_COLS]; + R[0][0] = cos(psi)*cos(theta); + R[0][1] = -sin(psi)*cos(phi) + cos(psi)*sin(theta)*sin(phi); + R[0][2] = sin(psi)*sin(phi) + cos(psi)*sin(theta)*cos(phi); + R[1][0] = sin(psi)*cos(theta); + R[1][1] = cos(psi)*cos(phi) + sin(psi)*sin(theta)*sin(phi); + R[1][2] = -cos(psi)*sin(phi) + sin(psi)*sin(theta)*cos(phi); + R[2][0] = -sin(theta); + R[2][1] = cos(theta)*sin(phi); + R[2][2] = cos(theta)*cos(phi); + + rotation_matrix rot_mat(R); + quaternion q = rot_mat2quat(rot_mat); + return q; } #endif //_EULER_ANGLES_H_ diff --git a/src/sensor_fusion/euler_angles.h b/src/sensor_fusion/euler_angles.h index 9c5af06..061dfb1 100644 --- a/src/sensor_fusion/euler_angles.h +++ b/src/sensor_fusion/euler_angles.h @@ -22,21 +22,25 @@ #include "vector.h" #include "quaternion.h" +#include "rotation_matrix.h" + +#define EULER_SIZE 3 template class euler_angles { public: - vect m_ang; + vect m_ang; euler_angles(); euler_angles(const TYPE roll, const TYPE pitch, const TYPE azimuth); - euler_angles(const vect v); + euler_angles(const vect v); euler_angles(const euler_angles& e); ~euler_angles(); euler_angles operator =(const euler_angles& e); template friend euler_angles quat2euler(const quaternion q); + template friend quaternion euler2quat(euler_angles euler); template friend euler_angles rad2deg(const euler_angles e); template friend euler_angles deg2rad(const euler_angles e); }; diff --git a/src/sensor_fusion/matrix.cpp b/src/sensor_fusion/matrix.cpp index 9eabcea..9d483f2 100755 --- a/src/sensor_fusion/matrix.cpp +++ b/src/sensor_fusion/matrix.cpp @@ -19,111 +19,50 @@ #ifdef _MATRIX_H_ -template -matrix::matrix(void) +TYPE_ROW_COL matrix::matrix(void) { - m_mat = NULL; + for (int i = 0; i < ROW; i++) + for (int j = 0; j < COL; j++) + m_mat[i][j] = 0; } -template -matrix::matrix(const int rows, const int cols) +TYPE_ROW_COL matrix::matrix(const matrix& m) { - m_rows = rows; - m_cols = cols; - m_mat = new TYPE *[m_rows]; - - for (int i = 0; i < m_rows; i++) - m_mat[i] = new TYPE [m_cols](); -} - -template -matrix::matrix(const matrix& m) -{ - m_rows = m.m_rows; - m_cols = m.m_cols; - m_mat = new TYPE *[m_rows]; - - for (int i = 0; i < m_rows; i++) - m_mat[i] = new TYPE [m_cols]; - - for (int p = 0; p < m_rows; p++) - for (int q = 0; q < m_cols; q++) + for (int p = 0; p < ROW; p++) + for (int q = 0; q < COL; q++) m_mat[p][q] = m.m_mat[p][q]; } -template -matrix::matrix(const int rows, const int cols, TYPE *mat_data) +TYPE_ROW_COL matrix::matrix(TYPE mat_data[ROW][COL]) { - m_rows = rows; - m_cols = cols; - m_mat = new TYPE *[m_rows]; - - for (int i = 0; i < m_rows; i++) - m_mat[i] = new TYPE [m_cols]; - - for (int i = 0; i < m_rows; i++) - for (int j = 0; j < m_cols; j++) - m_mat[i][j] = *mat_data++; + for (int i = 0; i < ROW; i++) + for (int j = 0; j < COL; j++) + m_mat[i][j] = mat_data[i][j]; } -template -matrix::~matrix() +TYPE_ROW_COL matrix::~matrix() { - if (m_mat != NULL) - { - for (int i = 0; i < m_rows; i++) - delete[] m_mat[i]; - delete[] m_mat; - } } -template -matrix matrix::operator =(const matrix& m) +TYPE_ROW_COL matrix matrix::operator =(const matrix& m) { if (this == &m) { return *this; } - if (m_mat == NULL) - { - m_rows = m.m_rows; - m_cols = m.m_cols; - m_mat = new TYPE *[m_rows]; - - for (int i = 0; i < m_rows; i++) - m_mat[i] = new TYPE [m_cols]; - } - else - { - if ((m_rows != m.m_rows) || (m_cols != m.m_cols)) - { - for (int i = 0; i < m_rows; i++) - delete[] m_mat[i]; - delete[] m_mat; - - m_rows = m.m_rows; - m_cols = m.m_cols; - m_mat = new TYPE *[m_rows]; - - for (int i = 0; i < m_rows; i++) - m_mat[i] = new TYPE [m_cols]; - } - } - - for (int p = 0; p < m_rows; p++) - for (int q = 0; q < m_cols; q++) + for (int p = 0; p < ROW; p++) + for (int q = 0; q < COL; q++) m_mat[p][q] = m.m_mat[p][q]; return *this; } -template -ostream& operator <<(ostream& dout, matrix& m) +T_R_C ostream& operator <<(ostream& dout, matrix& m) { - for (int i = 0; i < m.m_rows; i++) + for (int i = 0; i < R; i++) { - for (int j = 0; j < m.m_cols; j++) + for (int j = 0; j < C; j++) { dout << m.m_mat[i][j] << "\t"; } @@ -132,74 +71,60 @@ ostream& operator <<(ostream& dout, matrix& m) return dout; } -template -matrix operator +(const matrix m1, const matrix m2) +T_R_C matrix operator +(const matrix m1, const matrix m2) { - assert(m1.m_rows == m2.m_rows); - assert(m1.m_cols == m2.m_cols); - - matrix m3(m1.m_rows, m1.m_cols); + matrix m3; - for (int i = 0; i < m1.m_rows; i++) - for (int j = 0; j < m1.m_cols; j++) + for (int i = 0; i < R; i++) + for (int j = 0; j < C; j++) m3.m_mat[i][j] = m1.m_mat[i][j] + m2.m_mat[i][j]; return m3; } -template -matrix operator +(const matrix m, const T val) +T_R_C matrix operator +(const matrix m, const T val) { - matrix m1(m.m_rows, m.m_cols); + matrix m1; - for (int i = 0; i < m.m_rows; i++) - for (int j = 0; j < m.m_cols; j++) + for (int i = 0; i < R; i++) + for (int j = 0; j < C; j++) m1.m_mat[i][j] = m.m_mat[i][j] + val; return m1; } -template -matrix operator -(const matrix m1, const matrix m2) +T_R_C matrix operator -(const matrix m1, const matrix m2) { - assert(m1.m_rows == m2.m_rows); - assert(m1.m_cols == m2.m_cols); + matrix m3; - matrix m3(m1.m_rows, m1.m_cols); - - for (int i = 0; i < m1.m_rows; i++) - for (int j = 0; j < m1.m_cols; j++) + for (int i = 0; i < R; i++) + for (int j = 0; j < C; j++) m3.m_mat[i][j] = m1.m_mat[i][j] - m2.m_mat[i][j]; return m3; } -template -matrix operator -(const matrix m, const T val) +T_R_C matrix operator -(const matrix m, const T val) { - matrix m1(m.m_rows, m.m_cols); + matrix m1; - for (int i = 0; i < m.m_rows; i++) - for (int j = 0; j < m.m_cols; j++) + for (int i = 0; i < R; i++) + for (int j = 0; j < C; j++) m1.m_mat[i][j] = m.m_mat[i][j] - val; return m1; } -template -matrix operator *(const matrix m1, const matrix m2) +T_R_C_C2 matrix operator *(const matrix m1, const matrix m2) { - assert(m1.m_rows == m2.m_cols); - assert(m1.m_cols == m2.m_rows); - - matrix m3(m1.m_rows, m2.m_cols); + matrix m3; - for (int i = 0; i < m1.m_rows; i++) + for (int i = 0; i < R; i++) { - for (int j = 0; j < m2.m_cols; j++) + for (int j = 0; j < C2; j++) { m3.m_mat[i][j] = 0; - for (int k = 0; k < m2.m_rows; k++) + for (int k = 0; k < C; k++) m3.m_mat[i][j] += m1.m_mat[i][k] * m2.m_mat[k][j]; } } @@ -207,37 +132,34 @@ matrix operator *(const matrix m1, const matrix m2) return m3; } -template -matrix operator *(const matrix m, const T val) +T_R_C matrix operator *(const matrix m, const T val) { - matrix m1(m.m_rows, m.m_cols); + matrix m1; - for (int i = 0; i < m.m_rows; i++) - for (int j = 0; j < m.m_cols; j++) + for (int i = 0; i < R; i++) + for (int j = 0; j < C; j++) m1.m_mat[i][j] = m.m_mat[i][j] * val; return m1; } -template -matrix operator /(const matrix m1, const T val) +T_R_C matrix operator /(const matrix m1, const T val) { - matrix m3(m1.m_rows, m1.m_cols); + matrix m3; - for (int i = 0; i < m1.m_rows; i++) - for (int j = 0; j < m1.m_cols; j++) + for (int i = 0; i < R; i++) + for (int j = 0; j < C; j++) m3.m_mat[i][j] = m1.m_mat[i][j] / val; return m3; } -template -bool operator ==(const matrix m1, const matrix m2) +T_R1_C1_R2_C2 bool operator ==(const matrix m1, const matrix m2) { - if ((m1.m_rows == m2.m_rows) && (m1.m_cols == m2.m_cols)) + if ((R1 == R2) && (C1 == C2)) { - for (int i = 0; i < m1.m_rows; i++) - for (int j = 0; j < m2.m_cols; j++) + for (int i = 0; i < R1; i++) + for (int j = 0; j < C2; j++) if (m1.m_mat[i][j] != m2.m_mat[i][j]) return false; } @@ -247,41 +169,20 @@ bool operator ==(const matrix m1, const matrix m2) return true; } -template -bool operator !=(const matrix m1, const matrix m2) +T_R1_C1_R2_C2 bool operator !=(const matrix m1, const matrix m2) { return (!(m1 == m2)); } -template -matrix tran(const matrix m) +T_R_C matrix tran(const matrix m) { - matrix m1(m.m_cols, m.m_rows); + matrix m1; - for (int i = 0; i < m.m_rows; i++) - for (int j = 0; j < m.m_cols; j++) + for (int i = 0; i < R; i++) + for (int j = 0; j < C; j++) m1.m_mat[j][i] = m.m_mat[i][j]; return m1; } - -template -matrix mul(const matrix m1, const matrix m2) -{ - assert(m2.m_cols == 1); - assert(m1.m_cols == m2.m_rows); - - matrix m3(m1.m_rows, 1); - - for (int i = 0; i < m1.m_rows; i++) - { - m3.m_mat[i][0] = 0; - for (int k = 0; k < m2.m_rows; k++) - m3.m_mat[i][0] += m1.m_mat[i][k] * m2.m_mat[k][0]; - } - - return m3; -} - #endif //_MATRIX_H_ diff --git a/src/sensor_fusion/matrix.h b/src/sensor_fusion/matrix.h index 05030a0..b2c47bb 100755 --- a/src/sensor_fusion/matrix.h +++ b/src/sensor_fusion/matrix.h @@ -24,45 +24,33 @@ #include using namespace std; -template -class matrix { +#define TYPE_ROW_COL template +#define T_R_C template +#define T_R_C_C2 template +#define T_R1_C1_R2_C2 template + +TYPE_ROW_COL class matrix { public: - int m_rows; - int m_cols; - TYPE **m_mat; + TYPE m_mat[ROW][COL]; matrix(void); - matrix(const int rows, const int cols); - matrix(const int rows, const int cols, TYPE *mat_data); - matrix(const matrix& m); + matrix(TYPE mat_data[ROW][COL]); + matrix(const matrix& m); ~matrix(); - matrix operator =(const matrix& m); - - template friend ostream& operator << (ostream& dout, - matrix& m); - template friend matrix operator +(const matrix m1, - const matrix m2); - template friend matrix operator +(const matrix m, - const T val); - template friend matrix operator -(const matrix m1, - const matrix m2); - template friend matrix operator -(const matrix m, - const T val); - template friend matrix operator *(const matrix m1, - const matrix m2); - template friend matrix operator *(const matrix m, - const T val); - template friend matrix operator /(const matrix m1, - const T val); - template friend bool operator ==(const matrix m1, - const matrix m2); - template friend bool operator !=(const matrix m1, - const matrix m2); - - template friend matrix tran(const matrix m); - template friend matrix mul(const matrix m1, - const matrix m2); + matrix operator =(const matrix& m); + + T_R_C friend ostream& operator << (ostream& dout, matrix& m); + T_R_C friend matrix operator +(const matrix m1, const matrix m2); + T_R_C friend matrix operator +(const matrix m, const T val); + T_R_C friend matrix operator -(const matrix m1, const matrix m2); + T_R_C friend matrix operator -(const matrix m, const T val); + T_R_C_C2 friend matrix operator *(const matrix m1, const matrix m2); + T_R_C friend matrix operator *(const matrix m, const T val); + T_R_C friend matrix operator /(const matrix m1, const T val); + T_R1_C1_R2_C2 friend bool operator ==(const matrix m1, const matrix m2); + T_R1_C1_R2_C2 friend bool operator !=(const matrix m1, const matrix m2); + T_R_C friend matrix tran(const matrix m); }; #include "matrix.cpp" diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index b197f91..d9e353f 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -44,14 +44,6 @@ #define ABS(val) (((val) < 0) ? -(val) : (val)) -// M-matrix, V-vector, MxN=> matrix dimension, R-RowCount, C-Column count -#define M3X3R 3 -#define M3X3C 3 -#define M6X6R 6 -#define M6X6C 6 -#define V1x3S 3 -#define V1x4S 4 -#define V1x6S 6 template orientation_filter::orientation_filter() @@ -60,10 +52,7 @@ orientation_filter::orientation_filter() std::fill_n(arr, MOVING_AVERAGE_WINDOW_LENGTH, NON_ZERO_VAL); - vect vec(MOVING_AVERAGE_WINDOW_LENGTH, arr); - vect vec1x3(V1x3S); - vect vec1x6(V1x6S); - matrix mat6x6(M6X6R, M6X6C); + vect vec(arr); m_var_gyr_x = vec; m_var_gyr_y = vec; @@ -72,17 +61,6 @@ orientation_filter::orientation_filter() m_var_pitch = vec; m_var_azimuth = vec; - m_tran_mat = mat6x6; - m_measure_mat = mat6x6; - m_pred_cov = mat6x6; - m_driv_cov = mat6x6; - m_aid_cov = mat6x6; - - m_bias_correction = vec1x3; - m_state_new = vec1x6; - m_state_old = vec1x6; - m_state_error = vec1x6; - m_pitch_phase_compensation = 1; m_roll_phase_compensation = 1; m_azimuth_phase_compensation = 1; @@ -97,13 +75,12 @@ orientation_filter::~orientation_filter() } template -inline void orientation_filter::initialize_sensor_data(const sensor_data accel, +inline void orientation_filter::init_accel_gyro_mag_data(const sensor_data accel, const sensor_data gyro, const sensor_data magnetic) { unsigned long long sample_interval_gyro = SAMPLE_INTV; m_accel.m_data = accel.m_data; - m_gyro.m_data = gyro.m_data; m_magnetic.m_data = magnetic.m_data; if (m_gyro.m_time_stamp != 0 && gyro.m_time_stamp != 0) @@ -115,11 +92,11 @@ inline void orientation_filter::initialize_sensor_data(const sensor_data -inline void orientation_filter::initialize_sensor_data(const sensor_data accel, +inline void orientation_filter::init_accel_mag_data(const sensor_data accel, const sensor_data magnetic) { m_accel.m_data = accel.m_data; @@ -130,22 +107,41 @@ inline void orientation_filter::initialize_sensor_data(const sensor_data +inline void orientation_filter::init_accel_gyro_data(const sensor_data accel, + const sensor_data gyro) +{ + unsigned long long sample_interval_gyro = SAMPLE_INTV; + + m_accel.m_data = accel.m_data; + + if (m_gyro.m_time_stamp != 0 && gyro.m_time_stamp != 0) + sample_interval_gyro = gyro.m_time_stamp - m_gyro.m_time_stamp; + + m_gyro_dt = sample_interval_gyro * US2S; + + m_accel.m_time_stamp = accel.m_time_stamp; + m_gyro.m_time_stamp = gyro.m_time_stamp; + + m_gyro.m_data = gyro.m_data - m_bias_correction; +} + +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, (TYPE) m_magnetic_alignment_factor, 0.0}; - vect acc_e(V1x3S, arr_acc_e); - vect mag_e(V1x3S, arr_mag_e); + vect acc_e(arr_acc_e); + vect mag_e(arr_mag_e); - vect acc_b_x_mag_b = cross(m_accel.m_data, m_magnetic.m_data); - vect acc_e_x_mag_e = cross(acc_e, mag_e); + vect acc_b_x_mag_b = cross(m_accel.m_data, m_magnetic.m_data); + vect acc_e_x_mag_e = cross(acc_e, mag_e); - vect cross1 = cross(acc_b_x_mag_b, m_accel.m_data); - vect cross2 = cross(acc_e_x_mag_e, acc_e); + vect cross1 = cross(acc_b_x_mag_b, m_accel.m_data); + vect cross2 = cross(acc_e_x_mag_e, acc_e); - matrix mat_b(M3X3R, M3X3C); - matrix mat_e(M3X3R, M3X3C); + matrix mat_b; + matrix mat_e; for(int i = 0; i < M3X3R; i++) { @@ -157,13 +153,23 @@ inline void orientation_filter::orientation_triad_algorithm() mat_e.m_mat[i][2] = cross2.m_vec[i]; } - matrix mat_b_t = tran(mat_b); + matrix mat_b_t = tran(mat_b); rotation_matrix rot_mat(mat_e * mat_b_t); m_quat_aid = rot_mat2quat(rot_mat); } template +inline void orientation_filter::compute_accel_orientation() +{ + TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0}; + + vect acc_e(arr_acc_e); + + m_quat_aid = sensor_data2quat(m_accel, acc_e); +} + +template inline void orientation_filter::compute_covariance() { TYPE var_gyr_x, var_gyr_y, var_gyr_z; @@ -217,19 +223,10 @@ inline void orientation_filter::time_update() m_measure_mat.m_mat[2][2] = 1; if (is_initialized(m_state_old)) - m_state_new = transpose(mul(m_tran_mat, transpose(m_state_old))); + m_state_new = transpose(m_tran_mat * transpose(m_state_old)); m_pred_cov = (m_tran_mat * m_pred_cov * tran(m_tran_mat)) + m_driv_cov; - for (int j=0; j::time_update() template inline void orientation_filter::measurement_update() { - matrix gain(M6X6R, M6X6C); - matrix iden(M6X6R, M6X6C); - iden.m_mat[0][0] = iden.m_mat[1][1] = iden.m_mat[2][2] = 1; - iden.m_mat[3][3] = iden.m_mat[4][4] = iden.m_mat[5][5] = 1; + matrix gain; + TYPE iden = 0; for (int j=0; j temp = iden; + m_state_new.m_vec[i] = m_state_new.m_vec[i] + gain.m_mat[i][j] * m_state_error.m_vec[j]; - for (int i=0; i vec(V1x3S, arr_bias); + vect vec(arr_bias); m_bias_correction = vec; } @@ -317,7 +310,7 @@ euler_angles orientation_filter::get_orientation(const sensor_data cor_euler_ang; - initialize_sensor_data(accel, gyro, magnetic); + init_accel_gyro_mag_data(accel, gyro, magnetic); normalize(m_accel); m_gyro.m_data = m_gyro.m_data * (TYPE) PI; @@ -357,7 +350,7 @@ template quaternion orientation_filter::get_geomagnetic_quaternion(const sensor_data accel, const sensor_data magnetic) { - initialize_sensor_data(accel, magnetic); + init_accel_mag_data(accel, magnetic); normalize(m_accel); normalize(m_magnetic); diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h index 09f5b90..bf89c96 100644 --- a/src/sensor_fusion/orientation_filter.h +++ b/src/sensor_fusion/orientation_filter.h @@ -27,27 +27,37 @@ #include "euler_angles.h" #include "rotation_matrix.h" +#define MOVING_AVERAGE_WINDOW_LENGTH 20 + +#define M3X3R 3 +#define M3X3C 3 +#define M6X6R 6 +#define M6X6C 6 +#define V1x3S 3 +#define V1x4S 4 +#define V1x6S 6 + template class orientation_filter { public: sensor_data m_accel; sensor_data m_gyro; sensor_data m_magnetic; - 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_azimuth; - matrix m_driv_cov; - matrix m_aid_cov; - matrix m_tran_mat; - matrix m_measure_mat; - matrix m_pred_cov; - vect m_state_new; - vect m_state_old; - vect m_state_error; - vect m_bias_correction; + 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_azimuth; + matrix m_driv_cov; + matrix m_aid_cov; + matrix m_tran_mat; + matrix m_measure_mat; + matrix m_pred_cov; + 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; @@ -63,11 +73,14 @@ public: orientation_filter(); ~orientation_filter(); - inline void initialize_sensor_data(const sensor_data accel, + inline void init_accel_gyro_mag_data(const sensor_data accel, const sensor_data gyro, const sensor_data magnetic); - inline void initialize_sensor_data(const sensor_data accel, + inline void init_accel_mag_data(const sensor_data accel, const sensor_data magnetic); + inline void init_accel_gyro_data(const sensor_data accel, + const sensor_data gyro); inline void orientation_triad_algorithm(); + inline void compute_accel_orientation(); inline void compute_covariance(); inline void time_update(); inline void measurement_update(); diff --git a/src/sensor_fusion/quaternion.cpp b/src/sensor_fusion/quaternion.cpp index 7578326..caf2950 100755 --- a/src/sensor_fusion/quaternion.cpp +++ b/src/sensor_fusion/quaternion.cpp @@ -21,8 +21,6 @@ #include -#define QUAT_SIZE 4 - template int sgn(T val) { if (val >= 0) return 1; @@ -38,7 +36,7 @@ template T mag(T val) { } template -quaternion::quaternion() : m_quat(QUAT_SIZE) +quaternion::quaternion() : m_quat() { } @@ -47,12 +45,12 @@ quaternion::quaternion(const TYPE w, const TYPE x, const TYPE y, const TYP { TYPE vec_data[QUAT_SIZE] = {w, x, y, z}; - vect v(QUAT_SIZE, vec_data); + vect v(vec_data); m_quat = v; } template -quaternion::quaternion(const vect v) +quaternion::quaternion(const vect v) { m_quat = v; } @@ -95,9 +93,7 @@ void quaternion::quat_normalize() template quaternion operator *(const quaternion q, const T val) { - quaternion res; - res = q.m_quat * val; - return (res); + return (q.m_quat * val); } template @@ -147,4 +143,18 @@ quaternion phase_correction(const quaternion q1, const quaternion q2) return q; } + +template +quaternion axis2quat(const vect axis, const T angle) +{ + T w; + vect imag; + + w = cos(angle/2); + imag = axis * (T)(-sin(angle/2)); + + quaternion q(w, imag.m_vec[0], imag.m_vec[1], imag.m_vec[2]); + + return q; +} #endif //_QUATERNION_H_ diff --git a/src/sensor_fusion/quaternion.h b/src/sensor_fusion/quaternion.h index 225b129..539a952 100755 --- a/src/sensor_fusion/quaternion.h +++ b/src/sensor_fusion/quaternion.h @@ -22,14 +22,17 @@ #include "vector.h" +#define QUAT_SIZE 4 +#define REF_VEC_SIZE 3 + template class quaternion { public: - vect m_quat; + vect m_quat; quaternion(); quaternion(const TYPE w, const TYPE x, const TYPE y, const TYPE z); - quaternion(const vect v); + quaternion(const vect v); quaternion(const quaternion& q); ~quaternion(); @@ -44,6 +47,8 @@ public: const quaternion q2); template friend quaternion phase_correction(const quaternion q1, const quaternion q2); + template friend quaternion axis2quat(const vect axis, + const T angle); }; #include "quaternion.cpp" diff --git a/src/sensor_fusion/rotation_matrix.cpp b/src/sensor_fusion/rotation_matrix.cpp index f29e669..83ff5d3 100644 --- a/src/sensor_fusion/rotation_matrix.cpp +++ b/src/sensor_fusion/rotation_matrix.cpp @@ -20,8 +20,6 @@ #if defined (_ROTATION_MATRIX_H_) && defined (_MATRIX_H_) #define QUAT_LEN 4 -#define ROT_MAT_ROWS 3 -#define ROT_MAT_COLS 3 template T get_sign(T val) { @@ -29,21 +27,21 @@ template T get_sign(T val) } template -rotation_matrix::rotation_matrix() : m_rot_mat(ROT_MAT_ROWS, ROT_MAT_COLS) +rotation_matrix::rotation_matrix() : m_rot_mat() { } template -rotation_matrix::rotation_matrix(const matrix m) +rotation_matrix::rotation_matrix(const matrix m) { m_rot_mat = m; } template -rotation_matrix::rotation_matrix(const int rows, const int cols, TYPE *mat_data) +rotation_matrix::rotation_matrix(TYPE mat_data[ROT_MAT_ROWS][ROT_MAT_COLS]) { - matrix m(rows, cols, mat_data); + matrix m(mat_data); m_rot_mat = m; } @@ -88,7 +86,7 @@ rotation_matrix quat2rot_mat(quaternion q) R[2][1] = 2 * ((y * z) - (w * x)); R[2][2] = (2 * w * w) - 1 + (2 * z * z); - rotation_matrix rm(ROT_MAT_ROWS, ROT_MAT_COLS, &R[0][0]); + rotation_matrix rm(R); return rm; } diff --git a/src/sensor_fusion/rotation_matrix.h b/src/sensor_fusion/rotation_matrix.h index bb4aa78..5cdf430 100644 --- a/src/sensor_fusion/rotation_matrix.h +++ b/src/sensor_fusion/rotation_matrix.h @@ -23,14 +23,17 @@ #include "matrix.h" #include "quaternion.h" +#define ROT_MAT_ROWS 3 +#define ROT_MAT_COLS 3 + template class rotation_matrix { public: - matrix m_rot_mat; + matrix m_rot_mat; rotation_matrix(); - rotation_matrix(const matrix m); - rotation_matrix(const int rows, const int cols, TYPE *mat_data); + rotation_matrix(const matrix m); + rotation_matrix(TYPE mat_data[ROT_MAT_ROWS][ROT_MAT_COLS]); rotation_matrix(const rotation_matrix& rm); ~rotation_matrix(); diff --git a/src/sensor_fusion/sensor_data.cpp b/src/sensor_fusion/sensor_data.cpp index 4794b5e..7272800 100644 --- a/src/sensor_fusion/sensor_data.cpp +++ b/src/sensor_fusion/sensor_data.cpp @@ -21,10 +21,8 @@ #include "math.h" -#define SENSOR_DATA_SIZE 3 - template -sensor_data::sensor_data() : m_data(SENSOR_DATA_SIZE), m_time_stamp(0) +sensor_data::sensor_data() : m_data(), m_time_stamp(0) { } @@ -34,13 +32,13 @@ sensor_data::sensor_data(const TYPE x, const TYPE y, { TYPE vec_data[SENSOR_DATA_SIZE] = {x, y, z}; - vect v(SENSOR_DATA_SIZE, vec_data); + vect v(vec_data); m_data = v; m_time_stamp = time_stamp; } template -sensor_data::sensor_data(const vect v, +sensor_data::sensor_data(const vect v, const unsigned long long time_stamp) { m_data = v; @@ -71,7 +69,7 @@ sensor_data sensor_data::operator =(const sensor_data& s) template sensor_data operator +(sensor_data data1, sensor_data data2) { - sensor_data result(data1.m_data + data2.m_data, data1.m_time_stamp); + sensor_data result(data1.m_data + data2.m_data, 0); return result; } @@ -105,5 +103,18 @@ sensor_data scale_data(sensor_data data, T scaling_factor) return s; } + +template +quaternion sensor_data2quat(const sensor_data data, const vect ref_vec) +{ + vect axis; + T angle; + + axis = cross(data.m_data, ref_vec); + angle = acos(dot(data.m_data, ref_vec)); + + return axis2quat(axis, angle); +} + #endif /* _SENSOR_DATA_H_ */ diff --git a/src/sensor_fusion/sensor_data.h b/src/sensor_fusion/sensor_data.h index c95e5b4..7998b36 100644 --- a/src/sensor_fusion/sensor_data.h +++ b/src/sensor_fusion/sensor_data.h @@ -21,17 +21,20 @@ #define _SENSOR_DATA_H_ #include "vector.h" +#include "quaternion.h" + +#define SENSOR_DATA_SIZE 3 template class sensor_data { public: - vect 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 vect v, + sensor_data(const vect v, const unsigned long long time_stamp); sensor_data(const sensor_data& s); ~sensor_data(); @@ -44,6 +47,8 @@ public: template friend void normalize(sensor_data& data); template friend sensor_data scale_data(sensor_data data, T scaling_factor); + template friend quaternion sensor_data2quat(const sensor_data data, + const vect ref_vec); }; #include "sensor_data.cpp" diff --git a/src/sensor_fusion/test/test_projects/euler_angles_test/euler_angles_main.cpp b/src/sensor_fusion/test/test_projects/euler_angles_test/euler_angles_main.cpp index 06e449d..4a3104d 100644 --- a/src/sensor_fusion/test/test_projects/euler_angles_test/euler_angles_main.cpp +++ b/src/sensor_fusion/test/test_projects/euler_angles_test/euler_angles_main.cpp @@ -26,10 +26,10 @@ int main() float arr2[4] = {0.6, 0.6, -.18, -.44}; float arr3[4] = {-0.5, -0.36, .43, .03}; - vect v0(3, arr0); - vect v1(3, arr1); - vect v2(4, arr2); - vect v3(4, arr3); + vect v0(arr0); + vect v1(arr1); + vect v2(arr2); + vect v3(arr3); quaternion q1(v2); quaternion q2(v3); @@ -60,6 +60,11 @@ int main() cout << "input\t" << q2.m_quat << "\n"; cout << "output\t" << e8.m_ang << "\n\n"; + cout << "Euler to Quaternion\n"; + quaternion q3 = euler2quat(e8); + cout << "input\t" << e8.m_ang << "\n"; + cout << "output\t" << q3.m_quat << "\n\n"; + cout << "Radians to Degrees\n"; euler_angles e6 = deg2rad(e0); cout << "input\t" << e0.m_ang << "\n"; diff --git a/src/sensor_fusion/test/test_projects/matrix_test/matrix_main.cpp b/src/sensor_fusion/test/test_projects/matrix_test/matrix_main.cpp index d1a2153..8227c3a 100644 --- a/src/sensor_fusion/test/test_projects/matrix_test/matrix_main.cpp +++ b/src/sensor_fusion/test/test_projects/matrix_test/matrix_main.cpp @@ -32,19 +32,19 @@ int main() float arr8[3][3] = {{4.75, 0.65, 0.123}, {0.075, 5.302, 0.56}, {1.113, 0.475, 2.362}}; float arr9[3][3] = {{1, 2, 3}, {1, 2, 3}, {1, 2, 3}}; - matrix m1(2, 2, (float *) arr0); - matrix m2(2, 2, (float *) arr1); - matrix m3(2, 2); - matrix m10(3, 3, (float *) arr3); - matrix m11(3, 2, (float *) arr5); - matrix m6(3, 3); - matrix m13(3, 2); - matrix m12(3, 3, (float *) arr4); - matrix m15(3, 3, (float *) arr8); - matrix m20(1, 3, (float *) arr11); - matrix m21(3, 1, (float *) arr12); - matrix m22(2, 3, (float *) arr15); - matrix m9(3, 3, (float *) arr9); + matrix m1(arr0); + matrix m2(arr1); + matrix m3; + matrix m10(arr3); + matrix m11(arr5); + matrix m6; + matrix m13; + matrix m12(arr4); + matrix m15(arr8); + matrix m20(arr11); + matrix m21(arr12); + matrix m22(arr15); + matrix m9(arr9); cout<< "Constructor Test\n"; cout<< "\n" << m6; @@ -65,7 +65,7 @@ int main() cout<< "\n\n\nMultiplication\n"; m6 = m10 * m12; m3 = m1 * m2; - matrix m7(m20.m_rows, m21.m_cols); + matrix m7; m7 = m20 * m21; cout<< "\n" << m10 << "\n" << m12; cout<< "\nProduct:\n" << m6 << endl; @@ -74,7 +74,7 @@ int main() cout<< "\n" << m20 << "\n" << m21; cout<< "\nProduct:\n" << m7 << endl; cout<< "\n" << m9 << "\n" << m21; - m21 = mul(m9, m21); + m21 = m9 * m21; cout<< "\nProduct:\n" << m21 << endl; cout<< "\n\n\nDivision\n"; @@ -148,8 +148,8 @@ int main() cout<< "\n\nAssignment\n"; - m3 = m12; + matrix m30 = m12; cout<< "Input \n" << m12; - cout<< "\nOutput:\n" << m3 << endl; + cout<< "\nOutput:\n" << m30 << endl; } diff --git a/src/sensor_fusion/test/test_projects/quaternion_test/quaternion_main.cpp b/src/sensor_fusion/test/test_projects/quaternion_test/quaternion_main.cpp index 330a1ad..09f980f 100644 --- a/src/sensor_fusion/test/test_projects/quaternion_test/quaternion_main.cpp +++ b/src/sensor_fusion/test/test_projects/quaternion_test/quaternion_main.cpp @@ -23,9 +23,12 @@ int main() { float arr0[4] = {2344.98, 345.24, 456.12, 98.33}; float arr1[4] = {0.056, 0.34, -0.0076, 0.001}; + float axis1[3] = {6.5, 7.5, 8.3}; + float ang1 = 8.4; - vect v0(4, arr0); - vect v1(4, arr1); + vect v0(arr0); + vect v1(arr1); + vect v2(axis1); quaternion q0(v0); quaternion q1(v1); @@ -63,5 +66,13 @@ int main() cout << "input\t" << q1.m_quat << "\n"; q1.quat_normalize(); cout << "output\t" << q1.m_quat << "\n\n"; + + cout << "Axis2quat\n"; + cout << "input\t" << " " << v2 << endl; + cout << endl; + quaternion q11 = axis2quat(v2, ang1); + cout << "output\t" << q11.m_quat << "\n\n"; + cout << endl; + } diff --git a/src/sensor_fusion/test/test_projects/rotation_matrix_test/rotation_matrix_main.cpp b/src/sensor_fusion/test/test_projects/rotation_matrix_test/rotation_matrix_main.cpp index 21ecadf..9ab8c35 100644 --- a/src/sensor_fusion/test/test_projects/rotation_matrix_test/rotation_matrix_main.cpp +++ b/src/sensor_fusion/test/test_projects/rotation_matrix_test/rotation_matrix_main.cpp @@ -25,15 +25,15 @@ int main() float arr2[3][3] = {{2.24, 0.5, 0.023}, {3.675, 5.32, 0.556}, {1.023, 45.75, 621.6}}; float arr3[3][3] = {{4.75, 0.65, 0.123}, {0.075, 5.302, 0.56}, {1.113, 0.475, 2.362}}; - matrix m1(3, 3, (float *) arr1); - matrix m2(3, 3, (float *) arr2); - matrix m3(3, 3, (float *) arr3); + matrix m1(arr1); + matrix m2(arr2); + matrix m3(arr3); rotation_matrix rm0, rm5; rotation_matrix rm1(m1); rotation_matrix rm2(m2); rotation_matrix rm3(m3); - rotation_matrix rm4(3, 3, (float *) arr1); + rotation_matrix rm4(arr1); quaternion q0(-0.612372, 0.456436, 0.456436, 0.456436); quaternion q1; diff --git a/src/sensor_fusion/test/test_projects/sensor_data_test/sensor_data_main.cpp b/src/sensor_fusion/test/test_projects/sensor_data_test/sensor_data_main.cpp index 5f94e53..4477daa 100644 --- a/src/sensor_fusion/test/test_projects/sensor_data_test/sensor_data_main.cpp +++ b/src/sensor_fusion/test/test_projects/sensor_data_test/sensor_data_main.cpp @@ -22,13 +22,16 @@ int main() { float arr1[3] = {1.04, -4.678, -2.34}; - - vect v1(3, arr1); + float arr2[3] = {0, 0, 1}; + vect v1(arr1); + vect v2(arr2); sensor_data sd1(2.0, 3.0, 4.0, 140737488355328); sensor_data sd2(1.04, -4.678, -2.34, 0); sensor_data sd3(0.054, 1.097, 4.456, 140737488355328); + sensor_data sd10(v1, 140737488355328); + sensor_data sd11(0.2, 0.1, 0.3, 140737488355328); cout << "Constructor tests\n"; cout << "input\t" << v1 << "\n"; @@ -57,5 +60,10 @@ int main() sensor_data sd8 = scale_data(sd2, xx); cout<< "\n" << sd2.m_data << "\n" << xx; cout<< "\nResult:\n" << sd8.m_data << endl; + + cout<<"\n\n\nConvert Sensor Data to Quaternion:\n"; + quaternion q = sensor_data2quat(sd11, v2); + cout<< "\n" << sd11.m_data << "\n" << v2; + cout<< "\nResult:\n" << q.m_quat << endl; } diff --git a/src/sensor_fusion/test/test_projects/vector_test/vector_main.cpp b/src/sensor_fusion/test/test_projects/vector_test/vector_main.cpp index befdeee..bca9d2d 100644 --- a/src/sensor_fusion/test/test_projects/vector_test/vector_main.cpp +++ b/src/sensor_fusion/test/test_projects/vector_test/vector_main.cpp @@ -31,24 +31,32 @@ 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}; - 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); + vect v1(arr0); + vect v2(arr8); + vect v10(arr3); + vect v12(arr4); + vect v15(arr1); + vect v20(arr43); + vect v21(arr2); + vect v22(arr15); + vect v31(arr0); + vect v3; + vect vn; + vect v61(arr4); + vect v6; + vect vm; + vect v13; + vect v95; + vect v35(arr5); + vect vl; + vect vp; + vect vr(arr4); + vect vf; 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); + matrix m12(arr57); float arr67[3][1] = {{2.0}, {3.0}, {4.0}}; - matrix m32(3, 1, (float *) arr67); + matrix m32(arr67); cout<< "Constructor Test\n"; cout<< "\n" << v3; @@ -56,39 +64,41 @@ int main() cout<< "\n\nAddition\n"; v3 = v21 + v22; v95 = v15 + v20; + cout<< "\n\nNumbers added\n"; cout<< "\n" << v21 << "\n" << v22; cout<< "\nSum:\n" << v3 << endl; + cout<< "\n\nNumbers added\n"; cout<< "\n" << v15 << "\n" << v20; cout<< "\nSum:\n" << v95 << endl; float num = 5.5650; float num1 = -2.32; cout<< "\n\n\nScalar addition\n"; - v3 = v2 + num; - v6 = v10 + num1; + vl = v2 + num; + vm = v10 + num1; cout<< "\nNumber added:" << num; cout<< "\n\n" << v2; - cout<< "\nResult:\n\n" << v3; + cout<< "\nResult:\n\n" << vl; cout<< "\nNumber added:"<< num1; cout<< "\n\n" << v10; - cout<< "\nResult:\n\n"<< v6; + cout<< "\nResult:\n\n"<< vm; cout<< "\n\n\nSubtraction\n"; - v6 = v10 - v12; + vp = v10 - v12; cout<< "\n" << v10 << "\n" << v12; - cout<< "\nDifference:\n" << v6 << endl; + cout<< "\nDifference:\n" << vp << endl; float x = 4.0; float x1 = -2.5; cout<< "\n\n\nScalar subtraction\n"; v13 = v1 - x; - v6 = v10 - x1; + vp = v10 - x1; cout<< "\nNumber Subtracted:" << x; cout<< "\n\n" << v1; cout<< "\nResult:\n\n" << v13; cout<< "\nNumber Subtracted:" << x1; cout<< "\n\n" << v10; - cout<< "\nResult:\n\n" << v6; + cout<< "\nResult:\n\n" << vp; float xx = 7.2; cout<<"\n\n\nMultiplication\n"; @@ -97,23 +107,20 @@ int main() cout<< "\nProduct:\n" << v13 << endl; cout<< "\n\n\nMultiplication matrix x vector:\n"; - matrix m102 = m32 * v22; + matrix m102 = m32 * v22; cout<< "\n" << m32 <<"\n"<< v22; cout<< "\nProduct:\n"<< m102 << endl; cout<< "\n\n\nVector x Multiplication matrix:\n"; - vect v102 = (v22 * m12); + vect v102 = (v22 * m12); cout<< "\n" << v22 << "\n" << m12; cout<< "\nProduct:\n" << v102 << endl; - float val = mul(v22, m32); - cout<< "\n" << v22 << "\n" << m32; - cout<< "\nProduct:\n" << val << endl; float z = 3.50; float z1 = -5.567; cout<< "\n\n\nScalar multiplication\n"; v13 = v1 * z; - v6 = v12 * z1; + v61 = v12 * z1; cout<< "\nNumber Multiplied:" << z; cout<< "\n\n" << v1; cout<< "\nResult:\n\n" << v13; @@ -123,18 +130,18 @@ int main() float num2 = 5.5; cout<< "\n\n\nDivision\n"; - v3 = v1 / num2; + v31 = v1 / num2; cout<< "\n" << v1 << "\n" << num2; cout<< "\nResult:\n" << v3 << endl; cout<< "\n\n\nTranspose:"; cout << "\n\n" << v20; cout << "\nResult:\n\n"; - matrix m101 = (transpose(v20)); + matrix m101 = transpose(v20); cout << m101; cout << "\n\n" << m101; cout << "\nResult:\n\n"; - v20 = (transpose(m101)); + v20 = transpose(m101); cout << v20; cout << "\n\nv1:\n\n" << v1; @@ -148,9 +155,9 @@ int main() cout << (v10 == v10); cout << "\n\nv12:\n\n" << v12; - cout << "\n\nv15:\n\n" << v15; + cout << "\n\nv15:\n\n" << vr; cout << "\n\n\nv12 != v15 :"; - cout << (v12 != v15); + cout << (v12 != vr); cout << "\n\nv15:\n\n" << v15; cout << "\n\nv15:\n\n" << v15; @@ -158,16 +165,20 @@ int main() cout << (v15 != v15); cout<< "\n\nAssignment\n"; - v3 = v1; + v3 = vf; cout<< "Input \n" << v1; cout<< "\nOutput:\n" << v3 << endl; - - vect 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; + float val = dot(v21, v22); + cout<< "\n\n\nDot Product:"; + cout << "\n\n" << v21 << "\n\n" << v22; + cout << "\nResult:\n\n" << val; + cout << "\n\n\nQueue insert function:"; cout << "\nInput:\n\n" << v111; insert_end(v111, (float) 0.9191919); @@ -180,6 +191,6 @@ int main() cout << "\n\n\nIs Initialized:"; cout << "\nInput:\n\n" << v35; - cout << "\nResult:\n\n" << is_initialized(v35); + cout << "\nResult:\n\n" << is_initialized(v35) << endl; } diff --git a/src/sensor_fusion/vector.cpp b/src/sensor_fusion/vector.cpp index 5ab9862..2fc932a 100644 --- a/src/sensor_fusion/vector.cpp +++ b/src/sensor_fusion/vector.cpp @@ -19,82 +19,44 @@ #if defined (_VECTOR_H_) && defined (_MATRIX_H_) -template -vect::vect(void) +TYPE_SIZE vect::vect(void) { - m_vec = NULL; + for(int i=0;i -vect::vect(const int size) +TYPE_SIZE vect::vect(TYPE vec_data[SIZE]) { - m_size = size; - m_vec = new TYPE [m_size](); -} - -template -vect::vect(const int size, TYPE *vec_data) -{ - m_size = size; - m_vec = new TYPE [m_size]; - for (int j = 0; j < m_size; j++) - m_vec[j] = *vec_data++; + for (int j = 0; j < SIZE; j++) + m_vec[j] = vec_data[j]; } -template -vect::vect(const vect& v) +TYPE_SIZE vect::vect(const vect& v) { - m_size = v.m_size; - m_vec = new TYPE [m_size]; - - for (int q = 0; q < m_size; q++) + for (int q = 0; q < SIZE; q++) m_vec[q] = v.m_vec[q]; } -template -vect::~vect() + +TYPE_SIZE vect::~vect() { - if (m_vec != NULL) - delete[] m_vec; } -template -vect vect::operator =(const vect& v) +TYPE_SIZE vect vect::operator =(const vect& v) { if (this == &v) { return *this; } - - if (m_vec == NULL) - { - m_size = v.m_size; - m_vec = new TYPE [m_size]; - } - else - { - if (m_size != v.m_size) - { - delete[] m_vec; - - m_size = v.m_size; - m_vec = new TYPE [m_size]; - } - } - - - for (int q = 0; q < m_size; q++) + for (int q = 0; q < SIZE; q++) m_vec[q] = v.m_vec[q]; - - - return *this; + return *this; } -template -ostream& operator <<(ostream& dout, vect& v) +T_S ostream& operator <<(ostream& dout, vect& v) { - for (int j = 0; j < v.m_size; j++) + for (int j = 0; j < S; j++) { dout << v.m_vec[j] << "\t"; } @@ -104,65 +66,56 @@ ostream& operator <<(ostream& dout, vect& v) return dout; } -template -vect operator +(const vect v1, const vect v2) +T_S vect operator +(const vect v1, const vect v2) { - assert(v1.m_size == v2.m_size); + vect v3; - vect v3(v1.m_size); - - for (int j = 0; j < v1.m_size; j++) + for (int j = 0; j < S; j++) v3.m_vec[j] = v1.m_vec[j] + v2.m_vec[j]; return v3; } -template -vect operator +(const vect v, const T val) +T_S vect operator +(const vect v, const T val) { - vect v1(v.m_size); + vect v1; - for (int j = 0; j < v.m_size; j++) + for (int j = 0; j < S; j++) v1.m_vec[j] = v.m_vec[j] + val; return v1; } -template -vect operator -(const vect v1, const vect v2) +T_S vect operator -(const vect v1, const vect v2) { - assert(v1.m_size == v2.m_size); - - vect v3(v1.m_size); + vect v3; - for (int j = 0; j < v1.m_size; j++) + for (int j = 0; j < S; j++) v3.m_vec[j] = v1.m_vec[j] - v2.m_vec[j]; return v3; } -template -vect operator -(const vect v, const T val) +T_S vect operator -(const vect v, const T val) { - vect v1(v.m_size); + vect v1; - for (int j = 0; j < v.m_size; j++) + for (int j = 0; j < S; j++) v1.m_vec[j] = v.m_vec[j] - val; return v1; } -template -matrix operator *(const matrix m, const vect v) +T_S_R_C matrix operator *(const matrix m, const vect v) { - assert(m.m_rows == v.m_size); - assert(m.m_cols == 1); + assert(R == S); + assert(C == 1); - matrix m1(m.m_rows, v.m_size); + matrix m1; - for (int i = 0; i < m1.m_rows; i++) + for (int i = 0; i < R; i++) { - for (int j = 0; j < m1.m_cols; j++) + for (int j = 0; j < S; j++) { m1.m_mat[i][j] = m.m_mat[i][0] * v.m_vec[j]; } @@ -171,52 +124,47 @@ matrix operator *(const matrix m, const vect v) return m1; } -template -vect operator *(const vect v, const matrix m) +T_S_R_C vect operator *(const vect v, const matrix m) { - assert(m.m_rows == v.m_size); - assert(m.m_cols != 1); + assert(R == S); + assert(C != 1); + vect v1; - vect v1(m.m_cols); - - for (int j = 0; j < m.m_cols; j++) + for (int j = 0; j < C; j++) { v1.m_vec[j] = 0; - for (int k = 0; k < m.m_rows; k++) + for (int k = 0; k < R; k++) v1.m_vec[j] += v.m_vec[k] * m.m_mat[k][j]; } return v1; } -template -vect operator *(const vect v, const T val) +T_S vect operator *(const vect v, const T val) { - vect v1(v.m_size); + vect v1; - for (int j = 0; j < v.m_size; j++) + for (int j = 0; j < S; j++) v1.m_vec[j] = v.m_vec[j] * val; return v1; } -template -vect operator /(const vect v, const T val) +T_S vect operator /(const vect v, const T val) { - vect v1(v.m_size); + vect v1; - for (int j = 0; j < v.m_size; j++) + for (int j = 0; j < S; j++) v1.m_vec[j] = v.m_vec[j] / val; return v1; } -template -bool operator ==(const vect v1, const vect v2) +T_S1_S2 bool operator ==(const vect v1, const vect v2) { - if (v1.m_size == v2.m_size) + if (S1==S2) { - for (int i = 0; i < v1.m_size; i++) + for (int i = 0; i < S1; i++) if (v1.m_vec[i] != v2.m_vec[i]) return false; } @@ -226,62 +174,42 @@ bool operator ==(const vect v1, const vect v2) return true; } -template -bool operator !=(const vect v1, const vect v2) +T_S1_S2 bool operator !=(const vect v1, const vect v2) { return (!(v1 == v2)); } -template -matrix transpose(const vect v) +T_S matrix transpose(const vect v) { - matrix m(v.m_size, 1); + matrix m; - for (int i = 0; i < v.m_size; i++) + for (int i = 0; i < S; i++) m.m_mat[i][0] = v.m_vec[i]; return m; } -template -vect transpose(const matrix m) +T_S vect transpose(const matrix m) { - vect v(m.m_rows); + vect v; - for (int i = 0; i < m.m_rows; i++) + for (int i = 0; i < S; i++) v.m_vec[i] = m.m_mat[i][0]; return v; } -template -T mul(const vect v, const matrix m) +T_S void insert_end(vect& v, T val) { - assert(m.m_rows == v.m_size); - assert(m.m_cols == 1); - - T result = (T) 0; - - for (int k = 0; k < v.m_size; k++) - result += v.m_vec[k] * m.m_mat[k][0]; - - return result; -} - - -template -void insert_end(vect& v, T val) -{ - for (int i = 0; i < (v.m_size - 1); i++) + for (int i = 0; i < (S - 1); i++) v.m_vec[i] = v.m_vec[i+1]; - v.m_vec[v.m_size-1] = val; + v.m_vec[S-1] = val; } -template -vect cross(const vect v1, const vect v2) +T_S vect cross(const vect v1, const vect v2) { - vect v3(v1.m_size); + vect v3; v3.m_vec[0] = ((v1.m_vec[1] * v2.m_vec[2]) - (v1.m_vec[2] * v2.m_vec[1])); v3.m_vec[1] = ((v1.m_vec[2] * v2.m_vec[0]) - (v1.m_vec[0] * v2.m_vec[2])); @@ -290,10 +218,14 @@ vect cross(const vect v1, const vect v2) return v3; } -template -bool is_initialized(const vect v) +T_S T dot(const vect v1, const vect v2) +{ + return (v1.m_vec[0] * v2.m_vec[0] + v1.m_vec[1] * v2.m_vec[1] + v1.m_vec[2] * v2.m_vec[2]); +} + +T_S bool is_initialized(const vect v) { - vect v1(v.m_size); + vect v1; bool retval; retval = (v == v1) ? false : true; @@ -301,25 +233,24 @@ bool is_initialized(const vect v) return retval; } -template -T var(const vect v) +T_S T var(const vect v) { T val = 0; T mean, var, diff; - for (int i = 0; i < v.m_size; i++) + for (int i = 0; i < S; i++) val += v.m_vec[i]; - mean = val / v.m_size; + mean = val / S; val = 0; - for (int i = 0; i < v.m_size; i++) + for (int i = 0; i < S; i++) { diff = (v.m_vec[i] - mean); val += diff * diff; } - var = val / (v.m_size - 1); + var = val / (S - 1); return var; } diff --git a/src/sensor_fusion/vector.h b/src/sensor_fusion/vector.h index 5578f3c..c976ecc 100644 --- a/src/sensor_fusion/vector.h +++ b/src/sensor_fusion/vector.h @@ -22,53 +22,43 @@ #include "matrix.h" -template -class vect { -public: - int m_size; - TYPE *m_vec; +#define TYPE_SIZE template +#define T_S template +#define T_S_R_C template +#define T_S1_S2 template +TYPE_SIZE class vect { +public: + TYPE m_vec[SIZE]; vect(void); - vect(const int size); - vect(const int size, TYPE *vec_data); - vect(const vect& v); + vect(TYPE vec_data[SIZE]); + vect(const vect& v); ~vect(); - vect operator =(const vect& v); + 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); + T_S friend ostream& operator << (ostream& dout, vect& v); + T_S friend vect operator +(const vect v1, const vect v2); + T_S friend vect operator +(const vect v, const T val); + T_S friend vect operator -(const vect v1, const vect v2); + T_S friend vect operator -(const vect v, const T val); + T_S_R_C friend matrix operator *(const matrix v1, const vect v2); + T_S_R_C friend vect operator *(const vect v, const matrix m); + T_S friend vect operator *(const vect v, const T val); + T_S friend vect operator /(const vect v1, const T val); + T_S1_S2 friend bool operator ==(const vect v1, const vect v2); + T_S1_S2 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); + T_S friend void insert_end(vect& v, T val); + T_S friend matrix transpose(const vect v); + T_S friend vect transpose(const matrix m); + T_S friend vect cross(const vect v1, const vect v2); + T_S friend T dot(const vect v1, const vect v2); + T_S friend T var(const vect v); + T_S friend bool is_initialized(const vect v); }; #include "vector.cpp" #endif /* _VECTOR_H_ */ + -- 2.7.4 From d5af0f88316a87b01fafd5e454ff77735a1769df Mon Sep 17 00:00:00 2001 From: Ankur Date: Thu, 12 Feb 2015 19:32:03 +0530 Subject: [PATCH 10/16] Geomagnetic Sensor file - Feature merge from devel/tizen branch This is second of the four patches being submitted to merge the changes from devel/tizen to tizen branch. This contains the changes for adding geomagnetic rotation vector virtual sensor. Build has been checked for armv7l and aarch64 for public repo. Change-Id: Ie91229c349a452e8d24e7d831c258f1b9d26d72e --- packaging/sensord.spec | 4 +- src/CMakeLists.txt | 2 +- src/libsensord/CMakeLists.txt | 1 + src/libsensord/client_common.cpp | 2 + src/libsensord/sensor_geomag.h | 2 +- src/libsensord/sensor_geomagnetic_rv.h | 53 ++++ src/libsensord/sensor_internal.h | 1 + src/libsensord/sensor_internal_deprecated.h | 1 + src/libsensord/sensor_light.h | 2 +- src/libsensord/sensor_pressure.h | 2 +- src/libsensord/sensor_proxi.h | 2 +- src/libsensord/sensor_temperature.h | 2 +- src/rotation_vector/CMakeLists.txt | 3 + src/rotation_vector/geomagnetic_rv/CMakeLists.txt | 27 ++ .../geomagnetic_rv/geomagnetic_rv_sensor.cpp | 350 +++++++++++++++++++++ .../geomagnetic_rv/geomagnetic_rv_sensor.h | 75 +++++ .../documentation/equation/equation_13_updated.png | Bin 0 -> 1632 bytes .../design/documentation/sensor_fusion.htm | 2 +- src/sensor_fusion/design/lib/axis_rot2quat.m | 26 ++ .../design/lib/estimate_geomagnetic_rv.m | 17 +- .../design/lib/estimate_orientation.m | 2 +- src/sensor_fusion/design/lib/euler2quat.m | 38 +++ src/sensor_fusion/design/sf_geomagnetic_rv.m | 19 +- src/server/command_worker.cpp | 20 +- src/server/permission_checker.cpp | 11 +- src/server/permission_checker.h | 2 +- src/shared/sensor_common.h | 1 + test/CMakeLists.txt | 4 + virtual_sensors.xml | 26 ++ 29 files changed, 650 insertions(+), 47 deletions(-) create mode 100755 src/libsensord/sensor_geomagnetic_rv.h create mode 100755 src/rotation_vector/geomagnetic_rv/CMakeLists.txt create mode 100755 src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp create mode 100755 src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h create mode 100644 src/sensor_fusion/design/documentation/equation/equation_13_updated.png create mode 100644 src/sensor_fusion/design/lib/axis_rot2quat.m create mode 100644 src/sensor_fusion/design/lib/euler2quat.m diff --git a/packaging/sensord.spec b/packaging/sensord.spec index ecec809..e6d3ed6 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -28,6 +28,7 @@ BuildRequires: pkgconfig(capi-system-info) %define gravity_state ON %define linear_accel_state ON %define rv_state ON +%define geomagnetic_rv_state ON %define build_test_suite OFF %description @@ -78,6 +79,7 @@ cmake . -DCMAKE_INSTALL_PREFIX=%{_prefix} -DACCEL=%{accel_state} \ -DGEO=%{geo_state} -DPRESSURE=%{pressure_state} -DTEMPERATURE=%{temperature_state} \ -DORIENTATION=%{orientation_state} -DGRAVITY=%{gravity_state} \ -DLINEAR_ACCEL=%{linear_accel_state} -DRV=%{rv_state} \ + -DGEOMAGNETIC_RV=%{geomagnetic_rv_state} \ -DTEST_SUITE=%{build_test_suite} \ -DLIBDIR=%{_libdir} -DINCLUDEDIR=%{_includedir} @@ -145,6 +147,6 @@ systemctl daemon-reload %{_bindir}/temperature %{_bindir}/light %{_bindir}/rotation_vector +%{_bindir}/geomagnetic_rv %license LICENSE.APLv2 %endif - diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 60b93e3..853ce97 100755 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -38,7 +38,7 @@ IF("${ORIENTATION}" STREQUAL "ON") set(SENSOR_FUSION_ENABLE "1") set(ORIENTATION_ENABLE "1") ENDIF() -IF("${RV}" STREQUAL "ON") +IF("${GEOMAGNETIC_RV}" STREQUAL "ON") set(SENSOR_FUSION_ENABLE "1") ENDIF() IF("${GRAVITY}" STREQUAL "ON") diff --git a/src/libsensord/CMakeLists.txt b/src/libsensord/CMakeLists.txt index 5b57251..0fe82f6 100755 --- a/src/libsensord/CMakeLists.txt +++ b/src/libsensord/CMakeLists.txt @@ -56,6 +56,7 @@ install(FILES sensor_gravity.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES sensor_linear_accel.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES sensor_orientation.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES sensor_rv.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) +install(FILES sensor_geomagnetic_rv.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES sensor_temperature.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES sensor_motion.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES sensor_deprecated.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) diff --git a/src/libsensord/client_common.cpp b/src/libsensord/client_common.cpp index 22a74a4..831be17 100755 --- a/src/libsensord/client_common.cpp +++ b/src/libsensord/client_common.cpp @@ -41,6 +41,7 @@ log_element g_log_elements[] = { FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, ORIENTATION_SENSOR, 0, 1), FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, TEMPERATURE_SENSOR, 0, 1), FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, ROTATION_VECTOR_SENSOR, 0, 1), + FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, GEOMAGNETIC_RV_SENSOR, 0, 1), FILL_LOG_ELEMENT(LOG_ID_EVENT, GEOMAGNETIC_EVENT_CALIBRATION_NEEDED, 0, 1), FILL_LOG_ELEMENT(LOG_ID_EVENT, PROXIMITY_EVENT_CHANGE_STATE, 0,1), @@ -61,6 +62,7 @@ log_element g_log_elements[] = { FILL_LOG_ELEMENT(LOG_ID_EVENT, PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, TEMPERATURE_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), + FILL_LOG_ELEMENT(LOG_ID_EVENT, GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), FILL_LOG_ELEMENT(LOG_ID_DATA, ACCELEROMETER_BASE_DATA_SET, 0, 25), FILL_LOG_ELEMENT(LOG_ID_DATA, GYRO_BASE_DATA_SET, 0, 25), diff --git a/src/libsensord/sensor_geomag.h b/src/libsensord/sensor_geomag.h index 7929a99..c3dfb51 100755 --- a/src/libsensord/sensor_geomag.h +++ b/src/libsensord/sensor_geomag.h @@ -41,7 +41,7 @@ enum geomag_data_id { GEOMAGNETIC_RAW_DATA_SET = (GEOMAGNETIC_SENSOR << 16) | 0x0001, }; -enum geomag_evet_type { +enum geomag_event_type { GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME = (GEOMAGNETIC_SENSOR << 16) | 0x0001, GEOMAGNETIC_EVENT_CALIBRATION_NEEDED = (GEOMAGNETIC_SENSOR << 16) | 0x0002, GEOMAGNETIC_EVENT_UNPROCESSED_DATA_REPORT_ON_TIME = (GEOMAGNETIC_SENSOR << 16) | 0x0003, diff --git a/src/libsensord/sensor_geomagnetic_rv.h b/src/libsensord/sensor_geomagnetic_rv.h new file mode 100755 index 0000000..73106ae --- /dev/null +++ b/src/libsensord/sensor_geomagnetic_rv.h @@ -0,0 +1,53 @@ +/* + * libsensord + * + * Copyright (c) 2015 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_GEOMAGNETIC_RV_H__ +#define __SENSOR_GEOMAGNETIC_RV_H__ + +//! Pre-defined events for the geomagnetic rotation vector sensor +//! Sensor Plugin developer can add more event to their own headers + +#ifdef __cplusplus +extern "C" +{ +#endif + +/** + * @defgroup SENSOR_GEOMAGNETIC_RV Rotation Vector Sensor + * @ingroup SENSOR_FRAMEWORK + * + * These APIs are used to control the Geomagnetic Rotation Vector sensor. + * @{ + */ + +enum geomagnetic_rv_event_type { + GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME = (GEOMAGNETIC_RV_SENSOR << 16) | 0x0001, +}; + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif +//! End of a file + diff --git a/src/libsensord/sensor_internal.h b/src/libsensord/sensor_internal.h index 352154a..fcf11f7 100755 --- a/src/libsensord/sensor_internal.h +++ b/src/libsensord/sensor_internal.h @@ -51,6 +51,7 @@ extern "C" #include #include #include +#include #include diff --git a/src/libsensord/sensor_internal_deprecated.h b/src/libsensord/sensor_internal_deprecated.h index 69fa4ec..ee211d1 100755 --- a/src/libsensord/sensor_internal_deprecated.h +++ b/src/libsensord/sensor_internal_deprecated.h @@ -49,6 +49,7 @@ extern "C" #include #include #include +#include #include #include diff --git a/src/libsensord/sensor_light.h b/src/libsensord/sensor_light.h index 1ddb87a..bd35fb3 100755 --- a/src/libsensord/sensor_light.h +++ b/src/libsensord/sensor_light.h @@ -41,7 +41,7 @@ enum light_data_id { LIGHT_BASE_DATA_SET = (LIGHT_SENSOR << 16) | 0x0002, }; -enum light_evet_type { +enum light_event_type { LIGHT_EVENT_LUX_DATA_REPORT_ON_TIME = (LIGHT_SENSOR << 16) | 0x0001, LIGHT_EVENT_LEVEL_DATA_REPORT_ON_TIME = (LIGHT_SENSOR << 16) | 0x0002, LIGHT_EVENT_CHANGE_LEVEL = (LIGHT_SENSOR << 16) | 0x0004, diff --git a/src/libsensord/sensor_pressure.h b/src/libsensord/sensor_pressure.h index 5a113ed..dcf6b37 100755 --- a/src/libsensord/sensor_pressure.h +++ b/src/libsensord/sensor_pressure.h @@ -40,7 +40,7 @@ enum pressure_data_id { PRESSURE_BASE_DATA_SET = (PRESSURE_SENSOR << 16) | 0x0001, }; -enum pressure_evet_type { +enum pressure_event_type { PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME = (PRESSURE_SENSOR << 16) | 0x0001, }; diff --git a/src/libsensord/sensor_proxi.h b/src/libsensord/sensor_proxi.h index f4aa971..e7e2270 100755 --- a/src/libsensord/sensor_proxi.h +++ b/src/libsensord/sensor_proxi.h @@ -38,7 +38,7 @@ enum proxi_data_id { PROXIMITY_DISTANCE_DATA_SET = (PROXIMITY_SENSOR << 16) | 0x0002, }; -enum proxi_evet_type { +enum proxi_event_type { PROXIMITY_EVENT_CHANGE_STATE = (PROXIMITY_SENSOR << 16) | 0x0001, PROXIMITY_EVENT_STATE_REPORT_ON_TIME = (PROXIMITY_SENSOR << 16) | 0x0002, PROXIMITY_EVENT_DISTANCE_DATA_REPORT_ON_TIME = (PROXIMITY_SENSOR << 16) | 0x0004, diff --git a/src/libsensord/sensor_temperature.h b/src/libsensord/sensor_temperature.h index 9ef6a2e..52b5730 100755 --- a/src/libsensord/sensor_temperature.h +++ b/src/libsensord/sensor_temperature.h @@ -40,7 +40,7 @@ enum temperature_data_id { TEMPERATURE_BASE_DATA_SET = (TEMPERATURE_SENSOR << 16) | 0x0001, }; -enum temperature_evet_type { +enum temperature_event_type { TEMPERATURE_EVENT_RAW_DATA_REPORT_ON_TIME = (TEMPERATURE_SENSOR << 16) | 0x0001, }; diff --git a/src/rotation_vector/CMakeLists.txt b/src/rotation_vector/CMakeLists.txt index 7a527a7..3142605 100644 --- a/src/rotation_vector/CMakeLists.txt +++ b/src/rotation_vector/CMakeLists.txt @@ -1,3 +1,6 @@ IF("${RV}" STREQUAL "ON") add_subdirectory(rv) ENDIF() +IF("${GEOMAGNETIC_RV}" STREQUAL "ON") +add_subdirectory(geomagnetic_rv) +ENDIF() \ No newline at end of file diff --git a/src/rotation_vector/geomagnetic_rv/CMakeLists.txt b/src/rotation_vector/geomagnetic_rv/CMakeLists.txt new file mode 100755 index 0000000..36430ee --- /dev/null +++ b/src/rotation_vector/geomagnetic_rv/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 2.6) +project(geomagnetic_rv CXX) + +SET(SENSOR_NAME geomagnetic_rv_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(geomagnetic_rv_pkgs REQUIRED vconf) + +FOREACH(flag ${geomagnetic_rv_pkgs_LDFLAGS}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") +ENDFOREACH(flag) + +FOREACH(flag ${geomagnetic_rv_pkgs_CFLAGS}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") +ENDFOREACH(flag) + +add_library(${SENSOR_NAME} SHARED + geomagnetic_rv_sensor.cpp +) + +target_link_libraries(${SENSOR_NAME} ${geomagnetic_rv_pkgs_LDFLAGS} "-lm") + +install(TARGETS ${SENSOR_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR}/sensord) diff --git a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp new file mode 100755 index 0000000..ea5a446 --- /dev/null +++ b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp @@ -0,0 +1,350 @@ +/* + * sensord + * + * Copyright (c) 2015 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 +#include + +#define SENSOR_NAME "GEOMAGNETIC_RV_SENSOR" +#define SENSOR_TYPE_GEOMAGNETIC_RV "GEOMAGNETIC_ROTATION_VECTOR" + +#define ACCELEROMETER_ENABLED 0x01 +#define GEOMAGNETIC_ENABLED 0x02 +#define GEOMAGNETIC_RV_ENABLED 3 + +#define INITIAL_VALUE -1 + +#define MS_TO_US 1000 + +#define ELEMENT_NAME "NAME" +#define ELEMENT_VENDOR "VENDOR" +#define ELEMENT_RAW_DATA_UNIT "RAW_DATA_UNIT" +#define ELEMENT_DEFAULT_SAMPLING_TIME "DEFAULT_SAMPLING_TIME" +#define ELEMENT_ACCEL_STATIC_BIAS "ACCEL_STATIC_BIAS" +#define ELEMENT_GEOMAGNETIC_STATIC_BIAS "GEOMAGNETIC_STATIC_BIAS" +#define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION "ACCEL_ROTATION_DIRECTION_COMPENSATION" +#define ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION "GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION" +#define ELEMENT_ACCEL_SCALE "ACCEL_SCALE" +#define ELEMENT_GEOMAGNETIC_SCALE "GEOMAGNETIC_SCALE" +#define ELEMENT_MAGNETIC_ALIGNMENT_FACTOR "MAGNETIC_ALIGNMENT_FACTOR" + +void pre_process_data(sensor_data &data_out, const float *data_in, float *bias, int *sign, float scale) +{ + data_out.m_data.m_vec[0] = sign[0] * (data_in[0] - bias[0]) / scale; + data_out.m_data.m_vec[1] = sign[1] * (data_in[1] - bias[1]) / scale; + data_out.m_data.m_vec[2] = sign[2] * (data_in[2] - bias[2]) / scale; +} + +geomagnetic_rv_sensor::geomagnetic_rv_sensor() +: m_accel_sensor(NULL) +, m_magnetic_sensor(NULL) +, m_accuracy(-1) +, m_time(0) +{ + cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); + + m_name = string(SENSOR_NAME); + register_supported_event(GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME); + m_enable_geomagnetic_rv = 0; + + if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_VENDOR, m_vendor)) { + ERR("[VENDOR] is empty\n"); + throw ENXIO; + } + + INFO("m_vendor = %s", m_vendor.c_str()); + + if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) { + ERR("[DEFAULT_SAMPLING_TIME] is empty\n"); + throw ENXIO; + } + + INFO("m_default_sampling_time = %d", m_default_sampling_time); + + if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) { + ERR("[ACCEL_STATIC_BIAS] is empty\n"); + throw ENXIO; + } + + INFO("m_accel_static_bias = (%f, %f, %f)", m_accel_static_bias[0], m_accel_static_bias[1], m_accel_static_bias[2]); + + if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_GEOMAGNETIC_STATIC_BIAS, m_geomagnetic_static_bias, 3)) { + ERR("[GEOMAGNETIC_STATIC_BIAS] is empty\n"); + throw ENXIO; + } + + INFO("m_geomagnetic_static_bias = (%f, %f, %f)", m_geomagnetic_static_bias[0], m_geomagnetic_static_bias[1], m_geomagnetic_static_bias[2]); + + if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION, m_accel_rotation_direction_compensation, 3)) { + ERR("[ACCEL_ROTATION_DIRECTION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_accel_rotation_direction_compensation = (%d, %d, %d)", m_accel_rotation_direction_compensation[0], m_accel_rotation_direction_compensation[1], m_accel_rotation_direction_compensation[2]); + + if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION, m_geomagnetic_rotation_direction_compensation, 3)) { + ERR("[GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_geomagnetic_rotation_direction_compensation = (%d, %d, %d)", m_geomagnetic_rotation_direction_compensation[0], m_geomagnetic_rotation_direction_compensation[1], m_geomagnetic_rotation_direction_compensation[2]); + + if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_ACCEL_SCALE, &m_accel_scale)) { + ERR("[ACCEL_SCALE] is empty\n"); + throw ENXIO; + } + + INFO("m_accel_scale = %f", m_accel_scale); + + if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_GEOMAGNETIC_SCALE, &m_geomagnetic_scale)) { + ERR("[GEOMAGNETIC_SCALE] is empty\n"); + throw ENXIO; + } + + INFO("m_geomagnetic_scale = %f", m_geomagnetic_scale); + + if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_MAGNETIC_ALIGNMENT_FACTOR, &m_magnetic_alignment_factor)) { + ERR("[MAGNETIC_ALIGNMENT_FACTOR] is empty\n"); + throw ENXIO; + } + + INFO("m_magnetic_alignment_factor = %d", m_magnetic_alignment_factor); + + m_interval = m_default_sampling_time * MS_TO_US; + +} + +geomagnetic_rv_sensor::~geomagnetic_rv_sensor() +{ + INFO("geomagnetic_rv_sensor is destroyed!\n"); +} + +bool geomagnetic_rv_sensor::init() +{ + m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); + m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR); + + if (!m_accel_sensor || !m_magnetic_sensor) { + ERR("Failed to load sensors, accel: 0x%x, mag: 0x%x", + m_accel_sensor, m_magnetic_sensor); + return false; + } + + INFO("%s is created!\n", sensor_base::get_name()); + + return true; +} + +sensor_type_t geomagnetic_rv_sensor::get_type(void) +{ + return GEOMAGNETIC_RV_SENSOR; +} + +bool geomagnetic_rv_sensor::on_start(void) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); + m_accel_sensor->start(); + m_magnetic_sensor->add_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); + m_magnetic_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); + m_magnetic_sensor->start(); + + activate(); + return true; +} + +bool geomagnetic_rv_sensor::on_stop(void) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->delete_interval((intptr_t)this, false); + m_accel_sensor->stop(); + m_magnetic_sensor->delete_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); + m_magnetic_sensor->delete_interval((intptr_t)this, false); + m_magnetic_sensor->stop(); + + deactivate(); + return true; +} + +bool geomagnetic_rv_sensor::add_interval(int client_id, unsigned int interval) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->add_interval(client_id, interval, false); + m_magnetic_sensor->add_interval(client_id, interval, false); + + return sensor_base::add_interval(client_id, interval, false); +} + +bool geomagnetic_rv_sensor::delete_interval(int client_id) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->delete_interval(client_id, false); + m_magnetic_sensor->delete_interval(client_id, false); + + return sensor_base::delete_interval(client_id, false); +} + +void geomagnetic_rv_sensor::synthesize(const sensor_event_t& event, vector &outs) +{ + const float MIN_DELIVERY_DIFF_FACTOR = 0.75f; + unsigned long long diff_time; + + sensor_event_t rv_event; + quaternion quaternion_geo_rv; + + if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) { + diff_time = event.data.timestamp - m_time; + + if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale); + + m_accel.m_time_stamp = event.data.timestamp; + + m_enable_geomagnetic_rv |= ACCELEROMETER_ENABLED; + } + else if (event.event_type == GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME) { + diff_time = event.data.timestamp - m_time; + + if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + pre_process_data(m_magnetic, event.data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale); + + m_magnetic.m_time_stamp = event.data.timestamp; + + m_enable_geomagnetic_rv |= GEOMAGNETIC_ENABLED; + } + + if (m_enable_geomagnetic_rv == GEOMAGNETIC_RV_ENABLED) { + m_enable_geomagnetic_rv = 0; + + m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor; + + { + AUTOLOCK(m_fusion_mutex); + quaternion_geo_rv = m_orientation_filter.get_geomagnetic_quaternion(m_accel, m_magnetic); + } + + m_time = get_timestamp(); + + rv_event.sensor_id = get_id(); + rv_event.event_type = GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME; + rv_event.data.accuracy = SENSOR_ACCURACY_GOOD; + rv_event.data.timestamp = m_time; + rv_event.data.value_count = 4; + rv_event.data.values[0] = quaternion_geo_rv.m_quat.m_vec[1]; + rv_event.data.values[1] = quaternion_geo_rv.m_quat.m_vec[2]; + rv_event.data.values[2] = quaternion_geo_rv.m_quat.m_vec[3]; + rv_event.data.values[3] = quaternion_geo_rv.m_quat.m_vec[0]; + + push(rv_event); + } + + return; +} + +int geomagnetic_rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &data) +{ + sensor_data accel; + sensor_data magnetic; + + sensor_data_t accel_data; + sensor_data_t magnetic_data; + + quaternion quaternion_geo_rv; + + if (event_type != GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME) + return -1; + + m_accel_sensor->get_sensor_data(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME, accel_data); + m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, magnetic_data); + + pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale); + pre_process_data(magnetic, magnetic_data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale); + accel.m_time_stamp = accel_data.timestamp; + magnetic.m_time_stamp = magnetic_data.timestamp; + + m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor; + + { + AUTOLOCK(m_fusion_mutex); + quaternion_geo_rv = m_orientation_filter.get_geomagnetic_quaternion(m_accel, m_magnetic); + } + + data.accuracy = SENSOR_ACCURACY_GOOD; + data.timestamp = m_time; + data.value_count = 4; + data.values[0] = quaternion_geo_rv.m_quat.m_vec[1]; + data.values[1] = quaternion_geo_rv.m_quat.m_vec[2]; + data.values[2] = quaternion_geo_rv.m_quat.m_vec[3]; + data.values[3] = quaternion_geo_rv.m_quat.m_vec[0]; + + return 0; +} + +bool geomagnetic_rv_sensor::get_properties(sensor_properties_s &properties) +{ + properties.vendor = m_vendor; + properties.name = SENSOR_NAME; + properties.min_range = -1; + properties.max_range = 1; + properties.resolution = 0.000001; + properties.fifo_count = 0; + properties.max_batch_count = 0; + properties.min_interval = 1; + + return true; +} + +extern "C" sensor_module* create(void) +{ + geomagnetic_rv_sensor *sensor; + + try { + sensor = new(std::nothrow) geomagnetic_rv_sensor; + } catch (int err) { + ERR("Failed to create module, err: %d, cause: %s", err, strerror(err)); + return NULL; + } + + sensor_module *module = new(std::nothrow) sensor_module; + retvm_if(!module || !sensor, NULL, "Failed to allocate memory"); + + module->sensors.push_back(sensor); + return module; +} diff --git a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h new file mode 100755 index 0000000..9f3bdb1 --- /dev/null +++ b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h @@ -0,0 +1,75 @@ +/* + * sensord + * + * Copyright (c) 2015 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 _GEOMAGNETIC_RV_SENSOR_H_ +#define _GEOMAGNETIC_RV_SENSOR_H_ + +#include +#include +#include + +class geomagnetic_rv_sensor : public virtual_sensor { +public: + geomagnetic_rv_sensor(); + virtual ~geomagnetic_rv_sensor(); + + bool init(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_s &properties); + sensor_type_t get_type(void); + + int get_sensor_data(const unsigned int data_id, sensor_data_t &data); + +private: + sensor_base *m_accel_sensor; + sensor_base *m_magnetic_sensor; + + sensor_data m_accel; + sensor_data m_magnetic; + + cmutex m_value_mutex; + + orientation_filter m_orientation_filter; + + unsigned int m_enable_geomagnetic_rv; + + int m_accuracy; + unsigned long long m_time; + unsigned int m_interval; + + string m_vendor; + string m_raw_data_unit; + int m_default_sampling_time; + float m_accel_static_bias[3]; + float m_geomagnetic_static_bias[3]; + int m_accel_rotation_direction_compensation[3]; + int m_geomagnetic_rotation_direction_compensation[3]; + float m_accel_scale; + float m_geomagnetic_scale; + int m_magnetic_alignment_factor; + + bool on_start(void); + bool on_stop(void); +}; + +#endif /*_GEOMAGNETIC_RV_SENSOR_H_*/ diff --git a/src/sensor_fusion/design/documentation/equation/equation_13_updated.png b/src/sensor_fusion/design/documentation/equation/equation_13_updated.png new file mode 100644 index 0000000000000000000000000000000000000000..32f3f467fced35cf96d342ee896caba4c7259cbd GIT binary patch literal 1632 zcmV-m2A}zfP)Px#1ZP1_K>z@;j|==^1poj532;bRa{vGi!vFvd!vV){sAK>D1@%cpK~#8N?V16S zt11vhvo05u^<*`(nJgw>piz_tK_QX% tnk^tfjeJ`fw@c{q;fNl`90000TA!q>r z06Id@0ssJXgrEfg0O$xo3jhGn5rP%~0H7lTEdVIIKd7aeTK9ealt!c%#>;Ftt@!xapb7Tmu zaM#q0#hK3Z*)FBbu)5MgPj|2Tx%`vMK`S)eA+a35iNDWsM zNB)WS?`eyEvsiCPnh{9+$P*3xnO>^H!52I0dXcEoB>)EkK7Z_b@!$^92I_X}u27sGk_OEtW zbq7cvpnA|odtYj7J74;cs*w%a)T!3qgz~=WKfV6c$7(y*Y%s6H*dEw*R{jLHrB2M& zG}F^=l(~;$>Jip>>y3MKtqmjT?dhYtdG9+l3ecW}mjtzD>$zf&YnfM5dzqk>-=U+4 z+)c!%1v&B0Z^sKTW6Y^JHD?b}>vTAw=g&p>6xWUHEba6(`zLY;m01Uqk8b`L*r< zH>Bin>1sJ2#tRat(VQ)UHa->PO~dqGF6b99g~lUAe{{M&fm`eDhL6lc)_73?mIjeP?j%i zt+UsxhI{J034e`S=nil%oEPUqv^r?DTDu&yi3yT7VEf0WbaqkonCtD=RbY$-Lfd|4 z!siNC^|YU9#+ftsyk2<**4l7Oz1`US*D<*6039d^(dwYpo@llA5~xAz77?3@e}6x5 z`HdxvN*25}KbIJlW@7s>ic#bYB>c?$t&61??iaYGXMwqEb9+5gyGDY>=^cL`qiSuq zrQUX|(b>K&=h8)Z4Jsn(Tc$Mlps#bqDB%)P(M1Yu$xy(^u=K6j1JvyC?b2P~X9sQFsrL ziJO~N3~WCpWhBldwUf&cob(+|??nf@iFIQ}>vmWzqj1)>MR$M@Iph|G;5q7Q=eI!{ znYLN{*2JUWEf}-Mo;<3I2m@M>)8Tr$L+fv_-}hG6)M;`C-2no6|58&q|BZsy|I}v8 zV*dMOEMt9oeZOALL~>)EnG=iz#*wj{e>(U_vAP9%m<@JxQbg;ExDZ@&wX~?w9YAo| z(B~j2xsy5g?zzQJ$Lo`lGnmy>*3VbLj>bT1+|!b@ip>Qlk-%IuXKw=x&5wf?007|M zhM)xi0O$xo3jhGn5rP%~0H7lTEdT&OM+jN~0Dz7Vv;Y7A9U*7|0025d&;kGebcCP< e008L5=kp&A3Y$AH?8Msu0000
- +
diff --git a/src/sensor_fusion/design/lib/axis_rot2quat.m b/src/sensor_fusion/design/lib/axis_rot2quat.m new file mode 100644 index 0000000..b2756f5 --- /dev/null +++ b/src/sensor_fusion/design/lib/axis_rot2quat.m @@ -0,0 +1,26 @@ +% axis_rot2quat +% +% Copyright (c) 2015 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. + +% Axis rotation to quaternion function +% +% - convert sensor rotation axis and angle values to quaternion +function q = axis_rot2quat(axis, angle) + q0 = cos(angle/2); + q1 = -axis(1)*sin(angle/2); + q2 = -axis(2)*sin(angle/2); + q3 = -axis(3)*sin(angle/2); + q = [q0 q1 q2 q3]; +end diff --git a/src/sensor_fusion/design/lib/estimate_geomagnetic_rv.m b/src/sensor_fusion/design/lib/estimate_geomagnetic_rv.m index 340f4d0..c49c511 100755 --- a/src/sensor_fusion/design/lib/estimate_geomagnetic_rv.m +++ b/src/sensor_fusion/design/lib/estimate_geomagnetic_rv.m @@ -1,6 +1,6 @@ -% estimate_gravity +% estimate_geomagnetic_rv % -% Copyright (c) 2014 Samsung Electronics Co., Ltd. +% Copyright (c) 2015 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. @@ -14,23 +14,16 @@ % See the License for the specific language governing permissions and % limitations under the License. -% Gravitational Force Estimation function +% Geomagnetic Rotation Vector Estimation function % +% - Input orientation using only Accel and Geomagnetic sensors % - Orientation Estimation using estimate_orientation function -% - Project the orientation on the device reference axes to obtain -% gravitational force on specific reference axes - +% - Output aiding system quaternion as geomagnetic rotation vector function [Quat_aid] = estimate_geomagnetic_rv(Accel_data, Mag_data) -GRAVITY = 9.80665; -RAD2DEG = 57.2957795; - BUFFER_SIZE = size(Accel_data,2); -Gravity = zeros(3,BUFFER_SIZE); - -OR_driv = zeros(3,BUFFER_SIZE); Gyro_data = zeros(4,BUFFER_SIZE); Quat_driv = zeros(4,BUFFER_SIZE); diff --git a/src/sensor_fusion/design/lib/estimate_orientation.m b/src/sensor_fusion/design/lib/estimate_orientation.m index 0094b04..60541a5 100755 --- a/src/sensor_fusion/design/lib/estimate_orientation.m +++ b/src/sensor_fusion/design/lib/estimate_orientation.m @@ -1,6 +1,6 @@ % estimate_orientation % -% Copyright (c) 2014 Samsung Electronics Co., Ltd. +% Copyright (c) 2015 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. diff --git a/src/sensor_fusion/design/lib/euler2quat.m b/src/sensor_fusion/design/lib/euler2quat.m new file mode 100644 index 0000000..8442aa1 --- /dev/null +++ b/src/sensor_fusion/design/lib/euler2quat.m @@ -0,0 +1,38 @@ +% euler2quat +% +% Copyright (c) 2015 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. + +% Euler Angles to quaternion conversion +% +% - Implementation to convert orientation in terms of euler angles to quaternion + +function q = euler2quat(euler) + theta = euler(1); + phi = euler(2); + psi = euler(3); + R(1,1,1) = cos(psi)*cos(theta); + R(1,2,1) = -sin(psi)*cos(phi) + cos(psi)*sin(theta)*sin(phi); + R(1,3,1) = sin(psi)*sin(phi) + cos(psi)*sin(theta)*cos(phi); + + R(2,1,1) = sin(psi)*cos(theta); + R(2,2,1) = cos(psi)*cos(phi) + sin(psi)*sin(theta)*sin(phi); + R(2,3,1) = -cos(psi)*sin(phi) + sin(psi)*sin(theta)*cos(phi); + + R(3,1,1) = -sin(theta); + R(3,2,1) = cos(theta)*sin(phi); + R(3,3,1) = cos(theta)*cos(phi); + + q = rot_mat2quat(R); +end \ No newline at end of file diff --git a/src/sensor_fusion/design/sf_geomagnetic_rv.m b/src/sensor_fusion/design/sf_geomagnetic_rv.m index 78802eb..69bbdd5 100755 --- a/src/sensor_fusion/design/sf_geomagnetic_rv.m +++ b/src/sensor_fusion/design/sf_geomagnetic_rv.m @@ -75,16 +75,19 @@ hfig=(figure); scrsz = get(0,'ScreenSize'); set(hfig,'position',scrsz); % Geomagnetic Rotation Vector Plot Results +subplot(3,1,1) UA = Orientation_RV(1,:); p1 = plot(1:length(UA),UA(1,1:length(UA)),'k'); -hold on; -grid on; +legend(p1,'x-axis'); +title(['Pitch']); +subplot(3,1,2) UA = Orientation_RV(2,:); -p2 = plot(1:length(UA),UA(1,1:length(UA)),'b'); -hold on; -grid on; +p1 = plot(1:length(UA),UA(1,1:length(UA)),'b'); +legend(p1,'y-axis'); +title(['Roll']); +subplot(3,1,3) UA = Orientation_RV(3,:); -p3 = plot(1:length(UA),UA(1,1:length(UA)),'r'); -title(['Geomagnetic Rotation Vector']); -legend([p1 p2 p3],'x-axis', 'y-axis', 'z-axis'); +p1 = plot(1:length(UA),UA(1,1:length(UA)),'r'); +legend(p1,'z-axis'); +title(['Yaw']); diff --git a/src/server/command_worker.cpp b/src/server/command_worker.cpp index cd3edc9..c161171 100755 --- a/src/server/command_worker.cpp +++ b/src/server/command_worker.cpp @@ -30,9 +30,11 @@ #include #include #include +#include using std::string; using std::make_pair; +using std::set; command_worker::cmd_handler_t command_worker::m_cmd_handlers[]; sensor_raw_data_map command_worker::m_sensor_raw_data_map; @@ -847,21 +849,17 @@ csensor_event_dispatcher& command_worker::get_event_dispathcher(void) void insert_priority_list(unsigned int event_type) { - if (event_type == ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME) { + if (event_type == ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME || + event_type == LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME || + event_type == GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME || + event_type == ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME) { priority_list.insert(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); priority_list.insert(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); priority_list.insert(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); - } - - if (event_type == LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME) { - priority_list.insert(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); - priority_list.insert(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); - priority_list.insert(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); - } + } - if (event_type == GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME) { + if (event_type == GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME) { priority_list.insert(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); - priority_list.insert(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); priority_list.insert(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); - } + } } diff --git a/src/server/permission_checker.cpp b/src/server/permission_checker.cpp index aec5aef..ad05b02 100755 --- a/src/server/permission_checker.cpp +++ b/src/server/permission_checker.cpp @@ -27,10 +27,9 @@ #define SECURITY_LIB "/usr/lib/libsecurity-server-client.so.1" permission_checker::permission_checker() -: security_server_check_privilege_by_sockfd(NULL) +: m_security_server_check_privilege_by_sockfd(NULL) , m_security_handle(NULL) , m_permission_set(0) - { init(); } @@ -56,10 +55,10 @@ bool permission_checker::init_security_lib(void) return false; } - security_server_check_privilege_by_sockfd = + m_security_server_check_privilege_by_sockfd = (security_server_check_privilege_by_sockfd_t) dlsym(m_security_handle, "security_server_check_privilege_by_sockfd"); - if (!security_server_check_privilege_by_sockfd) { + if (!m_security_server_check_privilege_by_sockfd) { ERR("Failed to load symbol"); dlclose(m_security_handle); m_security_handle = NULL; @@ -94,8 +93,8 @@ int permission_checker::get_permission(int sock_fd) for (unsigned int i = 0; i < m_permission_infos.size(); ++i) { if (!m_permission_infos[i]->need_to_check) { permission |= m_permission_infos[i]->permission; - } else if ((m_permission_set & m_permission_infos[i]->permission) && security_server_check_privilege_by_sockfd) { - if (security_server_check_privilege_by_sockfd(sock_fd, m_permission_infos[i]->name.c_str(), m_permission_infos[i]->access_right.c_str()) == 1) { + } else if ((m_permission_set & m_permission_infos[i]->permission) && m_security_server_check_privilege_by_sockfd) { + if (m_security_server_check_privilege_by_sockfd(sock_fd, m_permission_infos[i]->name.c_str(), m_permission_infos[i]->access_right.c_str()) == 1) { permission |= m_permission_infos[i]->permission; } } diff --git a/src/server/permission_checker.h b/src/server/permission_checker.h index 5121e41..ad94708 100755 --- a/src/server/permission_checker.h +++ b/src/server/permission_checker.h @@ -55,7 +55,7 @@ private: bool init_security_lib(void); void init(); - security_server_check_privilege_by_sockfd_t security_server_check_privilege_by_sockfd; + security_server_check_privilege_by_sockfd_t m_security_server_check_privilege_by_sockfd; void *m_security_handle; permission_info_vector m_permission_infos; diff --git a/src/shared/sensor_common.h b/src/shared/sensor_common.h index 311ca62..aae9081 100755 --- a/src/shared/sensor_common.h +++ b/src/shared/sensor_common.h @@ -63,6 +63,7 @@ typedef enum { GRAVITY_SENSOR, LINEAR_ACCEL_SENSOR, ROTATION_VECTOR_SENSOR, + GEOMAGNETIC_RV_SENSOR, ORIENTATION_SENSOR, PIR_SENSOR, PIR_LONG_SENSOR, diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 6fb5085..5d65116 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -36,6 +36,7 @@ add_executable(pressure src/pressure.c) add_executable(temperature src/temperature.c) add_executable(light src/light.c) add_executable(rotation_vector src/rotation_vector.c) +add_executable(geomagnetic_rv src/geomagnetic_rv.c) SET_TARGET_PROPERTIES(accelerometer PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(geomagnetic PROPERTIES LINKER_LANGUAGE C) @@ -48,6 +49,7 @@ SET_TARGET_PROPERTIES(pressure PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(temperature PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(light PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(rotation_vector PROPERTIES LINKER_LANGUAGE C) +SET_TARGET_PROPERTIES(geomagnetic_rv PROPERTIES LINKER_LANGUAGE C) target_link_libraries(accelerometer glib-2.0 dlog sensor) target_link_libraries(geomagnetic glib-2.0 dlog sensor) @@ -60,6 +62,7 @@ target_link_libraries(pressure glib-2.0 dlog sensor) target_link_libraries(temperature glib-2.0 dlog sensor) target_link_libraries(light glib-2.0 dlog sensor) target_link_libraries(rotation_vector glib-2.0 dlog sensor) +target_link_libraries(geomagnetic_rv glib-2.0 dlog sensor) INSTALL(TARGETS accelerometer DESTINATION /usr/bin/) INSTALL(TARGETS geomagnetic DESTINATION /usr/bin/) @@ -72,4 +75,5 @@ INSTALL(TARGETS pressure DESTINATION /usr/bin/) INSTALL(TARGETS temperature DESTINATION /usr/bin/) INSTALL(TARGETS light DESTINATION /usr/bin/) INSTALL(TARGETS rotation_vector DESTINATION /usr/bin/) +INSTALL(TARGETS geomagnetic_rv DESTINATION /usr/bin/) diff --git a/virtual_sensors.xml b/virtual_sensors.xml index 1c20c0d..ab299df 100755 --- a/virtual_sensors.xml +++ b/virtual_sensors.xml @@ -55,6 +55,19 @@ + + + + + + + + + + + + + @@ -111,5 +124,18 @@ + + + + + + + + + + + + + -- 2.7.4 From 0728c03c5d8ba8a741bd4ff2c7ea22a5ece7bbfb Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Fri, 13 Feb 2015 15:33:50 +0530 Subject: [PATCH 11/16] Gaming rotation vector - Feature merge from devel/tizen branch This is third of the four patches being submitted to merge the changes from devel/tizen to tizen branch. This contains the changes for adding gaming rotation vector virtual sensor. Build has been checked for armv7l and aarch64 for public repo. Change-Id: I2da4478c01bf450d49240800695b57969ae22be7 --- src/sensor_fusion/design/lib/estimate_gaming_rv.m | 37 +++ .../design/lib/estimate_orientation.m | 271 +++++++++++++-------- src/sensor_fusion/design/sf_gaming_rv.m | 93 +++++++ src/sensor_fusion/orientation_filter.cpp | 86 +++++++ src/sensor_fusion/orientation_filter.h | 4 + src/sensor_fusion/test/orientation_sensor.cpp | 11 + src/sensor_fusion/test/orientation_sensor.h | 2 + .../orientation_sensor_main.cpp | 7 +- 8 files changed, 408 insertions(+), 103 deletions(-) create mode 100755 src/sensor_fusion/design/lib/estimate_gaming_rv.m create mode 100755 src/sensor_fusion/design/sf_gaming_rv.m diff --git a/src/sensor_fusion/design/lib/estimate_gaming_rv.m b/src/sensor_fusion/design/lib/estimate_gaming_rv.m new file mode 100755 index 0000000..cbaaca7 --- /dev/null +++ b/src/sensor_fusion/design/lib/estimate_gaming_rv.m @@ -0,0 +1,37 @@ +% estimate_gaming_rv +% +% Copyright (c) 2015 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. + +% Gaming Rotation Vector Estimation function +% +% - Input orientation using only Accel and Gyroscope sensors +% - Orientation Estimation using estimate_orientation function +% - Output driving system quaternion as gaming rotation vector + + +function [Quat_driv] = estimate_gaming_rv(Accel_data, Gyro_data) + +BUFFER_SIZE = size(Accel_data,2); + +Mag_data = zeros(4,BUFFER_SIZE); + +Quat_driv = zeros(4,BUFFER_SIZE); +Quat_aid = zeros(4,BUFFER_SIZE); +Quat_err = zeros(4,BUFFER_SIZE); + +% estimate orientation +[Quat_driv, Quat_aid, Quat_err] = estimate_orientation(Accel_data, Gyro_data, Mag_data); + +end diff --git a/src/sensor_fusion/design/lib/estimate_orientation.m b/src/sensor_fusion/design/lib/estimate_orientation.m index 60541a5..6f0e852 100755 --- a/src/sensor_fusion/design/lib/estimate_orientation.m +++ b/src/sensor_fusion/design/lib/estimate_orientation.m @@ -16,7 +16,10 @@ % Orientation Estimation function % -% - Orientation Estimation using Gyroscope as the driving system and Accelerometer+Geo-Magnetic Sensors as Aiding System. +% - Orientation and rotation vector Estimation using Gyroscope as the driving system and +% Accelerometer+Geo-Magnetic Sensors as Aiding System. +% - Gaming rotation vector Estimation using Accelerometer and Gyroscope sensors. +% - Geomagnetic rotation vector Estimation using Accelerometer and Geomagnetic sensors. % - Quaternion based approach % - Estimation and correction of orientation errors and bias errors for gyroscope using Kalman filter @@ -26,11 +29,16 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G MAGNETIC_ALIGNMENT_FACTOR = -1; GYRO_DATA_DISABLED = 0; + MAG_DATA_DISABLED = 0; if Gyro_data(4,1) == 0 GYRO_DATA_DISABLED = 1; end + if Mag_data(4,1) == 0 + MAG_DATA_DISABLED = 1; + end + GRAVITY = 9.80665; PI = 3.141593; NON_ZERO_VAL = 0.1; @@ -49,26 +57,31 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G Az = Accel_data(3,:); ATime = Accel_data(4,:); - Mx = Mag_data(1,:); - My = Mag_data(2,:); - Mz = Mag_data(3,:); - MTime = Mag_data(4,:); + mag_x = zeros(1,BUFFER_SIZE); + mag_y = zeros(1,BUFFER_SIZE); + mag_z = zeros(1,BUFFER_SIZE); + MTime = zeros(1,BUFFER_SIZE); + + if MAG_DATA_DISABLED != 1 + Mx = Mag_data(1,:); + My = Mag_data(2,:); + Mz = Mag_data(3,:); + MTime = Mag_data(4,:); + end acc_x = zeros(1,BUFFER_SIZE); acc_y = zeros(1,BUFFER_SIZE); acc_z = zeros(1,BUFFER_SIZE); - mag_x = zeros(1,BUFFER_SIZE); - mag_y = zeros(1,BUFFER_SIZE); - mag_z = zeros(1,BUFFER_SIZE); quat_aid = zeros(BUFFER_SIZE,4); quat_driv = zeros(BUFFER_SIZE,4); + quat_gaming_rv = zeros(BUFFER_SIZE,4); quat_error = zeros(BUFFER_SIZE,4); - + acc_e = [0.0;0.0;1.0]; % gravity vector in earth frame mag_e = [0.0;MAGNETIC_ALIGNMENT_FACTOR;0.0]; % magnetic field vector in earth frame - if GYRO_DATA_DISABLED != 1 + if GYRO_DATA_DISABLED != 1 % Gyroscope Bias Variables Bx = 0; By = 0; Bz = 0; @@ -114,6 +127,8 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G quat_driv(1,:) = [1 0 0 0]; end + q = [1 0 0 0]; + % first order filtering for i = 1:BUFFER_SIZE % normalize accelerometer measurements @@ -122,27 +137,32 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G acc_y(i) = norm_acc * Ay(i); acc_z(i) = norm_acc * Az(i); - % normalize magnetometer measurements - norm_mag = 1/sqrt(Mx(i)^2 + My(i)^2 + Mz(i)^2); - mag_x(i) = norm_mag * Mx(i); - mag_y(i) = norm_mag * My(i); - mag_z(i) = norm_mag * Mz(i); - - UA(i) = sqrt(acc_x(i)^2 + acc_y(i)^2 + acc_z(i)^2) - GRAVITY; - - % Aiding System (Accelerometer + Geomagnetic) quaternion generation % gravity vector in body frame acc_b =[acc_x(i);acc_y(i);acc_z(i)]; - % magnetic field vector in body frame - mag_b =[mag_x(i);mag_y(i);mag_z(i)]; - % compute measurement quaternion with TRIAD algorithm - acc_b_x_mag_b = cross(acc_b,mag_b); - acc_e_x_mag_e = cross(acc_e,mag_e); - M_b = [acc_b acc_b_x_mag_b cross(acc_b_x_mag_b,acc_b)]; - M_e = [acc_e acc_e_x_mag_e cross(acc_e_x_mag_e,acc_e)]; - Rot_m = M_e * M_b'; - quat_aid(i,:) = rot_mat2quat(Rot_m); + if MAG_DATA_DISABLED != 1 + % normalize magnetometer measurements + norm_mag = 1/sqrt(Mx(i)^2 + My(i)^2 + Mz(i)^2); + mag_x(i) = norm_mag * Mx(i); + mag_y(i) = norm_mag * My(i); + mag_z(i) = norm_mag * Mz(i); + + % Aiding System (Accelerometer + Geomagnetic) quaternion generation + % magnetic field vector in body frame + mag_b =[mag_x(i);mag_y(i);mag_z(i)]; + + % compute measurement quaternion with TRIAD algorithm + acc_b_x_mag_b = cross(acc_b,mag_b); + acc_e_x_mag_e = cross(acc_e,mag_e); + M_b = [acc_b acc_b_x_mag_b cross(acc_b_x_mag_b,acc_b)]; + M_e = [acc_e acc_e_x_mag_e cross(acc_e_x_mag_e,acc_e)]; + Rot_m = M_e * M_b'; + quat_aid(i,:) = rot_mat2quat(Rot_m); + else + axis = cross(acc_b, acc_e); + angle = acos(dot(acc_b, acc_e)); + quat_aid(i,:) = axis_rot2quat(axis, angle); + end if GYRO_DATA_DISABLED != 1 gyr_x(i) = Gx(i) * PI; @@ -195,9 +215,7 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G q = quat_aid(i,:); end - q_t = [q(2) q(3) q(4)]'; - Rtan = (q(1)^2 - q_t'*q_t)*eye(3) + 2*q_t*q_t' - 2*q(1)*[0 -q(4) q(3);q(4) 0 -q(2);-q(3) q(2) 0]; - F = [[0 gyr_z(i) -gyr_y(i);-gyr_z(i) 0 gyr_x(i);gyr_y(i) -gyr_x(i) 0] Rtan; zeros(3,3) (-(1/TauW) * eye(3))]; + F = [[0 gyr_z(i) -gyr_y(i);-gyr_z(i) 0 gyr_x(i);gyr_y(i) -gyr_x(i) 0] zeros(3,3); zeros(3,3) (-(1/TauW) * eye(3))]; % Time Update if i > 1 @@ -225,8 +243,16 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G x2 = euler_e(2)'/PI; x3 = euler_e(3)'/PI; - q = quat_prod(quat_driv(i,:), [1 x1 x2 x3]) * PI; - q = q / norm(q); + if MAG_DATA_DISABLED != 1 + q = quat_prod(quat_driv(i,:), [1 x1 x2 x3]) * PI; + q = q / norm(q); + else + euler_aid = quat2euler(quat_aid(i,:)); + euler_driv = quat2euler(quat_driv(i,:)); + + euler_gaming_rv = [euler_aid(2) euler_aid(1) euler_driv(3)]; + quat_gaming_rv(i,:) = euler2quat(euler_gaming_rv); + end if i > 1 e = [x1 x2 x3 x(4,i) x(5,i) x(6,i)]; @@ -247,8 +273,12 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G end end + if MAG_DATA_DISABLED == 1 + quat_driv = quat_gaming_rv; + end + if PLOT_SCALED_SENSOR_COMPARISON_DATA == 1 - % Accelerometer/Gyroscope/Magnetometer scaled Plot results + % Accelerometer/Gyroscope scaled Plot results hfig=(figure); scrsz = get(0,'ScreenSize'); set(hfig,'position',scrsz); @@ -256,32 +286,65 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G p1 = plot(1:BUFFER_SIZE,acc_x(1,1:BUFFER_SIZE),'r'); hold on; grid on; - p2 = plot(1:BUFFER_SIZE,Gx(1,1:BUFFER_SIZE),'b'); - hold on; - grid on; - p3 = plot(1:BUFFER_SIZE,mag_x(1,1:BUFFER_SIZE),'k'); - title(['Accelerometer/Gyroscope/Magnetometer X-Axis Plot']); - legend([p1 p2 p3],'Acc_X', 'Gyr_X', 'Mag_X'); + if GYRO_DATA_DISABLED != 1 + p2 = plot(1:BUFFER_SIZE,Gx(1,1:BUFFER_SIZE),'b'); + if MAG_DATA_DISABLED != 1 + hold on; + grid on; + p3 = plot(1:BUFFER_SIZE,mag_x(1,1:BUFFER_SIZE),'k'); + title(['Accelerometer/Gyroscope/Magnetometer X-Axis Plot']); + legend([p1 p2 p3],'Acc_X', 'Gyr_X', 'Mag_X'); + else + title(['Accelerometer/Gyroscope X-Axis Plot']); + legend([p1 p2],'Acc_X', 'Gyr_X'); + end + else + p2 = plot(1:BUFFER_SIZE,mag_x(1,1:BUFFER_SIZE),'k'); + title(['Accelerometer/Magnetometer X-Axis Plot']); + legend([p1 p2],'Acc_X', 'Mag_X'); + end subplot(3,1,2) p1 = plot(1:BUFFER_SIZE,acc_y(1,1:BUFFER_SIZE),'r'); hold on; grid on; - p2 = plot(1:BUFFER_SIZE,Gy(1,1:BUFFER_SIZE),'b'); - hold on; - grid on; - p3 = plot(1:BUFFER_SIZE,mag_y(1,1:BUFFER_SIZE),'k'); - title(['Accelerometer/Gyroscope/Magnetometer Y-Axis Plot']); - legend([p1 p2 p3],'Acc_Y', 'Gyr_Y', 'Mag_Y'); + if GYRO_DATA_DISABLED != 1 + p2 = plot(1:BUFFER_SIZE,Gy(1,1:BUFFER_SIZE),'b'); + if MAG_DATA_DISABLED != 1 + hold on; + grid on; + p3 = plot(1:BUFFER_SIZE,mag_y(1,1:BUFFER_SIZE),'k'); + title(['Accelerometer/Gyroscope/Magnetometer Y-Axis Plot']); + legend([p1 p2 p3],'Acc_Y', 'Gyr_Y', 'Mag_Y'); + else + title(['Accelerometer/Gyroscope Y-Axis Plot']); + legend([p1 p2],'Acc_Y', 'Gyr_Y'); + end + else + p2 = plot(1:BUFFER_SIZE,mag_y(1,1:BUFFER_SIZE),'k'); + title(['Accelerometer/Magnetometer Y-Axis Plot']); + legend([p1 p2],'Acc_X', 'Mag_Y'); + end subplot(3,1,3) p1 = plot(1:BUFFER_SIZE,acc_z(1,1:BUFFER_SIZE),'r'); hold on; grid on; - p2 = plot(1:BUFFER_SIZE,Gz(1,1:BUFFER_SIZE),'b'); - hold on; - grid on; - p3 = plot(1:BUFFER_SIZE,mag_z(1,1:BUFFER_SIZE),'k'); - title(['Accelerometer/Gyroscope/Magnetometer Z-Axis Plot']); - legend([p1 p2 p3],'Acc_Z', 'Gyr_Z', 'Mag_Z'); + if GYRO_DATA_DISABLED != 1 + p2 = plot(1:BUFFER_SIZE,Gz(1,1:BUFFER_SIZE),'b'); + if MAG_DATA_DISABLED != 1 + hold on; + grid on; + p3 = plot(1:BUFFER_SIZE,mag_z(1,1:BUFFER_SIZE),'k'); + title(['Accelerometer/Gyroscope/Magnetometer Z-Axis Plot']); + legend([p1 p2 p3],'Acc_Z', 'Gyr_Z', 'Mag_Z'); + else + title(['Accelerometer/Gyroscope Z-Axis Plot']); + legend([p1 p2],'Acc_Z', 'Gyr_Z'); + end + else + p2 = plot(1:BUFFER_SIZE,mag_z(1,1:BUFFER_SIZE),'k'); + title(['Accelerometer/Magnetometer Z-Axis Plot']); + legend([p1 p2],'Acc_Z', 'Mag_Z'); + end end if PLOT_INDIVIDUAL_SENSOR_INPUT_DATA == 1 @@ -311,56 +374,60 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G title(['Accelerometer Z-Axis Plot']); legend([p1 p2],'input signal','low-pass filtered signal'); - % Gyroscope Raw (vs) filtered output - hfig=(figure); - scrsz = get(0,'ScreenSize'); - set(hfig,'position',scrsz); - subplot(3,1,1) - p1 = plot(1:BUFFER_SIZE,Gx(1,1:BUFFER_SIZE),'r'); - hold on; - grid on; - p2 = plot(1:BUFFER_SIZE,Gx(1,1:BUFFER_SIZE),'b'); - title(['Gyroscope X-Axis Plot']); - legend([p1 p2],'input signal','low-pass filtered signal'); - subplot(3,1,2) - p1 = plot(1:BUFFER_SIZE,Gy(1,1:BUFFER_SIZE),'r'); - hold on; - grid on; - p2 = plot(1:BUFFER_SIZE,Gy(1,1:BUFFER_SIZE),'b'); - title(['Gyroscope Y-Axis Plot']); - legend([p1 p2],'input signal','low-pass filtered signal'); - subplot(3,1,3) - p1 = plot(1:BUFFER_SIZE,Gz(1,1:BUFFER_SIZE),'r'); - hold on; - grid on; - p2 = plot(1:BUFFER_SIZE,Gz(1,1:BUFFER_SIZE),'b'); - title(['Gyroscope Z-Axis Plot']); - legend([p1 p2],'input signal','low-pass filtered signal'); + if GYRO_DATA_DISABLED != 1 + % Gyroscope Raw (vs) filtered output + hfig=(figure); + scrsz = get(0,'ScreenSize'); + set(hfig,'position',scrsz); + subplot(3,1,1) + p1 = plot(1:BUFFER_SIZE,Gx(1,1:BUFFER_SIZE),'r'); + hold on; + grid on; + p2 = plot(1:BUFFER_SIZE,Gx(1,1:BUFFER_SIZE),'b'); + title(['Gyroscope X-Axis Plot']); + legend([p1 p2],'input signal','low-pass filtered signal'); + subplot(3,1,2) + p1 = plot(1:BUFFER_SIZE,Gy(1,1:BUFFER_SIZE),'r'); + hold on; + grid on; + p2 = plot(1:BUFFER_SIZE,Gy(1,1:BUFFER_SIZE),'b'); + title(['Gyroscope Y-Axis Plot']); + legend([p1 p2],'input signal','low-pass filtered signal'); + subplot(3,1,3) + p1 = plot(1:BUFFER_SIZE,Gz(1,1:BUFFER_SIZE),'r'); + hold on; + grid on; + p2 = plot(1:BUFFER_SIZE,Gz(1,1:BUFFER_SIZE),'b'); + title(['Gyroscope Z-Axis Plot']); + legend([p1 p2],'input signal','low-pass filtered signal'); + end - % Magnetometer Raw (vs) filtered output - hfig=(figure); - scrsz = get(0,'ScreenSize'); - set(hfig,'position',scrsz); - subplot(3,1,1) - p1 = plot(1:BUFFER_SIZE,Mx(1,1:BUFFER_SIZE),'r'); - hold on; - grid on; - p2 = plot(1:BUFFER_SIZE,Mx(1,1:BUFFER_SIZE),'b'); - title(['Magnetometer X-Axis Plot']); - legend([p1 p2],'input signal','low-pass filtered signal'); - subplot(3,1,2) - p1 = plot(1:BUFFER_SIZE,My(1,1:BUFFER_SIZE),'r'); - hold on; - grid on; - p2 = plot(1:BUFFER_SIZE,My(1,1:BUFFER_SIZE),'b'); - title(['Magnetometer Y-Axis Plot']); - legend([p1 p2],'input signal','low-pass filtered signal'); - subplot(3,1,3) - p1 = plot(1:BUFFER_SIZE,Mz(1,1:BUFFER_SIZE),'r'); - hold on; - grid on; - p2 = plot(1:BUFFER_SIZE,Mz(1,1:BUFFER_SIZE),'b'); - title(['Magnetometer Z-Axis Plot']); - legend([p1 p2],'input signal','low-pass filtered signal'); + if MAG_DATA_DISABLED != 1 + % Magnetometer Raw (vs) filtered output + hfig=(figure); + scrsz = get(0,'ScreenSize'); + set(hfig,'position',scrsz); + subplot(3,1,1) + p1 = plot(1:BUFFER_SIZE,Mx(1,1:BUFFER_SIZE),'r'); + hold on; + grid on; + p2 = plot(1:BUFFER_SIZE,Mx(1,1:BUFFER_SIZE),'b'); + title(['Magnetometer X-Axis Plot']); + legend([p1 p2],'input signal','low-pass filtered signal'); + subplot(3,1,2) + p1 = plot(1:BUFFER_SIZE,My(1,1:BUFFER_SIZE),'r'); + hold on; + grid on; + p2 = plot(1:BUFFER_SIZE,My(1,1:BUFFER_SIZE),'b'); + title(['Magnetometer Y-Axis Plot']); + legend([p1 p2],'input signal','low-pass filtered signal'); + subplot(3,1,3) + p1 = plot(1:BUFFER_SIZE,Mz(1,1:BUFFER_SIZE),'r'); + hold on; + grid on; + p2 = plot(1:BUFFER_SIZE,Mz(1,1:BUFFER_SIZE),'b'); + title(['Magnetometer Z-Axis Plot']); + legend([p1 p2],'input signal','low-pass filtered signal'); + end end end diff --git a/src/sensor_fusion/design/sf_gaming_rv.m b/src/sensor_fusion/design/sf_gaming_rv.m new file mode 100755 index 0000000..892b86e --- /dev/null +++ b/src/sensor_fusion/design/sf_gaming_rv.m @@ -0,0 +1,93 @@ +% sf_gaming_rv +% +% Copyright (c) 2015 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. + +% Sensor Fusion Implementation for Determination of Gaming rotation Vector +% +% - Input Accelerometer and Gyroscope sensor data +% - Call estimate_gaming_rotation +% - Plot results for gaming rotation on reference axis + +addpath('lib'); +clear +close all +clc + +GRAVITY = 9.80665; + +Max_Range_Accel = 39.203407; Min_Range_Accel = -39.204006; Res_Accel = 0.000598; +Max_Range_Gyro = 1146.862549; Min_Range_Gyro = -1146.880005; Res_Gyro = 0.017500; + +Bias_Ax = 0.098586; +Bias_Ay = 0.18385; +Bias_Az = 10.084 - GRAVITY; + +Bias_Gx = -5.3539; +Bias_Gy = 0.24325; +Bias_Gz = 2.3391; + +BUFFER_SIZE = 1095; + +Accel_data = zeros(4,BUFFER_SIZE); +Gyro_data = zeros(4,BUFFER_SIZE); + +Game_RV = zeros(4,BUFFER_SIZE); +Orientation_RV = zeros(3,BUFFER_SIZE); + +% Sensor Data simulating orientation motions + +% get accel x,y,z axis data from stored file +Accel_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,1))') - Bias_Ax)(1:BUFFER_SIZE); +Accel_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,2))') - Bias_Ay)(1:BUFFER_SIZE); +Accel_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,3))') - Bias_Az)(1:BUFFER_SIZE); +Accel_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,4))')(1:BUFFER_SIZE); + +% get gyro x,y,z axis data from stored file +Gyro_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,1))') - Bias_Gx)(1:BUFFER_SIZE); +Gyro_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,2))') - Bias_Gy)(1:BUFFER_SIZE); +Gyro_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,3))') - Bias_Gz)(1:BUFFER_SIZE); +Gyro_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,4))')(1:BUFFER_SIZE); + +scale_Gyro = 575; +Gyro_data(1,:) = Gyro_data(1,:)/scale_Gyro; +Gyro_data(2,:) = Gyro_data(2,:)/scale_Gyro; +Gyro_data(3,:) = Gyro_data(3,:)/scale_Gyro; + +% estimate orientation +Game_RV = estimate_gaming_rv(Accel_data, Gyro_data); + +for i = 1:BUFFER_SIZE + Orientation_RV(:,i) = quat2euler(Game_RV(i,:)); +end + +hfig=(figure); +scrsz = get(0,'ScreenSize'); +set(hfig,'position',scrsz); +% Gaming Rotation Vector Plot Results +subplot(3,1,1) +UA = Orientation_RV(1,:); +p1 = plot(1:length(UA),UA(1,1:length(UA)),'k'); +legend(p1,'x-axis'); +subplot(3,1,2) +UA = Orientation_RV(2,:); +p1 = plot(1:length(UA),UA(1,1:length(UA)),'b'); +legend(p1,'y-axis'); +subplot(3,1,3) +UA = Orientation_RV(3,:); +p1 = plot(1:length(UA),UA(1,1:length(UA)),'r'); +legend(p1,'z-axis'); +title(['Gaming Rotation Vector']); + + diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index d9e353f..470946d 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -272,6 +272,70 @@ inline void orientation_filter::time_update() } template +inline void orientation_filter::time_update_gaming_rv() +{ + quaternion quat_diff, quat_error, quat_output; + euler_angles euler_error; + euler_angles orientation; + euler_angles euler_aid; + euler_angles euler_driv; + + m_tran_mat.m_mat[0][1] = m_gyro.m_data.m_vec[2]; + m_tran_mat.m_mat[0][2] = -m_gyro.m_data.m_vec[1]; + m_tran_mat.m_mat[1][0] = -m_gyro.m_data.m_vec[2]; + m_tran_mat.m_mat[1][2] = m_gyro.m_data.m_vec[0]; + m_tran_mat.m_mat[2][0] = m_gyro.m_data.m_vec[1]; + m_tran_mat.m_mat[2][1] = -m_gyro.m_data.m_vec[0]; + m_tran_mat.m_mat[3][3] = (TYPE) F_CONST; + m_tran_mat.m_mat[4][4] = (TYPE) F_CONST; + m_tran_mat.m_mat[5][5] = (TYPE) F_CONST; + + m_measure_mat.m_mat[0][0] = 1; + m_measure_mat.m_mat[1][1] = 1; + m_measure_mat.m_mat[2][2] = 1; + + if (is_initialized(m_state_old)) + m_state_new = transpose(mul(m_tran_mat, transpose(m_state_old))); + + m_pred_cov = (m_tran_mat * m_pred_cov * tran(m_tran_mat)) + m_driv_cov; + + if(!is_initialized(m_quat_driv.m_quat)) + m_quat_driv = m_quat_aid; + + quaternion quat_rot_inc(0, m_gyro.m_data.m_vec[0], m_gyro.m_data.m_vec[1], + m_gyro.m_data.m_vec[2]); + + quat_diff = (m_quat_driv * quat_rot_inc) * (TYPE) 0.5; + + m_quat_driv = m_quat_driv + (quat_diff * (TYPE) m_gyro_dt * (TYPE) PI); + m_quat_driv.quat_normalize(); + + quat_output = phase_correction(m_quat_driv, m_quat_aid); + + quat_error = m_quat_aid * m_quat_driv; + + euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI; + + euler_aid = quat2euler(m_quat_aid); + euler_driv = quat2euler(quat_output); + + euler_angles euler_gaming_rv(euler_aid.m_ang.m_vec[0], euler_aid.m_ang.m_vec[1], + euler_driv.m_ang.m_vec[2]); + + m_quat_gaming_rv = euler2quat(euler_gaming_rv); + + if (is_initialized(m_state_new)) + { + m_state_error.m_vec[0] = euler_error.m_ang.m_vec[0]; + m_state_error.m_vec[1] = euler_error.m_ang.m_vec[1]; + m_state_error.m_vec[2] = euler_error.m_ang.m_vec[2]; + m_state_error.m_vec[3] = m_state_new.m_vec[3]; + m_state_error.m_vec[4] = m_state_new.m_vec[4]; + m_state_error.m_vec[5] = m_state_new.m_vec[5]; + } +} + +template inline void orientation_filter::measurement_update() { matrix gain; @@ -359,4 +423,26 @@ quaternion orientation_filter::get_geomagnetic_quaternion(const sens return m_quat_aid; } + +template +quaternion orientation_filter::get_gaming_quaternion(const sensor_data accel, + const sensor_data gyro) +{ + euler_angles cor_euler_ang; + + init_accel_gyro_data(accel, gyro); + + normalize(m_accel); + m_gyro.m_data = m_gyro.m_data * (TYPE) PI; + + compute_accel_orientation(); + + compute_covariance(); + + time_update_gaming_rv(); + + measurement_update(); + + return m_quat_gaming_rv; +} #endif //_ORIENTATION_FILTER_H_ diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h index bf89c96..a04f3f7 100644 --- a/src/sensor_fusion/orientation_filter.h +++ b/src/sensor_fusion/orientation_filter.h @@ -63,6 +63,7 @@ public: rotation_matrix m_rot_matrix; euler_angles m_orientation; quaternion m_quat_9axis; + quaternion m_quat_gaming_rv; TYPE m_gyro_dt; int m_pitch_phase_compensation; @@ -83,6 +84,7 @@ public: inline void compute_accel_orientation(); inline void compute_covariance(); inline void time_update(); + inline void time_update_gaming_rv(); inline void measurement_update(); euler_angles get_orientation(const sensor_data accel, @@ -93,6 +95,8 @@ public: const sensor_data gyro, const sensor_data magnetic); quaternion get_geomagnetic_quaternion(const sensor_data accel, const sensor_data magnetic); + quaternion get_gaming_quaternion(const sensor_data accel, + const sensor_data gyro); }; #include "orientation_filter.cpp" diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp index 06ebf5a..70746f5 100644 --- a/src/sensor_fusion/test/orientation_sensor.cpp +++ b/src/sensor_fusion/test/orientation_sensor.cpp @@ -105,4 +105,15 @@ quaternion orientation_sensor::get_geomagnetic_quaternion(sensor_data orientation_sensor::get_gaming_quaternion(sensor_data accel_data, + sensor_data gyro_data) +{ + 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); + + return orien_filter.get_gaming_quaternion(accel_data, gyro_data); +} #endif diff --git a/src/sensor_fusion/test/orientation_sensor.h b/src/sensor_fusion/test/orientation_sensor.h index e9fd2b8..358c476 100644 --- a/src/sensor_fusion/test/orientation_sensor.h +++ b/src/sensor_fusion/test/orientation_sensor.h @@ -35,6 +35,8 @@ public: sensor_data gyro, sensor_data magnetic); quaternion get_geomagnetic_quaternion(sensor_data accel, sensor_data magnetic); + quaternion get_gaming_quaternion(sensor_data accel, + sensor_data gyro); }; #include "orientation_sensor.cpp" diff --git a/src/sensor_fusion/test/test_projects/orientation_sensor_test/orientation_sensor_main.cpp b/src/sensor_fusion/test/test_projects/orientation_sensor_test/orientation_sensor_main.cpp index a30f8ee..c5593e9 100644 --- a/src/sensor_fusion/test/test_projects/orientation_sensor_test/orientation_sensor_main.cpp +++ b/src/sensor_fusion/test/test_projects/orientation_sensor_test/orientation_sensor_main.cpp @@ -39,7 +39,8 @@ int main() rotation_matrix orientation_mat; quaternion orientation_9axis_quat; quaternion orientation_geomagnetic_quat; - orientation_sensor orien_sensor1, orien_sensor2, orien_sensor3, orien_sensor4; + quaternion orientation_gaming_quat; + orientation_sensor orien_sensor1, orien_sensor2, orien_sensor3, orien_sensor4, orien_sensor5; accel_in.open(((string)ORIENTATION_DATA_PATH + (string)"accel.txt").c_str()); gyro_in.open(((string)ORIENTATION_DATA_PATH + (string)"gyro.txt").c_str()); @@ -95,6 +96,10 @@ int main() orientation_geomagnetic_quat = orien_sensor4.get_geomagnetic_quaternion(accel_data, magnetic_data); cout << "Orientation geomagnetic quaternion\t" << orientation_geomagnetic_quat.m_quat << "\n\n"; + + orientation_gaming_quat = orien_sensor5.get_gaming_quaternion(accel_data, gyro_data); + + cout << "Orientation gaming quaternion\t" << orientation_gaming_quat.m_quat << "\n\n"; } accel_in.close(); -- 2.7.4 From 6a105ee3b2c5599eea4876ac7808fb0f524ebcce Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Mon, 16 Feb 2015 18:03:49 +0530 Subject: [PATCH 12/16] Common test folder and automated testing of sensor APIs - Feature merge from devel/tizen branch -This is last of the four patches being submitted to merge the changes from devel/tizen to tizen branch. This contains the changes for making a common test folder for all sensors and an automatic test to test sensor APIs for sensors. -This patch also tests all the four patches submitted for merging features from devel/tizen to tizen branch. Change-Id: I2a101d14ea2ebbb92342232f89f4f17a3889c148 --- packaging/sensord.spec | 14 +- src/sensor_fusion/orientation_filter.cpp | 35 ++-- test/CMakeLists.txt | 57 +----- test/src/accelerometer.c | 145 --------------- test/src/auto_test.c | 235 ++++++++++++++++++++++++ test/src/geomagnetic.c | 140 --------------- test/src/gravity.c | 109 ------------ test/src/gyro.c | 146 --------------- test/src/light.c | 145 --------------- test/src/linear_acceleration.c | 111 ------------ test/src/orientation.c | 109 ------------ test/src/pressure.c | 146 --------------- test/src/proxi.c | 176 ------------------ test/src/rotation_vector.c | 144 --------------- test/src/tc-common.c | 294 +++++++++++++++++++++++++++++++ test/src/temperature.c | 145 --------------- 16 files changed, 563 insertions(+), 1588 deletions(-) delete mode 100644 test/src/accelerometer.c create mode 100644 test/src/auto_test.c delete mode 100755 test/src/geomagnetic.c delete mode 100755 test/src/gravity.c delete mode 100644 test/src/gyro.c delete mode 100644 test/src/light.c delete mode 100755 test/src/linear_acceleration.c delete mode 100755 test/src/orientation.c delete mode 100644 test/src/pressure.c delete mode 100644 test/src/proxi.c delete mode 100644 test/src/rotation_vector.c create mode 100644 test/src/tc-common.c delete mode 100644 test/src/temperature.c diff --git a/packaging/sensord.spec b/packaging/sensord.spec index e6d3ed6..987cb43 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -136,17 +136,7 @@ systemctl daemon-reload %if %{build_test_suite} == "ON" %files -n sensor-test %defattr(-,root,root,-) -%{_bindir}/accelerometer -%{_bindir}/geomagnetic -%{_bindir}/orientation -%{_bindir}/gravity -%{_bindir}/linear_acceleration -%{_bindir}/gyro -%{_bindir}/proxi -%{_bindir}/pressure -%{_bindir}/temperature -%{_bindir}/light -%{_bindir}/rotation_vector -%{_bindir}/geomagnetic_rv +%{_bindir}/auto_test +%{_bindir}/tc-common %license LICENSE.APLv2 %endif diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index 470946d..2bf7865 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -227,6 +227,15 @@ inline void orientation_filter::time_update() m_pred_cov = (m_tran_mat * m_pred_cov * tran(m_tran_mat)) + m_driv_cov; + for (int j=0; j inline void orientation_filter::measurement_update() { matrix gain; - TYPE iden = 0; + matrix iden; + iden.m_mat[0][0] = iden.m_mat[1][1] = iden.m_mat[2][2] = 1; + iden.m_mat[3][3] = iden.m_mat[4][4] = iden.m_mat[5][5] = 1; for (int j=0; j temp = iden; - m_pred_cov.m_mat[j][i] = (iden - (gain.m_mat[i][j] * m_measure_mat.m_mat[j][i])) * - m_pred_cov.m_mat[j][i]; + for (int i=0; i -#include -#include -#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 [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data->timestamp, data->values[0], data->values[1], data->values[2]); -} - -void printformat() -{ - printf("Usage : ./accelerometer (optional) (optional)\n\n"); - - printf("mode:"); - printf("[-p]\n"); - printf("p is for polling based,default mode is event driven\n"); - - printf("event:"); - printf("[RAW_DATA_REPORT_ON_TIME]\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, start_handle, stop_handle; - unsigned int event; - mainloop = g_main_loop_new(NULL, FALSE); - event = ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME; - event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); - event_condition->cond_op = CONDITION_EQUAL; - - sensor_type_t type = ACCELEROMETER_SENSOR; - - if (argc != 2 && argc != 3 && argc!=4) { - printformat(); - free(event_condition); - return 0; - } - - else if (argc>=3 && strcmp(argv[1], "-p") == 0 && strcmp(argv[2], "RAW_DATA_REPORT_ON_TIME") == 0) { - printf("Polling based\n"); - handle = sf_connect(type); - result = sf_start(handle, 1); - - if (result < 0) { - printf("Can't start accelerometer SENSOR\n"); - printf("Error\n\n\n\n"); - free(event_condition); - return -1; - } - - sensor_data_t data; - - while(1) { - result = sf_get_data(handle, ACCELEROMETER_BASE_DATA_SET , &data); - printf("Accelerometer [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2]); - usleep(100000); - } - - result = sf_disconnect(handle); - - if (result < 0) { - printf("Can't disconnect Accelerometer sensor\n"); - printf("Error\n\n\n\n"); - free(event_condition); - return -1; - } - } - - else if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) { - printf("Event based\n"); - - event_condition->cond_value1 = 100; - if (argc == 3) - event_condition->cond_value1 = atof(argv[2]); - - handle = sf_connect(type); - result = sf_register_event(handle, event, event_condition, callback, NULL); - - if (result < 0) - printf("Can't register accelerometer\n"); - - start_handle = sf_start(handle,0); - - if (start_handle < 0) { - printf("Error\n\n\n\n"); - sf_unregister_event(handle, event); - sf_disconnect(handle); - free(event_condition); - return -1; - } - - g_main_loop_run(mainloop); - g_main_loop_unref(mainloop); - - sf_unregister_event(handle, event); - - stop_handle = sf_stop(handle); - - if (stop_handle < 0) { - printf("Error\n\n"); - return -1; - free(event_condition); - } - - sf_disconnect(handle); - free(event_condition); - } - - else { - printformat(); - } - - return 0; -} - diff --git a/test/src/auto_test.c b/test/src/auto_test.c new file mode 100644 index 0000000..e897f74 --- /dev/null +++ b/test/src/auto_test.c @@ -0,0 +1,235 @@ +/* + * sensord + * + * Copyright (c) 2015 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 + +#define DEFAULT_EVENT_INTERVAL 100 + +static GMainLoop *mainloop; +FILE *fp; + +void callback(unsigned int event_type, sensor_event_data_t *event, void *user_data) +{ + g_main_loop_quit(mainloop); +} + +bool check_sensor_api(unsigned int event_type, int cond_value) +{ + int result, handle; + + mainloop = g_main_loop_new(NULL, FALSE); + + sensor_type_t sensor_type = event_type >> 16; + sensor_t sensor = sensord_get_sensor(sensor_type); + + handle = sensord_connect(sensor); + + if (handle < 0) { + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_connect\n", sensor_type, event_type); + return false; + } + + bool is_supported; + bool result_boolean = sensord_is_supported_event_type(sensor, event_type, &is_supported); + if (!result_boolean && !is_supported) { + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_is_supported_event\n", sensor_type, event_type); + return false; + } + + int output; + result_boolean = sensord_get_min_interval(sensor, &output); + if (!result_boolean) { + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_get_min_interval\n", sensor_type, event_type); + return false; + } + + float output3; + result_boolean = sensord_get_resolution(sensor, &output3); + if (!result_boolean) { + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_get_resolution\n", sensor_type, event_type); + return false; + } + + result_boolean = sensord_get_max_range(sensor, &output3); + if (!result_boolean) { + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_get_max_range\n", sensor_type, event_type); + return false; + } + + result_boolean = sensord_get_min_range(sensor, &output3); + if (!result_boolean) { + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_get_min_range\n", sensor_type, event_type); + return false; + } + + sensor_privilege_t output4; + result_boolean = sensord_get_privilege(sensor, &output4); + if (!result_boolean) { + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_get_privilege\n", sensor_type, event_type); + return false; + } + + const char* result_char = sensord_get_vendor(sensor); + if (!result_char) { + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_get_vendor\n", sensor_type, event_type); + return false; + } + + result_char = sensord_get_name(sensor); + if (!result_char) { + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_get_name\n", sensor_type, event_type); + return false; + } + + sensor_type_t output_type; + result_boolean = sensord_get_type(sensor, &output_type); + if (!result_boolean) { + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_get_type\n", sensor_type, event_type); + return false; + } + + unsigned int *output2; + result_boolean = sensord_get_supported_event_types(sensor, &output2, &output); + if (!result_boolean) { + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_get_supported_event_types\n", sensor_type, event_type); + return false; + } + + sensor_t *output_list; + result_boolean = sensord_get_sensor_list(sensor_type, &output_list, &output); + if (!result_boolean) { + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_get_sensor_list\n", sensor_type, event_type); + return false; + } + + result = sensord_register_event(handle, event_type, cond_value, 0, callback, NULL); + + if (result < 0) { + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_register_event\n", sensor_type, event_type); + return false; + } + + result_boolean = sensord_start(handle, 1); + + if (!result_boolean) { + sensord_unregister_event(handle, event_type); + sensord_disconnect(handle); + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_start\n", sensor_type, event_type); + return false; + } + + sensor_data_t data; + result_boolean = sensord_get_data(handle, event_type, &data); + if (!result_boolean) { + sensord_unregister_event(handle, event_type); + sensord_disconnect(handle); + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_get_data\n", sensor_type, event_type); + return false; + } + + g_main_loop_run(mainloop); + g_main_loop_unref(mainloop); + + result_boolean = sensord_change_event_interval(handle, event_type, 101); + if (!result_boolean) { + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_change_event_interval\n", sensor_type, event_type); + return false; + } + + result_boolean = sensord_set_option(handle, SENSOR_OPTION_ON_IN_SCREEN_OFF); + if (!result_boolean){ + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_change_sensor_option\n", sensor_type, event_type); + return false; + } + + result_boolean = sensord_unregister_event(handle, event_type); + + if (!result_boolean) { + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_unregister_event\n", sensor_type, event_type); + return false; + } + + result_boolean = sensord_stop(handle); + + if (!result_boolean) { + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_stop\n", sensor_type, event_type); + return false; + } + + result_boolean = sensord_disconnect(handle); + + if (!result_boolean) { + fprintf(fp, "Sensor - %d, event - %d, failed at sensord_disconnect\n", sensor_type, event_type); + return false; + } + + return true; +} + +int main(int argc, char **argv) +{ + bool result; + + int interval = DEFAULT_EVENT_INTERVAL; + if (argc == 2) + interval = atof(argv[1]); + + fp = fopen("auto_test.output", "w+"); + + result = check_sensor_api(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME, interval); + fprintf(fp, "Accelerometer - RAW_DATA_REPORT_ON_TIME - %d\n", result); + + result = check_sensor_api(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, interval); + fprintf(fp, "Geomagnetic - RAW_DATA_REPORT_ON_TIME - %d\n", result); + + result = check_sensor_api(GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME, interval); + fprintf(fp, "Gravity - RAW_DATA_REPORT_ON_TIME - %d\n", result); + + result = check_sensor_api(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME, interval); + fprintf(fp, "Gyroscope - RAW_DATA_REPORT_ON_TIME - %d\n", result); + + result = check_sensor_api(LIGHT_EVENT_LUX_DATA_REPORT_ON_TIME, interval); + fprintf(fp, "Light - RAW_DATA_REPORT_ON_TIME - %d\n", result); + + result = check_sensor_api(LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME, interval); + fprintf(fp, "Linear Accel - RAW_DATA_REPORT_ON_TIME - %d\n", result); + + result = check_sensor_api(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME, interval); + fprintf(fp, "Orientation - RAW_DATA_REPORT_ON_TIME - %d\n", result); + + result = check_sensor_api(PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME, interval); + fprintf(fp, "Pressure - RAW_DATA_REPORT_ON_TIME - %d\n", result); + + result = check_sensor_api(ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME, interval); + fprintf(fp, "Rotation Vector - RAW_DATA_REPORT_ON_TIME - %d\n", result); + + result = check_sensor_api(TEMPERATURE_EVENT_RAW_DATA_REPORT_ON_TIME, interval); + fprintf(fp, "Temperature - RAW_DATA_REPORT_ON_TIME - %d\n", result); + + printf("Logs printed in ./auto_test.output\n"); + fclose(fp); + return 0; +} diff --git a/test/src/geomagnetic.c b/test/src/geomagnetic.c deleted file mode 100755 index 1d72500..0000000 --- a/test/src/geomagnetic.c +++ /dev/null @@ -1,140 +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 - -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("Geomagnetic [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data->timestamp, data->values[0], data->values[1], data->values[2]); -} - -void printformat() -{ - printf("Usage : ./geomagnetic (optional) (optional)\n\n"); - - printf("mode:"); - printf("[-p]\n"); - printf("p is for polling based,default mode is event driven\n"); - - printf("event:"); - printf("[RAW_DATA_REPORT_ON_TIME]\n"); - - printf("interval:\n"); - printf("The time interval should be entered based on the sampling frequency supported by geomagnetic 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, start_handle, stop_handle; - unsigned int event; - mainloop = g_main_loop_new(NULL, FALSE); - event = GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME; - event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); - event_condition->cond_op = CONDITION_EQUAL; - - sensor_type_t type = GEOMAGNETIC_SENSOR; - - if (argc != 2 && argc != 3 && argc!=4) { - printformat(); - free(event_condition); - return 0; - } - - else if (argc>=3 && strcmp(argv[1], "-p") == 0 && strcmp(argv[2], "RAW_DATA_REPORT_ON_TIME") == 0) { - printf("Polling based\n"); - handle = sf_connect(type); - result = sf_start(handle, 1); - - if (result < 0) { - printf("Can't start geomagnetic SENSOR\n"); - printf("Error\n\n\n\n"); - return -1; - } - - sensor_data_t data; - - while(1) { - result = sf_get_data(handle, GEOMAGNETIC_BASE_DATA_SET , &data); - printf("Geomagnetic [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2]); - usleep(100000); - } - - result = sf_disconnect(handle); - - if (result < 0) { - printf("Can't disconnect geomagnetic sensor\n"); - printf("Error\n\n\n\n"); - return -1; - } - } - - else if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) { - printf("Event based\n"); - - event_condition->cond_value1 = 100; - if (argc == 3) - event_condition->cond_value1 = atof(argv[2]); - - handle = sf_connect(type); - result = sf_register_event(handle, event, event_condition, callback, NULL); - - if (result < 0) - printf("Can't register geomagnetic\n"); - - start_handle = sf_start(handle,0); - - if (start_handle < 0) { - 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); - - stop_handle = sf_stop(handle); - - if (stop_handle < 0) { - printf("Error\n\n"); - return -1; - } - - sf_disconnect(handle); - free(event_condition); - } - - else { - printformat(); - } - - return 0; -} - diff --git a/test/src/gravity.c b/test/src/gravity.c deleted file mode 100755 index d3cba2d..0000000 --- a/test/src/gravity.c +++ /dev/null @@ -1,109 +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 - -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("Gravity [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data->timestamp, data->values[0], data->values[1], data->values[2]); -} - -void printformat() -{ - printf("Usage : ./gravity (optional)\n\n"); - printf("event:\n"); - printf("RAW_DATA_REPORT_ON_TIME\n\n"); - printf("interval:\n"); - printf("The time interval should be entered based on the sampling " - "frequencies supported by accelerometer, gyroscope and " - "geomagnetic sensors driver on the device in ms. If no value " - "for gravity sensor is entered, a default value will be used.\n"); -} - -int main(int argc,char **argv) -{ - int result, handle, start_handle, stop_handle; - unsigned int event; - - mainloop = g_main_loop_new(NULL, FALSE); - sensor_type_t type = GRAVITY_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(); - free(event_condition); - return 0; - } - else { - if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) - event = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME; - else { - printformat(); - free(event_condition); - return 0; - } - - if (argc == 3) - event_condition->cond_value1 = atof(argv[2]); - } - - handle = sf_connect(type); - result = sf_register_event(handle, event, event_condition, callback, NULL); - - if (result < 0) - printf("Can't register gravity virtual sensor\n"); - - start_handle = sf_start(handle, 0); - - if (start_handle < 0) { - printf("Error\n\n\n\n"); - sf_unregister_event(handle, event); - sf_disconnect(handle); - free(event_condition); - return -1; - } - - g_main_loop_run(mainloop); - g_main_loop_unref(mainloop); - - sf_unregister_event(handle, event); - stop_handle = sf_stop(handle); - - if (stop_handle < 0) { - printf("Error\n\n"); - free(event_condition); - return -1; - } - - sf_disconnect(handle); - free(event_condition); - - return 0; -} diff --git a/test/src/gyro.c b/test/src/gyro.c deleted file mode 100644 index a3332f7..0000000 --- a/test/src/gyro.c +++ /dev/null @@ -1,146 +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 - -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("Gyroscope [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data->timestamp, data->values[0], data->values[1], data->values[2]); -} - -void printformat() -{ - printf("Usage : ./gyro (optional) (optional)\n\n"); - - printf("mode:"); - printf("[-p]\n"); - printf("p is for polling based,default mode is event driven\n"); - - printf("event:"); - printf("[RAW_DATA_REPORT_ON_TIME]\n"); - - printf("interval:\n"); - printf("The time interval should be entered based on the sampling frequency supported by gyroscope 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, start_handle, stop_handle; - unsigned int event; - mainloop = g_main_loop_new(NULL, FALSE); - event = GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME; - event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); - event_condition->cond_op = CONDITION_EQUAL; - - sensor_type_t type = GYROSCOPE_SENSOR; - - if (argc != 2 && argc != 3 && argc!=4) { - printformat(); - free(event_condition); - return 0; - } - - else if (argc>=3 && strcmp(argv[1], "-p") == 0 && strcmp(argv[2], "RAW_DATA_REPORT_ON_TIME") == 0) { - printf("Polling based\n"); - - handle = sf_connect(type); - result = sf_start(handle, 1); - - if (result < 0) { - printf("Can't start gyroscope SENSOR\n"); - printf("Error\n\n\n\n"); - free(event_condition); - return -1; - } - - sensor_data_t data; - - while(1) { - result = sf_get_data(handle, GYRO_BASE_DATA_SET , &data); - printf("Gyroscope [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2]); - usleep(100000); - } - - result = sf_disconnect(handle); - - if (result < 0) { - printf("Can't disconnect gyroscope sensor\n"); - printf("Error\n\n\n\n"); - free(event_condition); - return -1; - } - } - - else if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) { - printf("Event based\n"); - - event_condition->cond_value1 = 10.52; - if (argc == 3) - event_condition->cond_value1 = atof(argv[2]); - - handle = sf_connect(type); - result = sf_register_event(handle, event, event_condition, callback, NULL); - - if (result < 0) - printf("Can't register gyroscope\n"); - - start_handle = sf_start(handle,0); - - if (start_handle < 0) { - printf("Error\n\n\n\n"); - sf_unregister_event(handle, event); - sf_disconnect(handle); - free(event_condition); - return -1; - } - - g_main_loop_run(mainloop); - g_main_loop_unref(mainloop); - - sf_unregister_event(handle, event); - - stop_handle = sf_stop(handle); - - if (stop_handle < 0) { - printf("Error\n\n"); - free(event_condition); - return -1; - } - - sf_disconnect(handle); - free(event_condition); - } - - else { - printformat(); - } - - return 0; -} - diff --git a/test/src/light.c b/test/src/light.c deleted file mode 100644 index ba99732..0000000 --- a/test/src/light.c +++ /dev/null @@ -1,145 +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 - -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("Light :[%lld] [%6.6f]\n", data->timestamp, data->values[0]); -} - -void printformat() -{ - printf("Usage : ./light (optional) (optional)\n\n"); - - printf("mode:"); - printf("[-p]\n"); - printf("p is for polling based,default mode is event driven\n"); - - printf("event:"); - printf("[RAW_DATA_REPORT_ON_TIME]\n"); - - printf("interval:\n"); - printf("The time interval should be entered based on the sampling frequency supported by light 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, start_handle, stop_handle; - unsigned int event; - mainloop = g_main_loop_new(NULL, FALSE); - event = LIGHT_EVENT_LUX_DATA_REPORT_ON_TIME; - event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); - event_condition->cond_op = CONDITION_EQUAL; - - sensor_type_t type = LIGHT_SENSOR; - - if (argc != 2 && argc != 3 && argc!=4) { - printformat(); - free(event_condition); - return 0; - } - - else if (argc>=3 && strcmp(argv[1], "-p") == 0 && strcmp(argv[2], "RAW_DATA_REPORT_ON_TIME") == 0) { - printf("Polling based\n"); - - handle = sf_connect(type); - result = sf_start(handle, 1); - - if (result < 0) { - printf("Can't start light SENSOR\n"); - printf("Error\n\n\n\n"); - free(event_condition); - return -1; - } - - sensor_data_t data; - - while(1) { - result = sf_get_data(handle, LIGHT_LUX_DATA_SET , &data); - printf("Light : [%6.6f]\n", data.values[0]); - usleep(100000); - } - - result = sf_disconnect(handle); - if (result < 0) { - printf("Can't disconnect light sensor\n"); - printf("Error\n\n\n\n"); - free(event_condition); - return -1; - } - } - - else if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) { - printf("Event based\n"); - - event_condition->cond_value1 = 100; - if (argc == 3) - event_condition->cond_value1 = atof(argv[2]); - - handle = sf_connect(type); - result = sf_register_event(handle, event, event_condition, callback, NULL); - - if (result < 0) - printf("Can't register light\n"); - - start_handle = sf_start(handle,0); - - if (start_handle < 0) { - printf("Error\n\n\n\n"); - sf_unregister_event(handle, event); - sf_disconnect(handle); - free(event_condition); - return -1; - } - - g_main_loop_run(mainloop); - g_main_loop_unref(mainloop); - - sf_unregister_event(handle, event); - - stop_handle = sf_stop(handle); - - if (stop_handle < 0) { - printf("Error\n\n"); - free(event_condition); - return -1; - } - - sf_disconnect(handle); - free(event_condition); - } - - else { - printformat(); - } - - return 0; -} - diff --git a/test/src/linear_acceleration.c b/test/src/linear_acceleration.c deleted file mode 100755 index f992a68..0000000 --- a/test/src/linear_acceleration.c +++ /dev/null @@ -1,111 +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 - -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("Linear Acceleration [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data->timestamp, data->values[0], data->values[1], data->values[2]); -} - -void printformat() -{ - printf("Usage : ./linear_acceleration (optional)\n\n"); - printf("event:\n"); - printf("RAW_DATA_REPORT_ON_TIME\n\n"); - printf("interval:\n"); - printf("The time interval should be entered based on the sampling " - "frequencies supported by accelerometer, gyroscope and " - "geomagnetic sensors driver on the device in ms. If no value " - "for linear acceleration sensor is entered, a default value will be used.\n"); -} - -int main(int argc,char **argv) -{ - int result, handle, start_handle, stop_handle; - unsigned int event; - - mainloop = g_main_loop_new(NULL, FALSE); - sensor_type_t type = LINEAR_ACCEL_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(); - free(event_condition); - return 0; - } - else { - if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) - event = LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME; - else { - printformat(); - free(event_condition); - return 0; - } - - if (argc == 3) - event_condition->cond_value1 = atof(argv[2]); - } - - handle = sf_connect(type); - result = sf_register_event(handle, event, event_condition, callback, NULL); - - if (result < 0) - printf("Can't register linear acceleration virtual sensor\n"); - - start_handle = sf_start(handle, 0); - - if (start_handle < 0) { - printf("Error\n\n\n\n"); - sf_unregister_event(handle, event); - sf_disconnect(handle); - free(event_condition); - return -1; - } - - g_main_loop_run(mainloop); - g_main_loop_unref(mainloop); - - sf_unregister_event(handle, event); - - stop_handle = sf_stop(handle); - - if (stop_handle < 0) { - printf("Error\n\n"); - free(event_condition); - return -1; - } - - sf_disconnect(handle); - - free(event_condition); - - return 0; -} diff --git a/test/src/orientation.c b/test/src/orientation.c deleted file mode 100755 index 57bc82b..0000000 --- a/test/src/orientation.c +++ /dev/null @@ -1,109 +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 - -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("Orientation [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data->timestamp, data->values[0], data->values[1], data->values[2]); -} - -void printformat() -{ - printf("Usage : ./orientation (optional)\n\n"); - printf("event:\n"); - printf("RAW_DATA_REPORT_ON_TIME\n\n"); - printf("interval:\n"); - printf("The time interval should be entered based on the sampling " - "frequencies supported by accelerometer, gyroscope and " - "geomagnetic sensors driver on the device in ms. If no value " - "for orientation sensor is entered, a default value will be used.\n"); -} - -int main(int argc,char **argv) -{ - int result, handle, start_handle, stop_handle; - unsigned int event; - - mainloop = g_main_loop_new(NULL, FALSE); - sensor_type_t type = ORIENTATION_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(); - free(event_condition); - return 0; - } - else { - if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) - event = ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME; - else { - printformat(); - free(event_condition); - return 0; - } - - if (argc == 3) - event_condition->cond_value1 = atof(argv[2]); - } - - handle = sf_connect(type); - result = sf_register_event(handle, event, event_condition, callback, NULL); - - if (result < 0) - printf("Can't register orientation virtual sensor\n"); - - start_handle = sf_start(handle, 0); - - if (start_handle < 0) { - printf("Error\n\n\n\n"); - sf_unregister_event(handle, event); - sf_disconnect(handle); - free(event_condition); - return -1; - } - - g_main_loop_run(mainloop); - g_main_loop_unref(mainloop); - - sf_unregister_event(handle, event); - stop_handle = sf_stop(handle); - - if (stop_handle < 0) { - printf("Error\n\n"); - free(event_condition); - return -1; - } - sf_disconnect(handle); - - free(event_condition); - - return 0; -} diff --git a/test/src/pressure.c b/test/src/pressure.c deleted file mode 100644 index 7621160..0000000 --- a/test/src/pressure.c +++ /dev/null @@ -1,146 +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 - -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("Pressure [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data->timestamp, data->values[0], data->values[1], data->values[2]); -} - -void printformat() -{ - printf("Usage : ./pressure (optional) (optional)\n\n"); - - printf("mode:"); - printf("[-p]\n"); - printf("p is for polling based,default mode is event driven\n"); - - printf("event:"); - printf("[RAW_DATA_REPORT_ON_TIME]\n"); - - printf("interval:\n"); - printf("The time interval should be entered based on the sampling frequency supported by pressure 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, start_handle, stop_handle; - unsigned int event; - mainloop = g_main_loop_new(NULL, FALSE); - event = PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME; - event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); - event_condition->cond_op = CONDITION_EQUAL; - - sensor_type_t type = PRESSURE_SENSOR; - - if (argc != 2 && argc != 3 && argc!=4) { - printformat(); - free(event_condition); - return 0; - } - - else if (argc>=3 && strcmp(argv[1], "-p") == 0 && strcmp(argv[2], "RAW_DATA_REPORT_ON_TIME") == 0) { - printf("Polling based\n"); - - handle = sf_connect(type); - result = sf_start(handle, 1); - - if (result < 0) { - printf("Can't start pressure SENSOR\n"); - printf("Error\n\n\n\n"); - free(event_condition); - return -1; - } - - sensor_data_t data; - - while(1) { - result = sf_get_data(handle, PRESSURE_BASE_DATA_SET , &data); - printf("Pressure [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2]); - usleep(100000); - } - - result = sf_disconnect(handle); - - if (result < 0) { - printf("Can't disconnect pressure sensor\n"); - printf("Error\n\n\n\n"); - free(event_condition); - return -1; - } - } - - else if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) { - printf("Event based\n"); - - event_condition->cond_value1 = 10.52; - if (argc == 3) - event_condition->cond_value1 = atof(argv[2]); - - handle = sf_connect(type); - result = sf_register_event(handle, event, event_condition, callback, NULL); - - if (result < 0) - printf("Can't register pressure\n"); - - start_handle = sf_start(handle,0); - - if (start_handle < 0) { - printf("Error\n\n\n\n"); - sf_unregister_event(handle, event); - sf_disconnect(handle); - free(event_condition); - return -1; - } - - g_main_loop_run(mainloop); - g_main_loop_unref(mainloop); - - sf_unregister_event(handle, event); - - stop_handle = sf_stop(handle); - - if (stop_handle < 0) { - printf("Error\n\n"); - free(event_condition); - return -1; - } - - sf_disconnect(handle); - free(event_condition); - } - - else { - printformat(); - } - - return 0; -} - diff --git a/test/src/proxi.c b/test/src/proxi.c deleted file mode 100644 index 23da057..0000000 --- a/test/src/proxi.c +++ /dev/null @@ -1,176 +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 - -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("Proximity [%lld] [%6.6f]\n\n", data->timestamp, data->values[0]); -} - -void printformat() -{ - printf("Usage : ./proxi (optional) (optional)\n\n"); - - printf("mode:"); - printf("[-p]\n"); - printf("p is for polling based,default mode is event driven\n"); - - printf("event:"); - printf("[EVENT_CHANGE_STATE] "); - printf("[EVENT_STATE_REPORT_ON_TIME] "); - printf("[EVENT_DISTANCE_DATA_REPORT_ON_TIME]\n"); - - printf("interval:\n"); - printf("The time interval should be entered based on the sampling frequency supported by proximity 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, start_handle, stop_handle; - unsigned int event; - mainloop = g_main_loop_new(NULL, FALSE); - event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); - event_condition->cond_op = CONDITION_EQUAL; - sensor_type_t type = PROXIMITY_SENSOR; - - if (argc != 2 && argc != 3 && argc!=4) { - printformat(); - free(event_condition); - return 0; - } - - else if (argc>=3 && strcmp(argv[1], "-p") == 0) { - printf("Polling based\n"); - - if (strcmp(argv[2], "EVENT_CHANGE_STATE") == 0) { - event = PROXIMITY_BASE_DATA_SET; - } - else if (strcmp(argv[2], "EVENT_STATE_REPORT_ON_TIME") == 0) { - event = PROXIMITY_DISTANCE_DATA_SET; - } - else if (strcmp(argv[2], "EVENT_DISTANCE_DATA_REPORT_ON_TIME") == 0) { - event = PROXIMITY_EVENT_DISTANCE_DATA_REPORT_ON_TIME; - } - else { - printformat(); - free(event_condition); - return 0; - } - - handle = sf_connect(type); - result = sf_start(handle, 1); - - if (result < 0) { - printf("Can't start proximity SENSOR\n"); - printf("Error\n\n\n\n"); - free(event_condition); - return -1; - } - - sensor_data_t data; - - while(1) { - result = sf_get_data(handle, event , &data); - printf("Proximity [%6.6f]\n\n", data.values[0]); - usleep(100000); - } - - result = sf_disconnect(handle); - - if (result < 0) { - printf("Can't disconnect proximity sensor\n"); - printf("Error\n\n\n\n"); - free(event_condition); - return -1; - } - } - - else if (argc == 2 || argc ==3) { - printf("Event based\n"); - - if (strcmp(argv[1], "EVENT_CHANGE_STATE") == 0) { - event = PROXIMITY_EVENT_CHANGE_STATE; - } - else if (strcmp(argv[1], "EVENT_STATE_REPORT_ON_TIME") == 0) { - event = PROXIMITY_EVENT_STATE_REPORT_ON_TIME; - } - else if (strcmp(argv[1], "EVENT_DISTANCE_DATA_REPORT_ON_TIME") == 0) { - event = PROXIMITY_EVENT_DISTANCE_DATA_REPORT_ON_TIME; - } - else { - printformat(); - free(event_condition); - return 0; - } - - event_condition->cond_value1 = 100; - if (argc == 3) - event_condition->cond_value1 = atof(argv[2]); - - handle = sf_connect(type); - result = sf_register_event(handle, event, event_condition, callback, NULL); - - if (result < 0) - printf("Can't register proximity\n"); - - start_handle = sf_start(handle,0); - - if (start_handle < 0) { - printf("Error\n\n\n\n"); - sf_unregister_event(handle, event); - sf_disconnect(handle); - free(event_condition); - return -1; - } - - g_main_loop_run(mainloop); - g_main_loop_unref(mainloop); - - sf_unregister_event(handle, event); - - stop_handle = sf_stop(handle); - - if (stop_handle < 0) { - printf("Error\n\n"); - free(event_condition); - return -1; - } - - sf_disconnect(handle); - free(event_condition); - } - - else { - printformat(); - } - - return 0; -} - diff --git a/test/src/rotation_vector.c b/test/src/rotation_vector.c deleted file mode 100644 index be9adef..0000000 --- a/test/src/rotation_vector.c +++ /dev/null @@ -1,144 +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 - -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("Rotation Vector [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data->timestamp, data->values[0], - data->values[1], data->values[2]); -} - -void printformat() -{ - printf("Usage : ./rotation_vector (optional) (optional)\n\n"); - - printf("mode:"); - printf("[-p]\n"); - printf("p is for polling based,default mode is event driven\n"); - - printf("event:"); - printf("[RAW_DATA_REPORT_ON_TIME]\n"); - - printf("interval:\n"); - printf("The time interval should be entered based on the sampling frequencies supported by " - "accelerometer, gyroscope and geomagnetic sensors driver on the device in ms. If " - "no value for rotation vector sensor is entered, a default value will be used.\n"); -} - -int main(int argc,char **argv) -{ - int result, handle, start_handle, stop_handle; - unsigned int event; - mainloop = g_main_loop_new(NULL, FALSE); - event = ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME; - event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); - event_condition->cond_op = CONDITION_EQUAL; - - sensor_type_t type = ROTATION_VECTOR_SENSOR; - - if (argc != 2 && argc != 3 && argc!=4) { - printformat(); - free(event_condition); - return 0; - } - - else if (argc>=3 && strcmp(argv[1], "-p") == 0 && strcmp(argv[2], "RAW_DATA_REPORT_ON_TIME") == 0) { - printf("Polling based\n"); - handle = sf_connect(type); - result = sf_start(handle, 1); - - if (result < 0) { - printf("Can't start Rotation Vector SENSOR\n"); - printf("Error\n\n\n\n"); - return -1; - } - - sensor_data_t data; - - while(1) { - result = sf_get_data(handle, ACCELEROMETER_BASE_DATA_SET , &data); - printf("Rotation Vector [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2]); - usleep(100000); - } - - result = sf_disconnect(handle); - - if (result < 0) { - printf("Can't disconnect ROTATION VECTOR sensor\n"); - printf("Error\n\n\n\n"); - return -1; - } - } - - else if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) { - printf("Event based\n"); - - event_condition->cond_value1 = 100; - if (argc == 3) - event_condition->cond_value1 = atof(argv[2]); - - handle = sf_connect(type); - result = sf_register_event(handle, event, event_condition, callback, NULL); - - if (result < 0) - printf("Can't register rotation vector event\n"); - - start_handle = sf_start(handle,0); - - if (start_handle < 0) { - 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); - - stop_handle = sf_stop(handle); - - if (stop_handle < 0) { - printf("Error\n\n"); - return -1; - } - - sf_disconnect(handle); - free(event_condition); - } - - else { - printformat(); - } - - return 0; -} - diff --git a/test/src/tc-common.c b/test/src/tc-common.c new file mode 100644 index 0000000..81d83d9 --- /dev/null +++ b/test/src/tc-common.c @@ -0,0 +1,294 @@ +/* + * sensord + * + * Copyright (c) 2014-15 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 + +#define DEFAULT_EVENT_INTERVAL 100 + +static GMainLoop *mainloop; + +void usage() +{ + printf("Usage : ./tc-common (optional) (optional)\n\n"); + + printf("Sensor_type: "); + printf("[accelerometer] "); + printf("[gyroscope] "); + printf("[pressure] "); + printf("[temperature] "); + printf("[geomagnetic] "); + printf("[orientation] "); + printf("[gravity] "); + printf("[linear_accel] "); + printf("[rotation_vector] "); + printf("[gaming_rotation_vector] "); + printf("[light]\n"); + printf("event:"); + printf("[RAW_DATA_REPORT_ON_TIME]\n"); + + printf("Sensor_type: "); + printf("[proximity]\n"); + printf("event:"); + printf("[EVENT_CHANGE_STATE]\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"); +} + +unsigned int get_event_driven(sensor_type_t sensor_type, char str[]) +{ + switch (sensor_type) { + case ACCELEROMETER_SENSOR: + if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + return ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME; + break; + case GYROSCOPE_SENSOR: + if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + return GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME; + break; + case PRESSURE_SENSOR: + if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + return PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME; + break; + case GEOMAGNETIC_SENSOR: + if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + return GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME; + break; + case LIGHT_SENSOR: + if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + return LIGHT_EVENT_LUX_DATA_REPORT_ON_TIME; + break; + case TEMPERATURE_SENSOR: + if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + return TEMPERATURE_EVENT_RAW_DATA_REPORT_ON_TIME; + break; + case PROXIMITY_SENSOR: + if (strcmp(str, "EVENT_CHANGE_STATE") == 0) + return PROXIMITY_EVENT_CHANGE_STATE; + break; + case ORIENTATION_SENSOR: + if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + return ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME; + break; + case GRAVITY_SENSOR: + if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + return GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME; + break; + case LINEAR_ACCEL_SENSOR: + if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + return LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME; + break; + case ROTATION_VECTOR_SENSOR: + if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + return ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME; + break; + case GEOMAGNETIC_RV_SENSOR: + if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + return GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME; + break; + default: + return -1; + } +} + +void callback(sensor_t sensor, unsigned int event_type, sensor_data_t *data, void *user_data) +{ + sensor_type_t sensor_type = event_type >> 16; + + switch (sensor_type) { + case ACCELEROMETER_SENSOR: + printf("Accelerometer [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data->timestamp, data->values[0], data->values[1], data->values[2]); + break; + case GYROSCOPE_SENSOR: + printf("Gyroscope [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data->timestamp, data->values[0], data->values[1], data->values[2]); + break; + case PRESSURE_SENSOR: + printf("Pressure [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data->timestamp, data->values[0], data->values[1], data->values[2]); + break; + case GEOMAGNETIC_SENSOR: + printf("Geomagnetic [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data->timestamp, data->values[0], data->values[1], data->values[2]); + break; + case LIGHT_SENSOR: + printf("Light [%lld] [%6.6f]\n\n", data->timestamp, data->values[0]); + break; + case TEMPERATURE_SENSOR : + printf("Temperature [%lld] [%6.6f]\n\n", data->timestamp, data->values[0]); + break; + case PROXIMITY_SENSOR: + printf("Proximity [%lld] [%6.6f]\n\n", data->timestamp, data->values[0]); + break; + case ORIENTATION_SENSOR : + printf("Orientation [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data->timestamp, data->values[0], data->values[1], data->values[2]); + break; + case GRAVITY_SENSOR: + printf("Gravity [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data->timestamp, data->values[0], data->values[1], data->values[2]); + break; + case LINEAR_ACCEL_SENSOR: + printf("Linear acceleration [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data->timestamp, data->values[0], data->values[1], data->values[2]); + break; + case ROTATION_VECTOR_SENSOR: + printf("Rotation vector [%lld] [%6.6f] [%6.6f] [%6.6f] [%6.6f]\n\n", data->timestamp, data->values[0], data->values[1], data->values[2], data->values[3]); + break; + case GEOMAGNETIC_RV_SENSOR: + printf("Geomagnetic rotation vector [%lld] [%6.6f] [%6.6f] [%6.6f] [%6.6f]\n\n", data->timestamp, data->values[0], data->values[1], data->values[2], data->values[3]); + break; + default: + return; + } +} + +int main(int argc, char **argv) +{ + int result, handle, start_handle, stop_handle, interval; + char *end1, *end2; + unsigned int event; + bool EVENT_NOT_ENTERED = TRUE; + sensor_type_t sensor_type; + mainloop = g_main_loop_new(NULL, FALSE); + + if (argc < 2 || argc > 4) { + printf("Wrong number of arguments\n"); + usage(); + return 0; + } + + if (strcmp(argv[1], "accelerometer") == 0) { + sensor_type = ACCELEROMETER_SENSOR; + event = ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME; + } + else if (strcmp(argv[1], "gyroscope") == 0) { + sensor_type = GYROSCOPE_SENSOR; + event = GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME; + } + else if (strcmp(argv[1], "pressure") == 0) { + sensor_type = PRESSURE_SENSOR; + event = PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME; + } + else if (strcmp(argv[1], "temperature") == 0) { + sensor_type = TEMPERATURE_SENSOR; + event = TEMPERATURE_EVENT_RAW_DATA_REPORT_ON_TIME; + } + else if (strcmp(argv[1], "geomagnetic") == 0) { + sensor_type = GEOMAGNETIC_SENSOR; + event = GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME; + } + else if (strcmp(argv[1], "orientation") == 0) { + sensor_type = ORIENTATION_SENSOR; + event = ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME; + } + else if (strcmp(argv[1], "gravity") == 0) { + sensor_type = GRAVITY_SENSOR; + event = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME; + } + else if (strcmp(argv[1], "linear_accel") == 0) { + sensor_type = LINEAR_ACCEL_SENSOR; + event = LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME; + } + else if (strcmp(argv[1], "rotation_vector") == 0) { + sensor_type = ROTATION_VECTOR_SENSOR; + event = ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME; + } + else if (strcmp(argv[1], "gaming_rotation_vector") == 0) { + sensor_type = GEOMAGNETIC_RV_SENSOR; + event = GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME; + } + else if (strcmp(argv[1], "light") == 0) { + sensor_type = LIGHT_SENSOR; + event = LIGHT_EVENT_LUX_DATA_REPORT_ON_TIME; + } + else if (strcmp(argv[1], "proximity") == 0) { + sensor_type = PROXIMITY_SENSOR; + event = PROXIMITY_EVENT_CHANGE_STATE; + } + else { + usage(); + } + + interval = DEFAULT_EVENT_INTERVAL; + + if (argc > 2) { + event = get_event_driven(sensor_type, argv[2]); + + if (event == -1) { + usage(); + return -1; + } + + EVENT_NOT_ENTERED = FALSE; + } + + if (argc == 4) { + interval = strtol(argv[3], &end1, 10); + + if (*end1) { + printf("Conversion error, non-convertible part: %s\n", end1); + return -1; + } + } + + if (argc == 3 && EVENT_NOT_ENTERED) { + interval = strtol(argv[2], &end2, 10); + + if (*end2) { + printf("Conversion error, non-convertible part: %s\n", end2); + return -1; + } + } + + sensor_t sensor = sensord_get_sensor(sensor_type); + handle = sensord_connect(sensor); + + result = sensord_register_event(handle, event, interval, 0, callback, NULL); + + if (result < 0) { + printf("Can't register %s\n", argv[1]); + return -1; + } + + start_handle = sensord_start(handle, 0); + + if (start_handle < 0) { + printf("Error\n\n\n\n"); + sensord_unregister_event(handle, event); + sensord_disconnect(handle); + return -1; + } + + g_main_loop_run(mainloop); + g_main_loop_unref(mainloop); + + sensord_unregister_event(handle, event); + stop_handle = sensord_stop(handle); + + if (stop_handle < 0) { + printf("Error\n\n"); + return -1; + } + + sensord_disconnect(handle); + + return 0; +} diff --git a/test/src/temperature.c b/test/src/temperature.c deleted file mode 100644 index 01e6e81..0000000 --- a/test/src/temperature.c +++ /dev/null @@ -1,145 +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 - -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("Temperature [%lld] [%6.6f] \n\n", data->timestamp, data->values[0]); -} - -void printformat() -{ - printf("Usage : ./temperature (optional) (optional)\n\n"); - - printf("mode:"); - printf("[-p]\n"); - printf("p is for polling based,default mode is event driven\n"); - - printf("event:"); - printf("[RAW_DATA_REPORT_ON_TIME]\n"); - - printf("interval:\n"); - printf("The time interval should be entered based on the sampling frequency supported by temperature 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, start_handle, stop_handle; - unsigned int event; - mainloop = g_main_loop_new(NULL, FALSE); - event = TEMPERATURE_EVENT_RAW_DATA_REPORT_ON_TIME; - event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); - event_condition->cond_op = CONDITION_EQUAL; - - sensor_type_t type = TEMPERATURE_SENSOR; - - if (argc != 2 && argc != 3 && argc!=4) { - printformat(); - free(event_condition); - return 0; - } - - else if (argc>=3 && strcmp(argv[1], "-p") == 0 && strcmp(argv[2], "RAW_DATA_REPORT_ON_TIME") == 0) { - printf("Polling based\n"); - handle = sf_connect(type); - result = sf_start(handle, 1); - - if (result < 0) { - printf("Can't start temperature SENSOR\n"); - printf("Error\n\n\n\n"); - free(event_condition); - return -1; - } - - sensor_data_t data; - - while(1) { - result = sf_get_data(handle, TEMPERATURE_BASE_DATA_SET , &data); - printf("Temperature [%6.6f] \n\n", data.values[0]); - usleep(100000); - } - - result = sf_disconnect(handle); - - if (result < 0) { - printf("Can't disconnect temperature sensor\n"); - printf("Error\n\n\n\n"); - free(event_condition); - return -1; - } - } - - else if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) { - printf("Event based\n"); - - event_condition->cond_value1 = 100; - if (argc == 3) - event_condition->cond_value1 = atof(argv[2]); - - handle = sf_connect(type); - result = sf_register_event(handle, event, event_condition, callback, NULL); - - if (result < 0) - printf("Can't register temperature\n"); - - start_handle = sf_start(handle,0); - - if (start_handle < 0) { - printf("Error\n\n\n\n"); - sf_unregister_event(handle, event); - sf_disconnect(handle); - free(event_condition); - return -1; - } - - g_main_loop_run(mainloop); - g_main_loop_unref(mainloop); - - sf_unregister_event(handle, event); - - stop_handle = sf_stop(handle); - - if (stop_handle < 0) { - printf("Error\n\n"); - free(event_condition); - return -1; - } - - sf_disconnect(handle); - free(event_condition); - } - - else { - printformat(); - } - - return 0; -} - -- 2.7.4 From 8a764233038e4f8997fee8b515192bf931b586ca Mon Sep 17 00:00:00 2001 From: Ankur Date: Wed, 18 Feb 2015 15:57:13 +0530 Subject: [PATCH 13/16] Removing redundant accelerometer events ACCELEROMETER_BASE_DATA_SET and ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME were the same ACCELEROMETER_BASE_DATA_SET has been removed. ACCELEROMETER_BASE_DATA_SET was used for polling events and ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME was used for event driven Since both these represent the same accelerometer event, DATA_SET type has been removed. And ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME has been renamed to ACCELEROMETER_RAW_DATA_EVENT to make it simpler. The change has been tested on rd-pq device for all sensord API and all API were found to be working. Change-Id: I7602335045a1847b9ea3e58247b6ddcc82bbda7f --- src/accel/accel_sensor.cpp | 14 +++++++------- src/auto_rotation/auto_rotation_sensor.cpp | 6 +++--- src/libsensord/client_common.cpp | 5 ++--- src/libsensord/sensor_accel.h | 8 ++------ src/linear_accel/linear_accel_sensor.cpp | 8 ++++---- src/orientation/orientation_sensor.cpp | 8 ++++---- .../geomagnetic_rv/geomagnetic_rv_sensor.cpp | 8 ++++---- src/rotation_vector/rv/rv_sensor.cpp | 8 ++++---- src/server/command_worker.cpp | 4 ++-- test/src/auto_test.c | 2 +- test/src/tc-common.c | 4 ++-- 11 files changed, 35 insertions(+), 40 deletions(-) diff --git a/src/accel/accel_sensor.cpp b/src/accel/accel_sensor.cpp index 8bb436d..1dc28ff 100755 --- a/src/accel/accel_sensor.cpp +++ b/src/accel/accel_sensor.cpp @@ -42,8 +42,8 @@ accel_sensor::accel_sensor() m_name = string(SENSOR_NAME); vector supported_events = { - ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME, - ACCELEROMETER_EVENT_UNPROCESSED_DATA_REPORT_ON_TIME, + ACCELEROMETER_RAW_DATA_EVENT, + ACCELEROMETER_UNPROCESSED_DATA_EVENT, }; for_each(supported_events.begin(), supported_events.end(), @@ -104,15 +104,15 @@ bool accel_sensor::process_event(void) AUTOLOCK(m_mutex); AUTOLOCK(m_client_info_mutex); - if (get_client_cnt(ACCELEROMETER_EVENT_UNPROCESSED_DATA_REPORT_ON_TIME)) { + if (get_client_cnt(ACCELEROMETER_UNPROCESSED_DATA_EVENT)) { base_event.sensor_id = get_id(); - base_event.event_type = ACCELEROMETER_EVENT_UNPROCESSED_DATA_REPORT_ON_TIME; + base_event.event_type = ACCELEROMETER_UNPROCESSED_DATA_EVENT; push(base_event); } - if (get_client_cnt(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME)) { + if (get_client_cnt(ACCELEROMETER_RAW_DATA_EVENT)) { base_event.sensor_id = get_id(); - base_event.event_type = ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME; + base_event.event_type = ACCELEROMETER_RAW_DATA_EVENT; raw_to_base(base_event.data); push(base_event); } @@ -152,7 +152,7 @@ int accel_sensor::get_sensor_data(unsigned int type, sensor_data_t &data) return -1; } - if (type == ACCELEROMETER_BASE_DATA_SET) { + if (type == ACCELEROMETER_RAW_DATA_EVENT) { raw_to_base(data); } else { ERR("Does not support type: 0x%x", type); diff --git a/src/auto_rotation/auto_rotation_sensor.cpp b/src/auto_rotation/auto_rotation_sensor.cpp index efefb8b..cdf7609 100755 --- a/src/auto_rotation/auto_rotation_sensor.cpp +++ b/src/auto_rotation/auto_rotation_sensor.cpp @@ -110,7 +110,7 @@ bool auto_rotation_sensor::on_start(void) m_alg->start(); - m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); m_accel_sensor->add_interval((int)this , SAMPLING_TIME, true); m_accel_sensor->start(); @@ -119,7 +119,7 @@ bool auto_rotation_sensor::on_start(void) bool auto_rotation_sensor::on_stop(void) { - m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); m_accel_sensor->delete_interval((int)this , true); m_accel_sensor->stop(); @@ -130,7 +130,7 @@ void auto_rotation_sensor::synthesize(const sensor_event_t& event, vectoradd_interval((intptr_t)this, (m_interval/MS_TO_US), false); m_gravity_sensor->start(); - m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); m_accel_sensor->start(); @@ -165,7 +165,7 @@ bool linear_accel_sensor::on_stop(void) m_gravity_sensor->delete_interval((intptr_t)this, false); m_gravity_sensor->stop(); - m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); m_accel_sensor->delete_interval((intptr_t)this, false); m_accel_sensor->stop(); @@ -197,7 +197,7 @@ void linear_accel_sensor::synthesize(const sensor_event_t &event, vectorget_sensor_data(GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME, gravity_data); - m_accel_sensor->get_sensor_data(ACCELEROMETER_BASE_DATA_SET, accel_data); + m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data); accel_data.values[0] = m_accel_rotation_direction_compensation[0] * (accel_data.values[0] - m_accel_static_bias[0]) / m_accel_scale; accel_data.values[1] = m_accel_rotation_direction_compensation[1] * (accel_data.values[1] - m_accel_static_bias[1]) / m_accel_scale; diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp index 7629132..0719a1b 100755 --- a/src/orientation/orientation_sensor.cpp +++ b/src/orientation/orientation_sensor.cpp @@ -232,7 +232,7 @@ bool orientation_sensor::on_start(void) { AUTOLOCK(m_mutex); - m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); m_accel_sensor->start(); m_gyro_sensor->add_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); @@ -250,7 +250,7 @@ bool orientation_sensor::on_stop(void) { AUTOLOCK(m_mutex); - m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); m_accel_sensor->delete_interval((intptr_t)this, false); m_accel_sensor->stop(); m_gyro_sensor->delete_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); @@ -294,7 +294,7 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vector euler_orientation; float azimuth_offset; - if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) { + if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) { diff_time = event.data.timestamp - m_time; if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) @@ -388,7 +388,7 @@ int orientation_sensor::get_sensor_data(const unsigned int event_type, sensor_da 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_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, 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); diff --git a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp index ea5a446..3f6646e 100755 --- a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp +++ b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp @@ -171,7 +171,7 @@ bool geomagnetic_rv_sensor::on_start(void) { AUTOLOCK(m_mutex); - m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); m_accel_sensor->start(); m_magnetic_sensor->add_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); @@ -186,7 +186,7 @@ bool geomagnetic_rv_sensor::on_stop(void) { AUTOLOCK(m_mutex); - m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); m_accel_sensor->delete_interval((intptr_t)this, false); m_accel_sensor->stop(); m_magnetic_sensor->delete_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); @@ -225,7 +225,7 @@ void geomagnetic_rv_sensor::synthesize(const sensor_event_t& event, vector quaternion_geo_rv; - if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) { + if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) { diff_time = event.data.timestamp - m_time; if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) @@ -291,7 +291,7 @@ int geomagnetic_rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_ if (event_type != GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME) return -1; - m_accel_sensor->get_sensor_data(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME, accel_data); + m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data); m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, magnetic_data); pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale); diff --git a/src/rotation_vector/rv/rv_sensor.cpp b/src/rotation_vector/rv/rv_sensor.cpp index 7846940..392d930 100755 --- a/src/rotation_vector/rv/rv_sensor.cpp +++ b/src/rotation_vector/rv/rv_sensor.cpp @@ -198,7 +198,7 @@ bool rv_sensor::on_start(void) { AUTOLOCK(m_mutex); - m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); m_accel_sensor->start(); m_gyro_sensor->add_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); @@ -216,7 +216,7 @@ bool rv_sensor::on_stop(void) { AUTOLOCK(m_mutex); - m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); m_accel_sensor->delete_interval((intptr_t)this, false); m_accel_sensor->stop(); m_gyro_sensor->delete_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); @@ -260,7 +260,7 @@ void rv_sensor::synthesize(const sensor_event_t& event, vector & sensor_event_t rv_event; quaternion quaternion_orientation; - if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) { + if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) { diff_time = event.data.timestamp - m_time; if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) @@ -343,7 +343,7 @@ int rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &data) if (event_type != ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME) return -1; - m_accel_sensor->get_sensor_data(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME, accel_data); + m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, 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); diff --git a/src/server/command_worker.cpp b/src/server/command_worker.cpp index c161171..d9c5baa 100755 --- a/src/server/command_worker.cpp +++ b/src/server/command_worker.cpp @@ -853,13 +853,13 @@ void insert_priority_list(unsigned int event_type) event_type == LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME || event_type == GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME || event_type == ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME) { - priority_list.insert(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + priority_list.insert(ACCELEROMETER_RAW_DATA_EVENT); priority_list.insert(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); priority_list.insert(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); } if (event_type == GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME) { - priority_list.insert(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + priority_list.insert(ACCELEROMETER_RAW_DATA_EVENT); priority_list.insert(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); } } diff --git a/test/src/auto_test.c b/test/src/auto_test.c index e897f74..283aaf1 100644 --- a/test/src/auto_test.c +++ b/test/src/auto_test.c @@ -199,7 +199,7 @@ int main(int argc, char **argv) fp = fopen("auto_test.output", "w+"); - result = check_sensor_api(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME, interval); + result = check_sensor_api(ACCELEROMETER_RAW_DATA_EVENT, interval); fprintf(fp, "Accelerometer - RAW_DATA_REPORT_ON_TIME - %d\n", result); result = check_sensor_api(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, interval); diff --git a/test/src/tc-common.c b/test/src/tc-common.c index 81d83d9..368143a 100644 --- a/test/src/tc-common.c +++ b/test/src/tc-common.c @@ -63,7 +63,7 @@ unsigned int get_event_driven(sensor_type_t sensor_type, char str[]) switch (sensor_type) { case ACCELEROMETER_SENSOR: if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) - return ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME; + return ACCELEROMETER_RAW_DATA_EVENT; break; case GYROSCOPE_SENSOR: if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) @@ -177,7 +177,7 @@ int main(int argc, char **argv) if (strcmp(argv[1], "accelerometer") == 0) { sensor_type = ACCELEROMETER_SENSOR; - event = ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME; + event = ACCELEROMETER_RAW_DATA_EVENT; } else if (strcmp(argv[1], "gyroscope") == 0) { sensor_type = GYROSCOPE_SENSOR; -- 2.7.4 From c7a9a0568a32cac9fff4de85180745721c954962 Mon Sep 17 00:00:00 2001 From: Ankur Date: Wed, 18 Feb 2015 16:55:35 +0530 Subject: [PATCH 14/16] Removing redundant gyroscope events GYRO_BASE_DATA_SET and GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME were the same GYRO_BASE_DATA_SET has been removed. GYRO_BASE_DATA_SET was used for polling events and GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME was used for event driven Since both these represent the same gyroscope event, DATA_SET type has been removed. And GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME has been renamed to GYROSCOPE_RAW_DATA_EVENT to make it simpler. The change has been tested on rd-pq device for all sensord API and all API were found to be working. Change-Id: Ie4d6642691a8d57abed5f0b6387922fc871904c4 --- src/gyro/gyro_sensor.cpp | 14 +++++++------- src/libsensord/client_common.cpp | 5 ++--- src/libsensord/sensor_gyro.h | 8 ++------ src/orientation/orientation_sensor.cpp | 8 ++++---- src/rotation_vector/rv/rv_sensor.cpp | 8 ++++---- src/server/command_worker.cpp | 2 +- test/src/auto_test.c | 2 +- test/src/tc-common.c | 4 ++-- 8 files changed, 23 insertions(+), 28 deletions(-) diff --git a/src/gyro/gyro_sensor.cpp b/src/gyro/gyro_sensor.cpp index cdf4109..c7b0001 100755 --- a/src/gyro/gyro_sensor.cpp +++ b/src/gyro/gyro_sensor.cpp @@ -35,8 +35,8 @@ gyro_sensor::gyro_sensor() { m_name = string(SENSOR_NAME); - register_supported_event(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); - register_supported_event(GYROSCOPE_EVENT_UNPROCESSED_DATA_REPORT_ON_TIME); + register_supported_event(GYROSCOPE_RAW_DATA_EVENT); + register_supported_event(GYROSCOPE_UNPROCESSED_DATA_EVENT); physical_sensor::set_poller(gyro_sensor::working, this); } @@ -91,15 +91,15 @@ bool gyro_sensor::process_event(void) AUTOLOCK(m_client_info_mutex); - if (get_client_cnt(GYROSCOPE_EVENT_UNPROCESSED_DATA_REPORT_ON_TIME)) { + if (get_client_cnt(GYROSCOPE_UNPROCESSED_DATA_EVENT)) { event.sensor_id = get_id(); - event.event_type = GYROSCOPE_EVENT_UNPROCESSED_DATA_REPORT_ON_TIME; + event.event_type = GYROSCOPE_UNPROCESSED_DATA_EVENT; push(event); } - if (get_client_cnt(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME)) { + if (get_client_cnt(GYROSCOPE_RAW_DATA_EVENT)) { event.sensor_id = get_id(); - event.event_type = GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME; + event.event_type = GYROSCOPE_RAW_DATA_EVENT; raw_to_base(event.data); push(event); } @@ -136,7 +136,7 @@ int gyro_sensor::get_sensor_data(unsigned int type, sensor_data_t &data) { int state; - if (type != GYRO_BASE_DATA_SET) + if (type != GYROSCOPE_RAW_DATA_EVENT) return -1; state = m_sensor_hal->get_sensor_data(data); diff --git a/src/libsensord/client_common.cpp b/src/libsensord/client_common.cpp index f022992..f97e9b0 100755 --- a/src/libsensord/client_common.cpp +++ b/src/libsensord/client_common.cpp @@ -51,7 +51,7 @@ log_element g_log_elements[] = { FILL_LOG_ELEMENT(LOG_ID_EVENT, CONTEXT_EVENT_REPORT, 0, 1), FILL_LOG_ELEMENT(LOG_ID_EVENT, AUTO_ROTATION_EVENT_CHANGE_STATE, 0, 1), FILL_LOG_ELEMENT(LOG_ID_EVENT, ACCELEROMETER_RAW_DATA_EVENT, 0, 10), - FILL_LOG_ELEMENT(LOG_ID_EVENT, GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), + FILL_LOG_ELEMENT(LOG_ID_EVENT, GYROSCOPE_RAW_DATA_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, LIGHT_EVENT_LEVEL_DATA_REPORT_ON_TIME, 0, 10), @@ -64,7 +64,6 @@ log_element g_log_elements[] = { FILL_LOG_ELEMENT(LOG_ID_EVENT, ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), - FILL_LOG_ELEMENT(LOG_ID_DATA, GYRO_BASE_DATA_SET, 0, 25), FILL_LOG_ELEMENT(LOG_ID_DATA, PROXIMITY_BASE_DATA_SET, 0, 25), FILL_LOG_ELEMENT(LOG_ID_DATA, PROXIMITY_DISTANCE_DATA_SET, 0, 25), FILL_LOG_ELEMENT(LOG_ID_DATA, PRESSURE_BASE_DATA_SET, 0, 25), @@ -154,7 +153,7 @@ bool is_ontime_event(unsigned int event_type) switch (event_type ) { case ACCELEROMETER_RAW_DATA_EVENT: case PROXIMITY_EVENT_STATE_REPORT_ON_TIME: - case GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME: + case GYROSCOPE_RAW_DATA_EVENT: case LIGHT_EVENT_LEVEL_DATA_REPORT_ON_TIME: case GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME: case LIGHT_EVENT_LUX_DATA_REPORT_ON_TIME: diff --git a/src/libsensord/sensor_gyro.h b/src/libsensord/sensor_gyro.h index 4a5ce87..0425afe 100755 --- a/src/libsensord/sensor_gyro.h +++ b/src/libsensord/sensor_gyro.h @@ -36,13 +36,9 @@ extern "C" * @{ */ -enum gyro_data_id { - GYRO_BASE_DATA_SET = (GYROSCOPE_SENSOR << 16) | 0x0001, -}; - enum gyro_event_type { - GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME = (GYROSCOPE_SENSOR << 16) | 0x0001, - GYROSCOPE_EVENT_UNPROCESSED_DATA_REPORT_ON_TIME = (GYROSCOPE_SENSOR << 16) | 0x0002, + GYROSCOPE_RAW_DATA_EVENT = (GYROSCOPE_SENSOR << 16) | 0x0001, + GYROSCOPE_UNPROCESSED_DATA_EVENT = (GYROSCOPE_SENSOR << 16) | 0x0002, }; /** diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp index 0719a1b..424db60 100755 --- a/src/orientation/orientation_sensor.cpp +++ b/src/orientation/orientation_sensor.cpp @@ -235,7 +235,7 @@ bool orientation_sensor::on_start(void) m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); m_accel_sensor->start(); - m_gyro_sensor->add_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); + m_gyro_sensor->add_client(GYROSCOPE_RAW_DATA_EVENT); m_gyro_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); m_gyro_sensor->start(); m_magnetic_sensor->add_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); @@ -253,7 +253,7 @@ bool orientation_sensor::on_stop(void) m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); m_accel_sensor->delete_interval((intptr_t)this, false); m_accel_sensor->stop(); - m_gyro_sensor->delete_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); + m_gyro_sensor->delete_client(GYROSCOPE_RAW_DATA_EVENT); m_gyro_sensor->delete_interval((intptr_t)this, false); m_gyro_sensor->stop(); m_magnetic_sensor->delete_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); @@ -306,7 +306,7 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vectorget_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data); - m_gyro_sensor->get_sensor_data(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME, gyro_data); + m_gyro_sensor->get_sensor_data(GYROSCOPE_RAW_DATA_EVENT, gyro_data); m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, magnetic_data); pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale); diff --git a/src/rotation_vector/rv/rv_sensor.cpp b/src/rotation_vector/rv/rv_sensor.cpp index 392d930..9c80252 100755 --- a/src/rotation_vector/rv/rv_sensor.cpp +++ b/src/rotation_vector/rv/rv_sensor.cpp @@ -201,7 +201,7 @@ bool rv_sensor::on_start(void) m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); m_accel_sensor->start(); - m_gyro_sensor->add_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); + m_gyro_sensor->add_client(GYROSCOPE_RAW_DATA_EVENT); m_gyro_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); m_gyro_sensor->start(); m_magnetic_sensor->add_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); @@ -219,7 +219,7 @@ bool rv_sensor::on_stop(void) m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); m_accel_sensor->delete_interval((intptr_t)this, false); m_accel_sensor->stop(); - m_gyro_sensor->delete_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); + m_gyro_sensor->delete_client(GYROSCOPE_RAW_DATA_EVENT); m_gyro_sensor->delete_interval((intptr_t)this, false); m_gyro_sensor->stop(); m_magnetic_sensor->delete_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); @@ -272,7 +272,7 @@ void rv_sensor::synthesize(const sensor_event_t& event, vector & m_enable_orientation |= ACCELEROMETER_ENABLED; } - else if (event.event_type == GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME) { + else if (event.event_type == GYROSCOPE_RAW_DATA_EVENT) { diff_time = event.data.timestamp - m_time; if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) @@ -344,7 +344,7 @@ int rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &data) return -1; m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data); - m_gyro_sensor->get_sensor_data(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME, gyro_data); + m_gyro_sensor->get_sensor_data(GYROSCOPE_RAW_DATA_EVENT, gyro_data); m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, magnetic_data); pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale); diff --git a/src/server/command_worker.cpp b/src/server/command_worker.cpp index d9c5baa..048f2d5 100755 --- a/src/server/command_worker.cpp +++ b/src/server/command_worker.cpp @@ -854,7 +854,7 @@ void insert_priority_list(unsigned int event_type) event_type == GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME || event_type == ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME) { priority_list.insert(ACCELEROMETER_RAW_DATA_EVENT); - priority_list.insert(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); + priority_list.insert(GYROSCOPE_RAW_DATA_EVENT); priority_list.insert(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); } diff --git a/test/src/auto_test.c b/test/src/auto_test.c index 283aaf1..a770c69 100644 --- a/test/src/auto_test.c +++ b/test/src/auto_test.c @@ -208,7 +208,7 @@ int main(int argc, char **argv) result = check_sensor_api(GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME, interval); fprintf(fp, "Gravity - RAW_DATA_REPORT_ON_TIME - %d\n", result); - result = check_sensor_api(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME, interval); + result = check_sensor_api(GYROSCOPE_RAW_DATA_EVENT, interval); fprintf(fp, "Gyroscope - RAW_DATA_REPORT_ON_TIME - %d\n", result); result = check_sensor_api(LIGHT_EVENT_LUX_DATA_REPORT_ON_TIME, interval); diff --git a/test/src/tc-common.c b/test/src/tc-common.c index 368143a..12dcde6 100644 --- a/test/src/tc-common.c +++ b/test/src/tc-common.c @@ -67,7 +67,7 @@ unsigned int get_event_driven(sensor_type_t sensor_type, char str[]) break; case GYROSCOPE_SENSOR: if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) - return GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME; + return GYROSCOPE_RAW_DATA_EVENT; break; case PRESSURE_SENSOR: if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) @@ -181,7 +181,7 @@ int main(int argc, char **argv) } else if (strcmp(argv[1], "gyroscope") == 0) { sensor_type = GYROSCOPE_SENSOR; - event = GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME; + event = GYROSCOPE_RAW_DATA_EVENT; } else if (strcmp(argv[1], "pressure") == 0) { sensor_type = PRESSURE_SENSOR; -- 2.7.4 From 3ef120f1e032d5a74fc1daaf2f25cffb23236132 Mon Sep 17 00:00:00 2001 From: Ankur Date: Wed, 18 Feb 2015 17:21:21 +0530 Subject: [PATCH 15/16] Removing redundant proximity events PROXIMITY_BASE_DATA_SET and PROXIMITY_EVENT_CHANGE_STATE were the same PROXIMITY_BASE_DATA_SET has been removed. PROXIMITY_BASE_DATA_SET was used for polling events and PROXIMITY_EVENT_CHANGE_STATE was used for event driven Since both these represent the same proximity event, DATA_SET type has been removed. And PROXIMITY_EVENT_CHANGE_STATE has been renamed to PROXIMITY_CHANGE_STATE_EVENT to make it simpler. The change has been tested on rd-pq device for all sensord API and all API were found to be working. Change-Id: Idb5a5213fdeecdeef0503b8f7388cdb2b0b36385 --- src/libsensord/client_common.cpp | 14 ++++++-------- src/libsensord/sensor_proxi.h | 11 +++-------- src/proxi/proxi_sensor.cpp | 16 ++++++++-------- test/src/tc-common.c | 4 ++-- 4 files changed, 19 insertions(+), 26 deletions(-) diff --git a/src/libsensord/client_common.cpp b/src/libsensord/client_common.cpp index f97e9b0..340f281 100755 --- a/src/libsensord/client_common.cpp +++ b/src/libsensord/client_common.cpp @@ -44,10 +44,10 @@ log_element g_log_elements[] = { FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, GEOMAGNETIC_RV_SENSOR, 0, 1), FILL_LOG_ELEMENT(LOG_ID_EVENT, GEOMAGNETIC_EVENT_CALIBRATION_NEEDED, 0, 1), - FILL_LOG_ELEMENT(LOG_ID_EVENT, PROXIMITY_EVENT_CHANGE_STATE, 0,1), + FILL_LOG_ELEMENT(LOG_ID_EVENT, PROXIMITY_CHANGE_STATE_EVENT, 0,1), FILL_LOG_ELEMENT(LOG_ID_EVENT, LIGHT_EVENT_CHANGE_LEVEL, 0, 1), - FILL_LOG_ELEMENT(LOG_ID_EVENT, PROXIMITY_EVENT_STATE_REPORT_ON_TIME, 0, 10), - FILL_LOG_ELEMENT(LOG_ID_EVENT, PROXIMITY_EVENT_DISTANCE_DATA_REPORT_ON_TIME, 0, 10), + FILL_LOG_ELEMENT(LOG_ID_EVENT, PROXIMITY_STATE_EVENT, 0, 10), + FILL_LOG_ELEMENT(LOG_ID_EVENT, PROXIMITY_DISTANCE_DATA_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, CONTEXT_EVENT_REPORT, 0, 1), FILL_LOG_ELEMENT(LOG_ID_EVENT, AUTO_ROTATION_EVENT_CHANGE_STATE, 0, 1), FILL_LOG_ELEMENT(LOG_ID_EVENT, ACCELEROMETER_RAW_DATA_EVENT, 0, 10), @@ -64,8 +64,6 @@ log_element g_log_elements[] = { FILL_LOG_ELEMENT(LOG_ID_EVENT, ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), - FILL_LOG_ELEMENT(LOG_ID_DATA, PROXIMITY_BASE_DATA_SET, 0, 25), - FILL_LOG_ELEMENT(LOG_ID_DATA, PROXIMITY_DISTANCE_DATA_SET, 0, 25), FILL_LOG_ELEMENT(LOG_ID_DATA, PRESSURE_BASE_DATA_SET, 0, 25), FILL_LOG_ELEMENT(LOG_ID_DATA, GEOMAGNETIC_BASE_DATA_SET, 0, 25), FILL_LOG_ELEMENT(LOG_ID_DATA, LIGHT_BASE_DATA_SET, 0, 25), @@ -152,12 +150,12 @@ bool is_ontime_event(unsigned int event_type) { switch (event_type ) { case ACCELEROMETER_RAW_DATA_EVENT: - case PROXIMITY_EVENT_STATE_REPORT_ON_TIME: + case PROXIMITY_STATE_EVENT: case GYROSCOPE_RAW_DATA_EVENT: case LIGHT_EVENT_LEVEL_DATA_REPORT_ON_TIME: case GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME: case LIGHT_EVENT_LUX_DATA_REPORT_ON_TIME: - case PROXIMITY_EVENT_DISTANCE_DATA_REPORT_ON_TIME: + case PROXIMITY_DISTANCE_DATA_EVENT: case GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME: case LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME: case ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME: @@ -179,7 +177,7 @@ bool is_single_state_event(unsigned int event_type) switch (event_type) { case GEOMAGNETIC_EVENT_CALIBRATION_NEEDED: case LIGHT_EVENT_CHANGE_LEVEL: - case PROXIMITY_EVENT_CHANGE_STATE: + case PROXIMITY_CHANGE_STATE_EVENT: case AUTO_ROTATION_EVENT_CHANGE_STATE: return true; break; diff --git a/src/libsensord/sensor_proxi.h b/src/libsensord/sensor_proxi.h index e7e2270..22939d9 100755 --- a/src/libsensord/sensor_proxi.h +++ b/src/libsensord/sensor_proxi.h @@ -33,15 +33,10 @@ extern "C" * @{ */ -enum proxi_data_id { - PROXIMITY_BASE_DATA_SET = (PROXIMITY_SENSOR << 16) | 0x0001, - PROXIMITY_DISTANCE_DATA_SET = (PROXIMITY_SENSOR << 16) | 0x0002, -}; - enum proxi_event_type { - PROXIMITY_EVENT_CHANGE_STATE = (PROXIMITY_SENSOR << 16) | 0x0001, - PROXIMITY_EVENT_STATE_REPORT_ON_TIME = (PROXIMITY_SENSOR << 16) | 0x0002, - PROXIMITY_EVENT_DISTANCE_DATA_REPORT_ON_TIME = (PROXIMITY_SENSOR << 16) | 0x0004, + PROXIMITY_CHANGE_STATE_EVENT = (PROXIMITY_SENSOR << 16) | 0x0001, + PROXIMITY_STATE_EVENT = (PROXIMITY_SENSOR << 16) | 0x0002, + PROXIMITY_DISTANCE_DATA_EVENT = (PROXIMITY_SENSOR << 16) | 0x0004, }; enum proxi_change_state { diff --git a/src/proxi/proxi_sensor.cpp b/src/proxi/proxi_sensor.cpp index 31ef1d6..65809b9 100755 --- a/src/proxi/proxi_sensor.cpp +++ b/src/proxi/proxi_sensor.cpp @@ -30,9 +30,9 @@ proxi_sensor::proxi_sensor() { m_name = string(SENSOR_NAME); - register_supported_event(PROXIMITY_EVENT_CHANGE_STATE); - register_supported_event(PROXIMITY_EVENT_STATE_REPORT_ON_TIME); - register_supported_event(PROXIMITY_EVENT_DISTANCE_DATA_REPORT_ON_TIME); + register_supported_event(PROXIMITY_CHANGE_STATE_EVENT); + register_supported_event(PROXIMITY_STATE_EVENT); + register_supported_event(PROXIMITY_DISTANCE_DATA_EVENT); physical_sensor::set_poller(proxi_sensor::working, this); } @@ -80,8 +80,8 @@ bool proxi_sensor::process_event(void) AUTOLOCK(m_mutex); event.sensor_id = get_id(); - if (get_client_cnt(PROXIMITY_EVENT_DISTANCE_DATA_REPORT_ON_TIME)) { - event.event_type = PROXIMITY_EVENT_DISTANCE_DATA_REPORT_ON_TIME; + if (get_client_cnt(PROXIMITY_DISTANCE_DATA_EVENT)) { + event.event_type = PROXIMITY_DISTANCE_DATA_EVENT; raw_to_base(event.data); push(event); } @@ -92,8 +92,8 @@ bool proxi_sensor::process_event(void) AUTOLOCK(m_value_mutex); m_state = state; - if (get_client_cnt(PROXIMITY_EVENT_CHANGE_STATE)) { - event.event_type = PROXIMITY_EVENT_CHANGE_STATE; + if (get_client_cnt(PROXIMITY_CHANGE_STATE_EVENT)) { + event.event_type = PROXIMITY_CHANGE_STATE_EVENT; raw_to_base(event.data); push(event); } @@ -136,7 +136,7 @@ int proxi_sensor::get_sensor_data(unsigned int type, sensor_data_t &data) { int state; - if ((type != PROXIMITY_BASE_DATA_SET) && (type != PROXIMITY_DISTANCE_DATA_SET)) + if ((type != PROXIMITY_CHANGE_STATE_EVENT) && (type != PROXIMITY_STATE_EVENT)) return -1; state = m_sensor_hal->get_sensor_data(data); diff --git a/test/src/tc-common.c b/test/src/tc-common.c index 12dcde6..5913216 100644 --- a/test/src/tc-common.c +++ b/test/src/tc-common.c @@ -87,7 +87,7 @@ unsigned int get_event_driven(sensor_type_t sensor_type, char str[]) break; case PROXIMITY_SENSOR: if (strcmp(str, "EVENT_CHANGE_STATE") == 0) - return PROXIMITY_EVENT_CHANGE_STATE; + return PROXIMITY_CHANGE_STATE_EVENT; break; case ORIENTATION_SENSOR: if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) @@ -221,7 +221,7 @@ int main(int argc, char **argv) } else if (strcmp(argv[1], "proximity") == 0) { sensor_type = PROXIMITY_SENSOR; - event = PROXIMITY_EVENT_CHANGE_STATE; + event = PROXIMITY_CHANGE_STATE_EVENT; } else { usage(); -- 2.7.4 From 13e0b7b261fd2b46f918a651761d51233bf8616e Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Wed, 18 Feb 2015 18:04:15 +0530 Subject: [PATCH 16/16] Removing redundant pressure sensor events PRESSURE_BASE_DATA_SET and PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME were the same PRESSURE_BASE_DATA_SET has been removed. PRESSURE_BASE_DATA_SET was used for polling events and PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME was used for event driven Since both these represent the same pressure event, DATA_SET type has been removed. And PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME has been renamed to PRESSURE_RAW_DATA_EVENT to make it simpler. The change has been tested on rd-pq device for all sensord API and all API were found to be working. Change-Id: I6d4a0c81787dab67b3f61a2c511b181c50a763d1 --- src/libsensord/client_common.cpp | 8 +++----- src/libsensord/sensor_pressure.h | 6 +----- src/pressure/pressure_sensor.cpp | 8 ++++---- test/src/auto_test.c | 2 +- test/src/tc-common.c | 4 ++-- 5 files changed, 11 insertions(+), 17 deletions(-) diff --git a/src/libsensord/client_common.cpp b/src/libsensord/client_common.cpp index 340f281..ba15264 100755 --- a/src/libsensord/client_common.cpp +++ b/src/libsensord/client_common.cpp @@ -53,18 +53,17 @@ log_element g_log_elements[] = { FILL_LOG_ELEMENT(LOG_ID_EVENT, ACCELEROMETER_RAW_DATA_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, GYROSCOPE_RAW_DATA_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), - FILL_LOG_ELEMENT(LOG_ID_EVENT, PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), + FILL_LOG_ELEMENT(LOG_ID_EVENT, PRESSURE_RAW_DATA_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, LIGHT_EVENT_LEVEL_DATA_REPORT_ON_TIME, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, LIGHT_EVENT_LUX_DATA_REPORT_ON_TIME, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), - FILL_LOG_ELEMENT(LOG_ID_EVENT, PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), + FILL_LOG_ELEMENT(LOG_ID_EVENT, PRESSURE_RAW_DATA_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, TEMPERATURE_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), - FILL_LOG_ELEMENT(LOG_ID_DATA, PRESSURE_BASE_DATA_SET, 0, 25), FILL_LOG_ELEMENT(LOG_ID_DATA, GEOMAGNETIC_BASE_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), @@ -73,7 +72,6 @@ log_element g_log_elements[] = { 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_DATA, PRESSURE_BASE_DATA_SET, 0, 25), FILL_LOG_ELEMENT(LOG_ID_DATA, TEMPERATURE_BASE_DATA_SET, 0, 25), }; @@ -159,7 +157,7 @@ bool is_ontime_event(unsigned int event_type) case GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME: case LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME: case ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME: - case PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME: + case PRESSURE_RAW_DATA_EVENT: return true; break; } diff --git a/src/libsensord/sensor_pressure.h b/src/libsensord/sensor_pressure.h index dcf6b37..e06b39f 100755 --- a/src/libsensord/sensor_pressure.h +++ b/src/libsensord/sensor_pressure.h @@ -36,12 +36,8 @@ extern "C" * @{ */ -enum pressure_data_id { - PRESSURE_BASE_DATA_SET = (PRESSURE_SENSOR << 16) | 0x0001, -}; - enum pressure_event_type { - PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME = (PRESSURE_SENSOR << 16) | 0x0001, + PRESSURE_RAW_DATA_EVENT = (PRESSURE_SENSOR << 16) | 0x0001, }; /** diff --git a/src/pressure/pressure_sensor.cpp b/src/pressure/pressure_sensor.cpp index b70f48d..407c6f9 100755 --- a/src/pressure/pressure_sensor.cpp +++ b/src/pressure/pressure_sensor.cpp @@ -43,7 +43,7 @@ pressure_sensor::pressure_sensor() { m_name = string(SENSOR_NAME); - register_supported_event(PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME); + register_supported_event(PRESSURE_RAW_DATA_EVENT); physical_sensor::set_poller(pressure_sensor::working, this); } @@ -122,9 +122,9 @@ bool pressure_sensor::process_event(void) AUTOLOCK(m_client_info_mutex); - if (get_client_cnt(PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME)) { + if (get_client_cnt(PRESSURE_RAW_DATA_EVENT)) { event.sensor_id = get_id(); - event.event_type = PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME; + event.event_type = PRESSURE_RAW_DATA_EVENT; raw_to_base(event.data); push(event); } @@ -166,7 +166,7 @@ int pressure_sensor::get_sensor_data(unsigned int type, sensor_data_t &data) if (ret < 0) return -1; - if (type == PRESSURE_BASE_DATA_SET) { + if (type == PRESSURE_RAW_DATA_EVENT) { raw_to_base(data); return 0; } diff --git a/test/src/auto_test.c b/test/src/auto_test.c index a770c69..c369b26 100644 --- a/test/src/auto_test.c +++ b/test/src/auto_test.c @@ -220,7 +220,7 @@ int main(int argc, char **argv) result = check_sensor_api(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME, interval); fprintf(fp, "Orientation - RAW_DATA_REPORT_ON_TIME - %d\n", result); - result = check_sensor_api(PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME, interval); + result = check_sensor_api(PRESSURE_RAW_DATA_EVENT, interval); fprintf(fp, "Pressure - RAW_DATA_REPORT_ON_TIME - %d\n", result); result = check_sensor_api(ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME, interval); diff --git a/test/src/tc-common.c b/test/src/tc-common.c index 5913216..9c3b633 100644 --- a/test/src/tc-common.c +++ b/test/src/tc-common.c @@ -71,7 +71,7 @@ unsigned int get_event_driven(sensor_type_t sensor_type, char str[]) break; case PRESSURE_SENSOR: if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) - return PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME; + return PRESSURE_RAW_DATA_EVENT; break; case GEOMAGNETIC_SENSOR: if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) @@ -185,7 +185,7 @@ int main(int argc, char **argv) } else if (strcmp(argv[1], "pressure") == 0) { sensor_type = PRESSURE_SENSOR; - event = PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME; + event = PRESSURE_RAW_DATA_EVENT; } else if (strcmp(argv[1], "temperature") == 0) { sensor_type = TEMPERATURE_SENSOR; -- 2.7.4