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