src/sensor/gravity/gravity_lowpass_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_lowpass_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_lowpass"
  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 GRAVITY 9.80665
  34 #define US_PER_SEC 1000000
  35 #define TAU_LOW 0.4
  36 #define TAU_MID 0.75
  37 #define TAU_HIGH 0.99
  38 #define NORM(x, y, z) sqrt((x)*(x) + (y)*(y) + (z)*(z))
  39
  40 static sensor_info2_t sensor_info = {
  41         id: 0x1,
  42         type: GRAVITY_SENSOR,
  43         uri: NAME_SENSOR,
  44         vendor: NAME_VENDOR,
  45         min_range: -19.6,
  46         max_range: 19.6,
  47         resolution: 0.01,
  48         min_interval: 5,
  49         max_batch_count: 0,
  50         wakeup_supported: false,
  51         privilege:"",
  52 };
  53
  54 static required_sensor_s required_sensors[] = {
  55         {SRC_ID_ACC, SRC_STR_ACC}
  56 };
  57
  58 gravity_lowpass_sensor::gravity_lowpass_sensor()
  59 : m_x(-1)
  60 , m_y(-1)
  61 , m_z(-1)
  62 , m_accuracy(-1)
  63 , m_time(0)
  64 {
  65         _I("Gravity Sensor is created!");
  66 }
  67
  68 gravity_lowpass_sensor::~gravity_lowpass_sensor()
  69 {
  70 }
  71
  72 int gravity_lowpass_sensor::get_sensor_info(const sensor_info2_t **info)
  73 {
  74         *info = &sensor_info;
  75         return OP_SUCCESS;
  76 }
  77
  78 int gravity_lowpass_sensor::get_required_sensors(const required_sensor_s **sensors)
  79 {
  80         *sensors = required_sensors;
  81         return 1;
  82 }
  83
  84 int gravity_lowpass_sensor::update(uint32_t id, sensor_data_t *data, int len)
  85 {
  86         float x, y, z, norm, alpha, tau, err;
  87
  88         norm = NORM(data->values[0], data->values[1], data->values[2]);
  89         x = data->values[0] / norm * GRAVITY;
  90         y = data->values[1] / norm * GRAVITY;
  91         z = data->values[2] / norm * GRAVITY;
  92
  93         if (m_time > 0) {
  94                 err = fabs(norm - GRAVITY) / GRAVITY;
  95                 tau = (err < 0.1 ? TAU_LOW : err > 0.9 ? TAU_HIGH : TAU_MID);
  96                 alpha = tau / (tau + (float)(data->timestamp - m_time) / US_PER_SEC);
  97                 x = alpha * m_x + (1 - alpha) * x;
  98                 y = alpha * m_y + (1 - alpha) * y;
  99                 z = alpha * m_z + (1 - alpha) * z;
 100                 norm = NORM(x, y, z);
 101                 x = x / norm * GRAVITY;
 102                 y = y / norm * GRAVITY;
 103                 z = z / norm * GRAVITY;
 104         }
 105
 106         m_time = data->timestamp;
 107         m_accuracy = data->accuracy;
 108         m_x = x;
 109         m_y = y;
 110         m_z = z;
 111
 112         return OP_SUCCESS;
 113 }
 114
 115 int gravity_lowpass_sensor::get_data(sensor_data_t **data, int *len)
 116 {
 117         sensor_data_t *sensor_data;
 118         sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 119         retvm_if(!sensor_data, -ENOMEM, "Failed to allocate memory");
 120
 121         sensor_data->accuracy = m_accuracy;
 122         sensor_data->timestamp = m_time;
 123         sensor_data->value_count = 3;
 124         sensor_data->values[0] = m_x;
 125         sensor_data->values[1] = m_y;
 126         sensor_data->values[2] = m_z;
 127
 128         *data = sensor_data;
 129         *len = sizeof(sensor_data_t);
 130
 131         return 0;
 132 }