src/gravity/gravity_sensor.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 <stdio.h>
  21 #include <stdlib.h>
  22 #include <unistd.h>
  23 #include <errno.h>
  24 #include <math.h>
  25 #include <time.h>
  26 #include <sys/types.h>
  27 #include <dlfcn.h>
  28 #include <common.h>
  29 #include <sf_common.h>
  30 #include <sensor_base.h>
  31 #include <virtual_sensor.h>
  32 #include <gravity_sensor.h>
  33 #include <sensor_plugin_loader.h>
  34
  35 #define INITIAL_VALUE -1
  36 #define INITIAL_TIME 0
  37 #define TIME_CONSTANT  0.18
  38 #define GRAVITY 9.80665
  39
  40 #define SENSOR_NAME "GRAVITY_SENSOR"
  41
  42 gravity_sensor::gravity_sensor()
  43 : m_accel_sensor(NULL)
  44 , m_x(INITIAL_VALUE)
  45 , m_y(INITIAL_VALUE)
  46 , m_z(INITIAL_VALUE)
  47 , m_time(INITIAL_TIME)
  48 {
  49         m_name = string(SENSOR_NAME);
  50
  51         register_supported_event(GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME);
  52 }
  53
  54 gravity_sensor::~gravity_sensor()
  55 {
  56         INFO("gravity_sensor is destroyed!");
  57 }
  58
  59 bool gravity_sensor::init()
  60 {
  61         m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
  62
  63         if (!m_accel_sensor) {
  64                 ERR("cannot load accel sensor_hal[%s]", sensor_base::get_name());
  65                 return false;
  66         }
  67
  68         INFO("%s is created!", sensor_base::get_name());
  69         return true;
  70 }
  71
  72 sensor_type_t gravity_sensor::get_type(void)
  73 {
  74         return GRAVITY_SENSOR;
  75 }
  76
  77 bool gravity_sensor::on_start(void)
  78 {
  79         AUTOLOCK(m_mutex);
  80
  81         m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
  82         m_accel_sensor->start();
  83
  84         activate();
  85         return true;
  86 }
  87
  88 bool gravity_sensor::on_stop(void)
  89 {
  90         AUTOLOCK(m_mutex);
  91
  92         m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
  93         m_accel_sensor->stop();
  94
  95         deactivate();
  96         return true;
  97 }
  98
  99 bool gravity_sensor::add_interval(int client_id, unsigned int interval, bool is_processor)
 100 {
 101         m_accel_sensor->add_interval(client_id, interval, true);
 102         return sensor_base::add_interval(client_id, interval, true);
 103 }
 104
 105 bool gravity_sensor::delete_interval(int client_id, bool is_processor)
 106 {
 107         m_accel_sensor->delete_interval(client_id, true);
 108         return sensor_base::delete_interval(client_id, true);
 109 }
 110
 111 void gravity_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
 112 {
 113         if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) {
 114                 float x, y, z;
 115                 calibrate_gravity(event, x, y, z);
 116
 117                 sensor_event_t event;
 118                 event.event_type = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME;
 119                 event.data.data_accuracy = SENSOR_ACCURACY_GOOD;
 120                 event.data.data_unit_idx = SENSOR_UNIT_METRE_PER_SECOND_SQUARED;
 121                 event.data.timestamp = m_time;
 122                 event.data.values_num = 3;
 123                 event.data.values[0] = x;
 124                 event.data.values[1] = y;
 125                 event.data.values[2] = z;
 126                 outs.push_back(event);
 127
 128                 AUTOLOCK(m_value_mutex);
 129                 m_x = x;
 130                 m_y = y;
 131                 m_z = z;
 132
 133                 return;
 134         }
 135 }
 136
 137 int gravity_sensor::get_sensor_data(const unsigned int data_id, sensor_data_t &data)
 138 {
 139         if (data_id != GRAVITY_BASE_DATA_SET)
 140                 return -1;
 141
 142         AUTOLOCK(m_value_mutex);
 143
 144         data.data_accuracy = SENSOR_ACCURACY_GOOD;
 145         data.data_unit_idx = SENSOR_UNIT_METRE_PER_SECOND_SQUARED;
 146         data.time_stamp = m_time;
 147         data.values_num = 3;
 148         data.values[0] = m_x;
 149         data.values[1] = m_y;
 150         data.values[2] = m_z;
 151
 152         return 0;
 153 }
 154
 155 bool gravity_sensor::get_properties(const unsigned int type, sensor_properties_t &properties)
 156 {
 157         m_accel_sensor->get_properties(type, properties);
 158
 159         if (type != GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME)
 160                 return true;
 161
 162         properties.sensor_min_range = properties.sensor_min_range / GRAVITY;
 163         properties.sensor_max_range = properties.sensor_max_range / GRAVITY;
 164         properties.sensor_resolution = properties.sensor_resolution / GRAVITY;
 165         strncpy(properties.sensor_name, "Gravity Sensor", MAX_KEY_LENGTH);
 166
 167         return true;
 168 }
 169
 170 void gravity_sensor::calibrate_gravity(const sensor_event_t &raw, float &x, float &y, float &z)
 171 {
 172         unsigned long long timestamp;
 173         float dt;
 174         float alpha;
 175         float last_x = 0, last_y = 0, last_z = 0;
 176
 177         {
 178                 AUTOLOCK(m_value_mutex);
 179                 last_x = m_x;
 180                 last_y = m_y;
 181                 last_z = m_z;
 182         }
 183
 184         timestamp = get_timestamp();
 185         dt = (timestamp - m_time) / US_TO_SEC;
 186         m_time = timestamp;
 187         alpha = TIME_CONSTANT / (TIME_CONSTANT + dt);
 188
 189         if (dt > 1.0)
 190                 alpha = 0.0;
 191
 192         x = (alpha * last_x) + ((1 - alpha) * raw.data.values[0] / GRAVITY);
 193         y = (alpha * last_y) + ((1 - alpha) * raw.data.values[1] / GRAVITY);
 194         z = (alpha * last_z) + ((1 - alpha) * raw.data.values[2] / GRAVITY);
 195 }
 196
 197 extern "C" void *create(void)
 198 {
 199         gravity_sensor *inst;
 200
 201         try {
 202                 inst = new gravity_sensor();
 203         } catch (int ErrNo) {
 204                 ERR("Failed to create gravity_sensor class, errno : %d, errstr : %s", err, strerror(err));
 205                 return NULL;
 206         }
 207
 208         return (void *)inst;
 209 }
 210
 211 extern "C" void destroy(void *inst)
 212 {
 213         delete (gravity_sensor *)inst;
 214 }