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 <gravity_sensor.h>
  31 #include <sensor_plugin_loader.h>
  32 #include <cvirtual_sensor_config.h>
  33
  34 #define INITIAL_VALUE -1
  35 #define GRAVITY 9.80665
  36
  37 #define DEG2RAD (M_PI/180)
  38 #define DEVIATION 0.1
  39
  40 #define SENSOR_NAME "GRAVITY_SENSOR"
  41 #define SENSOR_TYPE_GRAVITY             "GRAVITY"
  42 #define SENSOR_TYPE_ORIENTATION         "ORIENTATION"
  43
  44 #define MS_TO_US 1000
  45 #define MIN_DELIVERY_DIFF_FACTOR 0.75f
  46
  47 #define ELEMENT_NAME                                                                                    "NAME"
  48 #define ELEMENT_VENDOR                                                                                  "VENDOR"
  49 #define ELEMENT_RAW_DATA_UNIT                                                                   "RAW_DATA_UNIT"
  50 #define ELEMENT_DEFAULT_SAMPLING_TIME                                                   "DEFAULT_SAMPLING_TIME"
  51 #define ELEMENT_GRAVITY_SIGN_COMPENSATION                                               "GRAVITY_SIGN_COMPENSATION"
  52 #define ELEMENT_ORIENTATION_DATA_UNIT                                                   "RAW_DATA_UNIT"
  53
  54 gravity_sensor::gravity_sensor()
  55 : m_orientation_sensor(NULL)
  56 , m_x(INITIAL_VALUE)
  57 , m_y(INITIAL_VALUE)
  58 , m_z(INITIAL_VALUE)
  59 {
  60         cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance();
  61
  62         m_name = string(SENSOR_NAME);
  63         m_timestamp = get_timestamp();
  64         register_supported_event(GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME);
  65
  66         if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_VENDOR, m_vendor)) {
  67                 ERR("[VENDOR] is empty\n");
  68                 throw ENXIO;
  69         }
  70
  71         INFO("m_vendor = %s", m_vendor.c_str());
  72
  73         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ORIENTATION_DATA_UNIT, m_orientation_data_unit)) {
  74                 ERR("[ORIENTATION_DATA_UNIT] is empty\n");
  75                 throw ENXIO;
  76         }
  77
  78         INFO("m_orientation_data_unit = %s", m_orientation_data_unit.c_str());
  79
  80         if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) {
  81                 ERR("[RAW_DATA_UNIT] is empty\n");
  82                 throw ENXIO;
  83         }
  84
  85         INFO("m_raw_data_unit = %s", m_raw_data_unit.c_str());
  86
  87         if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) {
  88                 ERR("[DEFAULT_SAMPLING_TIME] is empty\n");
  89                 throw ENXIO;
  90         }
  91
  92         INFO("m_default_sampling_time = %d", m_default_sampling_time);
  93
  94         if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_GRAVITY_SIGN_COMPENSATION, m_gravity_sign_compensation, 3)) {
  95                 ERR("[GRAVITY_SIGN_COMPENSATION] is empty\n");
  96                 throw ENXIO;
  97         }
  98
  99         INFO("m_gravity_sign_compensation = (%d, %d, %d)", m_gravity_sign_compensation[0], m_gravity_sign_compensation[1], m_gravity_sign_compensation[2]);
 100
 101         m_interval = m_default_sampling_time * MS_TO_US;
 102 }
 103
 104 gravity_sensor::~gravity_sensor()
 105 {
 106         INFO("gravity_sensor is destroyed!\n");
 107 }
 108
 109 bool gravity_sensor::init()
 110 {
 111         m_orientation_sensor = sensor_plugin_loader::get_instance().get_sensor(ORIENTATION_SENSOR);
 112
 113         if (!m_orientation_sensor) {
 114                 ERR("Failed to load orientation sensor: 0x%x", m_orientation_sensor);
 115                 return false;
 116         }
 117
 118         INFO("%s is created!", sensor_base::get_name());
 119         return true;
 120 }
 121
 122 sensor_type_t gravity_sensor::get_type(void)
 123 {
 124         return GRAVITY_SENSOR;
 125 }
 126
 127 bool gravity_sensor::on_start(void)
 128 {
 129         AUTOLOCK(m_mutex);
 130
 131         m_orientation_sensor->add_client(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME);
 132         m_orientation_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), true);
 133         m_orientation_sensor->start();
 134
 135         activate();
 136         return true;
 137 }
 138
 139 bool gravity_sensor::on_stop(void)
 140 {
 141         AUTOLOCK(m_mutex);
 142
 143         m_orientation_sensor->delete_client(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME);
 144         m_orientation_sensor->delete_interval((intptr_t)this, true);
 145         m_orientation_sensor->stop();
 146
 147         deactivate();
 148         return true;
 149 }
 150
 151 bool gravity_sensor::add_interval(int client_id, unsigned int interval)
 152 {
 153         AUTOLOCK(m_mutex);
 154         m_orientation_sensor->add_interval(client_id , interval, true);
 155
 156         return sensor_base::add_interval(client_id, interval, true);
 157 }
 158
 159 bool gravity_sensor::delete_interval(int client_id)
 160 {
 161         AUTOLOCK(m_mutex);
 162         m_orientation_sensor->delete_interval(client_id , true);
 163
 164         return sensor_base::delete_interval(client_id, true);
 165 }
 166
 167 void gravity_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
 168 {
 169         sensor_event_t gravity_event;
 170         float pitch, roll, azimuth;
 171
 172         azimuth = event.data.values[0];
 173         pitch = event.data.values[1];
 174         roll = event.data.values[2];
 175
 176         unsigned long long diff_time;
 177
 178         if(m_orientation_data_unit == "DEGREES") {
 179                 azimuth *= DEG2RAD;
 180                 pitch *= DEG2RAD;
 181                 roll *= DEG2RAD;
 182         }
 183
 184         if (event.event_type == ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME) {
 185                 diff_time = event.data.timestamp - m_timestamp;
 186
 187                 if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 188                         return;
 189
 190                 gravity_event.sensor_id = get_id();
 191                 gravity_event.event_type = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME;
 192                 m_timestamp = get_timestamp();
 193                 if ((roll >= (M_PI/2)-DEVIATION && roll <= (M_PI/2)+DEVIATION) ||
 194                                 (roll >= -(M_PI/2)-DEVIATION && roll <= -(M_PI/2)+DEVIATION)) {
 195                         gravity_event.data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll) * cos(azimuth);
 196                         gravity_event.data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(azimuth);
 197                         gravity_event.data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll);
 198                 } else if ((pitch >= (M_PI/2)-DEVIATION && pitch <= (M_PI/2)+DEVIATION) ||
 199                                 (pitch >= -(M_PI/2)-DEVIATION && pitch <= -(M_PI/2)+DEVIATION)) {
 200                         gravity_event.data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(azimuth);
 201                         gravity_event.data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch) * cos(azimuth);
 202                         gravity_event.data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(pitch);
 203                 } else {
 204                         gravity_event.data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll);
 205                         gravity_event.data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch);
 206                         gravity_event.data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll) * cos(pitch);
 207                 }
 208                 gravity_event.data.value_count = 3;
 209                 gravity_event.data.timestamp = m_timestamp;
 210                 gravity_event.data.accuracy = SENSOR_ACCURACY_GOOD;
 211
 212                 push(gravity_event);
 213         }
 214 }
 215
 216 int gravity_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
 217 {
 218         sensor_data_t orientation_data;
 219         float pitch, roll, azimuth;
 220
 221         m_orientation_sensor->get_sensor_data(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME, orientation_data);
 222
 223         azimuth = orientation_data.values[0];
 224         pitch = orientation_data.values[1];
 225         roll = orientation_data.values[2];
 226
 227         if(m_orientation_data_unit == "DEGREES") {
 228                 azimuth *= DEG2RAD;
 229                 pitch *= DEG2RAD;
 230                 roll *= DEG2RAD;
 231         }
 232
 233         if (event_type != GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME)
 234                 return -1;
 235
 236         data.accuracy = SENSOR_ACCURACY_GOOD;
 237         data.timestamp = get_timestamp();
 238         if ((roll >= (M_PI/2)-DEVIATION && roll <= (M_PI/2)+DEVIATION) ||
 239                         (roll >= -(M_PI/2)-DEVIATION && roll <= -(M_PI/2)+DEVIATION)) {
 240                 data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll) * cos(azimuth);
 241                 data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(azimuth);
 242                 data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll);
 243         } else if ((pitch >= (M_PI/2)-DEVIATION && pitch <= (M_PI/2)+DEVIATION) ||
 244                         (pitch >= -(M_PI/2)-DEVIATION && pitch <= -(M_PI/2)+DEVIATION)) {
 245                 data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(azimuth);
 246                 data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch) * cos(azimuth);
 247                 data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(pitch);
 248         } else {
 249                 data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll);
 250                 data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch);
 251                 data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll) * cos(pitch);
 252         }
 253         data.value_count = 3;
 254
 255         return 0;
 256 }
 257
 258 bool gravity_sensor::get_properties(sensor_properties_s &properties)
 259 {
 260         properties.min_range = -GRAVITY;
 261         properties.max_range = GRAVITY;
 262         properties.resolution = 0.000001;
 263         properties.vendor = m_vendor;
 264         properties.name = SENSOR_NAME;
 265
 266         return true;
 267 }
 268
 269 extern "C" void *create(void)
 270 {
 271         gravity_sensor *inst;
 272
 273         try {
 274                 inst = new gravity_sensor();
 275         } catch (int err) {
 276                 ERR("Failed to create gravity_sensor class, errno : %d, errstr : %s", err, strerror(err));
 277                 return NULL;
 278         }
 279
 280         return (void *)inst;
 281 }
 282
 283 extern "C" void destroy(void *inst)
 284 {
 285         delete (gravity_sensor *)inst;
 286 }