src/tilt/tilt_sensor.cpp

   1 /*
   2  * sensord
   3  *
   4  * Copyright (c) 2015 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 <tilt_sensor.h>
  31 #include <sensor_plugin_loader.h>
  32 #include <orientation_filter.h>
  33 #include <cvirtual_sensor_config.h>
  34
  35 #define SENSOR_NAME                     "TILT_SENSOR"
  36 #define SENSOR_TYPE_TILT        "TILT"
  37
  38 #define MIN_DELIVERY_DIFF_FACTOR 0.75f
  39
  40 #define INITIAL_VALUE -1
  41
  42 #define MS_TO_US 1000
  43
  44 #define ELEMENT_NAME                                                                                    "NAME"
  45 #define ELEMENT_VENDOR                                                                                  "VENDOR"
  46 #define ELEMENT_RAW_DATA_UNIT                                                                   "RAW_DATA_UNIT"
  47 #define ELEMENT_DEFAULT_SAMPLING_TIME                                                   "DEFAULT_SAMPLING_TIME"
  48 #define ELEMENT_PITCH_ROTATION_COMPENSATION                                             "PITCH_ROTATION_COMPENSATION"
  49 #define ELEMENT_ROLL_ROTATION_COMPENSATION                                              "ROLL_ROTATION_COMPENSATION"
  50
  51 void pre_process_data(sensor_data<float> &data_out, const float *data_in, float *bias, int *sign, float scale)
  52 {
  53         data_out.m_data.m_vec[0] = sign[0] * (data_in[0] - bias[0]) / scale;
  54         data_out.m_data.m_vec[1] = sign[1] * (data_in[1] - bias[1]) / scale;
  55         data_out.m_data.m_vec[2] = sign[2] * (data_in[2] - bias[2]) / scale;
  56 }
  57
  58 tilt_sensor::tilt_sensor()
  59 : m_accel_sensor(NULL)
  60 , m_fusion_sensor(NULL)
  61 , m_time(0)
  62 {
  63         cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance();
  64
  65         m_name = string(SENSOR_NAME);
  66         register_supported_event(TILT_RAW_DATA_EVENT);
  67         m_enable_tilt = 0;
  68
  69         if (!config.get(SENSOR_TYPE_TILT, ELEMENT_VENDOR, m_vendor)) {
  70                 ERR("[VENDOR] is empty\n");
  71                 throw ENXIO;
  72         }
  73
  74         INFO("m_vendor = %s", m_vendor.c_str());
  75
  76         if (!config.get(SENSOR_TYPE_TILT, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) {
  77                 ERR("[RAW_DATA_UNIT] is empty\n");
  78                 throw ENXIO;
  79         }
  80
  81         INFO("m_raw_data_unit = %s", m_raw_data_unit.c_str());
  82
  83         if (!config.get(SENSOR_TYPE_TILT, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) {
  84                 ERR("[DEFAULT_SAMPLING_TIME] is empty\n");
  85                 throw ENXIO;
  86         }
  87
  88         INFO("m_default_sampling_time = %d", m_default_sampling_time);
  89
  90         if (!config.get(SENSOR_TYPE_TILT, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) {
  91                 ERR("[PITCH_ROTATION_COMPENSATION] is empty\n");
  92                 throw ENXIO;
  93         }
  94
  95         INFO("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation);
  96
  97         if (!config.get(SENSOR_TYPE_TILT, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) {
  98                 ERR("[ROLL_ROTATION_COMPENSATION] is empty\n");
  99                 throw ENXIO;
 100         }
 101
 102         INFO("m_roll_rotation_compensation = %d", m_roll_rotation_compensation);
 103
 104         m_interval = m_default_sampling_time * MS_TO_US;
 105
 106         INFO("ramasamy 1: Constructor completed");
 107 }
 108
 109 tilt_sensor::~tilt_sensor()
 110 {
 111         INFO("tilt_sensor is destroyed!\n");
 112 }
 113
 114 bool tilt_sensor::init(void)
 115 {
 116         m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
 117         m_fusion_sensor = sensor_plugin_loader::get_instance().get_sensor(FUSION_SENSOR);
 118
 119         if (!m_accel_sensor || !m_fusion_sensor) {
 120                 ERR("Failed to load sensors,  accel: 0x%x, fusion: 0x%x",
 121                         m_accel_sensor, m_fusion_sensor);
 122                 return false;
 123         }
 124
 125         INFO("%s is created!\n", sensor_base::get_name());
 126
 127         INFO("ramasamy 2: Init completed");
 128
 129         return true;
 130 }
 131
 132 sensor_type_t tilt_sensor::get_type(void)
 133 {
 134         return TILT_SENSOR;
 135 }
 136
 137 bool tilt_sensor::on_start(void)
 138 {
 139         AUTOLOCK(m_mutex);
 140
 141         m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT);
 142         m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
 143         m_accel_sensor->start();
 144
 145         m_fusion_sensor->register_supported_event(FUSION_EVENT);
 146         m_fusion_sensor->register_supported_event(FUSION_TILT_ENABLED);
 147         m_fusion_sensor->add_client(FUSION_EVENT);
 148         m_fusion_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
 149         m_fusion_sensor->start();
 150
 151         INFO("ramasamy 3: ON_START completed");
 152
 153         activate();
 154         return true;
 155 }
 156
 157 bool tilt_sensor::on_stop(void)
 158 {
 159         AUTOLOCK(m_mutex);
 160
 161         m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT);
 162         m_accel_sensor->delete_interval((intptr_t)this, false);
 163         m_accel_sensor->stop();
 164
 165         m_fusion_sensor->delete_client(FUSION_EVENT);
 166         m_fusion_sensor->delete_interval((intptr_t)this, false);
 167         m_fusion_sensor->unregister_supported_event(FUSION_EVENT);
 168         m_fusion_sensor->unregister_supported_event(FUSION_TILT_ENABLED);
 169         m_fusion_sensor->stop();
 170
 171         INFO("ramasamy 4: ON_STOP completed");
 172
 173         deactivate();
 174         return true;
 175 }
 176
 177 bool tilt_sensor::add_interval(int client_id, unsigned int interval)
 178 {
 179         AUTOLOCK(m_mutex);
 180
 181         m_accel_sensor->add_interval(client_id, interval, false);
 182         m_fusion_sensor->add_interval(client_id, interval, false);
 183
 184         INFO("ramasamy 5: ADD_INTERVAL completed");
 185
 186         return sensor_base::add_interval(client_id, interval, false);
 187 }
 188
 189 bool tilt_sensor::delete_interval(int client_id)
 190 {
 191         AUTOLOCK(m_mutex);
 192
 193         m_accel_sensor->delete_interval(client_id, false);
 194         m_fusion_sensor->delete_interval(client_id, false);
 195
 196         INFO("ramasamy 6: DELETE_INTERVAL completed");
 197
 198         return sensor_base::delete_interval(client_id, false);
 199 }
 200
 201 void tilt_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
 202 {
 203         sensor_event_t tilt_event;
 204         unsigned long long diff_time;
 205
 206         INFO("ramasamy 7: SYNTHESIZE STARTED");
 207
 208         if (event.event_type == FUSION_EVENT) {
 209
 210                 INFO("ramasamy 8: SYNTHESIZE FUSION EVENT RECEIVED");
 211                 diff_time = event.data.timestamp - m_time;
 212
 213                 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 214                         return;
 215
 216                 quaternion<float> quat(event.data.values[0], event.data.values[1],
 217                                 event.data.values[2], event.data.values[3]);
 218
 219                 euler_angles<float> euler = quat2euler(quat);
 220
 221                 if(m_raw_data_unit == "DEGREES") {
 222                         euler = rad2deg(euler);
 223                 }
 224
 225                 euler.m_ang.m_vec[0] *= m_pitch_rotation_compensation;
 226                 euler.m_ang.m_vec[1] *= m_roll_rotation_compensation;
 227
 228                 m_time = get_timestamp();
 229                 tilt_event.sensor_id = get_id();
 230                 tilt_event.event_type = TILT_RAW_DATA_EVENT;
 231                 tilt_event.data.accuracy = event.data.accuracy;
 232                 tilt_event.data.timestamp = m_time;
 233                 tilt_event.data.value_count = 2;
 234                 tilt_event.data.values[0] = euler.m_ang.m_vec[0];
 235                 tilt_event.data.values[1] = euler.m_ang.m_vec[1];
 236
 237                 INFO("ramasamy 8: SYNTHESIZE FUSION EVENT SENT");
 238
 239                 push(tilt_event);
 240         }
 241
 242         return;
 243 }
 244
 245 int tilt_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
 246 {
 247         sensor_data_t fusion_data;
 248
 249         if (event_type != TILT_RAW_DATA_EVENT)
 250                 return -1;
 251
 252         m_fusion_sensor->get_sensor_data(FUSION_TILT_ENABLED, fusion_data);
 253
 254         quaternion<float> quat(fusion_data.values[0], fusion_data.values[1],
 255                         fusion_data.values[2], fusion_data.values[3]);
 256
 257         euler_angles<float> euler = quat2euler(quat);
 258
 259         if(m_raw_data_unit == "DEGREES") {
 260                 euler = rad2deg(euler);
 261         }
 262
 263         data.accuracy = fusion_data.accuracy;
 264         data.timestamp = get_timestamp();
 265         data.value_count = 2;
 266         data.values[0] = euler.m_ang.m_vec[0];
 267         data.values[1] = euler.m_ang.m_vec[1];
 268
 269         data.values[0] *= m_pitch_rotation_compensation;
 270         data.values[1] *= m_roll_rotation_compensation;
 271
 272         return 0;
 273 }
 274
 275 bool tilt_sensor::get_properties(sensor_properties_s &properties)
 276 {
 277         if(m_raw_data_unit == "DEGREES") {
 278                 properties.min_range = -180;
 279                 properties.max_range = 180;
 280         }
 281         else {
 282                 properties.min_range = -PI;
 283                 properties.max_range = PI;
 284         }
 285         properties.resolution = 0.000001;
 286         properties.vendor = m_vendor;
 287         properties.name = SENSOR_NAME;
 288         properties.min_interval = 1;
 289         properties.fifo_count = 0;
 290         properties.max_batch_count = 0;
 291
 292         return true;
 293 }
 294
 295 extern "C" sensor_module* create(void)
 296 {
 297         tilt_sensor *sensor;
 298
 299         try {
 300                 sensor = new(std::nothrow) tilt_sensor;
 301         } catch (int err) {
 302                 ERR("Failed to create module, err: %d, cause: %s", err, strerror(err));
 303                 return NULL;
 304         }
 305
 306         sensor_module *module = new(std::nothrow) sensor_module;
 307         retvm_if(!module || !sensor, NULL, "Failed to allocate memory");
 308
 309         module->sensors.push_back(sensor);
 310         return module;
 311 }