src/fusion-sensor/auto_rotation/auto_rotation_alg_emul.cpp

   1 /*
   2  * sensord
   3  *
   4  * Copyright (c) 2014 Samsung Electronics Co., Ltd.
   5  *
   6  * Licensed under the Apache License, Version 2.0 (the "License");
   7  * you may not use this file except in compliance with the License.
   8  * You may obtain a copy of the License at
   9  *
  10  * http://www.apache.org/licenses/LICENSE-2.0
  11  *
  12  * Unless required by applicable law or agreed to in writing, software
  13  * distributed under the License is distributed on an "AS IS" BASIS,
  14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  15  * See the License for the specific language governing permissions and
  16  * limitations under the License.
  17  *
  18  */
  19
  20 #include "auto_rotation_alg_emul.h"
  21
  22 #include <math.h>
  23 #include <stdlib.h>
  24 #include <sensor_types.h>
  25
  26 #define ROTATION_RULE_CNT 4
  27
  28 struct rotation_rule {
  29         int tilt;
  30         int angle;
  31 };
  32
  33 struct rotation_rule rot_rule[ROTATION_RULE_CNT] = {
  34         {40, 80},
  35         {50, 70},
  36         {60, 65},
  37         {90, 60},
  38 };
  39
  40 auto_rotation_alg_emul::auto_rotation_alg_emul()
  41 {
  42 }
  43
  44 auto_rotation_alg_emul::~auto_rotation_alg_emul()
  45 {
  46         close();
  47 }
  48
  49 int auto_rotation_alg_emul::convert_rotation(int prev_rotation,
  50                 float acc_pitch, float acc_theta)
  51 {
  52         const int ROTATION_0 = 0;
  53         const int ROTATION_90 = 90;
  54         const int ROTATION_180 = 180;
  55         const int ROTATION_360 = 360;
  56         const int TILT_MIN = 30;
  57         int tilt;
  58         int angle;
  59
  60         int new_rotation = AUTO_ROTATION_DEGREE_UNKNOWN;
  61
  62         for (int i = 0; i < ROTATION_RULE_CNT; ++i) {
  63                 tilt = rot_rule[i].tilt;
  64
  65                 if ((acc_pitch < TILT_MIN) || (acc_pitch > tilt))
  66                         continue;
  67
  68                 if ((prev_rotation == AUTO_ROTATION_DEGREE_0) || (prev_rotation == AUTO_ROTATION_DEGREE_180))
  69                         angle = rot_rule[i].angle;
  70                 else
  71                         angle = ROTATION_90 - rot_rule[i].angle;
  72
  73                 if ((acc_theta >= ROTATION_360 - angle && acc_theta <= ROTATION_360 - 1) ||
  74                         (acc_theta >= ROTATION_0 && acc_theta <= ROTATION_0 + angle))
  75                         new_rotation = AUTO_ROTATION_DEGREE_0;
  76                 else if (acc_theta >= ROTATION_0 + angle && acc_theta <= ROTATION_180 - angle)
  77                         new_rotation = AUTO_ROTATION_DEGREE_90;
  78                 else if (acc_theta >= ROTATION_180 - angle && acc_theta <= ROTATION_180 + angle)
  79                         new_rotation = AUTO_ROTATION_DEGREE_180;
  80                 else if (acc_theta >= ROTATION_180 + angle && acc_theta <= ROTATION_360 - angle)
  81                         new_rotation = AUTO_ROTATION_DEGREE_270;
  82
  83                 break;
  84         }
  85
  86         return new_rotation;
  87 }
  88
  89 bool auto_rotation_alg_emul::get_rotation(float acc[3],
  90                 unsigned long long timestamp, int prev_rotation, int &cur_rotation)
  91 {
  92         const int ROTATION_90 = 90;
  93         const int RADIAN = 57.29747;
  94
  95         double atan_value;
  96         int acc_theta;
  97         int acc_pitch;
  98         double realg;
  99         float x, y, z;
 100
 101         x = acc[0];
 102         y = acc[1];
 103         z = acc[2];
 104
 105         atan_value = atan2(x, y);
 106         acc_theta = (int)(atan_value * (RADIAN) + 360) % 360;
 107         realg = (double)sqrt((x * x) + (y * y) + (z * z));
 108         acc_pitch = ROTATION_90 - abs((int) (asin(z / realg) * RADIAN));
 109
 110         cur_rotation = convert_rotation(prev_rotation, acc_pitch, acc_theta);
 111
 112         if (cur_rotation == AUTO_ROTATION_DEGREE_UNKNOWN)
 113                 return false;
 114
 115         if (cur_rotation != prev_rotation)
 116                 return true;
 117
 118         return false;
 119 }