src/sensor/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 <sensor_log.h>
  29 #include <tilt_sensor.h>
  30 #include <sensor_loader.h>
  31 #include <orientation_filter.h>
  32 #include <virtual_sensor_config.h>
  33
  34 using std::string;
  35 using std::vector;
  36
  37 #define SENSOR_NAME                     "TILT_SENSOR"
  38 #define SENSOR_TYPE_TILT        "TILT"
  39
  40 #define MIN_DELIVERY_DIFF_FACTOR 0.75f
  41
  42 #define INITIAL_VALUE -1
  43
  44 #define MS_TO_US 1000
  45
  46 #define ELEMENT_NAME                                                                                    "NAME"
  47 #define ELEMENT_VENDOR                                                                                  "VENDOR"
  48 #define ELEMENT_RAW_DATA_UNIT                                                                   "RAW_DATA_UNIT"
  49 #define ELEMENT_DEFAULT_SAMPLING_TIME                                                   "DEFAULT_SAMPLING_TIME"
  50 #define ELEMENT_PITCH_ROTATION_COMPENSATION                                             "PITCH_ROTATION_COMPENSATION"
  51 #define ELEMENT_ROLL_ROTATION_COMPENSATION                                              "ROLL_ROTATION_COMPENSATION"
  52
  53 tilt_sensor::tilt_sensor()
  54 : m_accel_sensor(NULL)
  55 , m_fusion_sensor(NULL)
  56 , m_time(0)
  57 {
  58         virtual_sensor_config &config = virtual_sensor_config::get_instance();
  59
  60         m_name = string(SENSOR_NAME);
  61         register_supported_event(TILT_RAW_DATA_EVENT);
  62
  63         if (!config.get(SENSOR_TYPE_TILT, ELEMENT_VENDOR, m_vendor)) {
  64                 _W("[VENDOR] is empty\n");
  65                 throw ENXIO;
  66         }
  67
  68         _I("m_vendor = %s", m_vendor.c_str());
  69
  70         if (!config.get(SENSOR_TYPE_TILT, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) {
  71                 _W("[RAW_DATA_UNIT] is empty\n");
  72                 throw ENXIO;
  73         }
  74
  75         _I("m_raw_data_unit = %s", m_raw_data_unit.c_str());
  76
  77         if (!config.get(SENSOR_TYPE_TILT, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) {
  78                 _W("[DEFAULT_SAMPLING_TIME] is empty\n");
  79                 throw ENXIO;
  80         }
  81
  82         _I("m_default_sampling_time = %d", m_default_sampling_time);
  83
  84         if (!config.get(SENSOR_TYPE_TILT, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) {
  85                 _W("[PITCH_ROTATION_COMPENSATION] is empty\n");
  86                 throw ENXIO;
  87         }
  88
  89         _I("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation);
  90
  91         if (!config.get(SENSOR_TYPE_TILT, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) {
  92                 _W("[ROLL_ROTATION_COMPENSATION] is empty\n");
  93                 throw ENXIO;
  94         }
  95
  96         _I("m_roll_rotation_compensation = %d", m_roll_rotation_compensation);
  97
  98         m_interval = m_default_sampling_time * MS_TO_US;
  99 }
 100
 101 tilt_sensor::~tilt_sensor()
 102 {
 103         _I("tilt_sensor is destroyed!\n");
 104 }
 105
 106 bool tilt_sensor::init(void)
 107 {
 108         m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
 109         m_fusion_sensor = sensor_loader::get_instance().get_sensor(FUSION_SENSOR);
 110
 111         if (!m_accel_sensor || !m_fusion_sensor) {
 112                 _W("Failed to load sensors,  accel: %#x, fusion: %#x",
 113                         m_accel_sensor, m_fusion_sensor);
 114                 return false;
 115         }
 116
 117         _I("%s is created!\n", sensor_base::get_name());
 118
 119         return true;
 120 }
 121
 122 void tilt_sensor::get_types(vector<sensor_type_t> &types)
 123 {
 124         types.push_back(TILT_SENSOR);
 125 }
 126
 127 bool tilt_sensor::on_start(void)
 128 {
 129         AUTOLOCK(m_mutex);
 130
 131         m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT);
 132         m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
 133         m_accel_sensor->start();
 134
 135         m_fusion_sensor->register_supported_event(FUSION_EVENT);
 136         m_fusion_sensor->register_supported_event(FUSION_TILT_ENABLED);
 137         m_fusion_sensor->add_client(FUSION_EVENT);
 138         m_fusion_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
 139         m_fusion_sensor->start();
 140
 141         activate();
 142         return true;
 143 }
 144
 145 bool tilt_sensor::on_stop(void)
 146 {
 147         AUTOLOCK(m_mutex);
 148
 149         m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT);
 150         m_accel_sensor->delete_interval((intptr_t)this, false);
 151         m_accel_sensor->stop();
 152
 153         m_fusion_sensor->delete_client(FUSION_EVENT);
 154         m_fusion_sensor->delete_interval((intptr_t)this, false);
 155         m_fusion_sensor->unregister_supported_event(FUSION_EVENT);
 156         m_fusion_sensor->unregister_supported_event(FUSION_TILT_ENABLED);
 157         m_fusion_sensor->stop();
 158
 159         deactivate();
 160         return true;
 161 }
 162
 163 bool tilt_sensor::add_interval(int client_id, unsigned int interval)
 164 {
 165         AUTOLOCK(m_mutex);
 166
 167         m_accel_sensor->add_interval(client_id, interval, false);
 168         m_fusion_sensor->add_interval(client_id, interval, false);
 169
 170         return sensor_base::add_interval(client_id, interval, false);
 171 }
 172
 173 bool tilt_sensor::delete_interval(int client_id)
 174 {
 175         AUTOLOCK(m_mutex);
 176
 177         m_accel_sensor->delete_interval(client_id, false);
 178         m_fusion_sensor->delete_interval(client_id, false);
 179
 180         return sensor_base::delete_interval(client_id, false);
 181 }
 182
 183 void tilt_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
 184 {
 185         sensor_event_t tilt_event;
 186         unsigned long long diff_time;
 187
 188         if (event.event_type == FUSION_EVENT) {
 189                 diff_time = event.data.timestamp - m_time;
 190
 191                 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 192                         return;
 193
 194                 quaternion<float> quat(event.data.values[0], event.data.values[1],
 195                                 event.data.values[2], event.data.values[3]);
 196
 197                 euler_angles<float> euler = quat2euler(quat);
 198
 199                 if(m_raw_data_unit == "DEGREES") {
 200                         euler = rad2deg(euler);
 201                 }
 202
 203                 euler.m_ang.m_vec[0] *= m_pitch_rotation_compensation;
 204                 euler.m_ang.m_vec[1] *= m_roll_rotation_compensation;
 205
 206                 m_time = get_timestamp();
 207                 tilt_event.sensor_id = get_id();
 208                 tilt_event.event_type = TILT_RAW_DATA_EVENT;
 209                 tilt_event.data.accuracy = event.data.accuracy;
 210                 tilt_event.data.timestamp = m_time;
 211                 tilt_event.data.value_count = 2;
 212                 tilt_event.data.values[0] = euler.m_ang.m_vec[0];
 213                 tilt_event.data.values[1] = euler.m_ang.m_vec[1];
 214
 215                 push(tilt_event);
 216         }
 217
 218         return;
 219 }
 220
 221 int tilt_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
 222 {
 223         sensor_data_t fusion_data;
 224
 225         if (event_type != TILT_RAW_DATA_EVENT)
 226                 return -1;
 227
 228         m_fusion_sensor->get_sensor_data(FUSION_TILT_ENABLED, fusion_data);
 229
 230         quaternion<float> quat(fusion_data.values[0], fusion_data.values[1],
 231                         fusion_data.values[2], fusion_data.values[3]);
 232
 233         euler_angles<float> euler = quat2euler(quat);
 234
 235         if(m_raw_data_unit == "DEGREES") {
 236                 euler = rad2deg(euler);
 237         }
 238
 239         data.accuracy = fusion_data.accuracy;
 240         data.timestamp = get_timestamp();
 241         data.value_count = 2;
 242         data.values[0] = euler.m_ang.m_vec[0];
 243         data.values[1] = euler.m_ang.m_vec[1];
 244
 245         data.values[0] *= m_pitch_rotation_compensation;
 246         data.values[1] *= m_roll_rotation_compensation;
 247
 248         return 0;
 249 }
 250
 251 bool tilt_sensor::get_properties(sensor_type_t sensor_type, sensor_properties_s &properties)
 252 {
 253         if(m_raw_data_unit == "DEGREES") {
 254                 properties.min_range = -180;
 255                 properties.max_range = 180;
 256         } else {
 257                 properties.min_range = -PI;
 258                 properties.max_range = PI;
 259         }
 260         properties.resolution = 0.000001;
 261         properties.vendor = m_vendor;
 262         properties.name = SENSOR_NAME;
 263         properties.min_interval = 1;
 264         properties.fifo_count = 0;
 265         properties.max_batch_count = 0;
 266
 267         return true;
 268 }