sensord: enable rotation vector/orientation sensors

[platform/core/system/sensord.git] / src / sensor / fusion_util.cpp

diff --git a/src/sensor/fusion_util.cpp b/src/sensor/fusion_util.cpp
index c02c263..e14c223 100644 (file)
--- a/src/sensor/fusion_util.cpp
+++ b/src/sensor/fusion_util.cpp
@@ -16,15 +16,26 @@
  * limitations under the License.
  *
  */
-
-#include <fusion_util.h>
+#include <errno.h>
 #include <math.h>
 #include <stdlib.h>
 
+#include "fusion_util.h"
+
+#define RAD2DEGREE (180/M_PI)
+#define QUAT (M_PI/4)
+#define HALF (M_PI/2)
+#define ARCTAN(x, y) ((x) == 0 ? 0 : (y) != 0 ? atan2((x), (y)) : (x) > 0 ? M_PI/2.0 : -M_PI/2.0)
+
+static float clamp(float v)
+{
+       return (v < 0) ? 0.0 : v;
+}
+
 int quat_to_matrix(const float *quat, float *R)
 {
        if(quat == NULL || R == NULL)
-               return -1;
+               return -EINVAL;
 
        float q0 = quat[3];
        float q1 = quat[0];
@@ -53,3 +64,146 @@ int quat_to_matrix(const float *quat, float *R)
 
        return 0;
 }
+
+int matrix_to_quat(const float *R, float *quat)
+{
+       if (R == NULL || quat == NULL)
+               return -EINVAL;
+
+       const float Hx = R[0];
+       const float My = R[4];
+       const float Az = R[8];
+       quat[0] = sqrtf(clamp(Hx - My - Az + 1) * 0.25f);
+       quat[1] = sqrtf(clamp(-Hx + My - Az + 1) * 0.25f);
+       quat[2] = sqrtf(clamp(-Hx - My + Az + 1) * 0.25f);
+       quat[3] = sqrtf(clamp(Hx + My + Az + 1) * 0.25f);
+       quat[0] = copysignf(quat[0], R[7] - R[5]);
+       quat[1] = copysignf(quat[1], R[2] - R[6]);
+       quat[2] = copysignf(quat[2], R[3] - R[1]);
+
+       return 0;
+}
+
+int calculate_rotation_matrix(float *accel, float *geo, float *R, float *I)
+{
+       if (accel == NULL || geo == NULL || R == NULL || I == NULL)
+               return -EINVAL;
+
+       float Ax = accel[0];
+       float Ay = accel[1];
+       float Az = accel[2];
+       float Ex = geo[0];
+       float Ey = geo[1];
+       float Ez = geo[2];
+       float Hx = Ey*Az - Ez*Ay;
+       float Hy = Ez*Ax - Ex*Az;
+       float Hz = Ex*Ay - Ey*Ax;
+       float normH =  (float)sqrt(Hx*Hx + Hy*Hy + Hz*Hz);
+       if (normH < 0.1f)
+               return -EINVAL;
+
+       float invH = 1.0f / normH;
+       Hx *= invH;
+       Hy *= invH;
+       Hz *= invH;
+       float invA = 1.0f / (float)sqrt(Ax*Ax + Ay*Ay + Az*Az);
+       Ax *= invA;
+       Ay *= invA;
+       Az *= invA;
+       float Mx = Ay*Hz - Az*Hy;
+       float My = Az*Hx - Ax*Hz;
+       float Mz = Ax*Hy - Ay*Hx;
+
+       R[0] = Hx;  R[1] = Hy;  R[2] = Hz;
+       R[3] = Mx;  R[4] = My;  R[5] = Mz;
+       R[6] = Ax;  R[7] = Ay;  R[8] = Az;
+
+       float invE = 1.0 / (float)sqrt(Ex*Ex + Ey*Ey + Ez*Ez);
+       float c = (Ex*Mx + Ey*My + Ez*Mz) * invE;
+       float s = (Ex*Ax + Ey*Ay + Ez*Az) * invE;
+
+       I[0] = 1;     I[1] = 0;     I[2] = 0;
+       I[3] = 0;     I[4] = c;     I[5] = s;
+       I[6] = 0;     I[7] = -s;    I[8] = c;
+
+       return 0;
+}
+
+int quat_to_orientation(const float *quat, float &azimuth, float &pitch, float &roll)
+{
+       int error;
+       float g[3];
+       float R[9];
+
+       error = quat_to_matrix(quat, R);
+
+       if (error < 0)
+               return error;
+
+       float xyz_z = ARCTAN(R[3], R[0]);
+       float yxz_x = asinf(R[7]);
+       float yxz_y = ARCTAN(-R[6], R[8]);
+       float yxz_z = ARCTAN(-R[1], R[4]);
+
+       float a = fabs(yxz_x / HALF);
+       a = a * a;
+
+       float p = (fabs(yxz_y) / HALF - 1.0);
+
+       if (p < 0)
+               p = 0;
+
+       float v = 1 + (1 - a) / a * p;
+
+       if (v > 20)
+               v = 20;
+
+       if (yxz_x * yxz_y > 0) {
+               if (yxz_z > 0 && xyz_z < 0)
+                       xyz_z += M_PI * 2;
+       } else {
+               if (yxz_z < 0 && xyz_z > 0)
+                       xyz_z -= M_PI * 2;
+       }
+
+       g[0] = (1 - a * v) * yxz_z + (a * v) * xyz_z;
+       g[0] *= -1;
+
+       float tmp = R[7];
+
+       if (tmp > 1.0f)
+               tmp = 1.0f;
+       else if (tmp < -1.0f)
+               tmp = -1.0f;
+
+       g[1] = -asinf(tmp);
+       if (R[8] < 0)
+               g[1] = M_PI - g[1];
+
+       if (g[1] > M_PI)
+               g[1] -= M_PI * 2;
+
+       if ((fabs(R[7]) > QUAT))
+               g[2] = (float) atan2f(R[6], R[7]);
+       else
+               g[2] = (float) atan2f(R[6], R[8]);
+
+       if (g[2] > HALF)
+               g[2] = M_PI - g[2];
+       else if (g[2] < -HALF)
+               g[2] = -M_PI - g[2];
+
+       g[0] *= RAD2DEGREE;
+       g[1] *= RAD2DEGREE;
+       g[2] *= RAD2DEGREE;
+
+       if (g[0] < 0)
+               g[0] += 360;
+
+       azimuth = g[0];
+       pitch = g[1];
+       roll = g[2];
+
+       return 0;
+}
+