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