From c7faf9a41e75487df1a208a02eddb71f8aa68a1d Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 5 Jun 2015 16:45:00 +0900 Subject: [PATCH 01/16] Compensating drift before computing noise covariance - Improving accuracy of orientation detection by moving the drift compensation code before computing noise covariance. - resturcturing of the existing octave code. Change-Id: I09691d944ae0c3d731b6fabd098a3652991f7dc7 --- .../design/lib/estimate_orientation.m | 51 ++++++++++++---------- 1 file changed, 27 insertions(+), 24 deletions(-) diff --git a/src/sensor_fusion/design/lib/estimate_orientation.m b/src/sensor_fusion/design/lib/estimate_orientation.m index b2e3b73..511b646 100755 --- a/src/sensor_fusion/design/lib/estimate_orientation.m +++ b/src/sensor_fusion/design/lib/estimate_orientation.m @@ -172,15 +172,38 @@ function [quat_driv, quat_aid, quat_error, gyro_bias] = estimate_orientation(Ac gyr_y(i) = Gy(i) * PI; gyr_z(i) = Gz(i) * PI; - gyr_x(i) = gyr_x(i) - Bx; - gyr_y(i) = gyr_y(i) - By; - gyr_z(i) = gyr_z(i) - Bz; - euler = quat2euler(quat_aid(i,:)); roll(i) = euler(2); pitch(i) = euler(1); yaw(i) = euler(3); + if i > 1 + A_T(i) = ATime(i) - ATime(i-1); + G_T(i) = GTime(i) - GTime(i-1); + M_T(i) = MTime(i) - MTime(i-1); + end + + dt = G_T(i) * US2S; + + % Driving System (Gyroscope) quaternion generation + % convert scaled gyro data to rad/s + qDot = 0.5 * quat_prod(q, [0 gyr_x(i) gyr_y(i) gyr_z(i)]); + + % Integrate to yield quaternion + q = q + qDot * dt * PI; + + % normalise quaternion + quat_driv(i,:) = q / norm(q); + + % Kalman Filtering + quat_error(i,:) = quat_prod(quat_aid(i,:), quat_driv(i,:)); + + euler_e = quat2euler(quat_error(i,:)); + + gyr_x(i) = gyr_x(i) - (Bx + euler_e(1)); + gyr_y(i) = gyr_y(i) - (By + euler_e(2)); + gyr_z(i) = gyr_z(i) - (Bz + euler_e(3)); + if i <= MOVING_AVERAGE_WINDOW_LENGTH var_Gx(i) = NON_ZERO_VAL; var_Gy(i) = NON_ZERO_VAL; @@ -196,13 +219,7 @@ function [quat_driv, quat_aid, quat_error, gyro_bias] = estimate_orientation(Ac var_pitch(i) = var(pitch((i-MOVING_AVERAGE_WINDOW_LENGTH):i)); var_yaw(i) = var(yaw((i-MOVING_AVERAGE_WINDOW_LENGTH):i)); end - if i > 1 - A_T(i) = ATime(i) - ATime(i-1); - G_T(i) = GTime(i) - GTime(i-1); - M_T(i) = MTime(i) - MTime(i-1); - end - dt = G_T(i) * US2S; Qwn = [var_Gx(i) 0 0;0 var_Gy(i) 0;0 0 var_Gz(i);]; Qwb = (2 * (ZigmaW^2) / TauW) * eye(3); @@ -228,20 +245,6 @@ function [quat_driv, quat_aid, quat_error, gyro_bias] = estimate_orientation(Ac % compute covariance of prediction P = (F * P * F') + Q; - % Driving System (Gyroscope) quaternion generation - % convert scaled gyro data to rad/s - qDot = 0.5 * quat_prod(q, [0 gyr_x(i) gyr_y(i) gyr_z(i)]); - - % Integrate to yield quaternion - q = q + qDot * dt * PI; - - % normalise quaternion - quat_driv(i,:) = q / norm(q); - - % Kalman Filtering - quat_error(i,:) = quat_prod(quat_aid(i,:), quat_driv(i,:)); - - euler_e = quat2euler(quat_error(i,:)); x1 = euler_e(1)'/PI; x2 = euler_e(2)'/PI; x3 = euler_e(3)'/PI; -- 2.7.4 From 850ec1aa7dd85d5c252c64a0cd3b8771bca422cc Mon Sep 17 00:00:00 2001 From: Ankur Date: Fri, 29 May 2015 14:33:13 +0530 Subject: [PATCH 02/16] Adding tilt sensor event definitions [sensor_tilt.h] to avoid build break the sensor_tilt.h file containing the tilt sensor event definitions was missing. This was causing the build to break. Added the tilt sensor event definitions with tilt_raw_data_event. signed-off-by: Ankur Garg Change-Id: I2d7768d70d05c7e4c1be08910276b958e76ef761 --- src/libsensord/sensor_tilt.h | 52 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) create mode 100644 src/libsensord/sensor_tilt.h diff --git a/src/libsensord/sensor_tilt.h b/src/libsensord/sensor_tilt.h new file mode 100644 index 0000000..8b15d01 --- /dev/null +++ b/src/libsensord/sensor_tilt.h @@ -0,0 +1,52 @@ +/* + * 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_TILT_H__ +#define __SENSOR_TILT_H__ + +//! Pre-defined events for the tilt sensor +//! Sensor Plugin developer can add more event to their own headers + +#ifdef __cplusplus +extern "C" +{ +#endif + +/** + * @defgroup SENSOR_TILT tilt Sensor + * @ingroup SENSOR_FRAMEWORK + * + * These APIs are used to control the tilt sensor. + * @{ + */ + +enum tilt_event_type { + TILT_RAW_DATA_EVENT = (TILT_SENSOR << 16) | 0x0001, +}; + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif +//! End of a file -- 2.7.4 From 640f8468a27d4500c97f90114963d85394f48752 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 5 Jun 2015 17:03:36 +0900 Subject: [PATCH 03/16] Added code to compute gyro bias in orientation_filter - orientation_filter code will compute gyro bias by adding gyro drift and stocastic noise - null offset bias will be computed and added to gyro bias on individual virtual sensor code. - needed for uncalibrated gyroscope virtual sensor Change-Id: I09cc8af16c12ea77faee704d0ccf6ba0b4d72736 --- src/sensor_fusion/orientation_filter.cpp | 6 ++++++ src/sensor_fusion/orientation_filter.h | 1 + 2 files changed, 7 insertions(+) diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index c3a6c92..67ff072 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -234,6 +234,8 @@ inline void orientation_filter::time_update() euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI; + m_gyro_bias = euler_error.m_ang * (TYPE) PI; + quaternion quat_eu_er(1, euler_error.m_ang.m_vec[0], euler_error.m_ang.m_vec[1], euler_error.m_ang.m_vec[2]); @@ -298,6 +300,8 @@ inline void orientation_filter::time_update_gaming_rv() euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI; + m_gyro_bias = euler_error.m_ang * (TYPE) PI; + euler_aid = quat2euler(m_quat_aid); euler_driv = quat2euler(quat_output); @@ -351,6 +355,8 @@ inline void orientation_filter::measurement_update() vect vec(arr_bias); m_bias_correction = vec; + + m_gyro_bias = m_gyro_bias + vec; } template diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h index c89a6b8..892c87c 100644 --- a/src/sensor_fusion/orientation_filter.h +++ b/src/sensor_fusion/orientation_filter.h @@ -58,6 +58,7 @@ public: vect m_state_old; vect m_state_error; vect m_bias_correction; + vect m_gyro_bias; quaternion m_quat_aid; quaternion m_quat_driv; rotation_matrix m_rot_matrix; -- 2.7.4 From 356c0559f7aa15747284c6a958458c3cb7e62ccb Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 5 Jun 2015 17:08:57 +0900 Subject: [PATCH 04/16] Updating standalone c-library test code for orientation sensor - Updating the standalone c-library test code to test if gyro bias values are computed correctly in sensor fusion algorithm. - these test code generally does not follow coding guidelines as these files are not part of tizen build and is used only for internal testing purposes. Change-Id: Iada88a8e5f1122cb0381b4aaac0a57269b43edc5 --- src/sensor_fusion/test/orientation_sensor.cpp | 3 +++ .../test_projects/orientation_sensor_test/orientation_sensor_main.cpp | 4 ++++ 2 files changed, 7 insertions(+) diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp index 784ac3b..decb3cc 100644 --- a/src/sensor_fusion/test/orientation_sensor.cpp +++ b/src/sensor_fusion/test/orientation_sensor.cpp @@ -42,6 +42,7 @@ void pre_process_data(sensor_data *data_out, sensor_data *data_in, void orientation_sensor::get_device_orientation(sensor_data *accel_data, sensor_data *gyro_data, sensor_data *magnetic_data) { + vect vec_bias_gyro(bias_gyro); pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); normalize(*accel_data); @@ -52,6 +53,8 @@ void orientation_sensor::get_device_orientation(sensor_data *accel_data, orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor; orien_filter.get_device_orientation(accel_data, gyro_data, magnetic_data); + + orien_filter.m_gyro_bias = orien_filter.m_gyro_bias + vec_bias_gyro; } #endif 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 2710332..feb7e77 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 @@ -86,6 +86,8 @@ int main() orientation = orien_sensor.orien_filter.m_orientation; + cout << "Gyro Bias in radians\t" << orien_sensor.orien_filter.m_gyro_bias; + orientation = rad2deg(orientation); orientation.m_ang.m_vec[0] *= pitch_phase_compensation; @@ -97,6 +99,8 @@ int main() orien_file << orientation.m_ang; + orientation = orien_sensor.orien_filter.m_orientation; + 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 89028ed6f7e6839bf0cf7d286684007f33964383 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 5 Jun 2015 20:04:44 +0900 Subject: [PATCH 05/16] Moving drift compensation before computing covariance in c++ library - Moving drift compensation before computing noise covariance as this provides marginal improvement in orientation estimation. Change-Id: Ic236f8400203e70d9b870186c8e098c4bcc8395f --- src/sensor_fusion/orientation_filter.cpp | 92 +++++++++++++------------------- src/sensor_fusion/orientation_filter.h | 2 + 2 files changed, 38 insertions(+), 56 deletions(-) diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index 67ff072..ef2e228 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -148,6 +148,32 @@ inline void orientation_filter::compute_covariance() { TYPE var_gyr_x, var_gyr_y, var_gyr_z; TYPE var_roll, var_pitch, var_azimuth; + quaternion quat_diff, quat_error; + + 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(); + + m_quat_output = phase_correction(m_quat_driv, m_quat_aid); + + m_orientation = quat2euler(m_quat_output); + + quat_error = m_quat_aid * m_quat_driv; + + m_euler_error = (quat2euler(quat_error)).m_ang; + + m_gyro.m_data = m_gyro.m_data - m_euler_error.m_ang; + + m_euler_error.m_ang = m_euler_error.m_ang / (TYPE) PI; + + m_gyro_bias = m_euler_error.m_ang * (TYPE) PI; insert_end(m_var_gyr_x, m_gyro.m_data.m_vec[0]); insert_end(m_var_gyr_y, m_gyro.m_data.m_vec[1]); @@ -178,8 +204,6 @@ inline void orientation_filter::compute_covariance() template inline void orientation_filter::time_update() { - quaternion quat_diff, quat_error, quat_output; - euler_angles euler_error; euler_angles orientation; m_tran_mat.m_mat[0][1] = m_gyro.m_data.m_vec[2]; @@ -210,43 +234,22 @@ inline void orientation_filter::time_update() m_state_new.m_vec[j] = NEGLIGIBLE_VAL; } - 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); - - m_quat_9axis = quat_output; + m_quat_9axis = m_quat_output; m_quat_gaming_rv = m_quat_9axis; - m_orientation = quat2euler(quat_output); - m_rot_matrix = quat2rot_mat(m_quat_driv); - quat_error = m_quat_aid * m_quat_driv; - - euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI; - - m_gyro_bias = euler_error.m_ang * (TYPE) PI; - - quaternion quat_eu_er(1, euler_error.m_ang.m_vec[0], euler_error.m_ang.m_vec[1], - euler_error.m_ang.m_vec[2]); + quaternion quat_eu_er(1, m_euler_error.m_ang.m_vec[0], m_euler_error.m_ang.m_vec[1], + m_euler_error.m_ang.m_vec[2]); m_quat_driv = (m_quat_driv * quat_eu_er) * (TYPE) PI; m_quat_driv.quat_normalize(); 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[0] = m_euler_error.m_ang.m_vec[0]; + m_state_error.m_vec[1] = m_euler_error.m_ang.m_vec[1]; + m_state_error.m_vec[2] = m_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]; @@ -256,8 +259,6 @@ 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; @@ -281,29 +282,8 @@ inline void orientation_filter::time_update_gaming_rv() 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); - - m_orientation = quat2euler(quat_output); - - quat_error = m_quat_aid * m_quat_driv; - - euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI; - - m_gyro_bias = euler_error.m_ang * (TYPE) PI; - euler_aid = quat2euler(m_quat_aid); - euler_driv = quat2euler(quat_output); + euler_driv = quat2euler(m_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]); @@ -311,9 +291,9 @@ inline void orientation_filter::time_update_gaming_rv() 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[0] = m_euler_error.m_ang.m_vec[0]; + m_state_error.m_vec[1] = m_euler_error.m_ang.m_vec[1]; + m_state_error.m_vec[2] = m_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]; diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h index 892c87c..3133f8b 100644 --- a/src/sensor_fusion/orientation_filter.h +++ b/src/sensor_fusion/orientation_filter.h @@ -66,6 +66,8 @@ public: quaternion m_quat_9axis; quaternion m_quat_gaming_rv; quaternion m_quaternion; + quaternion m_quat_output; + euler_angles m_euler_error; TYPE m_gyro_dt; int m_magnetic_alignment_factor; -- 2.7.4 From 0cf26fae7c18563d9e95b3d95192176a6c48cf0f Mon Sep 17 00:00:00 2001 From: Ankur Date: Tue, 16 Jun 2015 14:50:48 +0530 Subject: [PATCH 06/16] Adding support for uncal_gyro sensor in fusion_sensor Modifying the fusion sensor to support uncallibrated gyroscope sensor Change-Id: I223e58181b0cbcd3d152548cc2c6401826b35348 --- src/fusion/fusion_sensor.cpp | 99 ++++++++++++++++++++++++++++++------------ src/libsensord/sensor_fusion.h | 14 +++--- 2 files changed, 80 insertions(+), 33 deletions(-) diff --git a/src/fusion/fusion_sensor.cpp b/src/fusion/fusion_sensor.cpp index 42a8e41..48ba309 100755 --- a/src/fusion/fusion_sensor.cpp +++ b/src/fusion/fusion_sensor.cpp @@ -44,6 +44,7 @@ #define GEOMAGNETIC_RV_ENABLED 5 #define ORIENTATION_ENABLED 7 #define ROTATION_VECTOR_ENABLED 7 +#define UNCAL_GYRO_ENABLED 7 #define INITIAL_VALUE -1 @@ -282,7 +283,8 @@ void fusion_sensor::synthesize(const sensor_event_t &event, vectorget_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data); @@ -373,7 +401,8 @@ int fusion_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t if (event_type == FUSION_ORIENTATION_ENABLED || event_type == FUSION_ROTATION_VECTOR_ENABLED || - event_type == FUSION_GAMING_ROTATION_VECTOR_ENABLED) + event_type == FUSION_GAMING_ROTATION_VECTOR_ENABLED || + event_type == FUSION_UNCAL_GYRO_ENABLED) { m_gyro_sensor->get_sensor_data(GYROSCOPE_RAW_DATA_EVENT, gyro_data); pre_process_data(gyro, gyro_data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale); @@ -382,7 +411,8 @@ int fusion_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t if (event_type == FUSION_ORIENTATION_ENABLED || event_type == FUSION_ROTATION_VECTOR_ENABLED || - event_type == FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED) + event_type == FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED || + event_type == FUSION_UNCAL_GYRO_ENABLED) { m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_RAW_DATA_EVENT, magnetic_data); pre_process_data(magnetic, magnetic_data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale); @@ -391,20 +421,35 @@ int fusion_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t m_orientation_filter_poll.m_magnetic_alignment_factor = m_magnetic_alignment_factor; - if (event_type == FUSION_ORIENTATION_ENABLED || event_type == FUSION_ROTATION_VECTOR_ENABLED) + if (event_type == FUSION_ORIENTATION_ENABLED || + event_type == FUSION_ROTATION_VECTOR_ENABLED || + event_type == FUSION_UNCAL_GYRO_ENABLED) m_orientation_filter_poll.get_device_orientation(&accel, &gyro, &magnetic); else if (event_type == FUSION_GAMING_ROTATION_VECTOR_ENABLED) m_orientation_filter_poll.get_device_orientation(&accel, &gyro, NULL); else if (event_type == FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED) m_orientation_filter_poll.get_device_orientation(&accel, NULL, &magnetic); - data.accuracy = SENSOR_ACCURACY_GOOD; - data.timestamp = get_timestamp(); - data.value_count = 4; - data.values[0] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[0]; - data.values[1] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[1]; - data.values[2] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[2]; - data.values[3] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[3]; + if (event_type == FUSION_UNCAL_GYRO_ENABLED) { + data.accuracy = SENSOR_ACCURACY_GOOD; + data.timestamp = get_timestamp(); + data.value_count = 3; + data.values[0] = m_orientation_filter_poll.m_gyro_bias.m_vec[0]; + data.values[1] = m_orientation_filter_poll.m_gyro_bias.m_vec[1]; + data.values[2] = m_orientation_filter_poll.m_gyro_bias.m_vec[2]; + } + else if (event_type != FUSION_ORIENTATION_ENABLED || + event_type != FUSION_ROTATION_VECTOR_ENABLED || + event_type != FUSION_GAMING_ROTATION_VECTOR_ENABLED || + event_type != FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED) { + data.accuracy = SENSOR_ACCURACY_GOOD; + data.timestamp = get_timestamp(); + data.value_count = 4; + data.values[0] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[0]; + data.values[1] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[1]; + data.values[2] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[2]; + data.values[3] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[3]; + } return 0; } diff --git a/src/libsensord/sensor_fusion.h b/src/libsensord/sensor_fusion.h index 8f80499..1a65ea3 100755 --- a/src/libsensord/sensor_fusion.h +++ b/src/libsensord/sensor_fusion.h @@ -37,12 +37,14 @@ extern "C" */ enum fusion_event_type { 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, + FUSION_UNCAL_GYRO_EVENT = (FUSION_SENSOR << 16) | 0x0002, + FUSION_CALIBRATION_NEEDED_EVENT = (FUSION_SENSOR << 16) | 0x0003, + FUSION_ORIENTATION_ENABLED = (FUSION_SENSOR << 16) | 0x0004, + FUSION_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0005, + FUSION_GAMING_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0006, + FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0007, + FUSION_TILT_ENABLED = (FUSION_SENSOR << 16) | 0x0008, + FUSION_UNCAL_GYRO_ENABLED = (FUSION_SENSOR << 16) | 0x0009, }; /** -- 2.7.4 From d8a2b899d9a0fdad534bdf70d98507624178a23f Mon Sep 17 00:00:00 2001 From: Ankur Date: Tue, 16 Jun 2015 18:18:42 +0530 Subject: [PATCH 07/16] Adding Uncal Gyro Sensor and related files Adding uncal_gyro sensor files. Change-Id: I5d76a4a40ba774c50fcf0130c78e32011bd106a9 --- packaging/sensord.spec | 2 + sensor_plugins.xml.in | 1 + src/CMakeLists.txt | 7 + src/libsensord/CMakeLists.txt | 1 + src/libsensord/client_common.cpp | 2 + src/libsensord/sensor_internal.h | 1 + src/libsensord/sensor_internal_deprecated.h | 1 + src/libsensord/sensor_uncal_gyro.h | 52 +++++ src/uncal_gyro/CMakeLists.txt | 24 +++ src/uncal_gyro/uncal_gyro_sensor.cpp | 319 ++++++++++++++++++++++++++++ src/uncal_gyro/uncal_gyro_sensor.h | 70 ++++++ virtual_sensors.xml | 21 ++ 12 files changed, 501 insertions(+) create mode 100644 src/libsensord/sensor_uncal_gyro.h create mode 100644 src/uncal_gyro/CMakeLists.txt create mode 100644 src/uncal_gyro/uncal_gyro_sensor.cpp create mode 100644 src/uncal_gyro/uncal_gyro_sensor.h diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 499c99e..5ca2327 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -31,6 +31,7 @@ BuildRequires: pkgconfig(capi-system-info) %define geomagnetic_rv_state ON %define gaming_rv_state ON %define tilt_state ON +%define uncal_gyro_state ON %define build_test_suite OFF %description @@ -82,6 +83,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} \ + -DUNCAL_GYRO=%{uncal_gyro_state} \ -DTILT=%{tilt_state} -DTEST_SUITE=%{build_test_suite} \ -DLIBDIR=%{_libdir} -DINCLUDEDIR=%{_includedir} diff --git a/sensor_plugins.xml.in b/sensor_plugins.xml.in index c1822ce..66d7d26 100755 --- a/sensor_plugins.xml.in +++ b/sensor_plugins.xml.in @@ -25,5 +25,6 @@ + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 65f0ec7..39dc767 100755 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -48,6 +48,10 @@ IF("${TILT}" STREQUAL "ON") set(SENSOR_FUSION_ENABLE "1") set(TILT_ENABLE "1") ENDIF() +IF("${UNCAL_GYRO}" STREQUAL "ON") +set(SENSOR_FUSION_ENABLE "1") +set(UNCAL_GYRO_ENABLE "1") +ENDIF() IF("${GRAVITY}" STREQUAL "ON") set(SENSOR_FUSION_ENABLE "1") set(ORIENTATION_ENABLE "1") @@ -75,6 +79,9 @@ ENDIF() IF("${TILT_ENABLE}" STREQUAL "1") add_subdirectory(tilt) ENDIF() +IF("${UNCAL_GYRO_ENABLE}" STREQUAL "1") +add_subdirectory(uncal_gyro) +ENDIF() add_subdirectory(rotation_vector) add_subdirectory(server) diff --git a/src/libsensord/CMakeLists.txt b/src/libsensord/CMakeLists.txt index a8b8aab..bd56d4b 100755 --- a/src/libsensord/CMakeLists.txt +++ b/src/libsensord/CMakeLists.txt @@ -62,4 +62,5 @@ install(FILES sensor_temperature.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/senso install(FILES sensor_motion.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES sensor_fusion.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES sensor_deprecated.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) +install(FILES sensor_uncal_gyro.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES ${PROJECT_NAME}.pc DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig) diff --git a/src/libsensord/client_common.cpp b/src/libsensord/client_common.cpp index 082c31d..29d056f 100755 --- a/src/libsensord/client_common.cpp +++ b/src/libsensord/client_common.cpp @@ -45,6 +45,7 @@ log_element g_log_elements[] = { FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, GAMING_RV_SENSOR, 0, 1), FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, FUSION_SENSOR, 0, 1), FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, TILT_SENSOR, 0, 1), + FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, UNCAL_GYROSCOPE_SENSOR, 0, 1), FILL_LOG_ELEMENT(LOG_ID_EVENT, GEOMAGNETIC_CALIBRATION_NEEDED_EVENT, 0, 1), FILL_LOG_ELEMENT(LOG_ID_EVENT, PROXIMITY_CHANGE_STATE_EVENT, 0,1), @@ -69,6 +70,7 @@ log_element g_log_elements[] = { FILL_LOG_ELEMENT(LOG_ID_EVENT, GAMING_RV_RAW_DATA_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, FUSION_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, TILT_RAW_DATA_EVENT, 0, 10), + FILL_LOG_ELEMENT(LOG_ID_EVENT, UNCAL_GYRO_RAW_DATA_EVENT, 0, 10), }; typedef unordered_map log_map; diff --git a/src/libsensord/sensor_internal.h b/src/libsensord/sensor_internal.h index 7842cda..477e6f0 100755 --- a/src/libsensord/sensor_internal.h +++ b/src/libsensord/sensor_internal.h @@ -56,6 +56,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 fdcbab1..3f6aede 100755 --- a/src/libsensord/sensor_internal_deprecated.h +++ b/src/libsensord/sensor_internal_deprecated.h @@ -55,6 +55,7 @@ extern "C" #include #include #include +#include #define MAX_KEY_LEN 30 diff --git a/src/libsensord/sensor_uncal_gyro.h b/src/libsensord/sensor_uncal_gyro.h new file mode 100644 index 0000000..92494a7 --- /dev/null +++ b/src/libsensord/sensor_uncal_gyro.h @@ -0,0 +1,52 @@ +/* + * 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_UNCAL_GYRO_H__ +#define __SENSOR_UNCAL_GYRO_H__ + +//! Pre-defined events for the uncal gyroscope sensor +//! Sensor Plugin developer can add more event to their own headers + +#ifdef __cplusplus +extern "C" +{ +#endif + +/** + * @defgroup UNCAL_SENSOR_GYRO Gyro Sensor + * @ingroup SENSOR_FRAMEWORK + * + * These APIs are used to control the gyro sensor. + * @{ + */ + +enum uncal_gyro_event_type { + UNCAL_GYRO_RAW_DATA_EVENT = (UNCAL_GYROSCOPE_SENSOR << 16) | 0x0001, +}; + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif +//! End of a file diff --git a/src/uncal_gyro/CMakeLists.txt b/src/uncal_gyro/CMakeLists.txt new file mode 100644 index 0000000..12d5b3d --- /dev/null +++ b/src/uncal_gyro/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 2.6) +project(uncal_gyro CXX) + +SET(SENSOR_NAME uncal_gyro_sensor) + +include_directories(${CMAKE_CURRENT_SOURCE_DIR}) +include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) +include_directories(${CMAKE_SOURCE_DIR}/src/sensor_fusion) + +FOREACH(flag ${gravity_pkgs_LDFLAGS}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") +ENDFOREACH(flag) + +FOREACH(flag ${uncal_gyro_pkgs_CFLAGS}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") +ENDFOREACH(flag) + +add_library(${SENSOR_NAME} SHARED + uncal_gyro_sensor.cpp + ) + +target_link_libraries(${SENSOR_NAME} ${uncal_gyro_pkgs_LDFLAGS} "-lm") + +install(TARGETS ${SENSOR_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR}/sensord) diff --git a/src/uncal_gyro/uncal_gyro_sensor.cpp b/src/uncal_gyro/uncal_gyro_sensor.cpp new file mode 100644 index 0000000..f173f3f --- /dev/null +++ b/src/uncal_gyro/uncal_gyro_sensor.cpp @@ -0,0 +1,319 @@ +/* + * 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 "UNCAL_GYROSCOPE_SENSOR" +#define SENSOR_TYPE_UNCAL_GYRO "UNCAL_GYROSCOPE" + +#define GYROSCOPE_ENABLED 0x01 +#define GYRO_BIAS_ENABLED 0x02 +#define UNCAL_GYRO_BIAS_ENABLED 3 + +#define INITIAL_VALUE -1 + +#define MS_TO_US 1000 +#define MIN_DELIVERY_DIFF_FACTOR 0.75f + +#define PI 3.141593 +#define AZIMUTH_OFFSET_DEGREES 360 +#define AZIMUTH_OFFSET_RADIANS (2 * PI) + +#define ELEMENT_NAME "NAME" +#define ELEMENT_VENDOR "VENDOR" +#define ELEMENT_RAW_DATA_UNIT "RAW_DATA_UNIT" +#define ELEMENT_DEFAULT_SAMPLING_TIME "DEFAULT_SAMPLING_TIME" + +uncal_gyro_sensor::uncal_gyro_sensor() +: m_accel_sensor(NULL) +, m_gyro_sensor(NULL) +, m_magnetic_sensor(NULL) +, m_fusion_sensor(NULL) +, m_time(0) +{ + cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); + + sensor_hal *fusion_sensor_hal = sensor_plugin_loader::get_instance().get_sensor_hal(FUSION_SENSOR); + if (!fusion_sensor_hal) + m_hardware_fusion = false; + else + m_hardware_fusion = true; + + m_name = string(SENSOR_NAME); + register_supported_event(UNCAL_GYRO_RAW_DATA_EVENT); + + if (!config.get(SENSOR_TYPE_UNCAL_GYRO, ELEMENT_VENDOR, m_vendor)) { + ERR("[VENDOR] is empty\n"); + throw ENXIO; + } + + INFO("m_vendor = %s", m_vendor.c_str()); + + if (!config.get(SENSOR_TYPE_UNCAL_GYRO, 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_UNCAL_GYRO, 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); + + m_interval = m_default_sampling_time * MS_TO_US; +} + +uncal_gyro_sensor::~uncal_gyro_sensor() +{ + INFO("uncal_gyro_sensor is destroyed!\n"); +} + +bool uncal_gyro_sensor::init(void) +{ + m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); + m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR); + m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR); + + m_fusion_sensor = sensor_plugin_loader::get_instance().get_sensor(FUSION_SENSOR); + + if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor || !m_fusion_sensor) { + ERR("Failed to load sensors, accel: 0x%x, gyro: 0x%x, mag: 0x%x, fusion: 0x%x", + m_accel_sensor, m_gyro_sensor, m_magnetic_sensor, m_fusion_sensor); + return false; + } + + INFO("%s is created!\n", sensor_base::get_name()); + + return true; +} + +sensor_type_t uncal_gyro_sensor::get_type(void) +{ + return UNCAL_GYROSCOPE_SENSOR; +} + +bool uncal_gyro_sensor::on_start(void) +{ + AUTOLOCK(m_mutex); + + if (!m_hardware_fusion) { + 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_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_RAW_DATA_EVENT); + m_magnetic_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); + m_magnetic_sensor->start(); + } + + m_fusion_sensor->register_supported_event(FUSION_UNCAL_GYRO_EVENT); + m_fusion_sensor->register_supported_event(FUSION_UNCAL_GYRO_ENABLED); + m_fusion_sensor->add_client(FUSION_UNCAL_GYRO_EVENT); + m_fusion_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); + m_fusion_sensor->start(); + + activate(); + return true; +} + +bool uncal_gyro_sensor::on_stop(void) +{ + AUTOLOCK(m_mutex); + + if (!m_hardware_fusion) { + 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_RAW_DATA_EVENT); + m_gyro_sensor->delete_interval((intptr_t)this, false); + m_gyro_sensor->stop(); + m_magnetic_sensor->delete_client(GEOMAGNETIC_RAW_DATA_EVENT); + m_magnetic_sensor->delete_interval((intptr_t)this, false); + m_magnetic_sensor->stop(); + } + + m_fusion_sensor->delete_client(FUSION_UNCAL_GYRO_EVENT); + m_fusion_sensor->delete_interval((intptr_t)this, false); + m_fusion_sensor->unregister_supported_event(FUSION_UNCAL_GYRO_EVENT); + m_fusion_sensor->unregister_supported_event(FUSION_UNCAL_GYRO_ENABLED); + m_fusion_sensor->stop(); + + deactivate(); + return true; +} + +bool uncal_gyro_sensor::add_interval(int client_id, unsigned int interval) +{ + AUTOLOCK(m_mutex); + + if (!m_hardware_fusion) { + m_accel_sensor->add_interval(client_id, interval, false); + m_gyro_sensor->add_interval(client_id, interval, false); + m_magnetic_sensor->add_interval(client_id, interval, false); + } + + m_fusion_sensor->add_interval(client_id, interval, false); + + return sensor_base::add_interval(client_id, interval, false); +} + +bool uncal_gyro_sensor::delete_interval(int client_id) +{ + AUTOLOCK(m_mutex); + + if (!m_hardware_fusion) { + m_accel_sensor->delete_interval(client_id, false); + m_gyro_sensor->delete_interval(client_id, false); + m_magnetic_sensor->delete_interval(client_id, false); + } + + m_fusion_sensor->delete_interval(client_id, false); + + return sensor_base::delete_interval(client_id, false); +} + +void uncal_gyro_sensor::synthesize(const sensor_event_t &event, vector &outs) +{ + sensor_event_t uncal_gyro_event; + unsigned long long diff_time; + float azimuth_offset; + + 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)) + return; + + m_gyro.m_data.m_vec[0] = event.data.values[0]; + m_gyro.m_data.m_vec[1] = event.data.values[1]; + m_gyro.m_data.m_vec[2] = event.data.values[2]; + + m_gyro.m_time_stamp = event.data.timestamp; + + m_enable_uncal_gyro |= GYROSCOPE_ENABLED; + } + + if (event.event_type == FUSION_UNCAL_GYRO_EVENT) { + diff_time = event.data.timestamp - m_time; + + if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + m_fusion.m_data.m_vec[0] = event.data.values[0]; + m_fusion.m_data.m_vec[1] = event.data.values[1]; + m_fusion.m_data.m_vec[2] = event.data.values[2]; + + m_fusion.m_time_stamp = event.data.timestamp; + + m_enable_uncal_gyro |= GYRO_BIAS_ENABLED; + } + + if (m_enable_uncal_gyro == UNCAL_GYRO_BIAS_ENABLED) { + m_enable_uncal_gyro = 0; + + m_time = get_timestamp(); + uncal_gyro_event.sensor_id = get_id(); + uncal_gyro_event.event_type = UNCAL_GYRO_RAW_DATA_EVENT; + uncal_gyro_event.data.value_count = 6; + uncal_gyro_event.data.timestamp = m_time; + uncal_gyro_event.data.accuracy = SENSOR_ACCURACY_GOOD; + uncal_gyro_event.data.values[0] = m_gyro.m_data.m_vec[0]; + uncal_gyro_event.data.values[1] = m_gyro.m_data.m_vec[1]; + uncal_gyro_event.data.values[2] = m_gyro.m_data.m_vec[2]; + + uncal_gyro_event.data.values[3] = m_fusion.m_data.m_vec[0]; + uncal_gyro_event.data.values[4] = m_fusion.m_data.m_vec[1]; + uncal_gyro_event.data.values[5] = m_fusion.m_data.m_vec[2]; + + push(uncal_gyro_event); + } + + return; +} + +int uncal_gyro_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data) +{ + sensor_data_t fusion_data, gyro_data; + + if (event_type != UNCAL_GYRO_RAW_DATA_EVENT) + return -1; + + m_fusion_sensor->get_sensor_data(FUSION_UNCAL_GYRO_ENABLED, fusion_data); + m_gyro_sensor->get_sensor_data(GYROSCOPE_RAW_DATA_EVENT, gyro_data); + + data.accuracy = fusion_data.accuracy; + data.timestamp = get_timestamp(); + data.value_count = 6; + data.values[0] = gyro_data.values[0]; + data.values[1] = gyro_data.values[1]; + data.values[2] = gyro_data.values[2]; + data.values[3] = fusion_data.values[0]; + data.values[4] = fusion_data.values[1]; + data.values[5] = fusion_data.values[2]; + + return 0; +} + +bool uncal_gyro_sensor::get_properties(sensor_properties_s &properties) +{ + 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) +{ + uncal_gyro_sensor *sensor; + + try { + sensor = new(std::nothrow) uncal_gyro_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/uncal_gyro/uncal_gyro_sensor.h b/src/uncal_gyro/uncal_gyro_sensor.h new file mode 100644 index 0000000..dccb6f0 --- /dev/null +++ b/src/uncal_gyro/uncal_gyro_sensor.h @@ -0,0 +1,70 @@ +/* + * 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 _UNCAL_GYRO_SENSOR_H_ +#define _UNCAL_GYRO_SENSOR_H_ + +#include +#include +#include + +class uncal_gyro_sensor : public virtual_sensor { +public: + uncal_gyro_sensor(); + virtual ~uncal_gyro_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_magnetic_sensor; + sensor_base *m_gyro_sensor; + sensor_base *m_fusion_sensor; + + sensor_data m_fusion; + sensor_data m_gyro; + + cmutex m_value_mutex; + + unsigned int m_enable_uncal_gyro; + + unsigned long long m_time; + unsigned int m_interval; + + string m_vendor; + string m_raw_data_unit; + int m_default_sampling_time; + float m_gyro_static_bias[3]; + int m_gyro_rotation_direction_compensation[3]; + float m_gyro_scale; + + bool on_start(void); + bool on_stop(void); +}; + +#endif /*_UNCAL_GYRO_SENSOR_H_*/ diff --git a/virtual_sensors.xml b/virtual_sensors.xml index 3005df6..06263c0 100755 --- a/virtual_sensors.xml +++ b/virtual_sensors.xml @@ -73,6 +73,13 @@ + + + + + + + @@ -148,6 +155,13 @@ + + + + + + + @@ -223,5 +237,12 @@ + + + + + + + -- 2.7.4 From ae70a6bddd6bf2a4a90e504b5895f7e2e47550d1 Mon Sep 17 00:00:00 2001 From: Ankur Date: Tue, 16 Jun 2015 18:24:09 +0530 Subject: [PATCH 08/16] Updating Test files to test the Uncal_gyro sensor Add test cases for uncal_gyro sensor to the test files Change-Id: I20243d3ec6e5967ee96a3120a3178af30ab2cd1d --- test/src/api-test.c | 3 +++ test/src/check-sensor.c | 12 ++++++++++++ test/src/sensor-test.c | 5 +++++ 3 files changed, 20 insertions(+) diff --git a/test/src/api-test.c b/test/src/api-test.c index 599041a..8910bd4 100644 --- a/test/src/api-test.c +++ b/test/src/api-test.c @@ -235,6 +235,9 @@ int main(int argc, char **argv) result = check_sensor_api(GAMING_RV_RAW_DATA_EVENT, interval); fprintf(fp, "Gaming Rotation Vector - RAW_DATA_REPORT_ON_TIME - %d\n", result); + result = check_sensor_api(UNCAL_GYROSCOPE_SENSOR, interval); + fprintf(fp, "Uncal Gyro Sensor - RAW_DATA_REPORT_ON_TIME - %d\n", result); + result = check_sensor_api(TEMPERATURE_RAW_DATA_EVENT, interval); fprintf(fp, "Temperature - RAW_DATA_REPORT_ON_TIME - %d\n", result); diff --git a/test/src/check-sensor.c b/test/src/check-sensor.c index 9bba712..6f86cf2 100644 --- a/test/src/check-sensor.c +++ b/test/src/check-sensor.c @@ -75,6 +75,9 @@ void printpollinglogs(sensor_type_t type,sensor_data_t data) 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; + case(UNCAL_GYROSCOPE_SENSOR): + printf("Uncal gyro [%lld] [%6.6f] [%6.6f] [%6.6f] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2], data.values[3], data.values[4], data.values[5]); + break; default: return; } @@ -139,6 +142,11 @@ int get_event(sensor_type_t sensor_type, char str[]) if (strcmp(str, "RAW_DATA_EVENT") == 0) return GAMING_RV_RAW_DATA_EVENT; break; + case UNCAL_GYROSCOPE_SENSOR: + if (strcmp(str, "RAW_DATA_EVENT") == 0) + return UNCAL_GYRO_RAW_DATA_EVENT; + break; + } return -1; } @@ -190,6 +198,10 @@ void callback(sensor_t sensor, unsigned int event_type, sensor_data_t *data, voi case GAMING_RV_SENSOR: printf("Gaming RV [%lld] [%6.6f] [%6.6f] [%6.6f] [%6.6f]\n", data->timestamp, data->values[0], data->values[1], data->values[2], data->values[3]); break; + case UNCAL_GYROSCOPE_SENSOR: + printf("Uncal gyro [%lld] [%6.6f] [%6.6f] [%6.6f] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2], data.values[3], data.values[4], data.values[5]); + break; + default: return; } diff --git a/test/src/sensor-test.c b/test/src/sensor-test.c index 99ea138..cb37818 100644 --- a/test/src/sensor-test.c +++ b/test/src/sensor-test.c @@ -45,6 +45,7 @@ void usage() printf("[geomagnetic_rv] "); printf("[gaming_rv] "); printf("[light]\n"); + printf("[uncal_gyro]"); printf("event:"); printf("[RAW_DATA_EVENT]\n"); printf("-p: [polling]\n"); @@ -128,6 +129,10 @@ int main(int argc, char **argv) sensor_type = PROXIMITY_SENSOR; event = PROXIMITY_CHANGE_STATE_EVENT; } + else if (strcmp(argv[1], "uncal_gyro") == 0) { + sensor_type = UNCAL_GYROSCOPE_SENSOR; + event = UNCAL_GYRO_RAW_DATA_EVENT; + } else { usage(); } -- 2.7.4 From 17813b8fc10bf1b3b919d4865c42ef47f5bcef09 Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Fri, 5 Jun 2015 14:44:13 +0530 Subject: [PATCH 09/16] Removing unused functions and variables from geomagnetic rotation vector Change-Id: Ibbfed62b6b494c46b07af31bb67b9b24d1aa0297 --- src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp | 12 ------------ src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h | 5 ----- 2 files changed, 17 deletions(-) diff --git a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp index d39f81a..723aca1 100755 --- a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp +++ b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp @@ -37,22 +37,11 @@ #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" -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) @@ -69,7 +58,6 @@ geomagnetic_rv_sensor::geomagnetic_rv_sensor() m_name = string(SENSOR_NAME); register_supported_event(GEOMAGNETIC_RV_RAW_DATA_EVENT); - m_enable_geomagnetic_rv = 0; if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_VENDOR, m_vendor)) { ERR("[VENDOR] is empty\n"); diff --git a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h index 3cf1999..c87d252 100755 --- a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h +++ b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h @@ -50,11 +50,6 @@ private: cmutex m_value_mutex; - orientation_filter m_orientation_filter; - orientation_filter m_orientation_filter_poll; - - unsigned int m_enable_geomagnetic_rv; - unsigned long long m_time; unsigned int m_interval; -- 2.7.4 From 711df134a305354b85c998014e1b508d714d1b66 Mon Sep 17 00:00:00 2001 From: Ankur Date: Mon, 29 Jun 2015 14:52:46 +0530 Subject: [PATCH 10/16] Fixing problem related to flushing output to file in Fusion data collection test case Change-Id: I5747038fd33e5f0115f167a62f8a7bdd21f64f12 --- test/src/fusion-data-collection.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/test/src/fusion-data-collection.c b/test/src/fusion-data-collection.c index 62e63f8..8c4c771 100644 --- a/test/src/fusion-data-collection.c +++ b/test/src/fusion-data-collection.c @@ -38,15 +38,19 @@ void callback(sensor_t sensor, unsigned int event_type, sensor_data_t *data, voi 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); + fflush(file_output[0]); 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); + fflush(file_output[1]); 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); + fflush(file_output[2]); break; case PROXIMITY_SENSOR: fprintf(file_output[MAXSIZE-1], "%6.6f %lld\n", data->values[0], data->timestamp); + fflush(file_output[MAXSIZE-1]); break; default: return; @@ -55,7 +59,7 @@ void callback(sensor_t sensor, unsigned int event_type, sensor_data_t *data, voi void usage() { - printf("Usage : ./one-client-multiple-sensors \n\n"); + printf("Usage : ./fusion-data-collection \n\n"); printf("interval:\n"); printf("The sampling interval in ms.\n"); -- 2.7.4 From 876d33fad9b40ce337cf8e001743a1dd6a4d10e1 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 2 Jul 2015 09:50:55 +0900 Subject: [PATCH 11/16] Improving the sensor fusion algorithm for orientation using accel & gyro - Improving algorithm by correcting gyroscope orientation using limited accelerometer orientation. - Change made and tested only on octave design code. Change-Id: Ic6abd19ab9f64cb06149622b11533eb1754279aa --- src/sensor_fusion/design/lib/estimate_orientation.m | 17 ++++++++++------- src/sensor_fusion/design/sf_gaming_rv.m | 8 ++++++++ src/sensor_fusion/design/sf_orientation.m | 2 +- 3 files changed, 19 insertions(+), 8 deletions(-) diff --git a/src/sensor_fusion/design/lib/estimate_orientation.m b/src/sensor_fusion/design/lib/estimate_orientation.m index 511b646..de28616 100755 --- a/src/sensor_fusion/design/lib/estimate_orientation.m +++ b/src/sensor_fusion/design/lib/estimate_orientation.m @@ -42,6 +42,7 @@ function [quat_driv, quat_aid, quat_error, gyro_bias] = estimate_orientation(Ac GRAVITY = 9.80665; PI = 3.141593; NON_ZERO_VAL = 0.1; + ROUNDOFF_VAL = 0.0025; PI_DEG = 180; MOVING_AVERAGE_WINDOW_LENGTH = 20; @@ -220,7 +221,6 @@ function [quat_driv, quat_aid, quat_error, gyro_bias] = estimate_orientation(Ac var_yaw(i) = var(yaw((i-MOVING_AVERAGE_WINDOW_LENGTH):i)); end - Qwn = [var_Gx(i) 0 0;0 var_Gy(i) 0;0 0 var_Gz(i);]; Qwb = (2 * (ZigmaW^2) / TauW) * eye(3); @@ -254,10 +254,13 @@ function [quat_driv, quat_aid, quat_error, gyro_bias] = estimate_orientation(Ac 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); + if (euler_aid(1)^2 < (ROUNDOFF_VAL * PI)) + if (euler_aid(2)^2 < (ROUNDOFF_VAL * PI)) + if (gyr_z(i)^2 < (NON_ZERO_VAL)) + q = quat_aid(i,:); + end + end + end end if i > 1 @@ -280,8 +283,8 @@ function [quat_driv, quat_aid, quat_error, gyro_bias] = estimate_orientation(Ac end end - if MAG_DATA_DISABLED == 1 - quat_driv = quat_gaming_rv; + if GYRO_DATA_DISABLED == 1 + quat_driv = quat_aid; end if PLOT_SCALED_SENSOR_COMPARISON_DATA == 1 diff --git a/src/sensor_fusion/design/sf_gaming_rv.m b/src/sensor_fusion/design/sf_gaming_rv.m index 892b86e..b8bb8b4 100755 --- a/src/sensor_fusion/design/sf_gaming_rv.m +++ b/src/sensor_fusion/design/sf_gaming_rv.m @@ -26,10 +26,15 @@ close all clc GRAVITY = 9.80665; +RAD2DEG = 57.2957795; 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; +PITCH_PHASE_CORRECTION = -1; +ROLL_PHASE_CORRECTION = -1; +YAW_PHASE_CORRECTION = -1; + Bias_Ax = 0.098586; Bias_Ay = 0.18385; Bias_Az = 10.084 - GRAVITY; @@ -70,6 +75,9 @@ Game_RV = estimate_gaming_rv(Accel_data, Gyro_data); for i = 1:BUFFER_SIZE Orientation_RV(:,i) = quat2euler(Game_RV(i,:)); + Orientation_RV(1,i) = ROLL_PHASE_CORRECTION * Orientation_RV(1,i) * RAD2DEG; + Orientation_RV(2,i) = PITCH_PHASE_CORRECTION * Orientation_RV(2,i) * RAD2DEG; + Orientation_RV(3,i) = YAW_PHASE_CORRECTION * Orientation_RV(3,i) * RAD2DEG; end hfig=(figure); diff --git a/src/sensor_fusion/design/sf_orientation.m b/src/sensor_fusion/design/sf_orientation.m index 1c181cb..0d6f3e0 100755 --- a/src/sensor_fusion/design/sf_orientation.m +++ b/src/sensor_fusion/design/sf_orientation.m @@ -88,7 +88,7 @@ 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 = 1150; +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; -- 2.7.4 From ef677f2a5a16b0c898b24cdfc53213f13cc6036c Mon Sep 17 00:00:00 2001 From: Ankur Date: Fri, 7 Aug 2015 17:37:32 +0530 Subject: [PATCH 12/16] Bug Fixing for gaming_rotation_vector in get_sensor_data function signed-off-by:"Ankur Garg " Change-Id: Ie973230ce18b8a043e77d0dd9257223eff95ab1b --- src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp b/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp index 36a8d4d..6982608 100755 --- a/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp +++ b/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp @@ -219,10 +219,10 @@ int gaming_rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &da data.accuracy = SENSOR_ACCURACY_GOOD; data.timestamp = get_timestamp(); data.value_count = 4; - data.values[0] = data.values[1]; - data.values[1] = data.values[2]; - data.values[2] = data.values[3]; - data.values[3] = data.values[0]; + data.values[0] = fusion_data.values[1]; + data.values[1] = fusion_data.values[2]; + data.values[2] = fusion_data.values[3]; + data.values[3] = fusion_data.values[0]; return 0; } -- 2.7.4 From 48e637c94110fc7baaee0769e4983058d31381d6 Mon Sep 17 00:00:00 2001 From: Ankur Date: Mon, 10 Aug 2015 16:09:13 +0530 Subject: [PATCH 13/16] Adding polling support for tilt sensor and polling fixing bug in fusion sensor -Added polling support for tilt sensor -Fixed polling bug in get_sensor_data function in fusion sensor file Change-Id: I95fcee8a33482488ac61d7ae46f1f06554a1ac35 --- src/fusion/fusion_sensor.cpp | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/src/fusion/fusion_sensor.cpp b/src/fusion/fusion_sensor.cpp index 48ba309..38e02e4 100755 --- a/src/fusion/fusion_sensor.cpp +++ b/src/fusion/fusion_sensor.cpp @@ -388,10 +388,11 @@ int fusion_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t euler_angles euler_orientation; - if (event_type != FUSION_ORIENTATION_ENABLED || - event_type != FUSION_ROTATION_VECTOR_ENABLED || - event_type != FUSION_GAMING_ROTATION_VECTOR_ENABLED || - event_type != FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED || + if (event_type != FUSION_ORIENTATION_ENABLED && + event_type != FUSION_ROTATION_VECTOR_ENABLED && + event_type != FUSION_GAMING_ROTATION_VECTOR_ENABLED && + event_type != FUSION_TILT_ENABLED && + event_type != FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED && event_type != FUSION_UNCAL_GYRO_ENABLED) return -1; @@ -429,6 +430,8 @@ int fusion_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t m_orientation_filter_poll.get_device_orientation(&accel, &gyro, NULL); else if (event_type == FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED) m_orientation_filter_poll.get_device_orientation(&accel, NULL, &magnetic); + else if (event_type == FUSION_TILT_ENABLED) + m_orientation_filter_poll.get_device_orientation(&accel, NULL, NULL); if (event_type == FUSION_UNCAL_GYRO_ENABLED) { data.accuracy = SENSOR_ACCURACY_GOOD; @@ -438,10 +441,11 @@ int fusion_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t data.values[1] = m_orientation_filter_poll.m_gyro_bias.m_vec[1]; data.values[2] = m_orientation_filter_poll.m_gyro_bias.m_vec[2]; } - else if (event_type != FUSION_ORIENTATION_ENABLED || - event_type != FUSION_ROTATION_VECTOR_ENABLED || - event_type != FUSION_GAMING_ROTATION_VECTOR_ENABLED || - event_type != FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED) { + else if (event_type == FUSION_ORIENTATION_ENABLED || + event_type == FUSION_ROTATION_VECTOR_ENABLED || + event_type == FUSION_GAMING_ROTATION_VECTOR_ENABLED || + event_type == FUSION_TILT_ENABLED || + event_type == FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED) { data.accuracy = SENSOR_ACCURACY_GOOD; data.timestamp = get_timestamp(); data.value_count = 4; -- 2.7.4 From a649d1f541e45ef780b4b6675b665993f0b0ab96 Mon Sep 17 00:00:00 2001 From: Ankur Date: Tue, 11 Aug 2015 15:07:53 +0530 Subject: [PATCH 14/16] Enabling Auto_rotation to support landscape/potrait 90 degree events Tested on rd-pq Change-Id: I9ce16a9fb8ba9f3235e19b5fb29e6063ab8f0f2a Signed-off-by: Ankur --- packaging/sensord.spec | 3 +- sensor_plugins.xml.in | 1 + src/auto_rotation/CMakeLists.txt | 17 +++++----- src/auto_rotation/auto_rotation_sensor.cpp | 48 ++++++++++++++++++++++++----- src/auto_rotation/auto_rotation_sensor.h | 12 +++++--- src/libsensord/sensor_internal_deprecated.h | 1 + virtual_sensors.xml | 14 +++++++++ 7 files changed, 73 insertions(+), 23 deletions(-) diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 5ca2327..b9bf107 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -18,6 +18,7 @@ BuildRequires: pkgconfig(libsystemd-daemon) BuildRequires: pkgconfig(capi-system-info) %define accel_state ON +%define auto_rotation_state ON %define gyro_state ON %define proxi_state ON %define light_state ON @@ -83,7 +84,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} \ - -DUNCAL_GYRO=%{uncal_gyro_state} \ + -DUNCAL_GYRO=%{uncal_gyro_state} -DAUTO_ROTATION=%{auto_rotation_state} \ -DTILT=%{tilt_state} -DTEST_SUITE=%{build_test_suite} \ -DLIBDIR=%{_libdir} -DINCLUDEDIR=%{_includedir} diff --git a/sensor_plugins.xml.in b/sensor_plugins.xml.in index 66d7d26..a088a2a 100755 --- a/sensor_plugins.xml.in +++ b/sensor_plugins.xml.in @@ -11,6 +11,7 @@ + diff --git a/src/auto_rotation/CMakeLists.txt b/src/auto_rotation/CMakeLists.txt index a0076b2..13b56e5 100644 --- a/src/auto_rotation/CMakeLists.txt +++ b/src/auto_rotation/CMakeLists.txt @@ -6,23 +6,20 @@ SET(SENSOR_NAME auto_rotation_sensor) include_directories(${CMAKE_CURRENT_SOURCE_DIR}) include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) -INCLUDE(FindPkgConfig) -PKG_CHECK_MODULES(auto_rot_pkgs REQUIRED vconf) - -FOREACH(flag ${auto_rot_pkgs_LDFLAGS}) +FOREACH(flag ${auto_rotation_pkgs_LDFLAGS}) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") ENDFOREACH(flag) -FOREACH(flag ${auto_rot_pkgs_CFLAGS}) +FOREACH(flag ${auto_rotation_pkgs_CFLAGS}) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") ENDFOREACH(flag) add_library(${SENSOR_NAME} SHARED - auto_rotation_sensor.cpp - auto_rotation_alg.cpp - auto_rotation_alg_emul.cpp - ) + auto_rotation_sensor.cpp + auto_rotation_alg.cpp + auto_rotation_alg_emul.cpp +) -target_link_libraries(${SENSOR_NAME} ${auto_rot_pkgs_LDFLAGS} "-lm") +target_link_libraries(${SENSOR_NAME} ${auto_rotation_pkgs_LDFLAGS} "-lm") install(TARGETS ${SENSOR_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR}/sensord) diff --git a/src/auto_rotation/auto_rotation_sensor.cpp b/src/auto_rotation/auto_rotation_sensor.cpp index 381e861..1b1954b 100755 --- a/src/auto_rotation/auto_rotation_sensor.cpp +++ b/src/auto_rotation/auto_rotation_sensor.cpp @@ -32,14 +32,24 @@ #include #include #include +#include #include #include using std::bind1st; using std::mem_fun; -#define SENSOR_NAME "AUTO_ROTATION_SENSOR" -#define AUTO_ROTATION_LIB "/usr/lib/sensord/libauto-rotation.so" +#define SENSOR_NAME "AUTO_ROTATION_SENSOR" +#define SENSOR_TYPE_AUTO_ROTATION "AUTO_ROTATION" + +#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 AUTO_ROTATION_LIB "/usr/lib/sensord/libauto_rotation_sensor.so" auto_rotation_sensor::auto_rotation_sensor() : m_accel_sensor(NULL) @@ -47,9 +57,34 @@ auto_rotation_sensor::auto_rotation_sensor() , m_rotation_time(1) // rotation state is valid from initial state, so set rotation time to non-zero value , m_alg(NULL) { + cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); m_name = string(SENSOR_NAME); register_supported_event(AUTO_ROTATION_CHANGE_STATE_EVENT); + + + if (!config.get(SENSOR_TYPE_AUTO_ROTATION, ELEMENT_VENDOR, m_vendor)) { + ERR("[VENDOR] is empty\n"); + throw ENXIO; + } + + INFO("m_vendor = %s", m_vendor.c_str()); + + if (!config.get(SENSOR_TYPE_AUTO_ROTATION, 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_AUTO_ROTATION, 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); + + m_interval = m_default_sampling_time * MS_TO_US; } auto_rotation_sensor::~auto_rotation_sensor() @@ -105,13 +140,12 @@ sensor_type_t auto_rotation_sensor::get_type(void) bool auto_rotation_sensor::on_start(void) { - const int SAMPLING_TIME = 60; m_rotation = AUTO_ROTATION_DEGREE_UNKNOWN; m_alg->start(); m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); - m_accel_sensor->add_interval((int)this , SAMPLING_TIME, true); + m_accel_sensor->add_interval((int)this , (m_interval/MS_TO_US), true); m_accel_sensor->start(); return activate(); @@ -159,9 +193,9 @@ void auto_rotation_sensor::synthesize(const sensor_event_t& event, vector -#include #include #include -using std::string; - class auto_rotation_sensor : public virtual_sensor { public: auto_rotation_sensor(); @@ -39,16 +36,21 @@ public: void synthesize(const sensor_event_t& event, vector &outs); - int get_sensor_data(unsigned int data_id, sensor_data_t &data); - virtual bool get_properties(sensor_properties_t &properties); + int get_sensor_data(const unsigned int event_type, sensor_data_t &data); + virtual bool get_properties(sensor_properties_s &properties); private: sensor_base *m_accel_sensor; cmutex m_value_mutex; int m_rotation; + unsigned int m_interval; unsigned long long m_rotation_time; auto_rotation_alg *m_alg; + string m_vendor; + string m_raw_data_unit; + int m_default_sampling_time; + auto_rotation_alg *get_alg(); virtual bool on_start(void); virtual bool on_stop(void); diff --git a/src/libsensord/sensor_internal_deprecated.h b/src/libsensord/sensor_internal_deprecated.h index 3f6aede..85bcce7 100755 --- a/src/libsensord/sensor_internal_deprecated.h +++ b/src/libsensord/sensor_internal_deprecated.h @@ -38,6 +38,7 @@ extern "C" /*header for each sensor type*/ #include +#include #include #include #include diff --git a/virtual_sensors.xml b/virtual_sensors.xml index 06263c0..52ce6fb 100755 --- a/virtual_sensors.xml +++ b/virtual_sensors.xml @@ -110,6 +110,13 @@ + + + + + + + @@ -192,6 +199,13 @@ + + + + + + + -- 2.7.4 From ca54636e30c5371089eb9b1c1b4f89ff77842413 Mon Sep 17 00:00:00 2001 From: Ankur Date: Tue, 11 Aug 2015 15:16:37 +0530 Subject: [PATCH 15/16] Adding test cases for auto_rotation sensor in the existing test files Change-Id: Ib467dbed75a582ccf85a4570c0096fc10afd9270 Signed-off-by: Ankur --- test/src/check-sensor.c | 10 ++++++++++ test/src/sensor-test.c | 5 +++++ 2 files changed, 15 insertions(+) diff --git a/test/src/check-sensor.c b/test/src/check-sensor.c index 6f86cf2..8b66fb4 100644 --- a/test/src/check-sensor.c +++ b/test/src/check-sensor.c @@ -36,6 +36,9 @@ void printpollinglogs(sensor_type_t type,sensor_data_t data) 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(AUTO_ROTATION_SENSOR): + printf("Auto Rotation [%lld] [%6.6f]\n\n", data.timestamp, data.values[0]); + 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; @@ -90,6 +93,10 @@ int get_event(sensor_type_t sensor_type, char str[]) if (strcmp(str, "RAW_DATA_EVENT") == 0) return ACCELEROMETER_RAW_DATA_EVENT; break; + case AUTO_ROTATION_SENSOR: + if (strcmp(str, "CHANGE_STATE_EVENT") == 0) + return AUTO_ROTATION_CHANGE_STATE_EVENT; + break; case GYROSCOPE_SENSOR: if (strcmp(str, "RAW_DATA_EVENT") == 0) return GYROSCOPE_RAW_DATA_EVENT; @@ -159,6 +166,9 @@ void callback(sensor_t sensor, unsigned int event_type, sensor_data_t *data, voi case ACCELEROMETER_SENSOR: printf("Accelerometer [%lld] [%6.6f] [%6.6f] [%6.6f]\n", data->timestamp, data->values[0], data->values[1], data->values[2]); break; + case AUTO_ROTATION_SENSOR: + printf("Auto Rotation [%lld] [%6.6f]\n", data->timestamp, data->values[0]); + break; case GYROSCOPE_SENSOR: printf("Gyroscope [%lld] [%6.6f] [%6.6f] [%6.6f]\n", data->timestamp, data->values[0], data->values[1], data->values[2]); break; diff --git a/test/src/sensor-test.c b/test/src/sensor-test.c index cb37818..ba70a2b 100644 --- a/test/src/sensor-test.c +++ b/test/src/sensor-test.c @@ -33,6 +33,7 @@ void usage() printf("Sensor_type: "); printf("[accelerometer] "); + printf("[auto_rotation]\n"); printf("[gyroscope] "); printf("[pressure] "); printf("[temperature] "); @@ -77,6 +78,10 @@ int main(int argc, char **argv) sensor_type = ACCELEROMETER_SENSOR; event = ACCELEROMETER_RAW_DATA_EVENT; } + else if (strcmp(argv[1], "auto_rotation") == 0) { + sensor_type = AUTO_ROTATION_SENSOR; + event = AUTO_ROTATION_CHANGE_STATE_EVENT; + } else if (strcmp(argv[1], "gyroscope") == 0) { sensor_type = GYROSCOPE_SENSOR; event = GYROSCOPE_RAW_DATA_EVENT; -- 2.7.4 From 000ec4b2d9bd5419705378e330cb122a888b09d1 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 24 Jul 2015 16:29:28 +0900 Subject: [PATCH 16/16] Adding Ultraviolet sensor support for Note 4 - Added/updated files to support ultraviolet sensor for Note 4. - Added build support - Added API support - Added configuration for Note 4 UV sensor Change-Id: I7de07724d3498372c9a39c5da272eec4674e6af2 --- packaging/sensord.spec | 4 +++- sensor_plugins.xml.in | 2 ++ sensors.xml.in | 7 +++++++ src/libsensord/CMakeLists.txt | 1 + src/libsensord/client_common.cpp | 2 ++ src/libsensord/sensor_internal.h | 2 +- src/libsensord/sensor_internal_deprecated.h | 1 + src/ultraviolet/CMakeLists.txt | 4 ++-- src/ultraviolet/ultraviolet_sensor.cpp | 30 ++++++++--------------------- src/ultraviolet/ultraviolet_sensor.h | 10 ++++------ src/ultraviolet/ultraviolet_sensor_hal.cpp | 9 ++++----- src/ultraviolet/ultraviolet_sensor_hal.h | 3 ++- 12 files changed, 37 insertions(+), 38 deletions(-) diff --git a/packaging/sensord.spec b/packaging/sensord.spec index b9bf107..aa45fd0 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -25,6 +25,7 @@ BuildRequires: pkgconfig(capi-system-info) %define geo_state ON %define pressure_state ON %define temperature_state ON +%define ultraviolet_state ON %define orientation_state ON %define gravity_state ON %define linear_accel_state ON @@ -85,7 +86,8 @@ cmake . -DCMAKE_INSTALL_PREFIX=%{_prefix} -DACCEL=%{accel_state} \ -DLINEAR_ACCEL=%{linear_accel_state} -DRV=%{rv_state} \ -DGEOMAGNETIC_RV=%{geomagnetic_rv_state} -DGAMING_RV=%{gaming_rv_state} \ -DUNCAL_GYRO=%{uncal_gyro_state} -DAUTO_ROTATION=%{auto_rotation_state} \ - -DTILT=%{tilt_state} -DTEST_SUITE=%{build_test_suite} \ + -DTILT=%{tilt_state} -DULTRAVIOLET=%{ultraviolet_state} \ + -DTEST_SUITE=%{build_test_suite} \ -DLIBDIR=%{_libdir} -DINCLUDEDIR=%{_includedir} %build diff --git a/sensor_plugins.xml.in b/sensor_plugins.xml.in index a088a2a..ba602e6 100755 --- a/sensor_plugins.xml.in +++ b/sensor_plugins.xml.in @@ -7,6 +7,7 @@ + @@ -27,5 +28,6 @@ + diff --git a/sensors.xml.in b/sensors.xml.in index b97ec83..58f7fb4 100755 --- a/sensors.xml.in +++ b/sensors.xml.in @@ -366,5 +366,12 @@ + + + + + + + diff --git a/src/libsensord/CMakeLists.txt b/src/libsensord/CMakeLists.txt index bd56d4b..db78c9a 100755 --- a/src/libsensord/CMakeLists.txt +++ b/src/libsensord/CMakeLists.txt @@ -61,6 +61,7 @@ install(FILES sensor_gaming_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_fusion.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) +install(FILES sensor_ultraviolet.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES sensor_deprecated.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES sensor_uncal_gyro.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES ${PROJECT_NAME}.pc DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig) diff --git a/src/libsensord/client_common.cpp b/src/libsensord/client_common.cpp index 29d056f..5783414 100755 --- a/src/libsensord/client_common.cpp +++ b/src/libsensord/client_common.cpp @@ -46,6 +46,7 @@ log_element g_log_elements[] = { FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, FUSION_SENSOR, 0, 1), FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, TILT_SENSOR, 0, 1), FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, UNCAL_GYROSCOPE_SENSOR, 0, 1), + FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, ULTRAVIOLET_SENSOR, 0, 1), FILL_LOG_ELEMENT(LOG_ID_EVENT, GEOMAGNETIC_CALIBRATION_NEEDED_EVENT, 0, 1), FILL_LOG_ELEMENT(LOG_ID_EVENT, PROXIMITY_CHANGE_STATE_EVENT, 0,1), @@ -71,6 +72,7 @@ log_element g_log_elements[] = { FILL_LOG_ELEMENT(LOG_ID_EVENT, FUSION_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, TILT_RAW_DATA_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, UNCAL_GYRO_RAW_DATA_EVENT, 0, 10), + FILL_LOG_ELEMENT(LOG_ID_EVENT, ULTRAVIOLET_RAW_DATA_EVENT, 0, 10), }; typedef unordered_map log_map; diff --git a/src/libsensord/sensor_internal.h b/src/libsensord/sensor_internal.h index 477e6f0..7711d01 100755 --- a/src/libsensord/sensor_internal.h +++ b/src/libsensord/sensor_internal.h @@ -57,7 +57,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); typedef void (*sensorhub_cb_t)(sensor_t sensor, unsigned int event_type, sensorhub_data_t *data, void *user_data); diff --git a/src/libsensord/sensor_internal_deprecated.h b/src/libsensord/sensor_internal_deprecated.h index 85bcce7..35f60e7 100755 --- a/src/libsensord/sensor_internal_deprecated.h +++ b/src/libsensord/sensor_internal_deprecated.h @@ -56,6 +56,7 @@ extern "C" #include #include #include +#include #include #define MAX_KEY_LEN 30 diff --git a/src/ultraviolet/CMakeLists.txt b/src/ultraviolet/CMakeLists.txt index 2ec2925..218c8dd 100644 --- a/src/ultraviolet/CMakeLists.txt +++ b/src/ultraviolet/CMakeLists.txt @@ -26,5 +26,5 @@ add_library(${SENSOR_HAL_NAME} SHARED target_link_libraries(${SENSOR_NAME} ${uv_pkgs_LDFLAGS} "-lm") target_link_libraries(${SENSOR_HAL_NAME} ${uv_pkgs_LDFLAGS}) -install(TARGETS ${SENSOR_NAME} DESTINATION lib/sensord) -install(TARGETS ${SENSOR_HAL_NAME} DESTINATION lib/sensord) +install(TARGETS ${SENSOR_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR}/sensord) +install(TARGETS ${SENSOR_HAL_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR}/sensord) diff --git a/src/ultraviolet/ultraviolet_sensor.cpp b/src/ultraviolet/ultraviolet_sensor.cpp index c3f221e..c673c02 100755 --- a/src/ultraviolet/ultraviolet_sensor.cpp +++ b/src/ultraviolet/ultraviolet_sensor.cpp @@ -35,7 +35,7 @@ ultraviolet_sensor::ultraviolet_sensor() { m_name = string(SENSOR_NAME); - register_supported_event(ULTRAVIOLET_EVENT_RAW_DATA_REPORT_ON_TIME); + register_supported_event(ULTRAVIOLET_RAW_DATA_EVENT); physical_sensor::set_poller(ultraviolet_sensor::working, this); } @@ -54,9 +54,9 @@ bool ultraviolet_sensor::init() return false; } - sensor_properties_t properties; + sensor_properties_s properties; - if (!m_sensor_hal->get_properties(properties)) { + if (m_sensor_hal->get_properties(properties) == false) { ERR("sensor->get_properties() is failed!\n"); return false; } @@ -88,11 +88,12 @@ bool ultraviolet_sensor::process_event(void) m_sensor_hal->get_sensor_data(event.data); + AUTOLOCK(m_client_info_mutex); - if (get_client_cnt(ULTRAVIOLET_EVENT_RAW_DATA_REPORT_ON_TIME)) { + if (get_client_cnt(ULTRAVIOLET_RAW_DATA_EVENT)) { event.sensor_id = get_id(); - event.event_type = ULTRAVIOLET_EVENT_RAW_DATA_REPORT_ON_TIME; + event.event_type = ULTRAVIOLET_RAW_DATA_EVENT; raw_to_base(event.data); push(event); } @@ -120,7 +121,7 @@ bool ultraviolet_sensor::on_stop(void) return stop_poll(); } -bool ultraviolet_sensor::get_properties(sensor_properties_t &properties) +bool ultraviolet_sensor::get_properties(sensor_properties_s &properties) { return m_sensor_hal->get_properties(properties); } @@ -134,7 +135,7 @@ int ultraviolet_sensor::get_sensor_data(unsigned int type, sensor_data_t &data) if (ret < 0) return -1; - if (type == ULTRAVIOLET_BASE_DATA_SET) { + if (type == ULTRAVIOLET_RAW_DATA_EVENT) { raw_to_base(data); return 0; } @@ -153,21 +154,6 @@ bool ultraviolet_sensor::set_interval(unsigned long interval) void ultraviolet_sensor::raw_to_base(sensor_data_t &data) { - /* - double lsb = data.values[0]; - double comp_lsb; - - const double c = 0.89; - const double e = 2.12; - const double f = -132.8; - - if (lsb > 108.8f) - comp_lsb = e * lsb + f; - else - comp_lsb = c * lsb; - - data.values[0] = comp_lsb * m_resolution; - */ data.values[0] = data.values[0] * m_resolution; data.value_count = 1; } diff --git a/src/ultraviolet/ultraviolet_sensor.h b/src/ultraviolet/ultraviolet_sensor.h index 945af49..95f404b 100755 --- a/src/ultraviolet/ultraviolet_sensor.h +++ b/src/ultraviolet/ultraviolet_sensor.h @@ -31,14 +31,12 @@ public: virtual ~ultraviolet_sensor(); bool init(); - sensor_type_t get_type(void); + virtual sensor_type_t get_type(void); static bool working(void *inst); - - bool set_interval(unsigned long interval); - virtual bool get_properties(sensor_properties_t &properties); - int get_sensor_data(unsigned int type, sensor_data_t &data); - + virtual bool set_interval(unsigned long interval); + virtual bool get_properties(sensor_properties_s &properties); + virtual int get_sensor_data(unsigned int type, sensor_data_t &data); private: sensor_hal *m_sensor_hal; float m_resolution; diff --git a/src/ultraviolet/ultraviolet_sensor_hal.cpp b/src/ultraviolet/ultraviolet_sensor_hal.cpp index 0729a29..9c63734 100755 --- a/src/ultraviolet/ultraviolet_sensor_hal.cpp +++ b/src/ultraviolet/ultraviolet_sensor_hal.cpp @@ -27,7 +27,7 @@ using std::ifstream; -#define SENSOR_TYPE_ULTRAVIOLET "ULTRAVIOLET" +#define SENSOR_TYPE_ULTRAVIOLET "ULTRAVIOLET" #define ELEMENT_NAME "NAME" #define ELEMENT_VENDOR "VENDOR" #define ELEMENT_RAW_DATA_UNIT "RAW_DATA_UNIT" @@ -211,7 +211,7 @@ bool ultraviolet_sensor_hal::update_value(bool wait) ++read_input_cnt; - if (ultraviolet_event.type == EV_REL && ultraviolet_event.code == REL_MISC) { + if (ultraviolet_event.type == EV_REL && ultraviolet_event.code == REL_X) { ultraviolet_raw = (int)ultraviolet_event.value - BIAS; ultraviolet = true; } else if (ultraviolet_event.type == EV_SYN) { @@ -246,15 +246,14 @@ int ultraviolet_sensor_hal::get_sensor_data(sensor_data_t &data) { AUTOLOCK(m_value_mutex); data.accuracy = SENSOR_ACCURACY_GOOD; - data.timestamp = m_fired_time ; + data.timestamp = m_fired_time; data.value_count = 1; data.values[0] = (float) m_ultraviolet; return 0; } - -bool ultraviolet_sensor_hal::get_properties(sensor_properties_t &properties) +bool ultraviolet_sensor_hal::get_properties(sensor_properties_s &properties) { properties.name = m_chip_name; properties.vendor = m_vendor; diff --git a/src/ultraviolet/ultraviolet_sensor_hal.h b/src/ultraviolet/ultraviolet_sensor_hal.h index 87caf83..1925b7a 100755 --- a/src/ultraviolet/ultraviolet_sensor_hal.h +++ b/src/ultraviolet/ultraviolet_sensor_hal.h @@ -37,7 +37,8 @@ public: bool set_interval(unsigned long val); bool is_data_ready(bool wait); virtual int get_sensor_data(sensor_data_t &data); - virtual bool get_properties(sensor_properties_t &properties); + bool get_properties(sensor_properties_s &properties); + private: string m_model_id; string m_vendor; -- 2.7.4