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