src/server/plugins/gravity/gravity_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 <sf_common.h>
  30 #include <gravity_sensor.h>
  31 #include <sensor_plugin_loader.h>
  32 #include <virtual_sensor_config.h>
  33
  34 using std::string;
  35 using std::vector;
  36
  37 #define INITIAL_VALUE -1
  38 #define GRAVITY 9.80665
  39
  40 #define DEVIATION 0.1
  41
  42 #define PI 3.141593
  43 #define AZIMUTH_OFFSET_DEGREES 360
  44 #define AZIMUTH_OFFSET_RADIANS (2 * PI)
  45
  46 #define SENSOR_NAME "GRAVITY_SENSOR"
  47 #define SENSOR_TYPE_GRAVITY             "GRAVITY"
  48 #define SENSOR_TYPE_ORIENTATION         "ORIENTATION"
  49
  50 #define MS_TO_US 1000
  51 #define MIN_DELIVERY_DIFF_FACTOR 0.75f
  52
  53 #define ELEMENT_NAME                                                                                    "NAME"
  54 #define ELEMENT_VENDOR                                                                                  "VENDOR"
  55 #define ELEMENT_RAW_DATA_UNIT                                                                   "RAW_DATA_UNIT"
  56 #define ELEMENT_DEFAULT_SAMPLING_TIME                                                   "DEFAULT_SAMPLING_TIME"
  57 #define ELEMENT_GRAVITY_SIGN_COMPENSATION                                               "GRAVITY_SIGN_COMPENSATION"
  58 #define ELEMENT_ORIENTATION_DATA_UNIT                                                   "RAW_DATA_UNIT"
  59 #define ELEMENT_PITCH_ROTATION_COMPENSATION                                             "PITCH_ROTATION_COMPENSATION"
  60 #define ELEMENT_ROLL_ROTATION_COMPENSATION                                              "ROLL_ROTATION_COMPENSATION"
  61 #define ELEMENT_AZIMUTH_ROTATION_COMPENSATION                                   "AZIMUTH_ROTATION_COMPENSATION"
  62
  63 gravity_sensor::gravity_sensor()
  64 : m_accel_sensor(NULL)
  65 , m_gyro_sensor(NULL)
  66 , m_magnetic_sensor(NULL)
  67 , m_fusion_sensor(NULL)
  68 , m_time(0)
  69 {
  70         virtual_sensor_config &config = virtual_sensor_config::get_instance();
  71
  72         sensor_hal *fusion_sensor_hal = sensor_plugin_loader::get_instance().get_sensor_hal(SENSOR_HAL_TYPE_FUSION);
  73         if (!fusion_sensor_hal)
  74                 m_hardware_fusion = false;
  75         else
  76                 m_hardware_fusion = true;
  77
  78         m_name = std::string(SENSOR_NAME);
  79         register_supported_event(GRAVITY_RAW_DATA_EVENT);
  80
  81         if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_VENDOR, m_vendor)) {
  82                 ERR("[VENDOR] is empty\n");
  83                 throw ENXIO;
  84         }
  85
  86         INFO("m_vendor = %s", m_vendor.c_str());
  87
  88         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ORIENTATION_DATA_UNIT, m_orientation_data_unit)) {
  89                 ERR("[ORIENTATION_DATA_UNIT] is empty\n");
  90                 throw ENXIO;
  91         }
  92
  93         INFO("m_orientation_data_unit = %s", m_orientation_data_unit.c_str());
  94
  95         if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) {
  96                 ERR("[RAW_DATA_UNIT] is empty\n");
  97                 throw ENXIO;
  98         }
  99
 100         INFO("m_raw_data_unit = %s", m_raw_data_unit.c_str());
 101
 102         if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) {
 103                 ERR("[DEFAULT_SAMPLING_TIME] is empty\n");
 104                 throw ENXIO;
 105         }
 106
 107         INFO("m_default_sampling_time = %d", m_default_sampling_time);
 108
 109         if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_GRAVITY_SIGN_COMPENSATION, m_gravity_sign_compensation, 3)) {
 110                 ERR("[GRAVITY_SIGN_COMPENSATION] is empty\n");
 111                 throw ENXIO;
 112         }
 113
 114         INFO("m_gravity_sign_compensation = (%d, %d, %d)", m_gravity_sign_compensation[0], m_gravity_sign_compensation[1], m_gravity_sign_compensation[2]);
 115
 116         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_AZIMUTH_ROTATION_COMPENSATION, &m_azimuth_rotation_compensation)) {
 117                 ERR("[AZIMUTH_ROTATION_COMPENSATION] is empty\n");
 118                 throw ENXIO;
 119         }
 120
 121         INFO("m_azimuth_rotation_compensation = %d", m_azimuth_rotation_compensation);
 122
 123         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) {
 124                 ERR("[PITCH_ROTATION_COMPENSATION] is empty\n");
 125                 throw ENXIO;
 126         }
 127
 128         INFO("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation);
 129
 130         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) {
 131                 ERR("[ROLL_ROTATION_COMPENSATION] is empty\n");
 132                 throw ENXIO;
 133         }
 134
 135         INFO("m_roll_rotation_compensation = %d", m_roll_rotation_compensation);
 136
 137         m_interval = m_default_sampling_time * MS_TO_US;
 138 }
 139
 140 gravity_sensor::~gravity_sensor()
 141 {
 142         INFO("gravity_sensor is destroyed!\n");
 143 }
 144
 145 bool gravity_sensor::init()
 146 {
 147         m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
 148         m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR);
 149         m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR);
 150
 151         m_fusion_sensor = sensor_plugin_loader::get_instance().get_sensor(FUSION_SENSOR);
 152
 153         if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor || !m_fusion_sensor) {
 154                 ERR("Failed to load sensors,  accel: 0x%x, gyro: 0x%x, mag: 0x%x, fusion: 0x%x",
 155                         m_accel_sensor, m_gyro_sensor, m_magnetic_sensor, m_fusion_sensor);
 156                 return false;
 157         }
 158
 159         INFO("%s is created!", sensor_base::get_name());
 160         return true;
 161 }
 162
 163 void gravity_sensor::get_types(vector<sensor_type_t> &types)
 164 {
 165         types.push_back(GRAVITY_SENSOR);
 166 }
 167
 168 bool gravity_sensor::on_start(void)
 169 {
 170         AUTOLOCK(m_mutex);
 171
 172         if (!m_hardware_fusion) {
 173                 m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT);
 174                 m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
 175                 m_accel_sensor->start();
 176                 m_gyro_sensor->add_client(GYROSCOPE_RAW_DATA_EVENT);
 177                 m_gyro_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
 178                 m_gyro_sensor->start();
 179                 m_magnetic_sensor->add_client(GEOMAGNETIC_RAW_DATA_EVENT);
 180                 m_magnetic_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
 181                 m_magnetic_sensor->start();
 182         }
 183
 184         m_fusion_sensor->register_supported_event(FUSION_EVENT);
 185         m_fusion_sensor->register_supported_event(FUSION_ORIENTATION_ENABLED);
 186         m_fusion_sensor->add_client(FUSION_EVENT);
 187         m_fusion_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
 188         m_fusion_sensor->start();
 189
 190         activate();
 191         return true;
 192 }
 193
 194 bool gravity_sensor::on_stop(void)
 195 {
 196         AUTOLOCK(m_mutex);
 197
 198         if (!m_hardware_fusion) {
 199                 m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT);
 200                 m_accel_sensor->delete_interval((intptr_t)this, false);
 201                 m_accel_sensor->stop();
 202                 m_gyro_sensor->delete_client(GYROSCOPE_RAW_DATA_EVENT);
 203                 m_gyro_sensor->delete_interval((intptr_t)this, false);
 204                 m_gyro_sensor->stop();
 205                 m_magnetic_sensor->delete_client(GEOMAGNETIC_RAW_DATA_EVENT);
 206                 m_magnetic_sensor->delete_interval((intptr_t)this, false);
 207                 m_magnetic_sensor->stop();
 208         }
 209
 210         m_fusion_sensor->delete_client(FUSION_EVENT);
 211         m_fusion_sensor->delete_interval((intptr_t)this, false);
 212         m_fusion_sensor->unregister_supported_event(FUSION_EVENT);
 213         m_fusion_sensor->unregister_supported_event(FUSION_ORIENTATION_ENABLED);
 214         m_fusion_sensor->stop();
 215
 216         deactivate();
 217         return true;
 218 }
 219
 220 bool gravity_sensor::add_interval(int client_id, unsigned int interval)
 221 {
 222         AUTOLOCK(m_mutex);
 223         if (!m_hardware_fusion) {
 224                 m_accel_sensor->add_interval(client_id, interval, false);
 225                 m_gyro_sensor->add_interval(client_id, interval, false);
 226                 m_magnetic_sensor->add_interval(client_id, interval, false);
 227         }
 228
 229         m_fusion_sensor->add_interval(client_id, interval, false);
 230
 231         return sensor_base::add_interval(client_id, interval, false);
 232 }
 233
 234 bool gravity_sensor::delete_interval(int client_id)
 235 {
 236         AUTOLOCK(m_mutex);
 237         if (!m_hardware_fusion) {
 238                 m_accel_sensor->delete_interval(client_id, false);
 239                 m_gyro_sensor->delete_interval(client_id, false);
 240                 m_magnetic_sensor->delete_interval(client_id, false);
 241         }
 242
 243         m_fusion_sensor->delete_interval(client_id, false);
 244
 245         return sensor_base::delete_interval(client_id, false);
 246 }
 247
 248 void gravity_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
 249 {
 250         sensor_event_t gravity_event;
 251         float pitch, roll, azimuth;
 252         unsigned long long diff_time;
 253         float azimuth_offset;
 254
 255         if (event.event_type == FUSION_EVENT) {
 256                 diff_time = event.data.timestamp - m_time;
 257
 258                 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 259                         return;
 260
 261                 quaternion<float> quat(event.data.values[0], event.data.values[1],
 262                                 event.data.values[2], event.data.values[3]);
 263
 264                 euler_angles<float> euler = quat2euler(quat);
 265
 266                 if(m_orientation_data_unit == "DEGREES") {
 267                         euler = rad2deg(euler);
 268                         azimuth_offset = AZIMUTH_OFFSET_DEGREES;
 269                 }
 270                 else {
 271                         azimuth_offset = AZIMUTH_OFFSET_RADIANS;
 272                 }
 273
 274                 euler.m_ang.m_vec[0] *= m_pitch_rotation_compensation;
 275                 euler.m_ang.m_vec[1] *= m_roll_rotation_compensation;
 276                 euler.m_ang.m_vec[2] *= m_azimuth_rotation_compensation;
 277
 278                 pitch = euler.m_ang.m_vec[0];
 279                 roll = euler.m_ang.m_vec[1];
 280                 if (euler.m_ang.m_vec[2] >= 0)
 281                         azimuth = euler.m_ang.m_vec[2];
 282                 else
 283                         azimuth = euler.m_ang.m_vec[2] + azimuth_offset;
 284
 285                 if(m_orientation_data_unit == "DEGREES") {
 286                         azimuth *= DEG2RAD;
 287                         pitch *= DEG2RAD;
 288                         roll *= DEG2RAD;
 289                 }
 290
 291                 m_time = get_timestamp();
 292                 gravity_event.sensor_id = get_id();
 293                 gravity_event.event_type = GRAVITY_RAW_DATA_EVENT;
 294
 295                 if ((roll >= (M_PI/2)-DEVIATION && roll <= (M_PI/2)+DEVIATION) ||
 296                                 (roll >= -(M_PI/2)-DEVIATION && roll <= -(M_PI/2)+DEVIATION)) {
 297                         gravity_event.data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll) * cos(azimuth);
 298                         gravity_event.data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(azimuth);
 299                         gravity_event.data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll);
 300                 } else if ((pitch >= (M_PI/2)-DEVIATION && pitch <= (M_PI/2)+DEVIATION) ||
 301                                 (pitch >= -(M_PI/2)-DEVIATION && pitch <= -(M_PI/2)+DEVIATION)) {
 302                         gravity_event.data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(azimuth);
 303                         gravity_event.data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch) * cos(azimuth);
 304                         gravity_event.data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(pitch);
 305                 } else {
 306                         gravity_event.data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll);
 307                         gravity_event.data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch);
 308                         gravity_event.data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll) * cos(pitch);
 309                 }
 310                 gravity_event.data.value_count = 3;
 311                 gravity_event.data.timestamp = m_time;
 312                 gravity_event.data.accuracy = SENSOR_ACCURACY_GOOD;
 313
 314                 push(gravity_event);
 315         }
 316
 317         return;
 318 }
 319
 320 int gravity_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
 321 {
 322         sensor_data_t fusion_data;
 323         float azimuth_offset;
 324         float pitch, roll, azimuth;
 325
 326         if (event_type != GRAVITY_RAW_DATA_EVENT)
 327                 return -1;
 328
 329         m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data);
 330
 331         quaternion<float> quat(fusion_data.values[0], fusion_data.values[1],
 332                         fusion_data.values[2], fusion_data.values[3]);
 333
 334         euler_angles<float> euler = quat2euler(quat);
 335
 336         if(m_orientation_data_unit == "DEGREES") {
 337                 euler = rad2deg(euler);
 338                 azimuth_offset = AZIMUTH_OFFSET_DEGREES;
 339         }
 340         else {
 341                 azimuth_offset = AZIMUTH_OFFSET_RADIANS;
 342         }
 343
 344         pitch = euler.m_ang.m_vec[0];
 345         roll = euler.m_ang.m_vec[1];
 346
 347         if (euler.m_ang.m_vec[2] >= 0)
 348                 azimuth = euler.m_ang.m_vec[2];
 349         else
 350                 azimuth = euler.m_ang.m_vec[2] + azimuth_offset;
 351
 352         if(m_orientation_data_unit == "DEGREES") {
 353                 azimuth *= DEG2RAD;
 354                 pitch *= DEG2RAD;
 355                 roll *= DEG2RAD;
 356         }
 357
 358         data.accuracy = SENSOR_ACCURACY_GOOD;
 359         data.timestamp = get_timestamp();
 360         if ((roll >= (M_PI/2)-DEVIATION && roll <= (M_PI/2)+DEVIATION) ||
 361                         (roll >= -(M_PI/2)-DEVIATION && roll <= -(M_PI/2)+DEVIATION)) {
 362                 data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll) * cos(azimuth);
 363                 data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(azimuth);
 364                 data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll);
 365         } else if ((pitch >= (M_PI/2)-DEVIATION && pitch <= (M_PI/2)+DEVIATION) ||
 366                         (pitch >= -(M_PI/2)-DEVIATION && pitch <= -(M_PI/2)+DEVIATION)) {
 367                 data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(azimuth);
 368                 data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch) * cos(azimuth);
 369                 data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(pitch);
 370         } else {
 371                 data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll);
 372                 data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch);
 373                 data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll) * cos(pitch);
 374         }
 375         data.value_count = 3;
 376
 377         return 0;
 378 }
 379
 380 bool gravity_sensor::get_properties(sensor_type_t sensor_type, sensor_properties_s &properties)
 381 {
 382         properties.min_range = -GRAVITY;
 383         properties.max_range = GRAVITY;
 384         properties.resolution = 0.000001;
 385         properties.vendor = m_vendor;
 386         properties.name = SENSOR_NAME;
 387         properties.fifo_count = 0;
 388         properties.max_batch_count = 0;
 389         properties.min_interval = 1;
 390
 391         return true;
 392 }