src/sensor/gravity/gravity_comp_sensor.cpp

   1 /*
   2  * sensord
   3  *
   4  * Copyright (c) 2017 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 "gravity_comp_sensor.h"
  21
  22 #include <sensor_log.h>
  23 #include <sensor_types.h>
  24 #include <fusion_util.h>
  25 #include <cmath>
  26
  27 #define NAME_SENSOR "http://tizen.org/sensor/general/gravity/tizen_complementary"
  28 #define NAME_VENDOR "tizen.org"
  29
  30 #define SRC_ID_ACC   0x1
  31 #define SRC_STR_ACC  "http://tizen.org/sensor/general/accelerometer"
  32
  33 #define SRC_ID_GYRO  0x2
  34 #define SRC_STR_GYRO "http://tizen.org/sensor/general/gyroscope"
  35
  36 #define GRAVITY 9.80665
  37
  38 #define PHASE_ACCEL_READY 0x01
  39 #define PHASE_GYRO_READY 0x02
  40 #define PHASE_FUSION_READY 0x03
  41 #define US_PER_SEC 1000000
  42 #define MS_PER_SEC 1000
  43 #define INV_ANGLE -1000
  44 #define TAU_LOW 0.4
  45 #define TAU_MID 0.75
  46 #define TAU_HIGH 0.99
  47
  48 #define DEG2RAD(x) ((x) * M_PI / 180.0)
  49 #define NORM(x, y, z) sqrt((x)*(x) + (y)*(y) + (z)*(z))
  50 #define ARCTAN(x, y) ((x) == 0 ? 0 : (y) != 0 ? atan2((x), (y)) : (x) > 0 ? M_PI/2.0 : -M_PI/2.0)
  51
  52 static sensor_info2_t sensor_info = {
  53         id: 0x1,
  54         type: GRAVITY_SENSOR,
  55         uri: NAME_SENSOR,
  56         vendor: NAME_VENDOR,
  57         min_range: -19.6,
  58         max_range: 19.6,
  59         resolution: 0.01,
  60         min_interval: 5,
  61         max_batch_count: 0,
  62         wakeup_supported: false,
  63         privilege:"",
  64 };
  65
  66 static required_sensor_s required_sensors[] = {
  67         {SRC_ID_ACC,  SRC_STR_ACC},
  68         {SRC_ID_GYRO, SRC_STR_GYRO},
  69 };
  70
  71 gravity_comp_sensor::gravity_comp_sensor()
  72 : m_fusion_phase(0)
  73 , m_x(-1)
  74 , m_y(-1)
  75 , m_z(-1)
  76 , m_accuracy(-1)
  77 , m_time(0)
  78 , m_accel_mag(0)
  79 , m_time_new(0)
  80 {
  81 }
  82
  83 gravity_comp_sensor::~gravity_comp_sensor()
  84 {
  85 }
  86
  87 int gravity_comp_sensor::get_sensor_info(const sensor_info2_t **info)
  88 {
  89         *info = &sensor_info;
  90         return OP_SUCCESS;
  91 }
  92
  93 int gravity_comp_sensor::get_required_sensors(const required_sensor_s **sensors)
  94 {
  95         *sensors = required_sensors;
  96         return 2;
  97 }
  98
  99 int gravity_comp_sensor::update(uint32_t id, sensor_data_t *data, int len)
 100 {
 101         if (id == SRC_ID_ACC) {
 102                 fusion_set_accel(data);
 103                 m_fusion_phase |= PHASE_ACCEL_READY;
 104         } else if (id == SRC_ID_GYRO) {
 105                 fusion_set_gyro(data);
 106                 m_fusion_phase |= PHASE_GYRO_READY;
 107         }
 108
 109         if (m_fusion_phase != PHASE_FUSION_READY)
 110                 return OP_ERROR;
 111
 112         m_fusion_phase = 0;
 113
 114         fusion_update_angle();
 115         fusion_get_gravity();
 116
 117         return OP_SUCCESS;
 118 }
 119
 120 void gravity_comp_sensor::fusion_set_accel(sensor_data_t *data)
 121 {
 122         double x = data->values[0];
 123         double y = data->values[1];
 124         double z = data->values[2];
 125
 126         m_accel_mag = NORM(x, y, z);
 127
 128         m_angle_n[0] = ARCTAN(z, y);
 129         m_angle_n[1] = ARCTAN(x, z);
 130         m_angle_n[2] = ARCTAN(y, x);
 131
 132         m_accuracy = data->accuracy;
 133         m_time_new = data->timestamp;
 134
 135         _D("AccIn: (%f, %f, %f)", x/m_accel_mag, y/m_accel_mag, z/m_accel_mag);
 136 }
 137
 138 void gravity_comp_sensor::fusion_set_gyro(sensor_data_t *data)
 139 {
 140         m_velocity[0] = -DEG2RAD(data->values[0]);
 141         m_velocity[1] = -DEG2RAD(data->values[1]);
 142         m_velocity[2] = -DEG2RAD(data->values[2]);
 143
 144         m_time_new = data->timestamp;
 145 }
 146
 147 void gravity_comp_sensor::fusion_update_angle(void)
 148 {
 149         _D("AngleIn: (%f, %f, %f)", m_angle_n[0], m_angle_n[1], m_angle_n[2]);
 150         _D("AngAccl: (%f, %f, %f)", m_velocity[0], m_velocity[1], m_velocity[2]);
 151         _D("Angle  : (%f, %f, %f)", m_angle[0], m_angle[1], m_angle[2]);
 152
 153         if (m_angle[0] == INV_ANGLE) {
 154                 /* 1st iteration */
 155                 m_angle[0] = m_angle_n[0];
 156                 m_angle[1] = m_angle_n[1];
 157                 m_angle[2] = m_angle_n[2];
 158         } else {
 159                 complementary(m_time_new - m_time);
 160         }
 161
 162         _D("Angle' : (%f, %f, %f)", m_angle[0], m_angle[1], m_angle[2]);
 163 }
 164
 165 void gravity_comp_sensor::fusion_get_gravity(void)
 166 {
 167         double x = 0, y = 0, z = 0;
 168         double norm;
 169         double vec[3][3];
 170
 171         /* Rotating along y-axis then z-axis */
 172         vec[0][2] = cos(m_angle[1]);
 173         vec[0][0] = sin(m_angle[1]);
 174         vec[0][1] = vec[0][0] * tan(m_angle[2]);
 175
 176         /* Rotating along z-axis then x-axis */
 177         vec[1][0] = cos(m_angle[2]);
 178         vec[1][1] = sin(m_angle[2]);
 179         vec[1][2] = vec[1][1] * tan(m_angle[0]);
 180
 181         /* Rotating along x-axis then y-axis */
 182         vec[2][1] = cos(m_angle[0]);
 183         vec[2][2] = sin(m_angle[0]);
 184         vec[2][0] = vec[2][2] * tan(m_angle[1]);
 185
 186         /* Normalize */
 187         for (int i = 0; i < 3; ++i) {
 188                 norm = NORM(vec[i][0], vec[i][1], vec[i][2]);
 189                 vec[i][0] /= norm;
 190                 vec[i][1] /= norm;
 191                 vec[i][2] /= norm;
 192                 x += vec[i][0];
 193                 y += vec[i][1];
 194                 z += vec[i][2];
 195         }
 196
 197         norm = NORM(x, y, z);
 198
 199         m_x = x / norm * GRAVITY;
 200         m_y = y / norm * GRAVITY;
 201         m_z = z / norm * GRAVITY;
 202         m_time = m_time_new;
 203 }
 204
 205 void gravity_comp_sensor::complementary(unsigned long long time_diff)
 206 {
 207         double err = fabs(m_accel_mag - GRAVITY) / GRAVITY;
 208         double tau = (err < 0.1 ? TAU_LOW : err > 0.9 ? TAU_HIGH : TAU_MID);
 209         double delta_t = (double)time_diff/ US_PER_SEC;
 210         double alpha = tau / (tau + delta_t);
 211
 212         _D("mag, err, tau, dt, alpha = %f, %f, %f, %f, %f", m_accel_mag, err, tau, delta_t, alpha);
 213
 214         m_angle[0] = complementary(m_angle[0], m_angle_n[0], m_velocity[0], delta_t, alpha);
 215         m_angle[1] = complementary(m_angle[1], m_angle_n[1], m_velocity[1], delta_t, alpha);
 216         m_angle[2] = complementary(m_angle[2], m_angle_n[2], m_velocity[2], delta_t, alpha);
 217 }
 218
 219 double gravity_comp_sensor::complementary(double angle, double angle_in, double vel, double delta_t, double alpha)
 220 {
 221         double s, c;
 222         angle = angle + vel * delta_t;
 223         s = alpha * sin(angle) + (1 - alpha) * sin(angle_in);
 224         c = alpha * cos(angle) + (1 - alpha) * cos(angle_in);
 225         return ARCTAN(s, c);
 226 }
 227
 228 int gravity_comp_sensor::get_data(sensor_data_t **data, int *len)
 229 {
 230         sensor_data_t *sensor_data;
 231         sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 232         retvm_if(!sensor_data, -ENOMEM, "Failed to allocate memory");
 233
 234         sensor_data->accuracy = m_accuracy;
 235         sensor_data->timestamp = m_time;
 236         sensor_data->value_count = 3;
 237         sensor_data->values[0] = m_x;
 238         sensor_data->values[1] = m_y;
 239         sensor_data->values[2] = m_z;
 240
 241         *data = sensor_data;
 242         *len = sizeof(sensor_data_t);
 243
 244         return 0;
 245 }