From ecde90333a1ea21280753978b2b08fb46975f20f Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 19 Dec 2014 11:11:18 +0530 Subject: [PATCH 01/16] Adding description for Measurement update system Adding description for the sensor fusion measurement update system in html documentation. Change-Id: I7dc24f7940cfb8d607313fe9b3cd405abf1666e2 --- .../design/documentation/sensor_fusion.htm | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/src/sensor_fusion/design/documentation/sensor_fusion.htm b/src/sensor_fusion/design/documentation/sensor_fusion.htm index 231b129..0003b4b 100755 --- a/src/sensor_fusion/design/documentation/sensor_fusion.htm +++ b/src/sensor_fusion/design/documentation/sensor_fusion.htm @@ -140,9 +140,9 @@ previous time instant and a '0' specifies it as an initialization value.

-

The accelerometer and magnetometer data are normalized based on equations (3) +

The accelerometer and magnetometer data are normalized based on equations (3) and (4) to obtain the calibrated accelerometer data (Ax, Ay, Az) and magnetometer -data (Mx, My, Mz).

+data (Mx, My, Mz).

@@ -426,6 +426,12 @@ apriori state vector error Δx- and prediction covariance P- based in [1].<

3.5. Measurement Update System

+

The measurement update system is used to determine the bias value that would be +deducted from the Gyroscope values (6). The equations (31), (32) and (33) are used to +compute the Kalman gain K, aposteriori state error computation Δx+(i) and +aposteriori prediction covariance P+, as shown in [4]. In equation (33), I denotes the +identity matrix H denotes the measurement matrix (identity matrix is used here) and the +apriori prediction covariance estimate P- (33).

@@ -445,6 +451,12 @@ apriori state vector error Δx- and prediction covariance P- based in [1].<
+

The bias compensation (Bx, By, Bz) obtained from Δx+(i) is used for removing +dynamic bias from the calibrated gyroscope values as shown in (6). The corrected +orientation that is determined using the above sensor fusion method, is obtained from +(22) by using the conversion function quat2euler [9] as shown in (34). This estimated +orientation is used in Section 3 to compute Gravity virtual sensor data.

+
@@ -501,7 +513,7 @@ gravitational effect on the devices reference axis.

When the device tilt values (pitch,roll) are changed from (0,0) to (0,Π/2), phone is rotated around x-axis, the y-axis gets aligned to earth's gravitational field -after rotation instead of the z-axis. When this rotation is applied to the equations +after rotation instead of the z-axis. When this rotation is applied to the equations given above, the values (GRx,GRy,GRz) are converted from (0,0,G) to (0,G,0) due to the shift in the axis which experiences the gravitational field (G is measure of Earth's gravity).

-- 2.7.4 From b560d96dcf4f5a1217c295cb809ed279af737786 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Mon, 22 Dec 2014 09:27:20 +0530 Subject: [PATCH 02/16] Completed adding html documentation for sensor fusion Added all sections for sensor fusion html documentation. Change-Id: Ibb50bad292c6056b6dd8d37b80ca0a8dda88ae74 --- .../design/documentation/sensor_fusion.htm | 68 ++++++++++++++++------ 1 file changed, 51 insertions(+), 17 deletions(-) diff --git a/src/sensor_fusion/design/documentation/sensor_fusion.htm b/src/sensor_fusion/design/documentation/sensor_fusion.htm index 0003b4b..d2536c4 100755 --- a/src/sensor_fusion/design/documentation/sensor_fusion.htm +++ b/src/sensor_fusion/design/documentation/sensor_fusion.htm @@ -512,18 +512,20 @@ gravitational effect on the devices reference axis.

When the device tilt values (pitch,roll) are changed from (0,0) to (0,Π/2), -phone is rotated around x-axis, the y-axis gets aligned to earth's gravitational field +phone is rotated around y-axis, the x-axis gets aligned to earth's gravitational field after rotation instead of the z-axis. When this rotation is applied to the equations -given above, the values (GRx,GRy,GRz) are converted from (0,0,G) to (0,G,0) due to the +given above, the values (GRx,GRy,GRz) are converted from (0,0,G) to (G,0,0) due to the shift in the axis which experiences the gravitational field (G is measure of Earth's gravity).

Determination of Linear Acceleration

-

Linear Acceleration virtual sensor data provides the measure of the acceleration of -a device after removing the Gravity components on the 3-axes. Accurate linear -acceleration data are calculated by subtracting gravity components from the 3-axes -calibrated accelerometer data.

+

Accurate linear acceleration data are calculated by subtracting gravity components +from the 3-axes calibrated accelerometer data as shown in (38), (39) and (40). As shown +in (38) the rotation of the device on y-axis (GRy) reflects on the accelerometer x-axis +sensor data (Ax). The linear acceleration measurement on x-axis (LAx) directly +corresponds to the accelerometer x-axis sensor data (Ax) meaning linear motion along +x-axis is directly measured on the accelerometer x-axis.

@@ -546,21 +548,53 @@ calibrated accelerometer data.

References

-

[1] Gebre-Egziabher, H., Rhayward, R. C. & Powell, J. D. Design -of Multi-Sensor Attitude Determination Systems. IEEE Transactions on -Aerospace and Electronic Systems, (Volume: 40, Issue: 2), 627 - 649 (2004)

+

[1] Gebre-Egziabher, H., Rhayward, R. C. & Powell, J. D. Design of Multi-Sensor +Attitude Determination Systems. IEEE Transactions on AESS, 40(2), 627 - 649 (2004)

-

[2] Abyarjoo1, F., Barreto1, A., Cofino1, J. & Ortega, F. R., Implementing -a Sensor Fusion Algorithm for 3D Orientation Detection with Inertial/Magnetic -Sensors. The International Joint Conferences on CISSE (2012)

+

[2] Abyarjoo1, et al. Implementing a Sensor Fusion Algorithm for 3D Orientation +Detection with Inertial/Magnetic Sensors. The International Joint Conferences on CISSE +(2012)

[3] Marcard, T. V., Design and Implementation of an attitude estimation system to -control orthopedic components. Chalmers University. Master thesis published on -the university link http://publications.lib.chalmers.se/records/fulltext/125985.pdf(2010)

+control orthopedic components. Chalmers University. Master thesis published on the +university link http://publications.lib.chalmers.se/records/fulltext/125985.pdf(2010)

-

[4] Welch, G., Bishop, G. An introduction to the Kalman Filter: SIGGRAPH (2001)

+

[4] Welch, G. & Bishop, G. An introduction to the Kalman Filter. SIGGRAPH 2001

-

[5] Grewal, M. S., Weill, L. R., Andrews, A. P., Global Positioning Systems, -Inertial Navigation, and Integration (John Wiley & Sons., 2001)

+

[5] Grewal, M. S., et al. Global Positioning Systems, Inertial Navigation, and +Integration (John Wiley & Sons., 2001)

+ +

[6] Greenberg, M. J., Euclidean and non-Euclidean geometry: Development and History +(Freeman, 1980)

+ +

[7] Conway, J. H & Smith, D. A. On Quaternions and Octonions: Their Geometry, +Arithmetic, and Symmetry (Peters, 2003)

+ +

[8] Zill, D. G & Cullen, M. R. "Definition 7.4: Cross product of two vectors". +Advanced engineering mathematics (Jones & Bartlett Learning, 2006)

+ +

[9] NASA Mission Planning and Analysis Division. Euler Angles, Quaternions, and +Transformation Matrices. Document Link- +http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf. Last Accessed +- July 2014.

+ +

[10] Android Sensor Fusion Library. Source code link: +https://android.googlesource.com/platform/frameworks/native/+/b398927/services/sensorservice/. +Last Accessed - July 2014.

+ +

[11] GNU Octave High Level Interpreter. Software Link: http://www.gnu.org/software/octave/. +Last Accessed - July 2014.

+ +

[12] Tizen OS - https://www.tizen.org/. Last Accessed - July 2014.

+ +

[13] Marins, J. L., et al. An Extended Kalman Filter for Quaternion-Based Orientation +Estimation Using MARG Sensors. IEEE/RSJ International Conference on Intelligent Robots and +Systems (2001)

+ +

[14] Favre, J., et al. Quaternion-based fusion of gyroscopes and accelerometers to improve +3D angle measurement. ELECTRONICS LETTERS, 25th May 2006, Vol. 42 No. 11

+ +

[15] Sabatini, A.M., Quaternion based attitude estimation algorithm applied to signals +from body-mounted gyroscopes. ELECTRONICS LETTERS, 13th May 2004, Vol. 40 No. 10

-- 2.7.4 From 4ba155308b2ed3a2b65087e7f2bbb1b7087cde91 Mon Sep 17 00:00:00 2001 From: Ankur Date: Mon, 22 Dec 2014 14:21:48 +0530 Subject: [PATCH 03/16] Removed compiler warnings related to unused variable -changed the variable (MS_TO_US) declaration to a macro as done in other files this removes the warning in csensor_event_listener.cpp regarding unused variable. -similarly, based on this, changed all occurrences of (MIN_DELIVERY_DIFF_FACTOR) from variable declaration to macro this removes the warning in csensor_event_listener.cpp regarding unused variable. Change-Id: I98708ed5eca0d84dd959d5a59ad865f5381c0b26 --- src/gravity/gravity_sensor.cpp | 2 +- src/libsensord/csensor_event_listener.cpp | 6 +++--- src/linear_accel/linear_accel_sensor.cpp | 2 +- src/orientation/orientation_sensor.cpp | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/gravity/gravity_sensor.cpp b/src/gravity/gravity_sensor.cpp index 64e39e7..f10f502 100755 --- a/src/gravity/gravity_sensor.cpp +++ b/src/gravity/gravity_sensor.cpp @@ -42,6 +42,7 @@ #define SENSOR_TYPE_ORIENTATION "ORIENTATION" #define MS_TO_US 1000 +#define MIN_DELIVERY_DIFF_FACTOR 0.75f #define ELEMENT_NAME "NAME" #define ELEMENT_VENDOR "VENDOR" @@ -172,7 +173,6 @@ void gravity_sensor::synthesize(const sensor_event_t &event, vector #include +#define MS_TO_US 1000 +#define MIN_DELIVERY_DIFF_FACTOR 0.75f + using std::thread; using std::pair; @@ -649,9 +652,6 @@ client_callback_info* csensor_event_listener::handle_calibration_cb(csensor_hand void csensor_event_listener::handle_events(void* event) { - const unsigned int MS_TO_US = 1000; - const float MIN_DELIVERY_DIFF_FACTOR = 0.75f; - unsigned long long cur_time; creg_event_info *event_info = NULL; sensor_event_data_t event_data; diff --git a/src/linear_accel/linear_accel_sensor.cpp b/src/linear_accel/linear_accel_sensor.cpp index 96e6cce..1af20bc 100755 --- a/src/linear_accel/linear_accel_sensor.cpp +++ b/src/linear_accel/linear_accel_sensor.cpp @@ -47,6 +47,7 @@ #define GRAVITY 9.80665 #define MS_TO_US 1000 +#define MIN_DELIVERY_DIFF_FACTOR 0.75f #define ACCELEROMETER_ENABLED 0x01 #define GRAVITY_ENABLED 0x02 @@ -197,7 +198,6 @@ void linear_accel_sensor::synthesize(const sensor_event_t &event, vector &outs) { - const float MIN_DELIVERY_DIFF_FACTOR = 0.75f; unsigned long long diff_time; sensor_event_t orientation_event; -- 2.7.4 From d57be8cc5575106d79022feb5549dc918605f53d Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Mon, 22 Dec 2014 14:32:22 +0530 Subject: [PATCH 04/16] Adding polling based support for testing of temperature sensor Change-Id: I3ff0750bd160c3259062a4f91542b69947597b53 --- packaging/sensord.spec | 1 + test/CMakeLists.txt | 4 ++ test/src/temperature.c | 140 +++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 145 insertions(+) create mode 100644 test/src/temperature.c diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 1cab59a..1eaccdf 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -144,6 +144,7 @@ systemctl daemon-reload /usr/bin/gyro /usr/bin/proxi /usr/bin/pressure +/usr/bin/temperature %license LICENSE.APLv2 %{_datadir}/license/test diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 24b24ce..5c89818 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -35,6 +35,7 @@ add_executable(linear_acceleration src/linear_acceleration.c) add_executable(gyro src/gyro.c) add_executable(proxi src/proxi.c) add_executable(pressure src/pressure.c) +add_executable(temperature src/temperature.c) SET_TARGET_PROPERTIES(accelerometer PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(geomagnetic PROPERTIES LINKER_LANGUAGE C) @@ -44,6 +45,7 @@ SET_TARGET_PROPERTIES(linear_acceleration PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(gyro PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(proxi PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(pressure PROPERTIES LINKER_LANGUAGE C) +SET_TARGET_PROPERTIES(temperature PROPERTIES LINKER_LANGUAGE C) target_link_libraries(accelerometer glib-2.0 dlog sensor) target_link_libraries(geomagnetic glib-2.0 dlog sensor) @@ -53,6 +55,7 @@ target_link_libraries(linear_acceleration glib-2.0 dlog sensor) target_link_libraries(gyro glib-2.0 dlog sensor) target_link_libraries(proxi glib-2.0 dlog sensor) target_link_libraries(pressure glib-2.0 dlog sensor) +target_link_libraries(temperature glib-2.0 dlog sensor) INSTALL(TARGETS accelerometer DESTINATION /usr/bin/) INSTALL(TARGETS geomagnetic DESTINATION /usr/bin/) @@ -62,4 +65,5 @@ INSTALL(TARGETS linear_acceleration DESTINATION /usr/bin/) INSTALL(TARGETS gyro DESTINATION /usr/bin/) INSTALL(TARGETS proxi DESTINATION /usr/bin/) INSTALL(TARGETS pressure DESTINATION /usr/bin/) +INSTALL(TARGETS temperature DESTINATION /usr/bin/) diff --git a/test/src/temperature.c b/test/src/temperature.c new file mode 100644 index 0000000..ffdb2e9 --- /dev/null +++ b/test/src/temperature.c @@ -0,0 +1,140 @@ +/* + * sensord + * + * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#include +#include +#include +#include +#include +#include +#include +#include + +static GMainLoop *mainloop; + +void callback(unsigned int event_type, sensor_event_data_t *event, void *user_data) +{ + sensor_data_t *data = (sensor_data_t *)event->event_data; + printf("Temperature [%lld] [%6.6f] \n\n", data->timestamp, data->values[0]); +} + +void printformat() +{ + printf("Usage : ./temperature (optional) (optional)\n\n"); + + printf("mode:"); + printf("[-p]\n"); + printf("p is for polling based,default mode is event driven\n"); + + printf("event:"); + printf("[RAW_DATA_REPORT_ON_TIME]\n"); + + printf("interval:\n"); + printf("The time interval should be entered based on the sampling frequency supported by temperature driver on the device in ms.If no value for sensor is entered default value by the driver will be used.\n"); +} + +int main(int argc,char **argv) +{ + int result, handle, start_handle, stop_handle; + unsigned int event; + mainloop = g_main_loop_new(NULL, FALSE); + event = TEMPERATURE_EVENT_RAW_DATA_REPORT_ON_TIME; + event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); + event_condition->cond_op = CONDITION_EQUAL; + + sensor_type_t type = TEMPERATURE_SENSOR; + + if (argc != 2 && argc != 3 && argc!=4) { + printformat(); + free(event_condition); + return 0; + } + + else if (argc>=3 && strcmp(argv[1], "-p") == 0 && strcmp(argv[2], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Polling based\n"); + handle = sf_connect(type); + result = sf_start(handle, 1); + + if (result < 0) { + printf("Can't start temperature SENSOR\n"); + printf("Error\n\n\n\n"); + return -1; + } + + sensor_data_t data; + + while(1) { + result = sf_get_data(handle, TEMPERATURE_BASE_DATA_SET , &data); + printf("Temperature [%6.6f] \n\n", data.values[0]); + usleep(100000); + } + + result = sf_disconnect(handle); + + if (result < 0) { + printf("Can't disconnect temperature sensor\n"); + printf("Error\n\n\n\n"); + return -1; + } + } + + else if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Event based\n"); + + event_condition->cond_value1 = 100; + if (argc == 3) + event_condition->cond_value1 = atof(argv[2]); + + handle = sf_connect(type); + result = sf_register_event(handle, event, event_condition, callback, NULL); + + if (result < 0) + printf("Can't register temperature\n"); + + start_handle = sf_start(handle,0); + + if (start_handle < 0) { + printf("Error\n\n\n\n"); + sf_unregister_event(handle, event); + sf_disconnect(handle); + return -1; + } + + g_main_loop_run(mainloop); + g_main_loop_unref(mainloop); + + sf_unregister_event(handle, event); + + stop_handle = sf_stop(handle); + + if (stop_handle < 0) { + printf("Error\n\n"); + return -1; + } + + sf_disconnect(handle); + free(event_condition); + } + + else { + printformat(); + } + + return 0; +} + -- 2.7.4 From d9f0239a7425ff1d549104dbde260fa28624f19c Mon Sep 17 00:00:00 2001 From: Ankur Date: Mon, 22 Dec 2014 15:12:25 +0530 Subject: [PATCH 05/16] Removed warnings related to comparison between signed and unsigned int -converted int variables to unsinged int to remove warnings for comparison between signed and unsigned integers. Change-Id: I3e0187d002b19b215a5d542d1206ba51ef663368 --- src/shared/csensor_event_dispatcher.cpp | 2 +- src/shared/sensor_info.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/shared/csensor_event_dispatcher.cpp b/src/shared/csensor_event_dispatcher.cpp index f9f90d5..cee0cf3 100755 --- a/src/shared/csensor_event_dispatcher.cpp +++ b/src/shared/csensor_event_dispatcher.cpp @@ -171,7 +171,7 @@ void csensor_event_dispatcher::dispatch_event(void) sort_sensor_events(sensor_events, event_cnt); - for (int i = 0; i < event_cnt; ++i) { + for (unsigned int i = 0; i < event_cnt; ++i) { if (is_record_event(sensor_events[i].event_type)) put_last_event(sensor_events[i].event_type, sensor_events[i]); } diff --git a/src/shared/sensor_info.cpp b/src/shared/sensor_info.cpp index 56da0c9..0521aea 100755 --- a/src/shared/sensor_info.cpp +++ b/src/shared/sensor_info.cpp @@ -211,8 +211,8 @@ void sensor_info::show(void) INFO("Fifo_count = %d", m_fifo_count); INFO("Max_batch_count = %d", m_max_batch_count); - for (int i = 0; i < m_supported_events.size(); ++i) - INFO("supported_events[%d] = 0x%x", i, m_supported_events[i]); + for (unsigned int i = 0; i < m_supported_events.size(); ++i) + INFO("supported_events[%u] = 0x%x", i, m_supported_events[i]); } -- 2.7.4 From 0258205da40c08ac1b7ff1e2f2041682e1edafd9 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Mon, 22 Dec 2014 15:46:46 +0530 Subject: [PATCH 06/16] Adding orientation filter to support quaternion output - orientation_filter would output device orientation as quaternion to support rotation_vector virtual sensor. Change-Id: I5e0cdcb1b218c59d32965deac76629f04c728899 --- src/sensor_fusion/orientation_filter.cpp | 13 +++++++++++++ src/sensor_fusion/orientation_filter.h | 3 +++ src/sensor_fusion/standalone/orientation_sensor.cpp | 18 +++++++++++++++++- src/sensor_fusion/standalone/orientation_sensor.h | 2 ++ .../orientation_sensor_main.cpp | 9 +++++++-- 5 files changed, 42 insertions(+), 3 deletions(-) diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index a615217..05537a3 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -218,8 +218,11 @@ inline void orientation_filter::time_update() 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_quaternion = quat_output; + orientation = quat2euler(quat_output); m_orientation.m_ang.m_vec[0] = orientation.m_ang.m_vec[0] * m_pitch_phase_compensation; @@ -313,4 +316,14 @@ rotation_matrix orientation_filter::get_rotation_matrix(const sensor return m_rot_matrix; } +template +quaternion orientation_filter::get_quaternion(const sensor_data accel, + const sensor_data gyro, const sensor_data magnetic) +{ + + get_orientation(accel, gyro, magnetic); + + return m_quaternion; +} + #endif //_ORIENTATION_FILTER_H_ diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h index f7bc4d7..40e2c50 100644 --- a/src/sensor_fusion/orientation_filter.h +++ b/src/sensor_fusion/orientation_filter.h @@ -52,6 +52,7 @@ public: quaternion m_quat_driv; rotation_matrix m_rot_matrix; euler_angles m_orientation; + quaternion m_quaternion; TYPE m_gyro_dt; int m_pitch_phase_compensation; @@ -73,6 +74,8 @@ public: const sensor_data gyro, const sensor_data magnetic); rotation_matrix get_rotation_matrix(const sensor_data accel, const sensor_data gyro, const sensor_data magnetic); + quaternion get_quaternion(const sensor_data accel, + const sensor_data gyro, const sensor_data magnetic); }; #include "orientation_filter.cpp" diff --git a/src/sensor_fusion/standalone/orientation_sensor.cpp b/src/sensor_fusion/standalone/orientation_sensor.cpp index c006922..82c227f 100644 --- a/src/sensor_fusion/standalone/orientation_sensor.cpp +++ b/src/sensor_fusion/standalone/orientation_sensor.cpp @@ -64,7 +64,6 @@ euler_angles orientation_sensor::get_orientation(sensor_data accel rotation_matrix orientation_sensor::get_rotation_matrix(sensor_data accel_data, sensor_data gyro_data, sensor_data magnetic_data) { - pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); normalize(accel_data); pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro); @@ -79,4 +78,21 @@ rotation_matrix orientation_sensor::get_rotation_matrix(sensor_data orientation_sensor::get_quaternion(sensor_data accel_data, + sensor_data gyro_data, sensor_data magnetic_data) +{ + pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); + normalize(accel_data); + pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro); + 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; + + return orien_filter.get_quaternion(accel_data, gyro_data, magnetic_data); +} + #endif diff --git a/src/sensor_fusion/standalone/orientation_sensor.h b/src/sensor_fusion/standalone/orientation_sensor.h index a02e2af..1ead53e 100644 --- a/src/sensor_fusion/standalone/orientation_sensor.h +++ b/src/sensor_fusion/standalone/orientation_sensor.h @@ -31,6 +31,8 @@ public: sensor_data gyro, sensor_data magnetic); rotation_matrix get_rotation_matrix(sensor_data accel, sensor_data gyro, sensor_data magnetic); + quaternion get_quaternion(sensor_data accel, + sensor_data gyro, sensor_data magnetic); }; #include "orientation_sensor.cpp" diff --git a/src/sensor_fusion/standalone/test/orientation_sensor_test/orientation_sensor_main.cpp b/src/sensor_fusion/standalone/test/orientation_sensor_test/orientation_sensor_main.cpp index b3ef1f5..12058e0 100644 --- a/src/sensor_fusion/standalone/test/orientation_sensor_test/orientation_sensor_main.cpp +++ b/src/sensor_fusion/standalone/test/orientation_sensor_test/orientation_sensor_main.cpp @@ -37,7 +37,8 @@ int main() unsigned long long time_stamp; euler_angles orientation; rotation_matrix orientation_mat; - orientation_sensor orien_sensor1, orien_sensor2; + quaternion orientation_quat; + orientation_sensor orien_sensor1, orien_sensor2, orien_sensor3; accel_in.open(((string)ORIENTATION_DATA_PATH + (string)"accel.txt").c_str()); gyro_in.open(((string)ORIENTATION_DATA_PATH + (string)"gyro.txt").c_str()); @@ -82,9 +83,13 @@ int main() cout << "Orientation angles\t" << orientation.m_ang << "\n\n"; - orientation_mat = orien_sensor1.get_rotation_matrix(accel_data, gyro_data, magnetic_data); + orientation_mat = orien_sensor2.get_rotation_matrix(accel_data, gyro_data, magnetic_data); cout << "Orientation matrix\t" << orientation_mat.m_rot_mat << "\n\n"; + + orientation_quat = orien_sensor3.get_quaternion(accel_data, gyro_data, magnetic_data); + + cout << "Orientation quaternion\t" << orientation_quat.m_quat << "\n\n"; } accel_in.close(); -- 2.7.4 From 11824fe5196cc1f71b42ef9d612dd9a83acd4b75 Mon Sep 17 00:00:00 2001 From: Ankur Date: Mon, 22 Dec 2014 16:37:30 +0530 Subject: [PATCH 07/16] Removed compiler warning for converting bool 'false' to pointer 'void *' -In the function sensor_get_sensor() in src/libsensord/client.cpp boolean 'false' was being converted to void * This was showing as a warning at compile time. For returning failed cases, NULL should be returned. The call to sensor_info_to_sensor in this function also returns NULL when failed. -Tested the code after change. Seems to be working fine. Change-Id: Id100370c9bc8b9f62c86acb6968f766b10370d7b --- src/libsensord/client.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/libsensord/client.cpp b/src/libsensord/client.cpp index 9acf884..2f1e649 100755 --- a/src/libsensord/client.cpp +++ b/src/libsensord/client.cpp @@ -423,7 +423,7 @@ API bool sensord_get_sensor_list(sensor_type_t type, sensor_t **list, int *senso API sensor_t sensord_get_sensor(sensor_type_t type) { - retvm_if (!get_sensor_list(), false, "Fail to get sensor list from server"); + retvm_if (!get_sensor_list(), NULL, "Fail to get sensor list from server"); const sensor_info *info; -- 2.7.4 From a7cede0b57291d01d4c196ca6af9a32481241d62 Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Mon, 22 Dec 2014 17:28:25 +0530 Subject: [PATCH 08/16] Adding polling based support for testing accelerometer Change-Id: Id1a4343e8ad9ca44d9bb5a1027526ed50f88b6e2 --- test/src/accelerometer.c | 108 ++++++++++++++++++++++++++++++----------------- 1 file changed, 70 insertions(+), 38 deletions(-) diff --git a/test/src/accelerometer.c b/test/src/accelerometer.c index c7ea6e6..fe61f37 100644 --- a/test/src/accelerometer.c +++ b/test/src/accelerometer.c @@ -22,7 +22,8 @@ #include #include #include -#include +#include +#include static GMainLoop *mainloop; @@ -34,10 +35,14 @@ void callback(unsigned int event_type, sensor_event_data_t *event, void *user_da void printformat() { - printf("Usage : ./accelerometer (optional)\n\n"); - printf("event:\n"); + printf("Usage : ./accelerometer (optional) (optional)\n\n"); - printf("RAW_DATA_REPORT_ON_TIME\n"); + printf("mode:"); + printf("[-p]\n"); + printf("p is for polling based,default mode is event driven\n"); + + printf("event:"); + printf("[RAW_DATA_REPORT_ON_TIME]\n"); printf("interval:\n"); printf("The time interval should be entered based on the sampling frequency supported by accelerometer driver on the device in ms.If no value for sensor is entered default value by the driver will be used.\n"); @@ -47,61 +52,88 @@ int main(int argc,char **argv) { int result, handle, start_handle, stop_handle; unsigned int event; - mainloop = g_main_loop_new(NULL, FALSE); - sensor_type_t type = ACCELEROMETER_SENSOR; + event = ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME; event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); event_condition->cond_op = CONDITION_EQUAL; - event_condition->cond_value1 = 100; - if (argc != 2 && argc != 3) { + sensor_type_t type = ACCELEROMETER_SENSOR; + + if (argc != 2 && argc != 3 && argc!=4) { printformat(); free(event_condition); return 0; } - if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) { - event = ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME; - } + else if (argc>=3 && strcmp(argv[1], "-p") == 0 && strcmp(argv[2], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Polling based\n"); + handle = sf_connect(type); + result = sf_start(handle, 1); - else { - printformat(); - free(event_condition); - return 0; - } - if (argc == 3) - event_condition->cond_value1 = atof(argv[2]); + if (result < 0) { + printf("Can't start accelerometer SENSOR\n"); + printf("Error\n\n\n\n"); + return -1; + } - handle = sf_connect(type); - result = sf_register_event(handle, event, event_condition, callback, NULL); + sensor_data_t data; - if (result < 0) - printf("Can't register accelerometer\n"); + while(1) { + result = sf_get_data(handle, ACCELEROMETER_BASE_DATA_SET , &data); + printf("Accelerometer [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2]); + usleep(100000); + } - start_handle = sf_start(handle,0); + result = sf_disconnect(handle); - if (start_handle < 0) { - printf("Error\n\n\n\n"); - sf_unregister_event(handle, event); - sf_disconnect(handle); - return -1; + if (result < 0) { + printf("Can't disconnect Accelerometer sensor\n"); + printf("Error\n\n\n\n"); + return -1; + } } - g_main_loop_run(mainloop); - g_main_loop_unref(mainloop); + else if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Event based\n"); - sf_unregister_event(handle, event); + event_condition->cond_value1 = 100; + if (argc == 3) + event_condition->cond_value1 = atof(argv[2]); - stop_handle = sf_stop(handle); + handle = sf_connect(type); + result = sf_register_event(handle, event, event_condition, callback, NULL); - if (stop_handle < 0) { - printf("Error\n\n"); - return -1; - } + if (result < 0) + printf("Can't register accelerometer\n"); - sf_disconnect(handle); + start_handle = sf_start(handle,0); - free(event_condition); + if (start_handle < 0) { + printf("Error\n\n\n\n"); + sf_unregister_event(handle, event); + sf_disconnect(handle); + return -1; + } + + g_main_loop_run(mainloop); + g_main_loop_unref(mainloop); + + sf_unregister_event(handle, event); + + stop_handle = sf_stop(handle); + + if (stop_handle < 0) { + printf("Error\n\n"); + return -1; + } + + sf_disconnect(handle); + free(event_condition); + } + + else { + printformat(); + } return 0; } -- 2.7.4 From 1efa8724e74e955ac8402718d58677c970c91c75 Mon Sep 17 00:00:00 2001 From: Ankur Date: Mon, 22 Dec 2014 17:35:34 +0530 Subject: [PATCH 09/16] Removed warnings related to unused variables -Removed two unused variables. Both variables were never being intialised or being assigned or being used anywhere in the code. The variables were just declared. Safe to remove such variables. Change-Id: I998a1c0c2978d48150c1b15e9b0cc083e430c73a --- src/libsensord/client.cpp | 2 +- src/orientation/orientation_sensor.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/libsensord/client.cpp b/src/libsensord/client.cpp index 2f1e649..79245d7 100755 --- a/src/libsensord/client.cpp +++ b/src/libsensord/client.cpp @@ -108,7 +108,7 @@ static void clean_up(void) static int get_power_save_state (void) { int state = 0; - int pm_state, ps_state; + int pm_state; vconf_get_int(VCONFKEY_PM_STATE, &pm_state); diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp index 325c004..dd0f818 100755 --- a/src/orientation/orientation_sensor.cpp +++ b/src/orientation/orientation_sensor.cpp @@ -295,7 +295,6 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vector euler_orientation; - float raw_data[3]; float azimuth_offset; if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) { -- 2.7.4 From ddbc8c8396afda438ec3907db2004ea555584eb0 Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Mon, 22 Dec 2014 17:39:15 +0530 Subject: [PATCH 10/16] Adding polling based support for testing gyroscope sensor Change-Id: Ibff596783a3d5c69f8fedc284df2f46cea144315 --- test/src/gyro.c | 112 ++++++++++++++++++++++++++++++++++++-------------------- 1 file changed, 73 insertions(+), 39 deletions(-) diff --git a/test/src/gyro.c b/test/src/gyro.c index ae34d95..b74d98d 100644 --- a/test/src/gyro.c +++ b/test/src/gyro.c @@ -16,28 +16,34 @@ * limitations under the License. * */ - #include #include #include #include #include #include -#include +#include +#include static GMainLoop *mainloop; void callback(unsigned int event_type, sensor_event_data_t *event, void *user_data) { sensor_data_t *data = (sensor_data_t *)event->event_data; - printf("Gyroscope [%lld] [%6.6f] [%6.6f] [%6.6f] \n\n", data->timestamp, data->values[0], data->values[1], data->values[2]); + printf("Gyroscope [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data->timestamp, data->values[0], data->values[1], data->values[2]); } void printformat() { - printf("Usage : ./gyro (optional)\n\n"); - printf("event:\n"); - printf("RAW_DATA_REPORT_ON_TIME\n"); + printf("Usage : ./gyro (optional) (optional)\n\n"); + + printf("mode:"); + printf("[-p]\n"); + printf("p is for polling based,default mode is event driven\n"); + + printf("event:"); + printf("[RAW_DATA_REPORT_ON_TIME]\n"); + printf("interval:\n"); printf("The time interval should be entered based on the sampling frequency supported by gyroscope driver on the device in ms.If no value for sensor is entered default value by the driver will be used.\n"); } @@ -46,61 +52,89 @@ int main(int argc,char **argv) { int result, handle, start_handle, stop_handle; unsigned int event; - mainloop = g_main_loop_new(NULL, FALSE); - sensor_type_t type = GYROSCOPE_SENSOR; + event = GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME; event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); event_condition->cond_op = CONDITION_EQUAL; - event_condition->cond_value1 = 10.52; - if (argc != 2 && argc != 3) { + sensor_type_t type = GYROSCOPE_SENSOR; + + if (argc != 2 && argc != 3 && argc!=4) { printformat(); free(event_condition); return 0; } - else { - if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) - event = GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME; - else { - printformat(); - free(event_condition); - return 0; + else if (argc>=3 && strcmp(argv[1], "-p") == 0 && strcmp(argv[2], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Polling based\n"); + + handle = sf_connect(type); + result = sf_start(handle, 1); + + if (result < 0) { + printf("Can't start gyroscope SENSOR\n"); + printf("Error\n\n\n\n"); + return -1; } - if(argc == 3) - event_condition->cond_value1 = atof(argv[2]); + sensor_data_t data; + + while(1) { + result = sf_get_data(handle, GYRO_BASE_DATA_SET , &data); + printf("Gyroscope [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2]); + usleep(100000); + } + + result = sf_disconnect(handle); + + if (result < 0) { + printf("Can't disconnect gyroscope sensor\n"); + printf("Error\n\n\n\n"); + return -1; + } } - handle = sf_connect(type); - result = sf_register_event(handle, event, event_condition, callback, NULL); + else if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Event based\n"); + + event_condition->cond_value1 = 10.52; + if (argc == 3) + event_condition->cond_value1 = atof(argv[2]); + + handle = sf_connect(type); + result = sf_register_event(handle, event, event_condition, callback, NULL); - if (result < 0) - printf("Can't register gyroscope\n"); + if (result < 0) + printf("Can't register gyroscope\n"); - start_handle = sf_start(handle, 0); + start_handle = sf_start(handle,0); + + if (start_handle < 0) { + printf("Error\n\n\n\n"); + sf_unregister_event(handle, event); + sf_disconnect(handle); + return -1; + } + + g_main_loop_run(mainloop); + g_main_loop_unref(mainloop); - if (start_handle < 0) { - printf("Error\n\n\n\n"); sf_unregister_event(handle, event); - sf_disconnect(handle); - return -1; - } - g_main_loop_run(mainloop); - g_main_loop_unref(mainloop); + stop_handle = sf_stop(handle); - sf_unregister_event(handle, event); - stop_handle = sf_stop(handle); + if (stop_handle < 0) { + printf("Error\n\n"); + return -1; + } - if (stop_handle < 0) { - printf("Error\n\n"); - return -1; + sf_disconnect(handle); + free(event_condition); } - sf_disconnect(handle); - - free(event_condition); + else { + printformat(); + } return 0; } -- 2.7.4 From a1ae1f819ee4830c3e565863b1fb4d75efdf3c7d Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Mon, 22 Dec 2014 17:50:40 +0530 Subject: [PATCH 11/16] Adding polling based support for testing pressure sensor Change-Id: I843b060db3f28a76cbac33c7deebb701055e8681 --- test/src/pressure.c | 111 ++++++++++++++++++++++++++++++++++------------------ 1 file changed, 73 insertions(+), 38 deletions(-) diff --git a/test/src/pressure.c b/test/src/pressure.c index e3ed4b4..a92ad59 100644 --- a/test/src/pressure.c +++ b/test/src/pressure.c @@ -16,14 +16,14 @@ * limitations under the License. * */ - #include #include #include #include #include #include -#include +#include +#include static GMainLoop *mainloop; @@ -35,71 +35,106 @@ void callback(unsigned int event_type, sensor_event_data_t *event, void *user_da void printformat() { - printf("Usage : ./pressure (optional)\n\n"); - printf("event:\n"); - printf("RAW_DATA_REPORT_ON_TIME\n"); + printf("Usage : ./pressure (optional) (optional)\n\n"); + + printf("mode:"); + printf("[-p]\n"); + printf("p is for polling based,default mode is event driven\n"); + + printf("event:"); + printf("[RAW_DATA_REPORT_ON_TIME]\n"); + printf("interval:\n"); printf("The time interval should be entered based on the sampling frequency supported by pressure driver on the device in ms.If no value for sensor is entered default value by the driver will be used.\n"); } -int main(int argc, char **argv) +int main(int argc,char **argv) { int result, handle, start_handle, stop_handle; unsigned int event; - mainloop = g_main_loop_new(NULL, FALSE); - sensor_type_t type = PRESSURE_SENSOR; + event = PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME; event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); event_condition->cond_op = CONDITION_EQUAL; - event_condition->cond_value1 = 100; - if (argc != 2 && argc != 3) { - printformat(); - free(event_condition); - return 0; - } + sensor_type_t type = PRESSURE_SENSOR; - if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") != 0) { + if (argc != 2 && argc != 3 && argc!=4) { printformat(); free(event_condition); return 0; } - event = PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME; + else if (argc>=3 && strcmp(argv[1], "-p") == 0 && strcmp(argv[2], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Polling based\n"); - if (argc == 3) - event_condition->cond_value1 = atof(argv[2]); + handle = sf_connect(type); + result = sf_start(handle, 1); - handle = sf_connect(type); - result = sf_register_event(handle, event, event_condition, callback, NULL); + if (result < 0) { + printf("Can't start pressure SENSOR\n"); + printf("Error\n\n\n\n"); + return -1; + } - if (result < 0) - printf("Can't register pressure\n"); + sensor_data_t data; - start_handle = sf_start(handle,0); + while(1) { + result = sf_get_data(handle, PRESSURE_BASE_DATA_SET , &data); + printf("Pressure [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2]); + usleep(100000); + } - if (start_handle < 0) { - printf("Error\n\n\n\n"); - sf_unregister_event(handle, event); - sf_disconnect(handle); - return -1; + result = sf_disconnect(handle); + + if (result < 0) { + printf("Can't disconnect pressure sensor\n"); + printf("Error\n\n\n\n"); + return -1; + } } - g_main_loop_run(mainloop); - g_main_loop_unref(mainloop); + else if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Event based\n"); - sf_unregister_event(handle, event); + event_condition->cond_value1 = 10.52; + if (argc == 3) + event_condition->cond_value1 = atof(argv[2]); - stop_handle = sf_stop(handle); + handle = sf_connect(type); + result = sf_register_event(handle, event, event_condition, callback, NULL); - if (stop_handle < 0) { - printf("Error\n\n"); - return -1; - } + if (result < 0) + printf("Can't register pressure\n"); + + start_handle = sf_start(handle,0); + + if (start_handle < 0) { + printf("Error\n\n\n\n"); + sf_unregister_event(handle, event); + sf_disconnect(handle); + return -1; + } + + g_main_loop_run(mainloop); + g_main_loop_unref(mainloop); + + sf_unregister_event(handle, event); - sf_disconnect(handle); + stop_handle = sf_stop(handle); - free(event_condition); + if (stop_handle < 0) { + printf("Error\n\n"); + return -1; + } + + sf_disconnect(handle); + free(event_condition); + } + + else { + printformat(); + } return 0; } -- 2.7.4 From 47655267960bc72d0b3be5fc495a991dbb982fb2 Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Mon, 22 Dec 2014 18:04:12 +0530 Subject: [PATCH 12/16] Adding polling based support for testing of light sensor Change-Id: Iab85d66ad648a2ecd5669938b448728ddfe898c7 --- packaging/sensord.spec | 1 + test/CMakeLists.txt | 4 ++ test/src/light.c | 140 +++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 145 insertions(+) create mode 100644 test/src/light.c diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 1eaccdf..2e1fff7 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -145,6 +145,7 @@ systemctl daemon-reload /usr/bin/proxi /usr/bin/pressure /usr/bin/temperature +/usr/bin/light %license LICENSE.APLv2 %{_datadir}/license/test diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 5c89818..3c103cc 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -36,6 +36,7 @@ add_executable(gyro src/gyro.c) add_executable(proxi src/proxi.c) add_executable(pressure src/pressure.c) add_executable(temperature src/temperature.c) +add_executable(light src/light.c) SET_TARGET_PROPERTIES(accelerometer PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(geomagnetic PROPERTIES LINKER_LANGUAGE C) @@ -46,6 +47,7 @@ SET_TARGET_PROPERTIES(gyro PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(proxi PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(pressure PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(temperature PROPERTIES LINKER_LANGUAGE C) +SET_TARGET_PROPERTIES(light PROPERTIES LINKER_LANGUAGE C) target_link_libraries(accelerometer glib-2.0 dlog sensor) target_link_libraries(geomagnetic glib-2.0 dlog sensor) @@ -56,6 +58,7 @@ target_link_libraries(gyro glib-2.0 dlog sensor) target_link_libraries(proxi glib-2.0 dlog sensor) target_link_libraries(pressure glib-2.0 dlog sensor) target_link_libraries(temperature glib-2.0 dlog sensor) +target_link_libraries(light glib-2.0 dlog sensor) INSTALL(TARGETS accelerometer DESTINATION /usr/bin/) INSTALL(TARGETS geomagnetic DESTINATION /usr/bin/) @@ -66,4 +69,5 @@ INSTALL(TARGETS gyro DESTINATION /usr/bin/) INSTALL(TARGETS proxi DESTINATION /usr/bin/) INSTALL(TARGETS pressure DESTINATION /usr/bin/) INSTALL(TARGETS temperature DESTINATION /usr/bin/) +INSTALL(TARGETS light DESTINATION /usr/bin/) diff --git a/test/src/light.c b/test/src/light.c new file mode 100644 index 0000000..454d5c2 --- /dev/null +++ b/test/src/light.c @@ -0,0 +1,140 @@ +/* + * sensord + * + * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#include +#include +#include +#include +#include +#include +#include +#include + +static GMainLoop *mainloop; + +void callback(unsigned int event_type, sensor_event_data_t *event, void *user_data) +{ + sensor_data_t *data = (sensor_data_t *)event->event_data; + printf("Light :[%lld] [%6.6f]\n", data->timestamp, data->values[0]); +} + +void printformat() +{ + printf("Usage : ./light (optional) (optional)\n\n"); + + printf("mode:"); + printf("[-p]\n"); + printf("p is for polling based,default mode is event driven\n"); + + printf("event:"); + printf("[RAW_DATA_REPORT_ON_TIME]\n"); + + printf("interval:\n"); + printf("The time interval should be entered based on the sampling frequency supported by light driver on the device in ms.If no value for sensor is entered default value by the driver will be used.\n"); +} + +int main(int argc,char **argv) +{ + int result, handle, start_handle, stop_handle; + unsigned int event; + mainloop = g_main_loop_new(NULL, FALSE); + event = LIGHT_EVENT_LUX_DATA_REPORT_ON_TIME; + event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); + event_condition->cond_op = CONDITION_EQUAL; + + sensor_type_t type = LIGHT_SENSOR; + + if (argc != 2 && argc != 3 && argc!=4) { + printformat(); + free(event_condition); + return 0; + } + + else if (argc>=3 && strcmp(argv[1], "-p") == 0 && strcmp(argv[2], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Polling based\n"); + + handle = sf_connect(type); + result = sf_start(handle, 1); + + if (result < 0) { + printf("Can't start light SENSOR\n"); + printf("Error\n\n\n\n"); + return -1; + } + + sensor_data_t data; + + while(1) { + result = sf_get_data(handle, LIGHT_LUX_DATA_SET , &data); + printf("Light : [%6.6f]\n", data.values[0]); + usleep(100000); + } + + result = sf_disconnect(handle); + if (result < 0) { + printf("Can't disconnect light sensor\n"); + printf("Error\n\n\n\n"); + return -1; + } + } + + else if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Event based\n"); + + event_condition->cond_value1 = 100; + if (argc == 3) + event_condition->cond_value1 = atof(argv[2]); + + handle = sf_connect(type); + result = sf_register_event(handle, event, event_condition, callback, NULL); + + if (result < 0) + printf("Can't register light\n"); + + start_handle = sf_start(handle,0); + + if (start_handle < 0) { + printf("Error\n\n\n\n"); + sf_unregister_event(handle, event); + sf_disconnect(handle); + return -1; + } + + g_main_loop_run(mainloop); + g_main_loop_unref(mainloop); + + sf_unregister_event(handle, event); + + stop_handle = sf_stop(handle); + + if (stop_handle < 0) { + printf("Error\n\n"); + return -1; + } + + sf_disconnect(handle); + free(event_condition); + } + + else { + printformat(); + } + + return 0; +} + -- 2.7.4 From 2b036ab72d4c1988623977d4e6aaa382c110bf2f Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Mon, 22 Dec 2014 18:15:52 +0530 Subject: [PATCH 13/16] Adding polling based support for testing proximity sensor Change-Id: I0a5c82eb75a615348a348f56bf9254bc0d4f17c9 --- test/src/proxi.c | 154 +++++++++++++++++++++++++++++++++++++------------------ 1 file changed, 105 insertions(+), 49 deletions(-) diff --git a/test/src/proxi.c b/test/src/proxi.c index 9ac965f..70185bd 100644 --- a/test/src/proxi.c +++ b/test/src/proxi.c @@ -16,14 +16,14 @@ * limitations under the License. * */ - #include #include #include #include #include #include -#include +#include +#include static GMainLoop *mainloop; @@ -35,11 +35,17 @@ void callback(unsigned int event_type, sensor_event_data_t *event, void *user_da void printformat() { - printf("Usage : ./proxi (optional)\n\n"); - printf("event:\n"); - printf("EVENT_CHANGE_STATE\n"); - printf("EVENT_STATE_REPORT_ON_TIME\n"); - printf("EVENT_DISTANCE_DATA_REPORT_ON_TIME\n"); + printf("Usage : ./proxi (optional) (optional)\n\n"); + + printf("mode:"); + printf("[-p]\n"); + printf("p is for polling based,default mode is event driven\n"); + + printf("event:"); + printf("[EVENT_CHANGE_STATE] "); + printf("[EVENT_STATE_REPORT_ON_TIME] "); + printf("[EVENT_DISTANCE_DATA_REPORT_ON_TIME]\n"); + printf("interval:\n"); printf("The time interval should be entered based on the sampling frequency supported by proximity driver on the device in ms.If no value for sensor is entered default value by the driver will be used.\n"); } @@ -48,68 +54,118 @@ int main(int argc,char **argv) { int result, handle, start_handle, stop_handle; unsigned int event; - mainloop = g_main_loop_new(NULL, FALSE); - sensor_type_t type = PROXIMITY_SENSOR; event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); event_condition->cond_op = CONDITION_EQUAL; - event_condition->cond_value1 = 100; + sensor_type_t type = PROXIMITY_SENSOR; - if (argc != 2 && argc != 3) { + if (argc != 2 && argc != 3 && argc!=4) { printformat(); free(event_condition); return 0; } - if (strcmp(argv[1], "EVENT_CHANGE_STATE") == 0) { - event = PROXIMITY_EVENT_CHANGE_STATE; - } - else if (strcmp(argv[1], "EVENT_STATE_REPORT_ON_TIME") == 0) { - event = PROXIMITY_EVENT_STATE_REPORT_ON_TIME; - } - else if (strcmp(argv[1], "EVENT_DISTANCE_DATA_REPORT_ON_TIME") == 0) { - event = PROXIMITY_EVENT_DISTANCE_DATA_REPORT_ON_TIME; - } - else { - printformat(); - free(event_condition); - return 0; + else if (argc>=3 && strcmp(argv[1], "-p") == 0) { + printf("Polling based\n"); + + if (strcmp(argv[2], "EVENT_CHANGE_STATE") == 0) { + event = PROXIMITY_BASE_DATA_SET; + } + else if (strcmp(argv[2], "EVENT_STATE_REPORT_ON_TIME") == 0) { + event = PROXIMITY_DISTANCE_DATA_SET; + } + else if (strcmp(argv[2], "EVENT_DISTANCE_DATA_REPORT_ON_TIME") == 0) { + event = PROXIMITY_EVENT_DISTANCE_DATA_REPORT_ON_TIME; + } + else { + printformat(); + free(event_condition); + return 0; + } + + handle = sf_connect(type); + result = sf_start(handle, 1); + + if (result < 0) { + printf("Can't start proximity SENSOR\n"); + printf("Error\n\n\n\n"); + return -1; + } + + sensor_data_t data; + + while(1) { + result = sf_get_data(handle, event , &data); + printf("Proximity [%6.6f]\n\n", data.values[0]); + usleep(100000); + } + + result = sf_disconnect(handle); + + if (result < 0) { + printf("Can't disconnect proximity sensor\n"); + printf("Error\n\n\n\n"); + return -1; + } } - if (argc == 3) - event_condition->cond_value1 = atof(argv[2]); + else if (argc == 2 || argc ==3) { + printf("Event based\n"); + + if (strcmp(argv[1], "EVENT_CHANGE_STATE") == 0) { + event = PROXIMITY_EVENT_CHANGE_STATE; + } + else if (strcmp(argv[1], "EVENT_STATE_REPORT_ON_TIME") == 0) { + event = PROXIMITY_EVENT_STATE_REPORT_ON_TIME; + } + else if (strcmp(argv[1], "EVENT_DISTANCE_DATA_REPORT_ON_TIME") == 0) { + event = PROXIMITY_EVENT_DISTANCE_DATA_REPORT_ON_TIME; + } + else { + printformat(); + free(event_condition); + return 0; + } + + event_condition->cond_value1 = 100; + if (argc == 3) + event_condition->cond_value1 = atof(argv[2]); + + handle = sf_connect(type); + result = sf_register_event(handle, event, event_condition, callback, NULL); + + if (result < 0) + printf("Can't register proximity\n"); + + start_handle = sf_start(handle,0); + + if (start_handle < 0) { + printf("Error\n\n\n\n"); + sf_unregister_event(handle, event); + sf_disconnect(handle); + return -1; + } + + g_main_loop_run(mainloop); + g_main_loop_unref(mainloop); - handle = sf_connect(type); - result = sf_register_event(handle, event, event_condition, callback, NULL); + sf_unregister_event(handle, event); - if (result < 0) - printf("Can't register proximity sensor\n"); + stop_handle = sf_stop(handle); - start_handle = sf_start(handle,0); + if (stop_handle < 0) { + printf("Error\n\n"); + return -1; + } - if (start_handle < 0) { - printf("Error\n\n\n\n"); - sf_unregister_event(handle, event); sf_disconnect(handle); - return -1; + free(event_condition); } - g_main_loop_run(mainloop); - g_main_loop_unref(mainloop); - - sf_unregister_event(handle, event); - - stop_handle = sf_stop(handle); - - if (stop_handle < 0) { - printf("Failed to stop proximity sensor\n\n"); - return -1; + else { + printformat(); } - sf_disconnect(handle); - - free(event_condition); - return 0; } -- 2.7.4 From 6749fe38425410aa54500081a4cae0f135ac3a0a Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 2 Jan 2015 08:50:16 +0530 Subject: [PATCH 14/16] Fix for orientation values being set to Nan - Fixing issue where low floating point values are rounded of to zero. - indexing issue. - Cleanup Change-Id: Ief4a37395b50f8946bf1d622c0895a881c3a7619 --- src/sensor_fusion/orientation_filter.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index 05537a3..5e0a354 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -40,6 +40,10 @@ #define QWB_CONST ((2 * (ZIGMA_W * ZIGMA_W)) / TAU_W) #define F_CONST (-1 / TAU_W) +#define NEGLIGIBLE_VAL 0.0000001 + +#define ABS(val) (((val) < 0) ? -(val) : (val)) + // M-matrix, V-vector, MxN=> matrix dimension, R-RowCount, C-Column count #define M3X3R 3 #define M3X3C 3 @@ -96,8 +100,6 @@ template inline void orientation_filter::initialize_sensor_data(const sensor_data accel, const sensor_data gyro, const sensor_data magnetic) { - vect acc_data(V1x3S); - vect gyr_data(V1x3S); unsigned long long sample_interval_gyro = SAMPLE_INTV; m_accel.m_data = accel.m_data; @@ -258,10 +260,8 @@ inline void orientation_filter::measurement_update() matrix gain(M6X6R, M6X6C); TYPE iden = 0; - for (int j = 0; j < M6X6C; j++) - { - for (int i = 0; i < M6X6R; i++) - { + for (int j=0; j::measurement_update() else iden = 0; - m_pred_cov.m_mat[i][j] = (iden - (gain.m_mat[i][j] * m_measure_mat.m_mat[j][i])) * - m_pred_cov.m_mat[i][j]; + m_pred_cov.m_mat[j][i] = (iden - (gain.m_mat[i][j] * m_measure_mat.m_mat[j][i])) * + m_pred_cov.m_mat[j][i]; + + if (ABS(m_pred_cov.m_mat[j][i]) < NEGLIGIBLE_VAL) + m_pred_cov.m_mat[j][i] = NEGLIGIBLE_VAL; } } -- 2.7.4 From 537339baada31e06a10bc201e4ccdab002f0afc9 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 2 Jan 2015 09:05:11 +0530 Subject: [PATCH 15/16] Updating linear acceleration range in get_properties - Updating the min_range and max_range values in get_properties to to shown according to accelerometer range. - cleanup Change-Id: I2f2c68b8e29aad658d1cea1eed8ef7fc3bd0a2c4 --- src/linear_accel/linear_accel_sensor.cpp | 4 +--- src/orientation/orientation_sensor.cpp | 2 +- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/linear_accel/linear_accel_sensor.cpp b/src/linear_accel/linear_accel_sensor.cpp index 1af20bc..fb2dabd 100755 --- a/src/linear_accel/linear_accel_sensor.cpp +++ b/src/linear_accel/linear_accel_sensor.cpp @@ -273,11 +273,9 @@ int linear_accel_sensor::get_sensor_data(const unsigned int event_type, sensor_d bool linear_accel_sensor::get_properties(sensor_properties_t &properties) { - m_gravity_sensor->get_properties(properties); + m_accel_sensor->get_properties(properties); properties.name = "Linear Acceleration Sensor"; properties.vendor = m_vendor; - properties.min_range = - 2 * GRAVITY; - properties.max_range = 2 * GRAVITY; properties.resolution = 0.000001; return true; diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp index dd0f818..7653ef3 100755 --- a/src/orientation/orientation_sensor.cpp +++ b/src/orientation/orientation_sensor.cpp @@ -436,7 +436,7 @@ bool orientation_sensor::get_properties(sensor_properties_t &properties) properties.min_range = -PI; properties.max_range = 2 * PI; } - properties.resolution = 0.000001;; + properties.resolution = 0.000001; properties.vendor = m_vendor; properties.name = SENSOR_NAME; -- 2.7.4 From 7cd4ed8647a5a0b42fdb8eec6b3f289472b8a204 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 2 Jan 2015 09:35:02 +0530 Subject: [PATCH 16/16] Adding Rotation Vector virtual sensor - Added 9-axis rotation vector based on sensor fusion - Added xml configuration support for virtual sensor - Added xml configuration for loading rotation vector library - Updated build and spec files Change-Id: I540b277019aa9a946e267b89c39a15e168dbe9f0 --- packaging/sensord.spec | 3 +- sensor_plugins.xml.in | 1 + src/libsensord/sensor_internal.h | 1 + src/libsensord/sensor_internal_deprecated.h | 1 + src/rotation_vector/CMakeLists.txt | 4 + src/rotation_vector/rv/CMakeLists.txt | 47 ++++ src/rotation_vector/rv/rv_sensor.cpp | 390 ++++++++++++++++++++++++++++ src/rotation_vector/rv/rv_sensor.h | 87 +++++++ virtual_sensors.xml.in | 16 ++ 9 files changed, 549 insertions(+), 1 deletion(-) create mode 100644 src/rotation_vector/CMakeLists.txt create mode 100755 src/rotation_vector/rv/CMakeLists.txt create mode 100755 src/rotation_vector/rv/rv_sensor.cpp create mode 100755 src/rotation_vector/rv/rv_sensor.h diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 2e1fff7..1d305ff 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -18,6 +18,7 @@ Source2: sensord.socket %define orientation_state ON %define gravity_state ON %define linear_accel_state ON +%define rv_state ON %define build_test_suite OFF @@ -79,7 +80,7 @@ cmake . -DCMAKE_INSTALL_PREFIX=%{_prefix} -DACCEL=%{accel_state} \ -DGYRO=%{gyro_state} -DPROXI=%{proxi_state} -DLIGHT=%{light_state} \ -DGEO=%{geo_state} -DPRESSURE=%{pressure_state} -DTEMPERATURE=%{temperature_state} \ -DORIENTATION=%{orientation_state} -DGRAVITY=%{gravity_state} \ - -DLINEAR_ACCEL=%{linear_accel_state} \ + -DLINEAR_ACCEL=%{linear_accel_state} -DRV=%{rv_state} \ -DTEST_SUITE=%{build_test_suite} make %{?jobs:-j%jobs} diff --git a/sensor_plugins.xml.in b/sensor_plugins.xml.in index f40f79a..aca8a81 100755 --- a/sensor_plugins.xml.in +++ b/sensor_plugins.xml.in @@ -20,5 +20,6 @@ + diff --git a/src/libsensord/sensor_internal.h b/src/libsensord/sensor_internal.h index bec1487..3bc8311 100755 --- a/src/libsensord/sensor_internal.h +++ b/src/libsensord/sensor_internal.h @@ -49,6 +49,7 @@ extern "C" #include #include #include +#include #include diff --git a/src/libsensord/sensor_internal_deprecated.h b/src/libsensord/sensor_internal_deprecated.h index f0a1a74..dc4a8b3 100755 --- a/src/libsensord/sensor_internal_deprecated.h +++ b/src/libsensord/sensor_internal_deprecated.h @@ -48,6 +48,7 @@ extern "C" #include #include #include +#include typedef struct { condition_op_t cond_op; diff --git a/src/rotation_vector/CMakeLists.txt b/src/rotation_vector/CMakeLists.txt new file mode 100644 index 0000000..c111931 --- /dev/null +++ b/src/rotation_vector/CMakeLists.txt @@ -0,0 +1,4 @@ +IF("${RV}" STREQUAL "ON") +add_subdirectory(rv) +ENDIF() + diff --git a/src/rotation_vector/rv/CMakeLists.txt b/src/rotation_vector/rv/CMakeLists.txt new file mode 100755 index 0000000..33ac289 --- /dev/null +++ b/src/rotation_vector/rv/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 2.6) +project(rv CXX) + +# to install pkgconfig setup file. +SET(EXEC_PREFIX "\${prefix}") +SET(LIBDIR "\${prefix}/lib") +SET(INCLUDEDIR "\${prefix}/include") +SET(VERSION 1.0) + +SET(SENSOR_NAME rv_sensor) + +include_directories(${CMAKE_CURRENT_SOURCE_DIR}) +include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) +include_directories(${CMAKE_SOURCE_DIR}/src/sensor_fusion) + +include(FindPkgConfig) +pkg_check_modules(rpkgs REQUIRED vconf) +add_definitions(${rpkgs_CFLAGS} -DUSE_ONLY_ONE_MODULE -DUSE_LCD_TYPE_CHECK) + +set(PROJECT_MAJOR_VERSION "0") +set(PROJECT_MINOR_VERSION "0") +set(PROJECT_RELEASE_VERSION "1") +set(CMAKE_VERBOSE_MAKEFILE OFF) + + +FIND_PROGRAM(UNAME NAMES uname) +EXEC_PROGRAM("${UNAME}" ARGS "-m" OUTPUT_VARIABLE "ARCH") +IF("${ARCH}" MATCHES "^arm.*") + ADD_DEFINITIONS("-DTARGET") + MESSAGE("add -DTARGET") +ELSE("${ARCH}" MATCHES "^arm.*") + ADD_DEFINITIONS("-DSIMULATOR") + MESSAGE("add -DSIMULATOR") +ENDIF("${ARCH}" MATCHES "^arm.*") + +add_definitions(-Wall -O3 -omit-frame-pointer) +add_definitions(-DUSE_DLOG_LOG) +#add_definitions(-Wall -g -D_DEBUG) +add_definitions(-Iinclude) + +add_library(${SENSOR_NAME} SHARED + rv_sensor.cpp +) + +target_link_libraries(${SENSOR_NAME} ${rpkgs_LDFLAGS} ${GLES_LDFLAGS} "-lm") + +install(TARGETS ${SENSOR_NAME} DESTINATION lib/sensord) diff --git a/src/rotation_vector/rv/rv_sensor.cpp b/src/rotation_vector/rv/rv_sensor.cpp new file mode 100755 index 0000000..cee3a94 --- /dev/null +++ b/src/rotation_vector/rv/rv_sensor.cpp @@ -0,0 +1,390 @@ +/* + * sensord + * + * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define SENSOR_NAME "RV_SENSOR" +#define SENSOR_TYPE_RV "ROTATION_VECTOR" + +#define ACCELEROMETER_ENABLED 0x01 +#define GYROSCOPE_ENABLED 0x02 +#define GEOMAGNETIC_ENABLED 0x04 +#define ORIENTATION_ENABLED 7 + +#define INITIAL_VALUE -1 + +#define MS_TO_US 1000 + +#define ELEMENT_NAME "NAME" +#define ELEMENT_VENDOR "VENDOR" +#define ELEMENT_RAW_DATA_UNIT "RAW_DATA_UNIT" +#define ELEMENT_DEFAULT_SAMPLING_TIME "DEFAULT_SAMPLING_TIME" +#define ELEMENT_ACCEL_STATIC_BIAS "ACCEL_STATIC_BIAS" +#define ELEMENT_GYRO_STATIC_BIAS "GYRO_STATIC_BIAS" +#define ELEMENT_GEOMAGNETIC_STATIC_BIAS "GEOMAGNETIC_STATIC_BIAS" +#define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION "ACCEL_ROTATION_DIRECTION_COMPENSATION" +#define ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION "GYRO_ROTATION_DIRECTION_COMPENSATION" +#define ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION "GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION" +#define ELEMENT_ACCEL_SCALE "ACCEL_SCALE" +#define ELEMENT_GYRO_SCALE "GYRO_SCALE" +#define ELEMENT_GEOMAGNETIC_SCALE "GEOMAGNETIC_SCALE" +#define ELEMENT_MAGNETIC_ALIGNMENT_FACTOR "MAGNETIC_ALIGNMENT_FACTOR" + +void pre_process_data(sensor_data &data_out, const float *data_in, float *bias, int *sign, float scale) +{ + data_out.m_data.m_vec[0] = sign[0] * (data_in[0] - bias[0]) / scale; + data_out.m_data.m_vec[1] = sign[1] * (data_in[1] - bias[1]) / scale; + data_out.m_data.m_vec[2] = sign[2] * (data_in[2] - bias[2]) / scale; +} + +rv_sensor::rv_sensor() +: m_accel_sensor(NULL) +, m_gyro_sensor(NULL) +, m_magnetic_sensor(NULL) +, m_x(-1) +, m_y(-1) +, m_z(-1) +, m_w(-1) +, m_accuracy(-1) +, m_time(0) +{ + 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); + m_enable_orientation = 0; + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_VENDOR, m_vendor)) { + ERR("[VENDOR] is empty\n"); + throw ENXIO; + } + + INFO("m_vendor = %s", m_vendor.c_str()); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) { + ERR("[DEFAULT_SAMPLING_TIME] is empty\n"); + throw ENXIO; + } + + INFO("m_default_sampling_time = %d", m_default_sampling_time); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) { + ERR("[ACCEL_STATIC_BIAS] is empty\n"); + throw ENXIO; + } + + INFO("m_accel_static_bias = (%f, %f, %f)", m_accel_static_bias[0], m_accel_static_bias[1], m_accel_static_bias[2]); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_GYRO_STATIC_BIAS, m_gyro_static_bias,3)) { + ERR("[GYRO_STATIC_BIAS] is empty\n"); + throw ENXIO; + } + + INFO("m_gyro_static_bias = (%f, %f, %f)", m_gyro_static_bias[0], m_gyro_static_bias[1], m_gyro_static_bias[2]); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_GEOMAGNETIC_STATIC_BIAS, m_geomagnetic_static_bias, 3)) { + ERR("[GEOMAGNETIC_STATIC_BIAS] is empty\n"); + throw ENXIO; + } + + INFO("m_geomagnetic_static_bias = (%f, %f, %f)", m_geomagnetic_static_bias[0], m_geomagnetic_static_bias[1], m_geomagnetic_static_bias[2]); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION, m_accel_rotation_direction_compensation, 3)) { + ERR("[ACCEL_ROTATION_DIRECTION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_accel_rotation_direction_compensation = (%d, %d, %d)", m_accel_rotation_direction_compensation[0], m_accel_rotation_direction_compensation[1], m_accel_rotation_direction_compensation[2]); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION, m_gyro_rotation_direction_compensation, 3)) { + ERR("[GYRO_ROTATION_DIRECTION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_gyro_rotation_direction_compensation = (%d, %d, %d)", m_gyro_rotation_direction_compensation[0], m_gyro_rotation_direction_compensation[1], m_gyro_rotation_direction_compensation[2]); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION, m_geomagnetic_rotation_direction_compensation, 3)) { + ERR("[GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_geomagnetic_rotation_direction_compensation = (%d, %d, %d)", m_geomagnetic_rotation_direction_compensation[0], m_geomagnetic_rotation_direction_compensation[1], m_geomagnetic_rotation_direction_compensation[2]); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_ACCEL_SCALE, &m_accel_scale)) { + ERR("[ACCEL_SCALE] is empty\n"); + throw ENXIO; + } + + INFO("m_accel_scale = %f", m_accel_scale); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_GYRO_SCALE, &m_gyro_scale)) { + ERR("[GYRO_SCALE] is empty\n"); + throw ENXIO; + } + + INFO("m_gyro_scale = %f", m_gyro_scale); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_GEOMAGNETIC_SCALE, &m_geomagnetic_scale)) { + ERR("[GEOMAGNETIC_SCALE] is empty\n"); + throw ENXIO; + } + + INFO("m_geomagnetic_scale = %f", m_geomagnetic_scale); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_MAGNETIC_ALIGNMENT_FACTOR, &m_magnetic_alignment_factor)) { + ERR("[MAGNETIC_ALIGNMENT_FACTOR] is empty\n"); + throw ENXIO; + } + + INFO("m_magnetic_alignment_factor = %d", m_magnetic_alignment_factor); + + m_interval = m_default_sampling_time * MS_TO_US; + +} + +rv_sensor::~rv_sensor() +{ + INFO("rv_sensor is destroyed!\n"); +} + +bool rv_sensor::init() +{ + m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); + m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR); + m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR); + + if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor) { + ERR("Failed to load sensors, accel: 0x%x, gyro: 0x%x, mag: 0x%x", + m_accel_sensor, m_gyro_sensor, m_magnetic_sensor); + return false; + } + + INFO("%s is created!\n", sensor_base::get_name()); + + return true; +} + +sensor_type_t rv_sensor::get_type(void) +{ + return ROTATION_VECTOR_SENSOR; +} + +bool rv_sensor::on_start(void) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->add_interval((int)this, (m_interval/MS_TO_US), false); + m_accel_sensor->start(); + m_gyro_sensor->add_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); + m_gyro_sensor->add_interval((int)this, (m_interval/MS_TO_US), false); + m_gyro_sensor->start(); + m_magnetic_sensor->add_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); + m_magnetic_sensor->add_interval((int)this, (m_interval/MS_TO_US), false); + m_magnetic_sensor->start(); + + activate(); + return true; +} + +bool rv_sensor::on_stop(void) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->delete_interval((int)this, false); + m_accel_sensor->stop(); + m_gyro_sensor->delete_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); + m_gyro_sensor->delete_interval((int)this, false); + m_gyro_sensor->stop(); + m_magnetic_sensor->delete_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); + m_magnetic_sensor->delete_interval((int)this, false); + m_magnetic_sensor->stop(); + + deactivate(); + return true; +} + +bool rv_sensor::add_interval(int client_id, unsigned int interval) +{ + AUTOLOCK(m_mutex); + + 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); + + return sensor_base::add_interval(client_id, interval, false); +} + +bool rv_sensor::delete_interval(int client_id) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->delete_interval(client_id, false); + m_gyro_sensor->delete_interval(client_id, false); + m_magnetic_sensor->delete_interval(client_id, false); + + return sensor_base::delete_interval(client_id, false); +} + +void rv_sensor::synthesize(const sensor_event_t& event, vector &outs) +{ + const float MIN_DELIVERY_DIFF_FACTOR = 0.75f; + unsigned long long diff_time; + + sensor_event_t rv_event; + quaternion quaternion_orientation; + + if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) { + diff_time = event.data.timestamp - m_time; + + if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale); + + m_accel.m_time_stamp = event.data.timestamp; + + m_enable_orientation |= ACCELEROMETER_ENABLED; + } + else if (event.event_type == GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME) { + diff_time = event.data.timestamp - m_time; + + if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + pre_process_data(m_gyro, event.data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale); + + m_gyro.m_time_stamp = event.data.timestamp; + + m_enable_orientation |= GYROSCOPE_ENABLED; + } + else if (event.event_type == GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME) { + diff_time = event.data.timestamp - m_time; + + if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + pre_process_data(m_magnetic, event.data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale); + + m_magnetic.m_time_stamp = event.data.timestamp; + + m_enable_orientation |= GEOMAGNETIC_ENABLED; + } + + if (m_enable_orientation == ORIENTATION_ENABLED) { + m_enable_orientation = 0; + + m_orientation.m_pitch_phase_compensation = m_pitch_rotation_compensation; + m_orientation.m_roll_phase_compensation = m_roll_rotation_compensation; + m_orientation.m_azimuth_phase_compensation = m_azimuth_rotation_compensation; + m_orientation.m_magnetic_alignment_factor = m_magnetic_alignment_factor; + + { + AUTOLOCK(m_fusion_mutex); + quaternion_orientation = m_orientation.get_quaternion(m_accel, m_gyro, m_magnetic); + } + + rv_event.sensor_id = get_id(); + rv_event.event_type = ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME; + rv_event.data.accuracy = SENSOR_ACCURACY_GOOD; + rv_event.data.timestamp = get_timestamp(); + rv_event.data.value_count = 4; + rv_event.data.values[0] = quaternion_orientation.m_quat.m_vec[1]; + rv_event.data.values[1] = quaternion_orientation.m_quat.m_vec[2]; + rv_event.data.values[2] = quaternion_orientation.m_quat.m_vec[3]; + rv_event.data.values[3] = quaternion_orientation.m_quat.m_vec[0]; + + push(rv_event); + + { + AUTOLOCK(m_value_mutex); + m_time = rv_event.data.value_count; + m_x = rv_event.data.values[0]; + m_y = rv_event.data.values[1]; + m_z = rv_event.data.values[2]; + m_w = rv_event.data.values[3]; + } + } + + return; +} + +int rv_sensor::get_sensor_data(unsigned int data_id, sensor_data_t &data) +{ + if (data_id != ROTATION_VECTOR_BASE_DATA_SET) + return -1; + + data.accuracy = SENSOR_ACCURACY_GOOD; + + AUTOLOCK(m_value_mutex); + data.timestamp = m_time; + data.values[0] = m_x; + data.values[1] = m_y; + data.values[2] = m_z; + data.values[3] = m_w; + data.value_count = 4; + + return 0; +} + +bool rv_sensor::get_properties(sensor_properties_t &properties) +{ + properties.vendor = m_vendor; + properties.name = SENSOR_NAME; + properties.min_range = 0; + properties.max_range = 1; + properties.resolution = 0.000001; + properties.fifo_count = 0; + properties.max_batch_count = 0; + properties.min_interval = 1; + + return true; +} + +extern "C" sensor_module* create(void) +{ + rv_sensor *sensor; + + try { + sensor = new(std::nothrow) rv_sensor; + } catch (int err) { + ERR("Failed to create module, err: %d, cause: %s", err, strerror(err)); + return NULL; + } + + sensor_module *module = new(std::nothrow) sensor_module; + retvm_if(!module || !sensor, NULL, "Failed to allocate memory"); + + module->sensors.push_back(sensor); + return module; +} diff --git a/src/rotation_vector/rv/rv_sensor.h b/src/rotation_vector/rv/rv_sensor.h new file mode 100755 index 0000000..3d84a80 --- /dev/null +++ b/src/rotation_vector/rv/rv_sensor.h @@ -0,0 +1,87 @@ +/* + * sensord + * + * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef _RV_SENSOR_H_ +#define _RV_SENSOR_H_ + +#include +#include +#include + +class rv_sensor : public virtual_sensor { +public: + rv_sensor(); + virtual ~rv_sensor(); + + bool init(void); + + void synthesize(const sensor_event_t &event, vector &outs); + + bool add_interval(int client_id, unsigned int interval); + bool delete_interval(int client_id); + bool get_properties(sensor_properties_t &properties); + sensor_type_t get_type(void); + + int get_sensor_data(const unsigned int data_id, sensor_data_t &data); + +private: + sensor_base *m_accel_sensor; + sensor_base *m_gyro_sensor; + sensor_base *m_magnetic_sensor; + + sensor_data m_accel; + sensor_data m_gyro; + sensor_data m_magnetic; + + cmutex m_value_mutex; + + orientation_filter m_orientation; + + unsigned int m_enable_orientation; + + float m_x; + float m_y; + float m_z; + float m_w; + int m_accuracy; + unsigned long long m_time; + unsigned int m_interval; + + string m_vendor; + string m_raw_data_unit; + int m_default_sampling_time; + float m_accel_static_bias[3]; + float m_gyro_static_bias[3]; + float m_geomagnetic_static_bias[3]; + int m_accel_rotation_direction_compensation[3]; + int m_gyro_rotation_direction_compensation[3]; + int m_geomagnetic_rotation_direction_compensation[3]; + float m_accel_scale; + 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); +}; + +#endif /*_RV_SENSOR_H_*/ diff --git a/virtual_sensors.xml.in b/virtual_sensors.xml.in index 42c9d72..680745e 100644 --- a/virtual_sensors.xml.in +++ b/virtual_sensors.xml.in @@ -38,4 +38,20 @@ + + + + + + + + + + + + + + + + -- 2.7.4