src/server/plugins/rotation_vector/gaming_rv/gaming_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 <gaming_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 "GAMING_RV_SENSOR"
  38 #define SENSOR_TYPE_GAMING_RV "GAMING_ROTATION_VECTOR"
  39
  40 #define ACCELEROMETER_ENABLED 0x01
  41 #define GYROSCOPE_ENABLED 0x02
  42 #define GAMING_RV_ENABLED 3
  43
  44 #define INITIAL_VALUE -1
  45
  46 #define MS_TO_US 1000
  47
  48 #define ELEMENT_NAME                                                                                    "NAME"
  49 #define ELEMENT_VENDOR                                                                                  "VENDOR"
  50 #define ELEMENT_RAW_DATA_UNIT                                                                   "RAW_DATA_UNIT"
  51 #define ELEMENT_DEFAULT_SAMPLING_TIME                                                   "DEFAULT_SAMPLING_TIME"
  52 #define ELEMENT_ACCEL_STATIC_BIAS                                                               "ACCEL_STATIC_BIAS"
  53 #define ELEMENT_GYRO_STATIC_BIAS                                                                "GYRO_STATIC_BIAS"
  54 #define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION                   "ACCEL_ROTATION_DIRECTION_COMPENSATION"
  55 #define ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION                    "GYRO_ROTATION_DIRECTION_COMPENSATION"
  56 #define ELEMENT_ACCEL_SCALE                                                                             "ACCEL_SCALE"
  57 #define ELEMENT_GYRO_SCALE                                                                              "GYRO_SCALE"
  58
  59 gaming_rv_sensor::gaming_rv_sensor()
  60 : m_accel_sensor(NULL)
  61 , m_gyro_sensor(NULL)
  62 , m_accuracy(-1)
  63 , m_time(0)
  64 {
  65         virtual_sensor_config &config = virtual_sensor_config::get_instance();
  66
  67         sensor_hal *fusion_sensor_hal = sensor_loader::get_instance().get_sensor_hal(SENSOR_HAL_TYPE_FUSION);
  68         if (!fusion_sensor_hal)
  69                 m_hardware_fusion = false;
  70         else
  71                 m_hardware_fusion = true;
  72
  73         INFO("m_hardware_fusion = %d", m_hardware_fusion);
  74
  75         m_name = string(SENSOR_NAME);
  76         register_supported_event(GAMING_RV_RAW_DATA_EVENT);
  77         m_enable_gaming_rv = 0;
  78
  79         if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_VENDOR, m_vendor)) {
  80                 ERR("[VENDOR] is empty\n");
  81                 throw ENXIO;
  82         }
  83
  84         INFO("m_vendor = %s", m_vendor.c_str());
  85
  86         if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) {
  87                 ERR("[DEFAULT_SAMPLING_TIME] is empty\n");
  88                 throw ENXIO;
  89         }
  90
  91         INFO("m_default_sampling_time = %d", m_default_sampling_time);
  92
  93         if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) {
  94                 ERR("[ACCEL_STATIC_BIAS] is empty\n");
  95                 throw ENXIO;
  96         }
  97
  98         INFO("m_accel_static_bias = (%f, %f, %f)", m_accel_static_bias[0], m_accel_static_bias[1], m_accel_static_bias[2]);
  99
 100         if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_GYRO_STATIC_BIAS, m_gyro_static_bias,3)) {
 101                 ERR("[GYRO_STATIC_BIAS] is empty\n");
 102                 throw ENXIO;
 103         }
 104
 105         INFO("m_gyro_static_bias = (%f, %f, %f)", m_gyro_static_bias[0], m_gyro_static_bias[1], m_gyro_static_bias[2]);
 106
 107         if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION, m_accel_rotation_direction_compensation, 3)) {
 108                 ERR("[ACCEL_ROTATION_DIRECTION_COMPENSATION] is empty\n");
 109                 throw ENXIO;
 110         }
 111
 112         INFO("m_accel_rotation_direction_compensation = (%d, %d, %d)", m_accel_rotation_direction_compensation[0], m_accel_rotation_direction_compensation[1], m_accel_rotation_direction_compensation[2]);
 113
 114         if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION, m_gyro_rotation_direction_compensation, 3)) {
 115                 ERR("[GYRO_ROTATION_DIRECTION_COMPENSATION] is empty\n");
 116                 throw ENXIO;
 117         }
 118
 119         INFO("m_gyro_rotation_direction_compensation = (%d, %d, %d)", m_gyro_rotation_direction_compensation[0], m_gyro_rotation_direction_compensation[1], m_gyro_rotation_direction_compensation[2]);
 120
 121         m_interval = m_default_sampling_time * MS_TO_US;
 122 }
 123
 124 gaming_rv_sensor::~gaming_rv_sensor()
 125 {
 126         INFO("gaming_rv_sensor is destroyed!\n");
 127 }
 128
 129 bool gaming_rv_sensor::init()
 130 {
 131         m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
 132         m_gyro_sensor = sensor_loader::get_instance().get_sensor(GYROSCOPE_SENSOR);
 133
 134         if (!m_accel_sensor || !m_gyro_sensor) {
 135                 ERR("Failed to load sensors,  accel: 0x%x, gyro: 0x%x",
 136                         m_accel_sensor, m_gyro_sensor);
 137                 return false;
 138         }
 139
 140         INFO("%s is created!\n", sensor_base::get_name());
 141
 142         return true;
 143 }
 144
 145 void gaming_rv_sensor::get_types(vector<sensor_type_t> &types)
 146 {
 147         types.push_back(GAMING_RV_SENSOR);
 148 }
 149
 150 bool gaming_rv_sensor::on_start(void)
 151 {
 152         AUTOLOCK(m_mutex);
 153
 154         if (!m_hardware_fusion) {
 155                 m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT);
 156                 m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
 157                 m_accel_sensor->start();
 158                 m_gyro_sensor->add_client(GYROSCOPE_RAW_DATA_EVENT);
 159                 m_gyro_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
 160                 m_gyro_sensor->start();
 161         }
 162
 163         activate();
 164         return true;
 165 }
 166
 167 bool gaming_rv_sensor::on_stop(void)
 168 {
 169         AUTOLOCK(m_mutex);
 170
 171         if (!m_hardware_fusion) {
 172                 m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT);
 173                 m_accel_sensor->delete_interval((intptr_t)this, false);
 174                 m_accel_sensor->stop();
 175                 m_gyro_sensor->delete_client(GYROSCOPE_RAW_DATA_EVENT);
 176                 m_gyro_sensor->delete_interval((intptr_t)this, false);
 177                 m_gyro_sensor->stop();
 178         }
 179
 180         deactivate();
 181         return true;
 182 }
 183
 184 bool gaming_rv_sensor::add_interval(int client_id, unsigned int interval)
 185 {
 186         AUTOLOCK(m_mutex);
 187
 188         if (!m_hardware_fusion) {
 189                 m_accel_sensor->add_interval(client_id, interval, false);
 190                 m_gyro_sensor->add_interval(client_id, interval, false);
 191         }
 192
 193         return sensor_base::add_interval(client_id, interval, false);
 194 }
 195
 196 bool gaming_rv_sensor::delete_interval(int client_id)
 197 {
 198         AUTOLOCK(m_mutex);
 199
 200         if (!m_hardware_fusion) {
 201                 m_accel_sensor->delete_interval(client_id, false);
 202                 m_gyro_sensor->delete_interval(client_id, false);
 203         }
 204
 205         return sensor_base::delete_interval(client_id, false);
 206 }
 207
 208 void gaming_rv_sensor::synthesize(const sensor_event_t& event, vector<sensor_event_t> &outs)
 209 {
 210         const float MIN_DELIVERY_DIFF_FACTOR = 0.75f;
 211         unsigned long long diff_time;
 212
 213         sensor_event_t rv_event;
 214         quaternion<float> quaternion_gaming_rv;
 215
 216         if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) {
 217                 diff_time = event.data.timestamp - m_time;
 218
 219                 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 220                         return;
 221
 222                 pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, ACCEL_SCALE);
 223
 224                 m_accel.m_time_stamp = event.data.timestamp;
 225
 226                 m_enable_gaming_rv |= ACCELEROMETER_ENABLED;
 227         } else if (event.event_type == GYROSCOPE_RAW_DATA_EVENT) {
 228                 diff_time = event.data.timestamp - m_time;
 229
 230                 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 231                         return;
 232
 233                 pre_process_data(m_gyro, event.data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, GYRO_SCALE);
 234
 235                 m_gyro.m_time_stamp = event.data.timestamp;
 236
 237                 m_enable_gaming_rv |= GYROSCOPE_ENABLED;
 238         }
 239
 240         if (m_enable_gaming_rv == GAMING_RV_ENABLED) {
 241                 m_enable_gaming_rv = 0;
 242
 243                 m_orientation_filter.get_device_orientation(&m_accel, &m_gyro, NULL);
 244
 245                 quaternion_gaming_rv = m_orientation_filter.m_quat_gaming_rv;
 246
 247                 m_time = get_timestamp();
 248                 rv_event.sensor_id = get_id();
 249                 rv_event.event_type = GAMING_RV_RAW_DATA_EVENT;
 250                 rv_event.data.accuracy = SENSOR_ACCURACY_GOOD;
 251                 rv_event.data.timestamp = m_time;
 252                 rv_event.data.value_count = 4;
 253                 rv_event.data.values[0] = quaternion_gaming_rv.m_quat.m_vec[1];
 254                 rv_event.data.values[1] = quaternion_gaming_rv.m_quat.m_vec[2];
 255                 rv_event.data.values[2] = quaternion_gaming_rv.m_quat.m_vec[3];
 256                 rv_event.data.values[3] = quaternion_gaming_rv.m_quat.m_vec[0];
 257
 258                 push(rv_event);
 259         }
 260
 261         return;
 262 }
 263
 264 int gaming_rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &data)
 265 {
 266         sensor_data<float> accel;
 267         sensor_data<float> gyro;
 268
 269         sensor_data_t accel_data;
 270         sensor_data_t gyro_data;
 271
 272         quaternion<float> quaternion_gaming_rv;
 273
 274         if (event_type != GAMING_RV_RAW_DATA_EVENT)
 275                 return -1;
 276
 277         m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data);
 278         m_gyro_sensor->get_sensor_data(GYROSCOPE_RAW_DATA_EVENT, gyro_data);
 279
 280         pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, ACCEL_SCALE);
 281         pre_process_data(gyro, gyro_data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, GYRO_SCALE);
 282         accel.m_time_stamp = accel_data.timestamp;
 283         gyro.m_time_stamp = gyro_data.timestamp;
 284
 285         m_orientation_filter_poll.get_device_orientation(&m_accel, &m_gyro, NULL);
 286
 287         quaternion_gaming_rv = m_orientation_filter_poll.m_quat_gaming_rv;
 288
 289         data.accuracy = SENSOR_ACCURACY_GOOD;
 290         data.timestamp = get_timestamp();
 291         data.value_count = 4;
 292         data.values[0] = quaternion_gaming_rv.m_quat.m_vec[1];
 293         data.values[1] = quaternion_gaming_rv.m_quat.m_vec[2];
 294         data.values[2] = quaternion_gaming_rv.m_quat.m_vec[3];
 295         data.values[3] = quaternion_gaming_rv.m_quat.m_vec[0];
 296
 297         return 0;
 298 }
 299
 300 bool gaming_rv_sensor::get_properties(sensor_type_t sensor_type, sensor_properties_s &properties)
 301 {
 302         properties.vendor = m_vendor;
 303         properties.name = SENSOR_NAME;
 304         properties.min_range = -1;
 305         properties.max_range = 1;
 306         properties.resolution = 0.000001;
 307         properties.fifo_count = 0;
 308         properties.max_batch_count = 0;
 309         properties.min_interval = 1;
 310
 311         return true;
 312 }
 313