From: TaeminYeom <taemin.yeom@samsung.com>
Date: Wed, 8 Jun 2022 02:11:55 +0000 (+0900)
Subject: Fix the initial value of yaw in gyroscope orientation
X-Git-Tag: submit/tizen_6.5/20220608.042616^0
X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=adf81478220834f96fa376688bf5bd2a1b9284b1;p=platform%2Fcore%2Fsystem%2Fsensord.git

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>
(cherry picked from commit 926ecd5e372ae46d317264086d725112ba1626b5)
---

diff --git a/src/fusion-sensor/fusion_util.cpp b/src/fusion-sensor/fusion_util.cpp
index 256376c6..e3ec8a0b 100644
--- 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 c3d6a52c..37768541 100644
--- 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 fb5e6c09..341e95e3 100644
--- 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);
     }