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