src/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 <common.h>
  29 #include <sf_common.h>
  30 #include <orientation_sensor.h>
  31 #include <sensor_plugin_loader.h>
  32 #include <orientation_filter.h>
  33 #include <cvirtual_sensor_config.h>
  34
  35 #define SENSOR_NAME "ORIENTATION_SENSOR"
  36 #define SENSOR_TYPE_ORIENTATION         "ORIENTATION"
  37
  38 #define ACCELEROMETER_ENABLED 0x01
  39 #define GYROSCOPE_ENABLED 0x02
  40 #define GEOMAGNETIC_ENABLED 0x04
  41 #define ORIENTATION_ENABLED 7
  42
  43 #define INITIAL_VALUE -1
  44
  45 #define MS_TO_US 1000
  46 #define MIN_DELIVERY_DIFF_FACTOR 0.75f
  47
  48 #define PI 3.141593
  49 #define AZIMUTH_OFFSET_DEGREES 360
  50 #define AZIMUTH_OFFSET_RADIANS (2 * PI)
  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_ACCEL_STATIC_BIAS                                                               "ACCEL_STATIC_BIAS"
  57 #define ELEMENT_GYRO_STATIC_BIAS                                                                "GYRO_STATIC_BIAS"
  58 #define ELEMENT_GEOMAGNETIC_STATIC_BIAS                                                 "GEOMAGNETIC_STATIC_BIAS"
  59 #define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION                   "ACCEL_ROTATION_DIRECTION_COMPENSATION"
  60 #define ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION                    "GYRO_ROTATION_DIRECTION_COMPENSATION"
  61 #define ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION             "GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION"
  62 #define ELEMENT_ACCEL_SCALE                                                                             "ACCEL_SCALE"
  63 #define ELEMENT_GYRO_SCALE                                                                              "GYRO_SCALE"
  64 #define ELEMENT_GEOMAGNETIC_SCALE                                                               "GEOMAGNETIC_SCALE"
  65 #define ELEMENT_MAGNETIC_ALIGNMENT_FACTOR                                               "MAGNETIC_ALIGNMENT_FACTOR"
  66 #define ELEMENT_PITCH_ROTATION_COMPENSATION                                             "PITCH_ROTATION_COMPENSATION"
  67 #define ELEMENT_ROLL_ROTATION_COMPENSATION                                              "ROLL_ROTATION_COMPENSATION"
  68 #define ELEMENT_AZIMUTH_ROTATION_COMPENSATION                                   "AZIMUTH_ROTATION_COMPENSATION"
  69
  70 void pre_process_data(sensor_data<float> &data_out, const float *data_in, float *bias, int *sign, float scale)
  71 {
  72         data_out.m_data.m_vec[0] = sign[0] * (data_in[0] - bias[0]) / scale;
  73         data_out.m_data.m_vec[1] = sign[1] * (data_in[1] - bias[1]) / scale;
  74         data_out.m_data.m_vec[2] = sign[2] * (data_in[2] - bias[2]) / scale;
  75 }
  76
  77 orientation_sensor::orientation_sensor()
  78 : m_accel_sensor(NULL)
  79 , m_gyro_sensor(NULL)
  80 , m_magnetic_sensor(NULL)
  81 , m_roll(INITIAL_VALUE)
  82 , m_pitch(INITIAL_VALUE)
  83 , m_azimuth(INITIAL_VALUE)
  84 {
  85         cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance();
  86
  87         m_name = string(SENSOR_NAME);
  88         register_supported_event(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME);
  89         m_timestamp = get_timestamp();
  90         m_enable_orientation = 0;
  91
  92         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_VENDOR, m_vendor)) {
  93                 ERR("[VENDOR] is empty\n");
  94                 throw ENXIO;
  95         }
  96
  97         INFO("m_vendor = %s", m_vendor.c_str());
  98
  99         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) {
 100                 ERR("[RAW_DATA_UNIT] is empty\n");
 101                 throw ENXIO;
 102         }
 103
 104         INFO("m_raw_data_unit = %s", m_raw_data_unit.c_str());
 105
 106         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) {
 107                 ERR("[DEFAULT_SAMPLING_TIME] is empty\n");
 108                 throw ENXIO;
 109         }
 110
 111         INFO("m_default_sampling_time = %d", m_default_sampling_time);
 112
 113         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) {
 114                 ERR("[ACCEL_STATIC_BIAS] is empty\n");
 115                 throw ENXIO;
 116         }
 117
 118         INFO("m_accel_static_bias = (%f, %f, %f)", m_accel_static_bias[0], m_accel_static_bias[1], m_accel_static_bias[2]);
 119
 120         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_GYRO_STATIC_BIAS, m_gyro_static_bias,3)) {
 121                 ERR("[GYRO_STATIC_BIAS] is empty\n");
 122                 throw ENXIO;
 123         }
 124
 125         INFO("m_gyro_static_bias = (%f, %f, %f)", m_gyro_static_bias[0], m_gyro_static_bias[1], m_gyro_static_bias[2]);
 126
 127         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_GEOMAGNETIC_STATIC_BIAS, m_geomagnetic_static_bias, 3)) {
 128                 ERR("[GEOMAGNETIC_STATIC_BIAS] is empty\n");
 129                 throw ENXIO;
 130         }
 131
 132         INFO("m_geomagnetic_static_bias = (%f, %f, %f)", m_geomagnetic_static_bias[0], m_geomagnetic_static_bias[1], m_geomagnetic_static_bias[2]);
 133
 134         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION, m_accel_rotation_direction_compensation, 3)) {
 135                 ERR("[ACCEL_ROTATION_DIRECTION_COMPENSATION] is empty\n");
 136                 throw ENXIO;
 137         }
 138
 139         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]);
 140
 141         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION, m_gyro_rotation_direction_compensation, 3)) {
 142                 ERR("[GYRO_ROTATION_DIRECTION_COMPENSATION] is empty\n");
 143                 throw ENXIO;
 144         }
 145
 146         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]);
 147
 148         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION, m_geomagnetic_rotation_direction_compensation, 3)) {
 149                 ERR("[GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION] is empty\n");
 150                 throw ENXIO;
 151         }
 152
 153         INFO("m_geomagnetic_rotation_direction_compensation = (%d, %d, %d)", m_geomagnetic_rotation_direction_compensation[0], m_geomagnetic_rotation_direction_compensation[1], m_geomagnetic_rotation_direction_compensation[2]);
 154
 155         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ACCEL_SCALE, &m_accel_scale)) {
 156                 ERR("[ACCEL_SCALE] is empty\n");
 157                 throw ENXIO;
 158         }
 159
 160         INFO("m_accel_scale = %f", m_accel_scale);
 161
 162         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_GYRO_SCALE, &m_gyro_scale)) {
 163                 ERR("[GYRO_SCALE] is empty\n");
 164                 throw ENXIO;
 165         }
 166
 167         INFO("m_gyro_scale = %f", m_gyro_scale);
 168
 169         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_GEOMAGNETIC_SCALE, &m_geomagnetic_scale)) {
 170                 ERR("[GEOMAGNETIC_SCALE] is empty\n");
 171                 throw ENXIO;
 172         }
 173
 174         INFO("m_geomagnetic_scale = %f", m_geomagnetic_scale);
 175
 176         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_MAGNETIC_ALIGNMENT_FACTOR, &m_magnetic_alignment_factor)) {
 177                 ERR("[MAGNETIC_ALIGNMENT_FACTOR] is empty\n");
 178                 throw ENXIO;
 179         }
 180
 181         INFO("m_magnetic_alignment_factor = %d", m_magnetic_alignment_factor);
 182
 183         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_AZIMUTH_ROTATION_COMPENSATION, &m_azimuth_rotation_compensation)) {
 184                 ERR("[AZIMUTH_ROTATION_COMPENSATION] is empty\n");
 185                 throw ENXIO;
 186         }
 187
 188         INFO("m_azimuth_rotation_compensation = %d", m_azimuth_rotation_compensation);
 189
 190         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) {
 191                 ERR("[PITCH_ROTATION_COMPENSATION] is empty\n");
 192                 throw ENXIO;
 193         }
 194
 195         INFO("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation);
 196
 197         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) {
 198                 ERR("[ROLL_ROTATION_COMPENSATION] is empty\n");
 199                 throw ENXIO;
 200         }
 201
 202         INFO("m_roll_rotation_compensation = %d", m_roll_rotation_compensation);
 203
 204         m_interval = m_default_sampling_time * MS_TO_US;
 205
 206 }
 207
 208 orientation_sensor::~orientation_sensor()
 209 {
 210         INFO("orientation_sensor is destroyed!\n");
 211 }
 212
 213 bool orientation_sensor::init(void)
 214 {
 215         m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
 216         m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR);
 217         m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR);
 218
 219         if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor) {
 220                 ERR("Failed to load sensors,  accel: 0x%x, gyro: 0x%x, mag: 0x%x",
 221                         m_accel_sensor, m_gyro_sensor, m_magnetic_sensor);
 222                 return false;
 223         }
 224
 225         INFO("%s is created!", sensor_base::get_name());
 226         return true;
 227 }
 228
 229 sensor_type_t orientation_sensor::get_type(void)
 230 {
 231         return ORIENTATION_SENSOR;
 232 }
 233
 234 bool orientation_sensor::on_start(void)
 235 {
 236         AUTOLOCK(m_mutex);
 237
 238         m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
 239         m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), true);
 240         m_accel_sensor->start();
 241         m_gyro_sensor->add_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME);
 242         m_gyro_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), true);
 243         m_gyro_sensor->start();
 244         m_magnetic_sensor->add_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME);
 245         m_magnetic_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), true);
 246         m_magnetic_sensor->start();
 247
 248         activate();
 249         return true;
 250 }
 251
 252 bool orientation_sensor::on_stop(void)
 253 {
 254         AUTOLOCK(m_mutex);
 255
 256         m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
 257         m_accel_sensor->delete_interval((intptr_t)this, true);
 258         m_accel_sensor->stop();
 259         m_gyro_sensor->delete_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME);
 260         m_gyro_sensor->delete_interval((intptr_t)this, true);
 261         m_gyro_sensor->stop();
 262         m_magnetic_sensor->delete_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME);
 263         m_magnetic_sensor->delete_interval((intptr_t)this, true);
 264         m_magnetic_sensor->stop();
 265
 266         deactivate();
 267         return true;
 268 }
 269
 270 bool orientation_sensor::add_interval(int client_id, unsigned int interval)
 271 {
 272         AUTOLOCK(m_mutex);
 273
 274         m_accel_sensor->add_interval(client_id, interval, true);
 275         m_gyro_sensor->add_interval(client_id, interval, true);
 276         m_magnetic_sensor->add_interval(client_id, interval, true);
 277
 278         return sensor_base::add_interval(client_id, interval, true);
 279 }
 280
 281 bool orientation_sensor::delete_interval(int client_id)
 282 {
 283         AUTOLOCK(m_mutex);
 284
 285         m_accel_sensor->delete_interval(client_id, true);
 286         m_gyro_sensor->delete_interval(client_id, true);
 287         m_magnetic_sensor->delete_interval(client_id, true);
 288
 289         return sensor_base::delete_interval(client_id, true);
 290 }
 291
 292 void orientation_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
 293 {
 294         unsigned long long diff_time;
 295
 296         sensor_event_t orientation_event;
 297         euler_angles<float> euler_orientation;
 298         float azimuth_offset;
 299
 300         if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) {
 301                 diff_time = event.data.timestamp - m_timestamp;
 302
 303                 if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 304                         return;
 305
 306                 pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale);
 307
 308                 m_accel.m_time_stamp = event.data.timestamp;
 309
 310                 m_enable_orientation |= ACCELEROMETER_ENABLED;
 311         }
 312         else if (event.event_type == GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME) {
 313                 diff_time = event.data.timestamp - m_timestamp;
 314
 315                 if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 316                         return;
 317
 318                 pre_process_data(m_gyro, event.data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale);
 319
 320                 m_gyro.m_time_stamp = event.data.timestamp;
 321
 322                 m_enable_orientation |= GYROSCOPE_ENABLED;
 323         }
 324         else if (event.event_type == GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME) {
 325                 diff_time = event.data.timestamp - m_timestamp;
 326
 327                 if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 328                         return;
 329
 330                 pre_process_data(m_magnetic, event.data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale);
 331
 332                 m_magnetic.m_time_stamp = event.data.timestamp;
 333
 334                 m_enable_orientation |= GEOMAGNETIC_ENABLED;
 335         }
 336
 337         if (m_enable_orientation == ORIENTATION_ENABLED) {
 338                 m_enable_orientation = 0;
 339                 m_timestamp = get_timestamp();
 340
 341                 m_orientation.m_pitch_phase_compensation = m_pitch_rotation_compensation;
 342                 m_orientation.m_roll_phase_compensation = m_roll_rotation_compensation;
 343                 m_orientation.m_azimuth_phase_compensation = m_azimuth_rotation_compensation;
 344                 m_orientation.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
 345
 346                 {
 347                         AUTOLOCK(m_fusion_mutex);
 348                         euler_orientation = m_orientation.get_orientation(m_accel, m_gyro, m_magnetic);
 349                 }
 350
 351                 if(m_raw_data_unit == "DEGREES") {
 352                         euler_orientation = rad2deg(euler_orientation);
 353                         azimuth_offset = AZIMUTH_OFFSET_DEGREES;
 354                 }
 355                 else {
 356                         azimuth_offset = AZIMUTH_OFFSET_RADIANS;
 357                 }
 358
 359                 orientation_event.sensor_id = get_id();
 360                 orientation_event.event_type = ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME;
 361                 orientation_event.data.accuracy = SENSOR_ACCURACY_GOOD;
 362                 orientation_event.data.timestamp = m_timestamp;
 363                 orientation_event.data.value_count = 3;
 364                 orientation_event.data.values[1] = euler_orientation.m_ang.m_vec[0];
 365                 orientation_event.data.values[2] = euler_orientation.m_ang.m_vec[1];
 366                 if (euler_orientation.m_ang.m_vec[2] >= 0)
 367                         orientation_event.data.values[0] = euler_orientation.m_ang.m_vec[2];
 368                 else
 369                         orientation_event.data.values[0] = euler_orientation.m_ang.m_vec[2] + azimuth_offset;
 370
 371                 push(orientation_event);
 372         }
 373
 374         return;
 375 }
 376
 377 int orientation_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
 378 {
 379         sensor_data<float> accel;
 380         sensor_data<float> gyro;
 381         sensor_data<float> magnetic;
 382
 383         sensor_data_t accel_data;
 384         sensor_data_t gyro_data;
 385         sensor_data_t magnetic_data;
 386
 387         euler_angles<float> euler_orientation;
 388         float azimuth_offset;
 389
 390         if (event_type != ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME)
 391                 return -1;
 392
 393         m_accel_sensor->get_sensor_data(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME, accel_data);
 394         m_gyro_sensor->get_sensor_data(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME, gyro_data);
 395         m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, magnetic_data);
 396
 397         pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale);
 398         pre_process_data(gyro, gyro_data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale);
 399         pre_process_data(magnetic, magnetic_data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale);
 400         accel.m_time_stamp = accel_data.timestamp;
 401         gyro.m_time_stamp = gyro_data.timestamp;
 402         magnetic.m_time_stamp = magnetic_data.timestamp;
 403
 404         m_orientation.m_pitch_phase_compensation = m_pitch_rotation_compensation;
 405         m_orientation.m_roll_phase_compensation = m_roll_rotation_compensation;
 406         m_orientation.m_azimuth_phase_compensation = m_azimuth_rotation_compensation;
 407         m_orientation.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
 408
 409         {
 410                 AUTOLOCK(m_fusion_mutex);
 411                 euler_orientation = m_orientation.get_orientation(m_accel, m_gyro, m_magnetic);
 412         }
 413
 414         if(m_raw_data_unit == "DEGREES") {
 415                 euler_orientation = rad2deg(euler_orientation);
 416                 azimuth_offset = AZIMUTH_OFFSET_DEGREES;
 417         }
 418         else {
 419                 azimuth_offset = AZIMUTH_OFFSET_RADIANS;
 420         }
 421
 422         data.accuracy = SENSOR_ACCURACY_GOOD;
 423         data.timestamp = get_timestamp();
 424         data.values[1] = euler_orientation.m_ang.m_vec[0];
 425         data.values[2] = euler_orientation.m_ang.m_vec[1];
 426         if (euler_orientation.m_ang.m_vec[2] >= 0)
 427                 data.values[0] = euler_orientation.m_ang.m_vec[2];
 428         else
 429                 data.values[0] = euler_orientation.m_ang.m_vec[2] + azimuth_offset;
 430         data.value_count = 3;
 431
 432         return 0;
 433 }
 434
 435 bool orientation_sensor::get_properties(sensor_properties_s &properties)
 436 {
 437         if(m_raw_data_unit == "DEGREES") {
 438                 properties.min_range = -180;
 439                 properties.max_range = 360;
 440         }
 441         else {
 442                 properties.min_range = -PI;
 443                 properties.max_range = 2 * PI;
 444         }
 445         properties.resolution = 0.000001;
 446
 447         properties.vendor = m_vendor;
 448         properties.name = SENSOR_NAME;
 449
 450         return true;
 451 }
 452
 453 extern "C" void *create(void)
 454 {
 455         orientation_sensor *inst;
 456
 457         try {
 458                 inst = new orientation_sensor();
 459         } catch (int err) {
 460                 ERR("orientation_sensor class create fail , errno : %d , errstr : %s", err, strerror(err));
 461                 return NULL;
 462         }
 463
 464         return (void *)inst;
 465 }
 466
 467 extern "C" void destroy(void *inst)
 468 {
 469         delete (orientation_sensor *)inst;
 470 }