Fix the initial value of yaw in gyroscope orientation

After sensord restarts, the inital value of yaw is 0

Change-Id: Ifc5a7fe679813abf2b7dbe372de6169e0df675c9
Signed-off-by: TaeminYeom <taemin.yeom@samsung.com>

diff --git a/src/fusion-sensor/fusion_util.cpp b/src/fusion-sensor/fusion_util.cpp
index 256376c66095673096912d828dbd381d6764c39b..e3ec8a0be7cf8005797fef94d99ece3f0126dc3f 100644 (file)
--- a/src/fusion-sensor/fusion_util.cpp
+++ b/src/fusion-sensor/fusion_util.cpp
@@ -148,3 +148,31 @@ int quat_to_orientation(const float *quat, float &azimuth, float &pitch, float &
        return 0;
 }
 
+/*
+       Euler angles to Quarternion conversion
+       q.w = cos(pitch / 2) * cos(roll / 2) * cos(yaw / 2) + sin(pitch / 2) * sin(roll / 2) * sin(yaw / 2)
+       q.x = sin(pitch / 2) * cos(roll / 2) * cos(yaw / 2) - cos(pitch / 2) * sin(roll / 2) * sin(yaw / 2)
+       q.y = cos(pitch / 2) * sin(roll / 2) * cos(yaw / 2) + sin(pitch / 2) * cos(roll / 2) * sin(yaw / 2)
+       q.z = cos(pitch / 2) * cos(roll / 2) * sin(yaw / 2) - sin(pitch / 2) * sin(roll / 2) * cos(yaw / 2)
+
+       Be careful about the definition of pitch and roll.
+       It can be different of axis direction each reference.
+*/
+
+void orientation_to_quat(float *quat, float azimuth, float pitch, float roll)
+{
+       azimuth /= -RAD2DEGREE;
+       pitch /= -RAD2DEGREE;
+       roll /= -RAD2DEGREE;
+       double cy = cos(azimuth * 0.5);
+       double sy = sin(azimuth * 0.5);
+       double cp = cos(pitch * 0.5);
+       double sp = sin(pitch * 0.5);
+       double cr = cos(roll * 0.5);
+       double sr = sin(roll * 0.5);
+
+       quat[3] = cp * cr * cy + sp * sr * sy;
+       quat[0] = sp * cr * cy - cp * sr * sy;
+       quat[1] = cp * sr * cy + sp * cr * sy;
+       quat[2] = cp * cr * sy - sp * sr * cy;
+}

diff --git a/src/fusion-sensor/fusion_util.h b/src/fusion-sensor/fusion_util.h
index c3d6a52c8c2461b7e310333c623aa6ef8b193ea7..37768541d0a0b2a7b4811afdecdb1a1347431b40 100644 (file)
--- a/src/fusion-sensor/fusion_util.h
+++ b/src/fusion-sensor/fusion_util.h
@@ -29,6 +29,7 @@ int quat_to_matrix(const float *quat, float *R);
 int matrix_to_quat(const float *R, float *quat);
 int calculate_rotation_matrix(float *accel, float *geo, float *R, float *I);
 int quat_to_orientation(const float *rotation, float &azimuth, float &pitch, float &roll);
+void orientation_to_quat(float *quat, float azimuth, float pitch, float roll);
 
 #ifdef __cplusplus
 }

diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.cpp b/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.cpp
index fb5e6c09cf66c7559cbfabb1b498b80c580707a5..341e95e3160e41bdb95db05b2fe2e55491e0eceb 100644 (file)
--- a/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.cpp
+++ b/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.cpp
@@ -17,6 +17,7 @@
 // released in android-11.0.0_r9
 
 #include "orientation_filter.h"
+#include "fusion_util.h"
 
 using namespace android;
 
@@ -300,7 +301,21 @@ bool orientation_filter::checkInitComplete(int what, const vec3_t& d, float dT)
 
         vec3_t north(cross_product(up, east));
         R << east << north << up;
-        const vec4_t q = matrixToQuat(R);
+        vec4_t q = matrixToQuat(R);
+
+        if (mMode == FUSION_NOMAG) {
+            float temp[4] = {q[0], q[1], q[2], q[3]};
+            float azimuth, pitch, roll;
+
+            quat_to_orientation(temp, azimuth, pitch, roll);
+            azimuth = 0;
+            orientation_to_quat(temp, azimuth, pitch, roll);
+
+            q[0] = temp[0];
+            q[1] = temp[1];
+            q[2] = temp[2];
+            q[3] = temp[3];
+        }
 
         initFusion(q, mGyroRate);
     }