From 653e9cebd7623972fd8bd4c69696e467585e4365 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Mon, 15 Dec 2014 09:19:38 +0530 Subject: [PATCH 01/16] Adding information for Driving System Updating documentation for 'Orientation Computation based on driving system' section in html documentation. Change-Id: Ibb483e5ba4e658f1710bec934666a7696e5ab92c --- .../design/documentation/sensor_fusion.htm | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/sensor_fusion/design/documentation/sensor_fusion.htm b/src/sensor_fusion/design/documentation/sensor_fusion.htm index 11a1b37..c1f9e3a 100755 --- a/src/sensor_fusion/design/documentation/sensor_fusion.htm +++ b/src/sensor_fusion/design/documentation/sensor_fusion.htm @@ -266,6 +266,19 @@ the same time they are computationally more efficient than using rotation matrix

3.3. Orientation Computation Based on Driving System

+

The sensor data obtained from the driving system (gyroscope) given by (Gx, Gy, Gz) +represents the calibrated angular rotation rate of the device in the 3-axes. Since +the calibrated gyroscope data provides the rate of change of angle in each axis, +the integration of the data acquired in each axis provides the orientation measure +of the device. During integration of the gyroscope data the noise present in each +instance of the measured data gets added up resulting in measured orientation +affected by drift. First the measured gyroscope data is converted to a quaternion +equivalent Qdriv [14, 15]. The initial value for the Qdriv is assigned based on the +computed Qaid initial value (15). The quaternion differential which denotes the +increment in rotation is computed by quaternion based multiplication [7, 9] of Qdriv +with gyroscope sensor data as shown in (16). The symbol ⊙ denotes quaternion +based multiplication in the following equations.

+
@@ -278,6 +291,13 @@ the same time they are computationally more efficient than using rotation matrix
+

Equation (17) represents the integration of the quaternion difference value with +the driving system quaternion to yield the measured quaternion value for that time +instant [1]. The value 'dt' represents the sampling interval for the gyroscope sensor +and 'Π' denotes the mathematical constant and denotes the ratio of a circle's +circumference to its diameter and is approximately equal to 3.14159. The driving +system quaternion is then normalized as shown in equation (18).

+
-- 2.7.4 From 30e4720a900b2d4896eb8f5ed9681ad4c6527b3c Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Tue, 16 Dec 2014 09:36:57 +0530 Subject: [PATCH 02/16] Adding information for time update system Adding description for Time update System section in sensor fusion html documentation. Change-Id: I94f87c6ccd3f4bfcfc005d37a480e4fd57fb11c8 --- .../design/documentation/sensor_fusion.htm | 40 ++++++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/src/sensor_fusion/design/documentation/sensor_fusion.htm b/src/sensor_fusion/design/documentation/sensor_fusion.htm index c1f9e3a..231b129 100755 --- a/src/sensor_fusion/design/documentation/sensor_fusion.htm +++ b/src/sensor_fusion/design/documentation/sensor_fusion.htm @@ -312,18 +312,37 @@ system quaternion is then normalized as shown in equation (18).

3.4. Time Update System

+

The orientation computed using the driving system sensor data is affected due to +the drift (from the integration of angular rates), the orientation computed using +the aiding system sensor data is not accurate (measurements are affected due to gravity). +To compensate for the deficiencies in the measurements of each system, the quaternion +orientation computations are combined using sensor fusion to obtain corrected/more +accurate device orientation data. The Quaternion error Qerr is computed in (19), as +described in [1].

+
+

We introduce the equations (20) and (21) to compute the Euler error from the +quaternion error and then using this Euler error to compensate for drift on the driving +system orientation quaternion. First, the quaternion error Qerr is converted to the Euler +angle representation Eerr based on (20). The quaternion representation of orientation +error is converted to Euler angles representation using the function quat2euler() for +which information can be found in [9].

+
+

The driving system quaternion is then compensated for the Euler angle error Eerr and +normalized as per (21) and (22) to correct for drift observed in the driving system sensor +data.

+
@@ -336,12 +355,25 @@ system quaternion is then normalized as shown in equation (18).

+

The quaternion Qdriv that is derived in (22) is now compensated for drift and corrected +for dynamic bias as shown in (6). The dynamic bias correction used in (6) is determined +using Kalman filtering as explained below. Based on [1], linear Kalman filtering is used +based on the error state equation, given in (23). The first three elements of the error +state vector are the Eerr values on the 3-axes (Φerr, Θerr, Ψerr) and the +next three elements are the bias compensation (Bx, By, Bz) that is mentioned in (6).

+
+

The variance in the gyroscope angular rate measurements denoted by Qwn over a windowed +period is computed in (24). The random driving process noise variance Qwb is computed in +(25), where the noise standard deviation σw and time constant τw are obtained from the +gyroscope sensor datasheet. Based on [1], the overall process noise covariance Q is +derived from the computed Qwn and Qwb as shown in (26).

+
@@ -360,12 +392,20 @@ system quaternion is then normalized as shown in equation (18).

+

The variance in the aiding system orientation (roll, pitch and yaw) measurements are +used to compute the measurement noise covariance R [1] which is shown in (27). The aiding +system orientation in terms of Euler angles Eaid can be obtained by converting the +quaternion obtained in (14) to its equivalent Euler angles.

+
+

The equations (28), (29) and (30) are used to determine the transformation matrix F, +apriori state vector error Δx- and prediction covariance P- based in [1].

+
-- 2.7.4 From 326d42647ffe8767d97ca60f147c3f6db5213a65 Mon Sep 17 00:00:00 2001 From: Ankur Date: Wed, 17 Dec 2014 16:41:59 +0530 Subject: [PATCH 03/16] Fixed Intialization Warnings -Reordered some variable intializations to remove warnings related to order of initializations. Change-Id: Ibf56efd6f19c11229a18fbe6a613ffcf2b172a16 --- src/geo/geo_sensor_hal.cpp | 6 +++--- src/libsensord/csensor_event_listener.cpp | 2 +- src/linear_accel/linear_accel_sensor.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/geo/geo_sensor_hal.cpp b/src/geo/geo_sensor_hal.cpp index d742b26..21a2276 100755 --- a/src/geo/geo_sensor_hal.cpp +++ b/src/geo/geo_sensor_hal.cpp @@ -40,13 +40,13 @@ using std::ifstream; #define GAUSS_TO_UTESLA(val) ((val) * 100.0f) geo_sensor_hal::geo_sensor_hal() -: m_x(0) +: m_polling_interval(POLL_1HZ_MS) +, m_x(0) , m_y(0) , m_z(0) , m_hdst(0) -, m_node_handle(-1) -, m_polling_interval(POLL_1HZ_MS) , m_fired_time(INITIAL_TIME) +, m_node_handle(-1) { const string sensorhub_interval_node_name = "mag_poll_delay"; csensor_config &config = csensor_config::get_instance(); diff --git a/src/libsensord/csensor_event_listener.cpp b/src/libsensord/csensor_event_listener.cpp index e093f21..78e705e 100755 --- a/src/libsensord/csensor_event_listener.cpp +++ b/src/libsensord/csensor_event_listener.cpp @@ -30,8 +30,8 @@ using std::pair; csensor_event_listener::csensor_event_listener() : m_client_id(CLIENT_ID_INVALID) -, m_thread_state(THREAD_STATE_TERMINATE) , m_poller(NULL) +, m_thread_state(THREAD_STATE_TERMINATE) , m_hup_observer(NULL) { } diff --git a/src/linear_accel/linear_accel_sensor.cpp b/src/linear_accel/linear_accel_sensor.cpp index f975733..96e6cce 100755 --- a/src/linear_accel/linear_accel_sensor.cpp +++ b/src/linear_accel/linear_accel_sensor.cpp @@ -53,8 +53,8 @@ #define LINEAR_ACCEL_ENABLED 3 linear_accel_sensor::linear_accel_sensor() -: m_gravity_sensor(NULL) -, m_accel_sensor(NULL) +: m_accel_sensor(NULL) +, m_gravity_sensor(NULL) , m_x(INITIAL_VALUE) , m_y(INITIAL_VALUE) , m_z(INITIAL_VALUE) -- 2.7.4 From c169c4e8efbe517eeb2a55f51d45e70df6047753 Mon Sep 17 00:00:00 2001 From: Ankur Date: Thu, 18 Dec 2014 16:13:02 +0530 Subject: [PATCH 04/16] Fixed Compilation warnings related to Intialization -Error fixed: Variable was being used before intialization -Testing will be done once the test cases are completed. Change-Id: I5316bce003717d97f6e3cd65a909d94e1c84ad0c --- src/gravity/gravity_sensor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/gravity/gravity_sensor.cpp b/src/gravity/gravity_sensor.cpp index acb73a6..64e39e7 100755 --- a/src/gravity/gravity_sensor.cpp +++ b/src/gravity/gravity_sensor.cpp @@ -218,6 +218,8 @@ int gravity_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t sensor_data_t orientation_data; float pitch, roll, azimuth; + m_orientation_sensor->get_sensor_data(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME, orientation_data); + azimuth = orientation_data.values[0]; pitch = orientation_data.values[1]; roll = orientation_data.values[2]; @@ -231,8 +233,6 @@ int gravity_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t if (event_type != GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME) return -1; - m_orientation_sensor->get_sensor_data(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME, orientation_data); - data.accuracy = SENSOR_ACCURACY_GOOD; data.timestamp = get_timestamp(); if ((roll >= (M_PI/2)-DEVIATION && roll <= (M_PI/2)+DEVIATION) || -- 2.7.4 From ed04630ae99f42a3a9ba4116b91f1f2c32ae88be Mon Sep 17 00:00:00 2001 From: Ankur Date: Thu, 18 Dec 2014 18:54:40 +0530 Subject: [PATCH 05/16] Fixed implicit declaration warnings -Fixed warnings related to implicit delcaration of strcmp() function. Error: missing #include from the files which are using the function strcmp Change-Id: Ia8c3fec21b7f622543b646063ec9718f5b1cba83 --- test/src/accelerometer.c | 1 + test/src/gyro.c | 1 + test/src/pressure.c | 1 + test/src/proxi.c | 1 + 4 files changed, 4 insertions(+) diff --git a/test/src/accelerometer.c b/test/src/accelerometer.c index 09028e4..c7ea6e6 100644 --- a/test/src/accelerometer.c +++ b/test/src/accelerometer.c @@ -22,6 +22,7 @@ #include #include #include +#include static GMainLoop *mainloop; diff --git a/test/src/gyro.c b/test/src/gyro.c index 884507b..ae34d95 100644 --- a/test/src/gyro.c +++ b/test/src/gyro.c @@ -23,6 +23,7 @@ #include #include #include +#include static GMainLoop *mainloop; diff --git a/test/src/pressure.c b/test/src/pressure.c index 0ac4e6d..e3ed4b4 100644 --- a/test/src/pressure.c +++ b/test/src/pressure.c @@ -23,6 +23,7 @@ #include #include #include +#include static GMainLoop *mainloop; diff --git a/test/src/proxi.c b/test/src/proxi.c index 7ed2d64..9ac965f 100644 --- a/test/src/proxi.c +++ b/test/src/proxi.c @@ -23,6 +23,7 @@ #include #include #include +#include static GMainLoop *mainloop; -- 2.7.4 From 00f95bdf1671616c0d60e2d2b1c56ee38a3bae59 Mon Sep 17 00:00:00 2001 From: Ankur Date: Thu, 18 Dec 2014 19:28:45 +0530 Subject: [PATCH 06/16] Removed warnings related to unintialized variables -Warnings related to variables being used without initialization. Intialized the variables. Actually, the variables were not beng used without initialization but the cases where the variables are being used are mutually exclusive. So, varaibles have to be intialized to some intial value to remove the warning. Change-Id: I4041e9d3256c40acb8cdbebbc810275123601603 --- src/shared/csensor_event_dispatcher.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/shared/csensor_event_dispatcher.cpp b/src/shared/csensor_event_dispatcher.cpp index f9f90d5..f00332d 100755 --- a/src/shared/csensor_event_dispatcher.cpp +++ b/src/shared/csensor_event_dispatcher.cpp @@ -189,8 +189,8 @@ void csensor_event_dispatcher::dispatch_event(void) void csensor_event_dispatcher::send_sensor_events(void* events, int event_cnt, bool is_hub_event) { - sensor_event_t *sensor_events; - sensorhub_event_t *sensor_hub_events; + sensor_event_t *sensor_events = NULL; + sensorhub_event_t *sensor_hub_events = NULL; cclient_info_manager& client_info_manager = get_client_info_manager(); const int RESERVED_CLIENT_CNT = 20; -- 2.7.4 From cb35b6dd8a1f44a8f103fdbcebae5b0dcf4bbbf5 Mon Sep 17 00:00:00 2001 From: Ankur Date: Thu, 18 Dec 2014 19:44:49 +0530 Subject: [PATCH 07/16] Added missing function call - Missing Function call to sf_stop() in one of the test cases - linear_acceleration.c - Also, it was leading to a warning about use before intialization warning. Change-Id: Ic0df7b6334945ac62048fee9ddcdb32fdf0fcba9 --- test/src/linear_acceleration.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/test/src/linear_acceleration.c b/test/src/linear_acceleration.c index 5e157e5..e45f4cc 100755 --- a/test/src/linear_acceleration.c +++ b/test/src/linear_acceleration.c @@ -94,6 +94,8 @@ int main(int argc,char **argv) sf_unregister_event(handle, event); + stop_handle = sf_stop(handle); + if (stop_handle < 0) { printf("Error\n\n"); return -1; -- 2.7.4 From b713babd37d30a84b9239b619dc8477c899d09d5 Mon Sep 17 00:00:00 2001 From: Ankur Date: Thu, 18 Dec 2014 19:58:08 +0530 Subject: [PATCH 08/16] Removed missing parantheses warning -Added missing parantheses arounf an assignment in a condition. -Removed Missing Parantheses Warning. Change-Id: Ib7814ef31a911896e868b11cfc057284573bee7a --- src/shared/sensor_plugin_loader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/shared/sensor_plugin_loader.cpp b/src/shared/sensor_plugin_loader.cpp index 09f6711..8aa70d1 100755 --- a/src/shared/sensor_plugin_loader.cpp +++ b/src/shared/sensor_plugin_loader.cpp @@ -227,7 +227,7 @@ bool sensor_plugin_loader::get_paths_from_dir(const string &dir_path, vectord_name); if (equal(PLUGIN_POSTFIX.rbegin(), PLUGIN_POSTFIX.rend(), name.rbegin())) { -- 2.7.4 From ecde90333a1ea21280753978b2b08fb46975f20f Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 19 Dec 2014 11:11:18 +0530 Subject: [PATCH 09/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 10/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 11/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 12/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 13/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 14/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 15/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 16/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