From dfb73ce3c70609da7bc614449059caefd3f598cb Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Mon, 16 Mar 2015 19:20:01 +0530 Subject: [PATCH 01/16] Fixing value assignment to m_orientation - fixing value assignment for m_orientation in orientation_filter - cleanup of fusion_sensor Change-Id: I3442dd275adc3952ff92b5911bcab2b6751ccf79 --- src/fusion/fusion_sensor.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/fusion/fusion_sensor.h b/src/fusion/fusion_sensor.h index 010c786..8b4013f 100755 --- a/src/fusion/fusion_sensor.h +++ b/src/fusion/fusion_sensor.h @@ -76,9 +76,6 @@ private: float m_gyro_scale; float m_geomagnetic_scale; int m_magnetic_alignment_factor; - int m_azimuth_rotation_compensation; - int m_pitch_rotation_compensation; - int m_roll_rotation_compensation; bool on_start(void); bool on_stop(void); -- 2.7.4 From ad424c3610364192c9704965295e75c42dc3ae07 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 18 Mar 2015 13:32:51 +0530 Subject: [PATCH 02/16] Improved rotation matrix to quaternion conversion - This has better azimuth computation response over previous implementation. Change-Id: Ie631f1f3e3815c4553d4bb51efdabf23738f680a --- src/sensor_fusion/rotation_matrix.cpp | 54 +++++++++++------------------------ 1 file changed, 16 insertions(+), 38 deletions(-) diff --git a/src/sensor_fusion/rotation_matrix.cpp b/src/sensor_fusion/rotation_matrix.cpp index 83ff5d3..fd3e6b5 100644 --- a/src/sensor_fusion/rotation_matrix.cpp +++ b/src/sensor_fusion/rotation_matrix.cpp @@ -95,44 +95,22 @@ template quaternion rot_mat2quat(rotation_matrix rm) { T q0, q1, q2, q3; - - T diag_sum = rm.m_rot_mat.m_mat[0][0] + rm.m_rot_mat.m_mat[1][1] + rm.m_rot_mat.m_mat[2][2]; - - if ( diag_sum > 0 ) - { - T val = (T) 0.5 / sqrt(diag_sum + (T) 1.0); - q0 = (T) 0.25 / val; - q1 = ( rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2] ) * val; - q2 = ( rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0] ) * val; - q3 = ( rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1] ) * val; - } - else - { - if ( rm.m_rot_mat.m_mat[0][0] > rm.m_rot_mat.m_mat[1][1] && rm.m_rot_mat.m_mat[0][0] > rm.m_rot_mat.m_mat[2][2] ) - { - T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] - rm.m_rot_mat.m_mat[2][2]); - q0 = (rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2] ) / val; - q1 = (T) 0.25 * val; - q2 = (rm.m_rot_mat.m_mat[0][1] + rm.m_rot_mat.m_mat[1][0] ) / val; - q3 = (rm.m_rot_mat.m_mat[0][2] + rm.m_rot_mat.m_mat[2][0] ) / val; - } - else if (rm.m_rot_mat.m_mat[1][1] > rm.m_rot_mat.m_mat[2][2]) - { - T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[1][1] - rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[2][2]); - q0 = (rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0] ) / val; - q1 = (rm.m_rot_mat.m_mat[0][1] + rm.m_rot_mat.m_mat[1][0] ) / val; - q2 = (T) 0.25 * val; - q3 = (rm.m_rot_mat.m_mat[1][2] + rm.m_rot_mat.m_mat[2][1] ) / val; - } - else - { - T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[2][2] - rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] ); - q0 = (rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1] ) / val; - q1 = (rm.m_rot_mat.m_mat[0][2] + rm.m_rot_mat.m_mat[2][0] ) / val; - q2 = (rm.m_rot_mat.m_mat[1][2] + rm.m_rot_mat.m_mat[2][1] ) / val; - q3 = (T) 0.25 * val; - } - } + T phi, theta, psi; + + phi = atan2(rm.m_rot_mat.m_mat[2][1], rm.m_rot_mat.m_mat[2][2]); + theta = atan2(-rm.m_rot_mat.m_mat[2][0], + sqrt((rm.m_rot_mat.m_mat[2][1] * rm.m_rot_mat.m_mat[2][1]) + + (rm.m_rot_mat.m_mat[2][2] * rm.m_rot_mat.m_mat[2][2]))); + psi = atan2(rm.m_rot_mat.m_mat[1][0], rm.m_rot_mat.m_mat[0][0]); + + q0 = (cos(phi/2) * cos(theta/2) * cos(psi/2)) + + (sin(phi/2) * sin(theta/2) * sin(psi/2)); + q1 = (-cos(phi/2) * sin(theta/2) * sin(psi/2)) + + (sin(phi/2) * cos(theta/2) * cos(psi/2)); + q2 = (cos(phi/2) * sin(theta/2) * cos(psi/2)) + + (sin(phi/2) * cos(theta/2) * sin(psi/2)); + q3 = (cos(phi/2) * cos(theta/2) * sin(psi/2)) - + (sin(phi/2) * sin(theta/2) * cos(psi/2)); quaternion q(q0, q1, q2, q3); -- 2.7.4 From cd5d6ad07f9a3fd3f51553ed5bb97bafe5835ded Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 18 Mar 2015 14:16:45 +0530 Subject: [PATCH 03/16] Fixing gyro data handling and cleanup - Fixing code related to input gyro data handling. - cleanup of orientation filter code. Change-Id: I9efd0c69ed7c2ab0dcc334e460723d9f8cdea668 --- src/sensor_fusion/orientation_filter.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index 47c7b34..5aa4968 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -88,9 +88,9 @@ inline void orientation_filter::initialize_sensor_data(const sensor_datam_time_stamp; - m_gyro.m_data = m_gyro.m_data * (TYPE) PI; + m_gyro.m_data = gyro->m_data * (TYPE) PI; - m_gyro.m_data = gyro->m_data - m_bias_correction; + m_gyro.m_data = m_gyro.m_data - m_bias_correction; } if (magnetic != NULL) { @@ -226,7 +226,7 @@ inline void orientation_filter::time_update() m_quat_9axis = quat_output; m_quat_gaming_rv = m_quat_9axis; - orientation = quat2euler(quat_output); + m_orientation = quat2euler(quat_output); m_rot_matrix = quat2rot_mat(m_quat_driv); @@ -292,6 +292,8 @@ inline void orientation_filter::time_update_gaming_rv() quat_output = phase_correction(m_quat_driv, m_quat_aid); + m_orientation = quat2euler(quat_output); + quat_error = m_quat_aid * m_quat_driv; euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI; -- 2.7.4 From 8465531093b0eadd1041680b9b2ae4e3c9cc469b Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 18 Mar 2015 14:27:50 +0530 Subject: [PATCH 04/16] Updating orientation sensor standalone test files - Updating orientation sensor standalone test files after restructuring or sensor fusion Change-Id: I58519ba358290142d585f2d5aea9e667c61a4f33 --- src/sensor_fusion/test/orientation_sensor.cpp | 7 ------- .../orientation_sensor_main.cpp | 18 ++++++++++++++++-- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp index b9ff4bb..784ac3b 100644 --- a/src/sensor_fusion/test/orientation_sensor.cpp +++ b/src/sensor_fusion/test/orientation_sensor.cpp @@ -28,10 +28,6 @@ int sign_magnetic[] = {+1, +1, +1}; float scale_accel = 1; float scale_gyro = 1150; float scale_magnetic = 1; - -int pitch_phase_compensation = -1; -int roll_phase_compensation = -1; -int azimuth_phase_compensation = -1; int magnetic_alignment_factor = -1; void pre_process_data(sensor_data *data_out, sensor_data *data_in, float *bias, int *sign, float scale) @@ -53,9 +49,6 @@ void orientation_sensor::get_device_orientation(sensor_data *accel_data, pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); normalize(*magnetic_data); - orien_filter.m_pitch_phase_compensation = pitch_phase_compensation; - orien_filter.m_roll_phase_compensation = roll_phase_compensation; - orien_filter.m_azimuth_phase_compensation = azimuth_phase_compensation; orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor; orien_filter.get_device_orientation(accel_data, gyro_data, magnetic_data); 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 53599d2..a35a6ce 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 @@ -26,6 +26,9 @@ using namespace std; #define ORIENTATION_DATA_PATH "../../../design/data/100ms/orientation/roll_pitch_yaw/" #define ORIENTATION_DATA_SIZE 1095 +int pitch_phase_compensation = -1; +int roll_phase_compensation = -1; +int azimuth_phase_compensation = -1; int main() { @@ -81,9 +84,20 @@ int main() orien_sensor.get_device_orientation(&accel_data, &gyro_data, &magnetic_data); - orien_file << orien_sensor.orien_filter.m_orientation.m_ang; + orientation = orien_sensor.orien_filter.m_orientation; - cout << "Orientation angles\t" << orien_sensor.orien_filter.m_orientation.m_ang << "\n\n"; + orientation = rad2deg(orientation); + + orientation.m_ang.m_vec[0] *= pitch_phase_compensation; + orientation.m_ang.m_vec[1] *= roll_phase_compensation; + orientation.m_ang.m_vec[2] *= azimuth_phase_compensation; + + if (orientation.m_ang.m_vec[2] < 0) + orientation.m_ang.m_vec[2] += 360; + + orien_file << orientation.m_ang; + + cout << "Orientation angles\t" << orientation.m_ang << "\n\n"; cout << "Orientation matrix\t" << orien_sensor.orien_filter.m_rot_matrix.m_rot_mat << "\n\n"; -- 2.7.4 From 08775ae0af1b0ca77342f741b807507b22c0a249 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 18 Mar 2015 14:34:29 +0530 Subject: [PATCH 05/16] Updating octave design files for orientation - Updating octave design files based on latest sensor fusion C/C++ library code. - Matching the functionalities between the octave code and C/C++ library code. Change-Id: I24f26381fc07c5237d3965ddddb01a9d8c2013c3 --- .../design/lib/estimate_orientation.m | 4 ++-- src/sensor_fusion/design/sf_orientation.m | 24 ++++++++++++++++++---- 2 files changed, 22 insertions(+), 6 deletions(-) diff --git a/src/sensor_fusion/design/lib/estimate_orientation.m b/src/sensor_fusion/design/lib/estimate_orientation.m index 6f0e852..d7f023a 100755 --- a/src/sensor_fusion/design/lib/estimate_orientation.m +++ b/src/sensor_fusion/design/lib/estimate_orientation.m @@ -24,8 +24,8 @@ % - Estimation and correction of orientation errors and bias errors for gyroscope using Kalman filter function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, Gyro_data, Mag_data) - PLOT_SCALED_SENSOR_COMPARISON_DATA = 0; - PLOT_INDIVIDUAL_SENSOR_INPUT_DATA = 0; + PLOT_SCALED_SENSOR_COMPARISON_DATA = 1; + PLOT_INDIVIDUAL_SENSOR_INPUT_DATA = 1; MAGNETIC_ALIGNMENT_FACTOR = -1; GYRO_DATA_DISABLED = 0; diff --git a/src/sensor_fusion/design/sf_orientation.m b/src/sensor_fusion/design/sf_orientation.m index 140487a..3a97580 100755 --- a/src/sensor_fusion/design/sf_orientation.m +++ b/src/sensor_fusion/design/sf_orientation.m @@ -48,6 +48,14 @@ Bias_Mx = 0; Bias_My = 0; Bias_Mz = 0; +Sign_Ax = 1; +Sign_Ay = 1; +Sign_Az = 1; + +Sign_Gx = 1; +Sign_Gy = 1; +Sign_Gz = 1; + Sign_Mx = 1; Sign_My = 1; Sign_Mz = 1; @@ -80,15 +88,15 @@ Gyro_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:, 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; +scale_Gyro = 1150; Gyro_data(1,:) = Gyro_data(1,:)/scale_Gyro; Gyro_data(2,:) = Gyro_data(2,:)/scale_Gyro; Gyro_data(3,:) = Gyro_data(3,:)/scale_Gyro; % get magnetometer x,y,z axis data from stored file -Mag_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,1))') + Bias_Mx)(1:BUFFER_SIZE) * Sign_Mx; -Mag_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,2))') + Bias_My)(1:BUFFER_SIZE) * Sign_My; -Mag_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,3))') + Bias_Mz)(1:BUFFER_SIZE) * Sign_Mz; +Mag_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,1))') - Bias_Mx)(1:BUFFER_SIZE) * Sign_Mx; +Mag_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,2))') - Bias_My)(1:BUFFER_SIZE) * Sign_My; +Mag_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,3))') - Bias_Mz)(1:BUFFER_SIZE) * Sign_Mz; Mag_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,4))')(1:BUFFER_SIZE); % estimate orientation @@ -104,6 +112,14 @@ for i = 1:BUFFER_SIZE OR_driv(1,i) = ROLL_PHASE_CORRECTION * euler_driv(i,2)' * RAD2DEG; OR_driv(2,i) = PITCH_PHASE_CORRECTION * euler_driv(i,1)' * RAD2DEG; OR_driv(3,i) = YAW_PHASE_CORRECTION * euler_driv(i,3)' * RAD2DEG; + + if (OR_driv(3,i) < 0) + OR_driv(3,i) = OR_driv(3,i) + 360; + end + + if (OR_aid(3,i) < 0) + OR_aid(3,i) = OR_aid(3,i) + 360; + end euler_err(i,:) = quat2euler(Quat_err(i,:)); OR_err(1,i) = euler_err(i,2)' * RAD2DEG; -- 2.7.4 From f58d91769d81840fc53a931ff48f9a8554e22146 Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Fri, 20 Mar 2015 15:43:37 +0530 Subject: [PATCH 06/16] Updating event representation for rotation vector virtual sensor -Updating rpresentation from ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME to ROTATION_VECTOR_RAW_DATA_EVENT Change-Id: Iaee8da2a4523f48a08a251ab931e4fc82ccb0911 --- src/libsensord/client_common.cpp | 2 +- src/libsensord/sensor_rv.h | 2 +- src/rotation_vector/rv/rv_sensor.cpp | 6 +++--- src/server/command_worker.cpp | 2 +- test/src/auto_test.c | 2 +- test/src/tc-common.c | 4 ++-- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/libsensord/client_common.cpp b/src/libsensord/client_common.cpp index dc00b87..c051c8c 100755 --- a/src/libsensord/client_common.cpp +++ b/src/libsensord/client_common.cpp @@ -63,7 +63,7 @@ log_element g_log_elements[] = { FILL_LOG_ELEMENT(LOG_ID_EVENT, ORIENTATION_RAW_DATA_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, PRESSURE_RAW_DATA_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, TEMPERATURE_RAW_DATA_EVENT, 0, 10), - FILL_LOG_ELEMENT(LOG_ID_EVENT, ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), + FILL_LOG_ELEMENT(LOG_ID_EVENT, ROTATION_VECTOR_RAW_DATA_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, GEOMAGNETIC_RV_RAW_DATA_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, GAMING_RV_RAW_DATA_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, FUSION_EVENT, 0, 10), diff --git a/src/libsensord/sensor_rv.h b/src/libsensord/sensor_rv.h index a7d6a99..3e65acc 100755 --- a/src/libsensord/sensor_rv.h +++ b/src/libsensord/sensor_rv.h @@ -37,7 +37,7 @@ extern "C" */ enum rot_event_type { - ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME = (ROTATION_VECTOR_SENSOR << 16) | 0x0001, + ROTATION_VECTOR_RAW_DATA_EVENT = (ROTATION_VECTOR_SENSOR << 16) | 0x0001, }; /** diff --git a/src/rotation_vector/rv/rv_sensor.cpp b/src/rotation_vector/rv/rv_sensor.cpp index b3330f1..f647ef0 100755 --- a/src/rotation_vector/rv/rv_sensor.cpp +++ b/src/rotation_vector/rv/rv_sensor.cpp @@ -62,7 +62,7 @@ rv_sensor::rv_sensor() cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); m_name = string(SENSOR_NAME); - register_supported_event(ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME); + register_supported_event(ROTATION_VECTOR_RAW_DATA_EVENT); m_enable_orientation = 0; if (!config.get(SENSOR_TYPE_RV, ELEMENT_VENDOR, m_vendor)) { @@ -199,7 +199,7 @@ void rv_sensor::synthesize(const sensor_event_t &event, vector & m_time = get_timestamp(); rv_event.sensor_id = get_id(); - rv_event.event_type = ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME; + rv_event.event_type = ROTATION_VECTOR_RAW_DATA_EVENT; rv_event.data.accuracy = SENSOR_ACCURACY_GOOD; rv_event.data.timestamp = m_time; rv_event.data.value_count = 4; @@ -218,7 +218,7 @@ int rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &data) { sensor_data_t fusion_data; - if (event_type != ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME) + if (event_type != ROTATION_VECTOR_RAW_DATA_EVENT) return -1; m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data); diff --git a/src/server/command_worker.cpp b/src/server/command_worker.cpp index 19965a0..7a8c59e 100755 --- a/src/server/command_worker.cpp +++ b/src/server/command_worker.cpp @@ -852,7 +852,7 @@ void insert_priority_list(unsigned int event_type) if (event_type == ORIENTATION_RAW_DATA_EVENT || event_type == LINEAR_ACCEL_RAW_DATA_EVENT || event_type == GRAVITY_RAW_DATA_EVENT || - event_type == ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME) { + event_type == ROTATION_VECTOR_RAW_DATA_EVENT) { priority_list.insert(ACCELEROMETER_RAW_DATA_EVENT); priority_list.insert(GYROSCOPE_RAW_DATA_EVENT); priority_list.insert(GEOMAGNETIC_RAW_DATA_EVENT); diff --git a/test/src/auto_test.c b/test/src/auto_test.c index 6bb0d29..67f5319 100644 --- a/test/src/auto_test.c +++ b/test/src/auto_test.c @@ -223,7 +223,7 @@ int main(int argc, char **argv) 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); + result = check_sensor_api(ROTATION_VECTOR_RAW_DATA_EVENT, interval); fprintf(fp, "Rotation Vector - RAW_DATA_REPORT_ON_TIME - %d\n", result); result = check_sensor_api(GEOMAGNETIC_RV_RAW_DATA_EVENT, interval); diff --git a/test/src/tc-common.c b/test/src/tc-common.c index 8606165..634af34 100644 --- a/test/src/tc-common.c +++ b/test/src/tc-common.c @@ -104,7 +104,7 @@ int get_event_driven(sensor_type_t sensor_type, char str[]) break; case ROTATION_VECTOR_SENSOR: if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) - return ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME; + return ROTATION_VECTOR_RAW_DATA_EVENT; break; case GEOMAGNETIC_RV_SENSOR: if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) @@ -217,7 +217,7 @@ int main(int argc, char **argv) } else if (strcmp(argv[1], "rotation_vector") == 0) { sensor_type = ROTATION_VECTOR_SENSOR; - event = ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME; + event = ROTATION_VECTOR_RAW_DATA_EVENT; } else if (strcmp(argv[1], "geomagnetic_rv") == 0) { sensor_type = GEOMAGNETIC_RV_SENSOR; -- 2.7.4 From 88fefc026292eeb306ad16a2bd288aa9d79d3c42 Mon Sep 17 00:00:00 2001 From: Ankur Date: Fri, 20 Mar 2015 16:08:08 +0530 Subject: [PATCH 07/16] Restructuring test folder to include parallel sensor test case Restructured the test folder to separate the functions from tc-common because they will also be used by a new test case (to be added in the next patch) for testing multiple test cases in parallel. Change-Id: If36de3b1123bd3811516fa859be73a753dc686e7 --- packaging/sensord.spec | 4 +- test/CMakeLists.txt | 16 ++-- test/src/{auto_test.c => api-test.c} | 0 test/src/{tc-common.c => check-sensor.c} | 135 +------------------------- test/src/check-sensor.h | 28 ++++++ test/src/sensor-test.c | 156 +++++++++++++++++++++++++++++++ 6 files changed, 199 insertions(+), 140 deletions(-) rename test/src/{auto_test.c => api-test.c} (100%) rename test/src/{tc-common.c => check-sensor.c} (61%) create mode 100644 test/src/check-sensor.h create mode 100644 test/src/sensor-test.c diff --git a/packaging/sensord.spec b/packaging/sensord.spec index e311020..af660f2 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -138,7 +138,7 @@ systemctl daemon-reload %if %{build_test_suite} == "ON" %files -n sensor-test %defattr(-,root,root,-) -%{_bindir}/auto_test -%{_bindir}/tc-common +%{_bindir}/api-test +%{_bindir}/sensor-test %license LICENSE.APLv2 %endif diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index e81b9c0..0f6c8d4 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -25,14 +25,14 @@ SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${EXTRA_CFLAGS}") link_directories(../src/libsensord/) -add_executable(auto_test src/auto_test.c) -add_executable(tc-common src/tc-common.c) +add_executable(api-test src/api-test.c) +add_executable(sensor-test src/sensor-test.c src/check-sensor.c) -SET_TARGET_PROPERTIES(auto_test PROPERTIES LINKER_LANGUAGE C) -SET_TARGET_PROPERTIES(tc-common PROPERTIES LINKER_LANGUAGE C) +SET_TARGET_PROPERTIES(api-test PROPERTIES LINKER_LANGUAGE C) +SET_TARGET_PROPERTIES(sensor-test PROPERTIES LINKER_LANGUAGE C) -target_link_libraries(auto_test glib-2.0 dlog sensor) -target_link_libraries(tc-common glib-2.0 dlog sensor) +target_link_libraries(api-test glib-2.0 dlog sensor) +target_link_libraries(sensor-test glib-2.0 dlog sensor) -INSTALL(TARGETS auto_test DESTINATION /usr/bin/) -INSTALL(TARGETS tc-common DESTINATION /usr/bin/) +INSTALL(TARGETS api-test DESTINATION /usr/bin/) +INSTALL(TARGETS sensor-test DESTINATION /usr/bin/) diff --git a/test/src/auto_test.c b/test/src/api-test.c similarity index 100% rename from test/src/auto_test.c rename to test/src/api-test.c diff --git a/test/src/tc-common.c b/test/src/check-sensor.c similarity index 61% rename from test/src/tc-common.c rename to test/src/check-sensor.c index 634af34..3c28feb 100644 --- a/test/src/tc-common.c +++ b/test/src/check-sensor.c @@ -26,39 +26,10 @@ #include #include -#define DEFAULT_EVENT_INTERVAL 100 +#include "check-sensor.h" 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("[geomagnetic_rv] "); - printf("[gaming_rv] "); - 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"); -} - int get_event_driven(sensor_type_t sensor_type, char str[]) { switch (sensor_type) { @@ -168,107 +139,11 @@ void callback(sensor_t sensor, unsigned int event_type, sensor_data_t *data, voi } } -int main(int argc, char **argv) +int check_sensor(sensor_type_t sensor_type, unsigned int event, int interval) { - int result, handle, start_handle, stop_handle, interval; - char *end1, *end2; - 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; - } + int handle, result, start_handle, stop_handle; - if (strcmp(argv[1], "accelerometer") == 0) { - sensor_type = ACCELEROMETER_SENSOR; - event = ACCELEROMETER_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "gyroscope") == 0) { - sensor_type = GYROSCOPE_SENSOR; - event = GYROSCOPE_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "pressure") == 0) { - sensor_type = PRESSURE_SENSOR; - event = PRESSURE_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "temperature") == 0) { - sensor_type = TEMPERATURE_SENSOR; - event = TEMPERATURE_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "geomagnetic") == 0) { - sensor_type = GEOMAGNETIC_SENSOR; - event = GEOMAGNETIC_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "orientation") == 0) { - sensor_type = ORIENTATION_SENSOR; - event = ORIENTATION_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "gravity") == 0) { - sensor_type = GRAVITY_SENSOR; - event = GRAVITY_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "linear_accel") == 0) { - sensor_type = LINEAR_ACCEL_SENSOR; - event = LINEAR_ACCEL_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "rotation_vector") == 0) { - sensor_type = ROTATION_VECTOR_SENSOR; - event = ROTATION_VECTOR_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "geomagnetic_rv") == 0) { - sensor_type = GEOMAGNETIC_RV_SENSOR; - event = GEOMAGNETIC_RV_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "gaming_rv") == 0) { - sensor_type = GAMING_RV_SENSOR; - event = GAMING_RV_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "light") == 0) { - sensor_type = LIGHT_SENSOR; - event = LIGHT_LUX_DATA_EVENT; - } - else if (strcmp(argv[1], "proximity") == 0) { - sensor_type = PROXIMITY_SENSOR; - event = PROXIMITY_CHANGE_STATE_EVENT; - } - 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; - } - } + mainloop = g_main_loop_new(NULL, FALSE); sensor_t sensor = sensord_get_sensor(sensor_type); handle = sensord_connect(sensor); @@ -276,7 +151,7 @@ int main(int argc, char **argv) result = sensord_register_event(handle, event, interval, 0, callback, NULL); if (result < 0) { - printf("Can't register %s\n", argv[1]); + printf("Can't register sensor\n"); return -1; } diff --git a/test/src/check-sensor.h b/test/src/check-sensor.h new file mode 100644 index 0000000..097a770 --- /dev/null +++ b/test/src/check-sensor.h @@ -0,0 +1,28 @@ +/* + * 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. + * + */ +#ifndef CHECK_SENSOR_H +#define CHECK_SENSOR_H + +#define DEFAULT_EVENT_INTERVAL 100 + +int get_event_driven(sensor_type_t sensor_type, char str[]); +void callback(sensor_t sensor, unsigned int event_type, sensor_data_t *data, void *user_data); +int check_sensor(sensor_type_t sensor_type, unsigned int event, int interval); + +#endif diff --git a/test/src/sensor-test.c b/test/src/sensor-test.c new file mode 100644 index 0000000..0b55112 --- /dev/null +++ b/test/src/sensor-test.c @@ -0,0 +1,156 @@ +/* + * 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 "check-sensor.h" + +void usage() +{ + printf("Usage : ./sensor-test (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("[geomagnetic_rv] "); + printf("[gaming_rv] "); + 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"); +} + +int main(int argc, char **argv) +{ + int interval; + unsigned int event; + sensor_type_t sensor_type; + + char *end1; + + 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_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "gyroscope") == 0) { + sensor_type = GYROSCOPE_SENSOR; + event = GYROSCOPE_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "pressure") == 0) { + sensor_type = PRESSURE_SENSOR; + event = PRESSURE_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "temperature") == 0) { + sensor_type = TEMPERATURE_SENSOR; + event = TEMPERATURE_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "geomagnetic") == 0) { + sensor_type = GEOMAGNETIC_SENSOR; + event = GEOMAGNETIC_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "orientation") == 0) { + sensor_type = ORIENTATION_SENSOR; + event = ORIENTATION_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "gravity") == 0) { + sensor_type = GRAVITY_SENSOR; + event = GRAVITY_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "linear_accel") == 0) { + sensor_type = LINEAR_ACCEL_SENSOR; + event = LINEAR_ACCEL_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "rotation_vector") == 0) { + sensor_type = ROTATION_VECTOR_SENSOR; + event = ROTATION_VECTOR_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "geomagnetic_rv") == 0) { + sensor_type = GEOMAGNETIC_RV_SENSOR; + event = GEOMAGNETIC_RV_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "gaming_rv") == 0) { + sensor_type = GAMING_RV_SENSOR; + event = GAMING_RV_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "light") == 0) { + sensor_type = LIGHT_SENSOR; + event = LIGHT_LUX_DATA_EVENT; + } + else if (strcmp(argv[1], "proximity") == 0) { + sensor_type = PROXIMITY_SENSOR; + event = PROXIMITY_CHANGE_STATE_EVENT; + } + else { + usage(); + } + + interval = DEFAULT_EVENT_INTERVAL; + + if (argc == 3) { + int temp_event = get_event_driven(sensor_type, argv[2]); + + if (temp_event == -1) { + interval = atoi(argv[2]); + if (interval == 0){ + usage(); + return -1; + } + } + else { + event = temp_event; + } + } + else if (argc == 4) { + event = get_event_driven(sensor_type, argv[2]); + interval = strtol(argv[3], &end1, 10); + + if (*end1) { + printf("Conversion error, non-convertible part: %s\n", end1); + return -1; + } + } + + return check_sensor(sensor_type, event, interval); +} -- 2.7.4 From ab1a0791801b6c91316a3e032b6dcfea5bbbae77 Mon Sep 17 00:00:00 2001 From: Ankur Date: Mon, 23 Mar 2015 12:35:03 +0530 Subject: [PATCH 08/16] Adding new test case for testing sensors in parallel This test case runs multiple sensor tests in parallel. It can be used for performance analysis of the various modules. Change-Id: I10a3e7ec98140ce6ecdcb6a29a0b8d6052e3287e --- packaging/sensord.spec | 1 + test/CMakeLists.txt | 4 ++ test/src/performance-test.c | 110 ++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 115 insertions(+) create mode 100644 test/src/performance-test.c diff --git a/packaging/sensord.spec b/packaging/sensord.spec index af660f2..0f3c296 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -140,5 +140,6 @@ systemctl daemon-reload %defattr(-,root,root,-) %{_bindir}/api-test %{_bindir}/sensor-test +%{_bindir}/performance-test %license LICENSE.APLv2 %endif diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 0f6c8d4..8716991 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -27,12 +27,16 @@ link_directories(../src/libsensord/) add_executable(api-test src/api-test.c) add_executable(sensor-test src/sensor-test.c src/check-sensor.c) +add_executable(performance-test src/performance-test.c src/check-sensor.c) SET_TARGET_PROPERTIES(api-test PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(sensor-test PROPERTIES LINKER_LANGUAGE C) +SET_TARGET_PROPERTIES(performance-test PROPERTIES LINKER_LANGUAGE C) target_link_libraries(api-test glib-2.0 dlog sensor) target_link_libraries(sensor-test glib-2.0 dlog sensor) +target_link_libraries(performance-test glib-2.0 dlog sensor) INSTALL(TARGETS api-test DESTINATION /usr/bin/) INSTALL(TARGETS sensor-test DESTINATION /usr/bin/) +INSTALL(TARGETS performance-test DESTINATION /usr/bin/) diff --git a/test/src/performance-test.c b/test/src/performance-test.c new file mode 100644 index 0000000..207b6fc --- /dev/null +++ b/test/src/performance-test.c @@ -0,0 +1,110 @@ +/* + * 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 "check-sensor.h" + +void usage() +{ + printf("Usage : ./performance-test (optional)\n\n"); + + printf("TIMEOUT:\n"); + printf("time for which the parallel sensor test cases should run\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.\n"); + printf("If no value for sensor is entered default value by the driver will be used.\n"); +} + +int main(int argc, char** argv) +{ + pid_t b = 1; + + int i = 0; + int interval = DEFAULT_EVENT_INTERVAL; + int TIMEOUT = 10; //in seconds for which all the sensor tests should run + + if(argc < 2) { + usage(); + return -1; + } + else if(argc == 2){ + TIMEOUT = atoi(argv[1]); + if (TIMEOUT == 0) { + usage(); + return -1; + } + } + else { + TIMEOUT = atoi(argv[1]); + interval = atoi(argv[2]); + if (TIMEOUT == 0 || interval == 0) { + usage(); + return -1; + } + } + + //make an array of size MAX and fill it with all the sensors needed to run + int MAX = 6; + pid_t pids[MAX]; + sensor_type_t sensor[MAX]; + + //Update the value of MAX and add more sensors here to test more sensors in parallel + sensor[0] = ACCELEROMETER_SENSOR; + sensor[1] = GYROSCOPE_SENSOR; + sensor[2] = GEOMAGNETIC_SENSOR; + sensor[3] = PRESSURE_SENSOR; + sensor[4] = PROXIMITY_SENSOR; + sensor[MAX-1] = LIGHT_SENSOR; + + while (i < MAX) { + if (b > 0) { + b = fork(); + if (b == -1) perror("Fork failed\n"); + else if (b == 0) { + break; + } + pids[i] = b; + i++; + } + } + + if (i < MAX) { + // call the sensord test tc-common for a sensor. + int event = (sensor[i] << 16) | 0x0001; + check_sensor(sensor[i], event, interval); + } + else { + // Main Parent Child. Waits for TIMEOUT and then kills all child processes. + sleep (TIMEOUT); + int j = 0; + + for (j = 0; j < MAX; j++) { + char command[100]; + sprintf(command, "kill %d", pids[j]); + system(command); + } + } + + return 0; +} -- 2.7.4 From eaba13ca4c498b2cdefe96cc02fbc0a4f0137063 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 25 Mar 2015 14:52:37 +0530 Subject: [PATCH 09/16] Restructuring get_device_orientation flow - restructuring orientation_filter::get_device_orientation flow for easier maintenance - cleanup Change-Id: Ie1bf8025d462d71146db2466b67a3e9f8585e31c --- src/sensor_fusion/orientation_filter.cpp | 34 +++++++++++++--------- .../orientation_sensor_main.cpp | 2 +- virtual_sensors.xml | 2 +- 3 files changed, 22 insertions(+), 16 deletions(-) diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index 5aa4968..ae88a46 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -359,29 +359,35 @@ void orientation_filter::get_device_orientation(const sensor_data *a { initialize_sensor_data(accel, gyro, magnetic); - if (magnetic != NULL) + if (gyro != NULL && magnetic != NULL) { + orientation_triad_algorithm(); - else if (gyro != NULL) - compute_accel_orientation(); - if (gyro != NULL) { compute_covariance(); - if (magnetic != NULL) - time_update(); - else - time_update_gaming_rv(); + time_update(); measurement_update(); - if (magnetic == NULL) { - m_quaternion = m_quat_gaming_rv; - } else { - m_quaternion = m_quat_9axis; - } + m_quaternion = m_quat_9axis; + + } else if (!gyro) { + + orientation_triad_algorithm(); - } else { m_quaternion = m_quat_aid; + + } else if (!magnetic) { + + compute_accel_orientation(); + + compute_covariance(); + + time_update_gaming_rv(); + + measurement_update(); + + m_quaternion = m_quat_gaming_rv; } } 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 a35a6ce..2710332 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 @@ -90,7 +90,7 @@ int main() orientation.m_ang.m_vec[0] *= pitch_phase_compensation; orientation.m_ang.m_vec[1] *= roll_phase_compensation; - orientation.m_ang.m_vec[2] *= azimuth_phase_compensation; + orientation.m_ang.m_vec[2] *= azimuth_phase_compensation; if (orientation.m_ang.m_vec[2] < 0) orientation.m_ang.m_vec[2] += 360; diff --git a/virtual_sensors.xml b/virtual_sensors.xml index 8371d60..3210e4f 100755 --- a/virtual_sensors.xml +++ b/virtual_sensors.xml @@ -131,7 +131,7 @@ - + -- 2.7.4 From cee985429d1a0ac7bced9d4cd83cca786f45c67e Mon Sep 17 00:00:00 2001 From: Ankur Date: Mon, 4 May 2015 11:20:25 +0530 Subject: [PATCH 10/16] Removing Compiler warning - unused variable Removed unused variable Change-Id: I73b3c36d2e30024fbb7e9f75014574bc1957ec77 --- src/fusion/fusion_sensor.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/fusion/fusion_sensor.cpp b/src/fusion/fusion_sensor.cpp index fa80416..26abfff 100755 --- a/src/fusion/fusion_sensor.cpp +++ b/src/fusion/fusion_sensor.cpp @@ -353,7 +353,6 @@ int fusion_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t sensor_data_t magnetic_data; euler_angles euler_orientation; - float azimuth_offset; if (event_type != FUSION_ORIENTATION_ENABLED || event_type != FUSION_ROTATION_VECTOR_ENABLED || -- 2.7.4 From 71749a7b3cbb642c51860410185ce1b5646dff00 Mon Sep 17 00:00:00 2001 From: Ankur Date: Tue, 5 May 2015 17:33:02 +0530 Subject: [PATCH 11/16] Removed Compiler warning - incompatible pointer type Change-Id: I73f52ea6bb3b7e6f5306bc1cc32c6d1cbf953648 --- test/src/api-test.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/src/api-test.c b/test/src/api-test.c index 67f5319..0ecea23 100644 --- a/test/src/api-test.c +++ b/test/src/api-test.c @@ -31,7 +31,7 @@ static GMainLoop *mainloop; FILE *fp; -void callback(unsigned int event_type, sensor_event_data_t *event, void *user_data) +void callback(sensor_t sensor, unsigned int event_type, sensor_data_t *data, void *user_data) { g_main_loop_quit(mainloop); } -- 2.7.4 From 3c3939235cbfb2b1af855b9d411e726a6ce971bf Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 25 Mar 2015 15:30:59 +0530 Subject: [PATCH 12/16] Modifying error/return conditions during fusion sensor initialization - Only failure of accel object initialization will return error - Failure of gyro and magnetic object initialization will print info Change-Id: I6cf67581d4a5fd18d87e418f095a067ceb07841e --- src/fusion/fusion_sensor.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/fusion/fusion_sensor.cpp b/src/fusion/fusion_sensor.cpp index fa80416..ae30dea 100755 --- a/src/fusion/fusion_sensor.cpp +++ b/src/fusion/fusion_sensor.cpp @@ -195,12 +195,17 @@ bool fusion_sensor::init(void) m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR); m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR); - if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor) { - ERR("Failed to load sensors, accel: 0x%x, gyro: 0x%x, mag: 0x%x", - m_accel_sensor, m_gyro_sensor, m_magnetic_sensor); + if (!m_accel_sensor) { + ERR("Failed to load accel sensor: 0x%x", m_accel_sensor); return false; } + if (!m_gyro_sensor) + INFO("Failed to load gyro sensor: 0x%x", m_gyro_sensor); + + if (!m_magnetic_sensor) + INFO("Failed to load geomagnetic sensor: 0x%x", m_magnetic_sensor); + INFO("%s is created!", sensor_base::get_name()); return true; } -- 2.7.4 From e1dcb072724002f72076fdf22e1f2ac590e4b009 Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Wed, 8 Apr 2015 17:36:30 +0530 Subject: [PATCH 13/16] Adding polling based test functionality for sensors -Adding functionality for getting sensor event logs using polling mechansim. -Change is being integrated in the test suite after there was amajor restructuring of the test folder to combine all tests into one. Change-Id: I23a817895a9e7db8f7c4284f0d0c6c697476f5e9 --- test/src/check-sensor.c | 109 +++++++++++++++++++++++++++++++++++++++++------- test/src/check-sensor.h | 4 +- test/src/sensor-test.c | 79 +++++++++++++++++++++++++---------- 3 files changed, 153 insertions(+), 39 deletions(-) diff --git a/test/src/check-sensor.c b/test/src/check-sensor.c index 3c28feb..aa15e34 100644 --- a/test/src/check-sensor.c +++ b/test/src/check-sensor.c @@ -30,63 +30,109 @@ static GMainLoop *mainloop; -int get_event_driven(sensor_type_t sensor_type, char str[]) +void printpollinglogs(sensor_type_t type,sensor_data_t data) +{ + switch(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 Rv [%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(GAMING_RV_SENSOR): + printf("Gaming Rv [%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 get_event(sensor_type_t sensor_type, char str[]) { switch (sensor_type) { case ACCELEROMETER_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return ACCELEROMETER_RAW_DATA_EVENT; break; case GYROSCOPE_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return GYROSCOPE_RAW_DATA_EVENT; break; case PRESSURE_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return PRESSURE_RAW_DATA_EVENT; break; case GEOMAGNETIC_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return GEOMAGNETIC_RAW_DATA_EVENT; break; case LIGHT_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return LIGHT_LUX_DATA_EVENT; break; case TEMPERATURE_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return TEMPERATURE_RAW_DATA_EVENT; break; case PROXIMITY_SENSOR: - if (strcmp(str, "EVENT_CHANGE_STATE") == 0) + if (strcmp(str, "CHANGE_STATE_EVENT") == 0) return PROXIMITY_CHANGE_STATE_EVENT; break; case ORIENTATION_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return ORIENTATION_RAW_DATA_EVENT; break; case GRAVITY_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return GRAVITY_RAW_DATA_EVENT; break; case LINEAR_ACCEL_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return LINEAR_ACCEL_RAW_DATA_EVENT; break; case ROTATION_VECTOR_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return ROTATION_VECTOR_RAW_DATA_EVENT; break; case GEOMAGNETIC_RV_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return GEOMAGNETIC_RV_RAW_DATA_EVENT; break; case GAMING_RV_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return GAMING_RV_RAW_DATA_EVENT; break; } - return -1; } @@ -185,3 +231,36 @@ int check_sensor(sensor_type_t sensor_type, unsigned int event, int interval) return 0; } + +int polling_sensor(sensor_type_t sensor_type, unsigned int event) +{ + int result, handle; + printf("Polling based\n"); + + handle = sf_connect(sensor_type); + result = sf_start(handle, 1); + + if (result < 0) { + printf("Can't start the sensor\n"); + printf("Error\n\n\n\n"); + return -1; + } + + sensor_data_t data; + + while(1) { + result = sf_get_data(handle, event , &data); + printpollinglogs(sensor_type, data); + usleep(100000); + } + + result = sf_disconnect(handle); + + if (result < 0) { + printf("Can't disconnect sensor\n"); + printf("Error\n\n\n\n"); + return -1; + } + + return 0; +} diff --git a/test/src/check-sensor.h b/test/src/check-sensor.h index 097a770..86b407b 100644 --- a/test/src/check-sensor.h +++ b/test/src/check-sensor.h @@ -21,8 +21,10 @@ #define DEFAULT_EVENT_INTERVAL 100 -int get_event_driven(sensor_type_t sensor_type, char str[]); +int get_event(sensor_type_t sensor_type, char str[]); void callback(sensor_t sensor, unsigned int event_type, sensor_data_t *data, void *user_data); int check_sensor(sensor_type_t sensor_type, unsigned int event, int interval); +void printpollinglogs(sensor_type_t type, sensor_data_t data); +int polling_sensor(sensor_type_t sensor_type, unsigned int event); #endif diff --git a/test/src/sensor-test.c b/test/src/sensor-test.c index 0b55112..5c5c3c1 100644 --- a/test/src/sensor-test.c +++ b/test/src/sensor-test.c @@ -29,7 +29,7 @@ void usage() { - printf("Usage : ./sensor-test (optional) (optional)\n\n"); + printf("Usage : ./sensor-test -p(optional) (optional) (optional)\n\n"); printf("Sensor_type: "); printf("[accelerometer] "); @@ -45,13 +45,13 @@ void usage() printf("[gaming_rv] "); printf("[light]\n"); printf("event:"); - printf("[RAW_DATA_REPORT_ON_TIME]\n"); - + printf("[RAW_DATA_EVENT]\n"); + printf("-p: [polling]\n"); printf("Sensor_type: "); printf("[proximity]\n"); printf("event:"); - printf("[EVENT_CHANGE_STATE]\n"); - + printf("[CHANGE_STATE_EVENT]\n"); + printf("-p: [polling]\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"); } @@ -61,10 +61,11 @@ int main(int argc, char **argv) int interval; unsigned int event; sensor_type_t sensor_type; + bool is_polling; char *end1; - if (argc < 2 || argc > 4) { + if (argc < 2 || argc > 5) { printf("Wrong number of arguments\n"); usage(); return 0; @@ -128,29 +129,61 @@ int main(int argc, char **argv) interval = DEFAULT_EVENT_INTERVAL; - if (argc == 3) { - int temp_event = get_event_driven(sensor_type, argv[2]); + is_polling = FALSE; - if (temp_event == -1) { - interval = atoi(argv[2]); - if (interval == 0){ - usage(); - return -1; + if(argc >= 3 && strcmp(argv[2], "-p") == 0) { + is_polling = TRUE; + } + + if (is_polling) { + if (argc == 4) { + int temp_event = get_event(sensor_type, argv[3]); + if (temp_event == -1) { + interval = atoi(argv[3]); + if (interval == 0){ + usage(); + return -1; + } + } + else { + event = temp_event; } } - else { - event = temp_event; + else if (argc == 5) { + event = get_event(sensor_type, argv[3]); + interval = strtol(argv[4], &end1, 10); + + if (*end1) { + printf("Conversion error, non-convertible part: %s\n", end1); + return -1; + } } + return polling_sensor(sensor_type, event); } - else if (argc == 4) { - event = get_event_driven(sensor_type, argv[2]); - interval = strtol(argv[3], &end1, 10); + else { + if (argc == 3) { + int temp_event = get_event(sensor_type, argv[2]); - if (*end1) { - printf("Conversion error, non-convertible part: %s\n", end1); - return -1; + if (temp_event == -1) { + interval = atoi(argv[2]); + if (interval == 0){ + usage(); + return -1; + } + } + else { + event = temp_event; + } } - } + else if (argc == 4) { + event = get_event(sensor_type, argv[2]); + interval = strtol(argv[3], &end1, 10); - return check_sensor(sensor_type, event, interval); + if (*end1) { + printf("Conversion error, non-convertible part: %s\n", end1); + return -1; + } + } + return check_sensor(sensor_type, event, interval); + } } -- 2.7.4 From b35f82121ecf4fac092e77eef13d547fcb6f1064 Mon Sep 17 00:00:00 2001 From: Ankur Date: Thu, 9 Apr 2015 17:27:40 +0530 Subject: [PATCH 14/16] Addition of Test case for fusion data collection This test case registers accelerometer, gyroscope, magnetometer and proximity sensors from one single client. This is useful for collecting data from these physical sensors for any particular motion. Change-Id: Ie2c866c5ec624290f9988b17ee33ea92bc8752ca --- packaging/sensord.spec | 1 + test/CMakeLists.txt | 4 ++ test/src/fusion-data-collection.c | 125 ++++++++++++++++++++++++++++++++++++++ 3 files changed, 130 insertions(+) create mode 100644 test/src/fusion-data-collection.c diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 0f3c296..48c8cf1 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -141,5 +141,6 @@ systemctl daemon-reload %{_bindir}/api-test %{_bindir}/sensor-test %{_bindir}/performance-test +%{_bindir}/fusion-data-collection %license LICENSE.APLv2 %endif diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 8716991..81213f1 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -28,15 +28,19 @@ link_directories(../src/libsensord/) add_executable(api-test src/api-test.c) add_executable(sensor-test src/sensor-test.c src/check-sensor.c) add_executable(performance-test src/performance-test.c src/check-sensor.c) +add_executable(fusion-data-collection src/fusion-data-collection.c) SET_TARGET_PROPERTIES(api-test PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(sensor-test PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(performance-test PROPERTIES LINKER_LANGUAGE C) +SET_TARGET_PROPERTIES(fusion-data-collection PROPERTIES LINKER_LANGUAGE C) target_link_libraries(api-test glib-2.0 dlog sensor) target_link_libraries(sensor-test glib-2.0 dlog sensor) target_link_libraries(performance-test glib-2.0 dlog sensor) +target_link_libraries(fusion-data-collection glib-2.0 dlog sensor) INSTALL(TARGETS api-test DESTINATION /usr/bin/) INSTALL(TARGETS sensor-test DESTINATION /usr/bin/) INSTALL(TARGETS performance-test DESTINATION /usr/bin/) +INSTALL(TARGETS fusion-data-collection DESTINATION /usr/bin/) diff --git a/test/src/fusion-data-collection.c b/test/src/fusion-data-collection.c new file mode 100644 index 0000000..62e63f8 --- /dev/null +++ b/test/src/fusion-data-collection.c @@ -0,0 +1,125 @@ +/* + * 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 MAXSIZE 4 + +static GMainLoop *mainloop; +FILE* file_output[MAXSIZE]; + +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: + fprintf(file_output[0], "%6.6f %6.6f %6.6f %lld\n", data->values[0], data->values[1], data->values[2], data->timestamp); + break; + case GEOMAGNETIC_SENSOR: + fprintf(file_output[1], "%6.6f %6.6f %6.6f %lld\n", data->values[0], data->values[1], data->values[2], data->timestamp); + break; + case GYROSCOPE_SENSOR: + fprintf(file_output[2], "%6.6f %6.6f %6.6f %lld\n", data->values[0], data->values[1], data->values[2], data->timestamp); + break; + case PROXIMITY_SENSOR: + fprintf(file_output[MAXSIZE-1], "%6.6f %lld\n", data->values[0], data->timestamp); + break; + default: + return; + } +} + +void usage() +{ + printf("Usage : ./one-client-multiple-sensors \n\n"); + + printf("interval:\n"); + printf("The sampling interval in ms.\n"); + exit(-1); +} + +int main(int argc, char **argv) +{ + int interval; + + if (argc == 2) { + interval = atoi(argv[1]); + if (interval <= 0) + usage(); + } + else + usage(); + + int i; + + int handle[MAXSIZE]; + int result[MAXSIZE], start_handle[MAXSIZE], stop_handle[MAXSIZE]; + unsigned int event[MAXSIZE]; + int sensors[MAXSIZE]; + + sensors[0] = ACCELEROMETER_SENSOR; + sensors[1] = GEOMAGNETIC_SENSOR; + sensors[2] = GYROSCOPE_SENSOR; + sensors[MAXSIZE-1] = PROXIMITY_SENSOR; + + mainloop = g_main_loop_new(NULL, FALSE); + + char file_name[50]; + + for (i = 0; i < MAXSIZE; i++) { + sprintf(file_name, "output_%d", sensors[i]); + file_output[i] = fopen(file_name, "w+"); + sensor_t sensor = sensord_get_sensor(sensors[i]); + handle[i] = sensord_connect(sensor); + event[i] = (sensors[i] << 16) | 0x0001; + result[i] = sensord_register_event(handle[i], event[i], interval, 0, callback, NULL); + + if (result[i] < 0) { + printf("error: unable to register sensor\n"); + return -1; + } + start_handle[i] = sensord_start(handle[i], 1); + + if (start_handle[i] < 0) { + printf("error: unable to start handle\n"); + sensord_unregister_event(handle[i], event[i]); + sensord_disconnect(handle[i]); + return -1; + } + } + + g_main_loop_run(mainloop); + g_main_loop_unref(mainloop); + + for (i = 0; i < MAXSIZE; i++) { + sensord_unregister_event(handle[i], event[i]); + sensord_stop(handle[i]); + sensord_disconnect(handle[i]); + } + + return 0; +} -- 2.7.4 From 5bfb197853878c3dd31072a612c1c64c6a7a4adc Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 15 May 2015 18:48:26 +0900 Subject: [PATCH 15/16] Adding support for computing tilt quaternion based on accelerometer - support for sensor fusion algorithm to compute tilt quaternion Change-Id: I32135f270acc4eb8de1811e6f29182614dd7ce54 --- src/sensor_fusion/orientation_filter.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index ae88a46..c3a6c92 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -371,6 +371,12 @@ void orientation_filter::get_device_orientation(const sensor_data *a m_quaternion = m_quat_9axis; + } else if (!gyro && !magnetic) { + + compute_accel_orientation(); + + m_quaternion = m_quat_aid; + } else if (!gyro) { orientation_triad_algorithm(); -- 2.7.4 From d104d0ac5b4772c105642ef6304809ce92808b59 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 15 May 2015 19:00:26 +0900 Subject: [PATCH 16/16] Adding Tilt Sensor and related files - Adding tilt sensor based on accelerometer only - uses intermediate quaternion output of sensor fusion - Used in Tizen Z1 for computing limited device orientation - Adding all api, build files related to tilt Change-Id: I7682da9238ca61e9d8fc1b392fb14afc33406d2d --- packaging/sensord.spec | 5 +- sensor_plugins.xml.in | 1 + src/CMakeLists.txt | 11 +- src/fusion/fusion_sensor.cpp | 4 +- src/libsensord/CMakeLists.txt | 1 + src/libsensord/client_common.cpp | 2 + src/libsensord/sensor_fusion.h | 13 +- src/libsensord/sensor_internal.h | 1 + src/libsensord/sensor_internal_deprecated.h | 1 + src/shared/sensor_common.h | 1 + src/tilt/CMakeLists.txt | 24 +++ src/tilt/tilt_sensor.cpp | 311 ++++++++++++++++++++++++++++ src/tilt/tilt_sensor.h | 69 ++++++ virtual_sensors.xml | 27 +++ 14 files changed, 459 insertions(+), 12 deletions(-) create mode 100755 src/tilt/CMakeLists.txt create mode 100755 src/tilt/tilt_sensor.cpp create mode 100755 src/tilt/tilt_sensor.h diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 48c8cf1..499c99e 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -30,7 +30,7 @@ BuildRequires: pkgconfig(capi-system-info) %define rv_state ON %define geomagnetic_rv_state ON %define gaming_rv_state ON -%define fusion_state ON +%define tilt_state ON %define build_test_suite OFF %description @@ -82,7 +82,7 @@ cmake . -DCMAKE_INSTALL_PREFIX=%{_prefix} -DACCEL=%{accel_state} \ -DORIENTATION=%{orientation_state} -DGRAVITY=%{gravity_state} \ -DLINEAR_ACCEL=%{linear_accel_state} -DRV=%{rv_state} \ -DGEOMAGNETIC_RV=%{geomagnetic_rv_state} -DGAMING_RV=%{gaming_rv_state} \ - -DFUSION=%{fusion_state} -DTEST_SUITE=%{build_test_suite} \ + -DTILT=%{tilt_state} -DTEST_SUITE=%{build_test_suite} \ -DLIBDIR=%{_libdir} -DINCLUDEDIR=%{_includedir} %build @@ -134,6 +134,7 @@ systemctl daemon-reload %{_libdir}/pkgconfig/sensor.pc %{_libdir}/pkgconfig/sf_common.pc %{_libdir}/pkgconfig/sensord-server.pc +%license LICENSE.APLv2 %if %{build_test_suite} == "ON" %files -n sensor-test diff --git a/sensor_plugins.xml.in b/sensor_plugins.xml.in index 9fde9d6..c1822ce 100755 --- a/sensor_plugins.xml.in +++ b/sensor_plugins.xml.in @@ -24,5 +24,6 @@ + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 556256a..65f0ec7 100755 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -44,6 +44,10 @@ ENDIF() IF("${GAMING_RV}" STREQUAL "ON") set(SENSOR_FUSION_ENABLE "1") ENDIF() +IF("${TILT}" STREQUAL "ON") +set(SENSOR_FUSION_ENABLE "1") +set(TILT_ENABLE "1") +ENDIF() IF("${GRAVITY}" STREQUAL "ON") set(SENSOR_FUSION_ENABLE "1") set(ORIENTATION_ENABLE "1") @@ -57,8 +61,6 @@ set(LINEAR_ACCELERATION_ENABLE "1") ENDIF() IF("${SENSOR_FUSION_ENABLE}" STREQUAL "1") add_subdirectory(sensor_fusion) -ENDIF() -IF("${FUSION}" STREQUAL "ON") add_subdirectory(fusion) ENDIF() IF("${ORIENTATION_ENABLE}" STREQUAL "1") @@ -70,8 +72,11 @@ ENDIF() IF("${LINEAR_ACCELERATION_ENABLE}" STREQUAL "1") add_subdirectory(linear_accel) ENDIF() -add_subdirectory(rotation_vector) +IF("${TILT_ENABLE}" STREQUAL "1") +add_subdirectory(tilt) +ENDIF() +add_subdirectory(rotation_vector) add_subdirectory(server) add_subdirectory(libsensord) add_subdirectory(shared) diff --git a/src/fusion/fusion_sensor.cpp b/src/fusion/fusion_sensor.cpp index 0bac151..42a8e41 100755 --- a/src/fusion/fusion_sensor.cpp +++ b/src/fusion/fusion_sensor.cpp @@ -39,6 +39,7 @@ #define ACCELEROMETER_ENABLED 0x01 #define GYROSCOPE_ENABLED 0x02 #define GEOMAGNETIC_ENABLED 0x04 +#define TILT_ENABLED 1 #define GAMING_RV_ENABLED 3 #define GEOMAGNETIC_RV_ENABLED 5 #define ORIENTATION_ENABLED 7 @@ -317,7 +318,8 @@ void fusion_sensor::synthesize(const sensor_event_t &event, vector log_map; diff --git a/src/libsensord/sensor_fusion.h b/src/libsensord/sensor_fusion.h index 7539be1..8f80499 100755 --- a/src/libsensord/sensor_fusion.h +++ b/src/libsensord/sensor_fusion.h @@ -36,12 +36,13 @@ extern "C" * @{ */ enum fusion_event_type { - FUSION_ORIENTATION_ENABLED = (FUSION_SENSOR << 16) | 0x0001, - FUSION_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0002, - FUSION_GAMING_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0003, - FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0004, - FUSION_EVENT = (FUSION_SENSOR << 16) | 0x0005, - FUSION_CALIBRATION_NEEDED_EVENT = (FUSION_SENSOR << 16) | 0x0006, + FUSION_EVENT = (FUSION_SENSOR << 16) | 0x0001, + FUSION_CALIBRATION_NEEDED_EVENT = (FUSION_SENSOR << 16) | 0x0002, + FUSION_ORIENTATION_ENABLED = (FUSION_SENSOR << 16) | 0x0003, + FUSION_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0004, + FUSION_GAMING_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0005, + FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0006, + FUSION_TILT_ENABLED = (FUSION_SENSOR << 16) | 0x0007, }; /** diff --git a/src/libsensord/sensor_internal.h b/src/libsensord/sensor_internal.h index 844e7af..7842cda 100755 --- a/src/libsensord/sensor_internal.h +++ b/src/libsensord/sensor_internal.h @@ -55,6 +55,7 @@ extern "C" #include #include #include +#include typedef void (*sensor_cb_t)(sensor_t sensor, unsigned int event_type, sensor_data_t *data, void *user_data); diff --git a/src/libsensord/sensor_internal_deprecated.h b/src/libsensord/sensor_internal_deprecated.h index 4420044..fdcbab1 100755 --- a/src/libsensord/sensor_internal_deprecated.h +++ b/src/libsensord/sensor_internal_deprecated.h @@ -54,6 +54,7 @@ extern "C" #include #include #include +#include #define MAX_KEY_LEN 30 diff --git a/src/shared/sensor_common.h b/src/shared/sensor_common.h index 48346ac..d1df649 100755 --- a/src/shared/sensor_common.h +++ b/src/shared/sensor_common.h @@ -66,6 +66,7 @@ typedef enum { GEOMAGNETIC_RV_SENSOR, GAMING_RV_SENSOR, ORIENTATION_SENSOR, + TILT_SENSOR, PIR_SENSOR, PIR_LONG_SENSOR, TEMPERATURE_SENSOR, diff --git a/src/tilt/CMakeLists.txt b/src/tilt/CMakeLists.txt new file mode 100755 index 0000000..44e6a1a --- /dev/null +++ b/src/tilt/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 2.6) +project(tilt CXX) + +SET(SENSOR_NAME tilt_sensor) + +include_directories(${CMAKE_CURRENT_SOURCE_DIR}) +include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) +include_directories(${CMAKE_SOURCE_DIR}/src/sensor_fusion) + +FOREACH(flag ${tilt_pkgs_LDFLAGS}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") +ENDFOREACH(flag) + +FOREACH(flag ${tilt_pkgs_CFLAGS}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") +ENDFOREACH(flag) + +add_library(${SENSOR_NAME} SHARED + tilt_sensor.cpp + ) + +target_link_libraries(${SENSOR_NAME} ${tilt_pkgs_LDFLAGS} "-lm") + +install(TARGETS ${SENSOR_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR}/sensord) diff --git a/src/tilt/tilt_sensor.cpp b/src/tilt/tilt_sensor.cpp new file mode 100755 index 0000000..db40acf --- /dev/null +++ b/src/tilt/tilt_sensor.cpp @@ -0,0 +1,311 @@ +/* + * 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 "TILT_SENSOR" +#define SENSOR_TYPE_TILT "TILT" + +#define MIN_DELIVERY_DIFF_FACTOR 0.75f + +#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_PITCH_ROTATION_COMPENSATION "PITCH_ROTATION_COMPENSATION" +#define ELEMENT_ROLL_ROTATION_COMPENSATION "ROLL_ROTATION_COMPENSATION" + +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; +} + +tilt_sensor::tilt_sensor() +: m_accel_sensor(NULL) +, m_fusion_sensor(NULL) +, m_time(0) +{ + cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); + + m_name = string(SENSOR_NAME); + register_supported_event(TILT_RAW_DATA_EVENT); + m_enable_tilt = 0; + + if (!config.get(SENSOR_TYPE_TILT, ELEMENT_VENDOR, m_vendor)) { + ERR("[VENDOR] is empty\n"); + throw ENXIO; + } + + INFO("m_vendor = %s", m_vendor.c_str()); + + if (!config.get(SENSOR_TYPE_TILT, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) { + ERR("[RAW_DATA_UNIT] is empty\n"); + throw ENXIO; + } + + INFO("m_raw_data_unit = %s", m_raw_data_unit.c_str()); + + if (!config.get(SENSOR_TYPE_TILT, 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_TILT, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) { + ERR("[PITCH_ROTATION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation); + + if (!config.get(SENSOR_TYPE_TILT, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) { + ERR("[ROLL_ROTATION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_roll_rotation_compensation = %d", m_roll_rotation_compensation); + + m_interval = m_default_sampling_time * MS_TO_US; + + INFO("ramasamy 1: Constructor completed"); +} + +tilt_sensor::~tilt_sensor() +{ + INFO("tilt_sensor is destroyed!\n"); +} + +bool tilt_sensor::init(void) +{ + m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); + m_fusion_sensor = sensor_plugin_loader::get_instance().get_sensor(FUSION_SENSOR); + + if (!m_accel_sensor || !m_fusion_sensor) { + ERR("Failed to load sensors, accel: 0x%x, fusion: 0x%x", + m_accel_sensor, m_fusion_sensor); + return false; + } + + INFO("%s is created!\n", sensor_base::get_name()); + + INFO("ramasamy 2: Init completed"); + + return true; +} + +sensor_type_t tilt_sensor::get_type(void) +{ + return TILT_SENSOR; +} + +bool tilt_sensor::on_start(void) +{ + AUTOLOCK(m_mutex); + + 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_fusion_sensor->register_supported_event(FUSION_EVENT); + m_fusion_sensor->register_supported_event(FUSION_TILT_ENABLED); + m_fusion_sensor->add_client(FUSION_EVENT); + m_fusion_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); + m_fusion_sensor->start(); + + INFO("ramasamy 3: ON_START completed"); + + activate(); + return true; +} + +bool tilt_sensor::on_stop(void) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); + m_accel_sensor->delete_interval((intptr_t)this, false); + m_accel_sensor->stop(); + + m_fusion_sensor->delete_client(FUSION_EVENT); + m_fusion_sensor->delete_interval((intptr_t)this, false); + m_fusion_sensor->unregister_supported_event(FUSION_EVENT); + m_fusion_sensor->unregister_supported_event(FUSION_TILT_ENABLED); + m_fusion_sensor->stop(); + + INFO("ramasamy 4: ON_STOP completed"); + + deactivate(); + return true; +} + +bool tilt_sensor::add_interval(int client_id, unsigned int interval) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->add_interval(client_id, interval, false); + m_fusion_sensor->add_interval(client_id, interval, false); + + INFO("ramasamy 5: ADD_INTERVAL completed"); + + return sensor_base::add_interval(client_id, interval, false); +} + +bool tilt_sensor::delete_interval(int client_id) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->delete_interval(client_id, false); + m_fusion_sensor->delete_interval(client_id, false); + + INFO("ramasamy 6: DELETE_INTERVAL completed"); + + return sensor_base::delete_interval(client_id, false); +} + +void tilt_sensor::synthesize(const sensor_event_t &event, vector &outs) +{ + sensor_event_t tilt_event; + unsigned long long diff_time; + + INFO("ramasamy 7: SYNTHESIZE STARTED"); + + if (event.event_type == FUSION_EVENT) { + + INFO("ramasamy 8: SYNTHESIZE FUSION EVENT RECEIVED"); + diff_time = event.data.timestamp - m_time; + + if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + quaternion quat(event.data.values[0], event.data.values[1], + event.data.values[2], event.data.values[3]); + + euler_angles euler = quat2euler(quat); + + if(m_raw_data_unit == "DEGREES") { + euler = rad2deg(euler); + } + + euler.m_ang.m_vec[0] *= m_pitch_rotation_compensation; + euler.m_ang.m_vec[1] *= m_roll_rotation_compensation; + + m_time = get_timestamp(); + tilt_event.sensor_id = get_id(); + tilt_event.event_type = TILT_RAW_DATA_EVENT; + tilt_event.data.accuracy = event.data.accuracy; + tilt_event.data.timestamp = m_time; + tilt_event.data.value_count = 2; + tilt_event.data.values[0] = euler.m_ang.m_vec[0]; + tilt_event.data.values[1] = euler.m_ang.m_vec[1]; + + INFO("ramasamy 8: SYNTHESIZE FUSION EVENT SENT"); + + push(tilt_event); + } + + return; +} + +int tilt_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data) +{ + sensor_data_t fusion_data; + + if (event_type != TILT_RAW_DATA_EVENT) + return -1; + + m_fusion_sensor->get_sensor_data(FUSION_TILT_ENABLED, fusion_data); + + quaternion quat(fusion_data.values[0], fusion_data.values[1], + fusion_data.values[2], fusion_data.values[3]); + + euler_angles euler = quat2euler(quat); + + if(m_raw_data_unit == "DEGREES") { + euler = rad2deg(euler); + } + + data.accuracy = fusion_data.accuracy; + data.timestamp = get_timestamp(); + data.value_count = 2; + data.values[0] = euler.m_ang.m_vec[0]; + data.values[1] = euler.m_ang.m_vec[1]; + + data.values[0] *= m_pitch_rotation_compensation; + data.values[1] *= m_roll_rotation_compensation; + + return 0; +} + +bool tilt_sensor::get_properties(sensor_properties_s &properties) +{ + if(m_raw_data_unit == "DEGREES") { + properties.min_range = -180; + properties.max_range = 180; + } + else { + properties.min_range = -PI; + properties.max_range = PI; + } + properties.resolution = 0.000001; + properties.vendor = m_vendor; + properties.name = SENSOR_NAME; + properties.min_interval = 1; + properties.fifo_count = 0; + properties.max_batch_count = 0; + + return true; +} + +extern "C" sensor_module* create(void) +{ + tilt_sensor *sensor; + + try { + sensor = new(std::nothrow) tilt_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/tilt/tilt_sensor.h b/src/tilt/tilt_sensor.h new file mode 100755 index 0000000..758751f --- /dev/null +++ b/src/tilt/tilt_sensor.h @@ -0,0 +1,69 @@ +/* + * 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 _TILT_SENSOR_H_ +#define _TILT_SENSOR_H_ + +#include +#include +#include + +class tilt_sensor : public virtual_sensor { +public: + tilt_sensor(); + virtual ~tilt_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 event_type, sensor_data_t &data); + +private: + sensor_base *m_accel_sensor; + sensor_base *m_fusion_sensor; + + sensor_data m_accel; + + cmutex m_value_mutex; + + orientation_filter m_orientation_filter; + orientation_filter m_orientation_filter_poll; + + unsigned int m_enable_tilt; + + unsigned long long m_time; + unsigned int m_interval; + + string m_vendor; + string m_raw_data_unit; + int m_default_sampling_time; + int m_pitch_rotation_compensation; + int m_roll_rotation_compensation; + + bool on_start(void); + bool on_stop(void); +}; + +#endif /*_TILT_SENSOR_H_*/ diff --git a/virtual_sensors.xml b/virtual_sensors.xml index 3210e4f..3005df6 100755 --- a/virtual_sensors.xml +++ b/virtual_sensors.xml @@ -64,6 +64,15 @@ + + + + + + + + + @@ -130,6 +139,15 @@ + + + + + + + + + @@ -196,5 +214,14 @@ + + + + + + + + + -- 2.7.4