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