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