src/fusion/fusion_sensor.cpp

   1 /*
   2  * sensord
   3  *
   4  * Copyright (c) 2015 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 <fusion_sensor.h>
  31 #include <sensor_plugin_loader.h>
  32 #include <orientation_filter.h>
  33 #include <cvirtual_sensor_config.h>
  34 #include <algorithm>
  35
  36 #define SENSOR_NAME "FUSION_SENSOR"
  37 #define SENSOR_TYPE_FUSION              "FUSION"
  38
  39 #define ACCELEROMETER_ENABLED 0x01
  40 #define GYROSCOPE_ENABLED 0x02
  41 #define GEOMAGNETIC_ENABLED 0x04
  42 #define TILT_ENABLED 1
  43 #define GAMING_RV_ENABLED 3
  44 #define GEOMAGNETIC_RV_ENABLED 5
  45 #define ORIENTATION_ENABLED 7
  46 #define ROTATION_VECTOR_ENABLED 7
  47
  48 #define INITIAL_VALUE -1
  49
  50 #define MS_TO_US 1000
  51 #define MIN_DELIVERY_DIFF_FACTOR 0.75f
  52
  53 #define PI 3.141593
  54 #define AZIMUTH_OFFSET_DEGREES 360
  55 #define AZIMUTH_OFFSET_RADIANS (2 * PI)
  56
  57 #define ELEMENT_NAME                                                                                    "NAME"
  58 #define ELEMENT_VENDOR                                                                                  "VENDOR"
  59 #define ELEMENT_RAW_DATA_UNIT                                                                   "RAW_DATA_UNIT"
  60 #define ELEMENT_DEFAULT_SAMPLING_TIME                                                   "DEFAULT_SAMPLING_TIME"
  61 #define ELEMENT_ACCEL_STATIC_BIAS                                                               "ACCEL_STATIC_BIAS"
  62 #define ELEMENT_GYRO_STATIC_BIAS                                                                "GYRO_STATIC_BIAS"
  63 #define ELEMENT_GEOMAGNETIC_STATIC_BIAS                                                 "GEOMAGNETIC_STATIC_BIAS"
  64 #define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION                   "ACCEL_ROTATION_DIRECTION_COMPENSATION"
  65 #define ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION                    "GYRO_ROTATION_DIRECTION_COMPENSATION"
  66 #define ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION             "GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION"
  67 #define ELEMENT_ACCEL_SCALE                                                                             "ACCEL_SCALE"
  68 #define ELEMENT_GYRO_SCALE                                                                              "GYRO_SCALE"
  69 #define ELEMENT_GEOMAGNETIC_SCALE                                                               "GEOMAGNETIC_SCALE"
  70 #define ELEMENT_MAGNETIC_ALIGNMENT_FACTOR                                               "MAGNETIC_ALIGNMENT_FACTOR"
  71 #define ELEMENT_PITCH_ROTATION_COMPENSATION                                             "PITCH_ROTATION_COMPENSATION"
  72 #define ELEMENT_ROLL_ROTATION_COMPENSATION                                              "ROLL_ROTATION_COMPENSATION"
  73 #define ELEMENT_AZIMUTH_ROTATION_COMPENSATION                                   "AZIMUTH_ROTATION_COMPENSATION"
  74
  75 void pre_process_data(sensor_data<float> &data_out, const float *data_in, float *bias, int *sign, float scale)
  76 {
  77         data_out.m_data.m_vec[0] = sign[0] * (data_in[0] - bias[0]) / scale;
  78         data_out.m_data.m_vec[1] = sign[1] * (data_in[1] - bias[1]) / scale;
  79         data_out.m_data.m_vec[2] = sign[2] * (data_in[2] - bias[2]) / scale;
  80 }
  81
  82 fusion_sensor::fusion_sensor()
  83 : m_accel_sensor(NULL)
  84 , m_gyro_sensor(NULL)
  85 , m_magnetic_sensor(NULL)
  86 , m_time(0)
  87 {
  88         cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance();
  89         m_name = string(SENSOR_NAME);
  90         m_enable_fusion = 0;
  91
  92         if (!config.get(SENSOR_TYPE_FUSION, 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_FUSION, 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_FUSION, 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_FUSION, 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_FUSION, 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_FUSION, 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_FUSION, 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_FUSION, 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_FUSION, 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_FUSION, 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_FUSION, 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_FUSION, 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_FUSION, 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         m_interval = m_default_sampling_time * MS_TO_US;
 184
 185         m_accel_ptr = m_gyro_ptr = m_magnetic_ptr = NULL;
 186 }
 187
 188 fusion_sensor::~fusion_sensor()
 189 {
 190         INFO("fusion_sensor is destroyed!\n");
 191 }
 192
 193 bool fusion_sensor::init(void)
 194 {
 195         m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
 196         m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR);
 197         m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR);
 198
 199         if (!m_accel_sensor) {
 200                 ERR("Failed to load accel sensor: 0x%x", m_accel_sensor);
 201                 return false;
 202         }
 203
 204         if (!m_gyro_sensor)
 205                 INFO("Failed to load gyro sensor: 0x%x", m_gyro_sensor);
 206
 207         if (!m_magnetic_sensor)
 208                 INFO("Failed to load geomagnetic sensor: 0x%x", m_magnetic_sensor);
 209
 210         INFO("%s is created!", sensor_base::get_name());
 211         return true;
 212 }
 213
 214 sensor_type_t fusion_sensor::get_type(void)
 215 {
 216         return FUSION_SENSOR;
 217 }
 218
 219 bool fusion_sensor::on_start(void)
 220 {
 221         AUTOLOCK(m_mutex);
 222         activate();
 223         return true;
 224 }
 225
 226 bool fusion_sensor::on_stop(void)
 227 {
 228         AUTOLOCK(m_mutex);
 229         deactivate();
 230         return true;
 231 }
 232
 233 bool fusion_sensor::add_interval(int client_id, unsigned int interval)
 234 {
 235         bool retval;
 236
 237         AUTOLOCK(m_mutex);
 238         retval = sensor_base::add_interval(client_id, interval, false);
 239
 240         m_interval = sensor_base::get_interval(client_id, false);
 241
 242         if (m_interval != 0)
 243                 retval = true;
 244
 245         return retval;
 246 }
 247
 248 bool fusion_sensor::delete_interval(int client_id)
 249 {
 250         bool retval;
 251
 252         AUTOLOCK(m_mutex);
 253         retval = sensor_base::delete_interval(client_id, false);
 254
 255         m_interval = sensor_base::get_interval(client_id, false);
 256
 257         if (m_interval != 0)
 258                 retval = true;
 259
 260         return retval;
 261 }
 262
 263 void fusion_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
 264 {
 265         unsigned long long diff_time;
 266         euler_angles<float> euler_orientation;
 267
 268         if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) {
 269                 diff_time = event.data.timestamp - m_time;
 270
 271                 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 272                         return;
 273
 274                 pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale);
 275
 276                 m_accel.m_time_stamp = event.data.timestamp;
 277
 278                 m_accel_ptr = &m_accel;
 279
 280                 m_enable_fusion |= ACCELEROMETER_ENABLED;
 281         }
 282
 283         if (sensor_base::is_supported(FUSION_ORIENTATION_ENABLED) ||
 284                         sensor_base::is_supported(FUSION_ROTATION_VECTOR_ENABLED) ||
 285                         sensor_base::is_supported(FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED)) {
 286                 if (event.event_type == GEOMAGNETIC_RAW_DATA_EVENT) {
 287                         diff_time = event.data.timestamp - m_time;
 288
 289                         if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 290                                 return;
 291
 292                         pre_process_data(m_magnetic, event.data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale);
 293
 294                         m_magnetic.m_time_stamp = event.data.timestamp;
 295
 296                         m_magnetic_ptr = &m_magnetic;
 297
 298                         m_enable_fusion |= GEOMAGNETIC_ENABLED;
 299                 }
 300         }
 301
 302         if (sensor_base::is_supported(FUSION_ORIENTATION_ENABLED) ||
 303                         sensor_base::is_supported(FUSION_ROTATION_VECTOR_ENABLED) ||
 304                         sensor_base::is_supported(FUSION_GAMING_ROTATION_VECTOR_ENABLED)) {
 305                 if (event.event_type == GYROSCOPE_RAW_DATA_EVENT) {
 306                                 diff_time = event.data.timestamp - m_time;
 307
 308                                 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 309                                         return;
 310
 311                                 pre_process_data(m_gyro, event.data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale);
 312
 313                                 m_gyro.m_time_stamp = event.data.timestamp;
 314
 315                                 m_gyro_ptr = &m_gyro;
 316
 317                                 m_enable_fusion |= GYROSCOPE_ENABLED;
 318                 }
 319         }
 320
 321         if ((m_enable_fusion == TILT_ENABLED && sensor_base::is_supported(FUSION_TILT_ENABLED)) ||
 322                         (m_enable_fusion == ORIENTATION_ENABLED && sensor_base::is_supported(FUSION_ORIENTATION_ENABLED)) ||
 323                         (m_enable_fusion == ROTATION_VECTOR_ENABLED && sensor_base::is_supported(FUSION_ROTATION_VECTOR_ENABLED)) ||
 324                         (m_enable_fusion == GAMING_RV_ENABLED && sensor_base::is_supported(FUSION_GAMING_ROTATION_VECTOR_ENABLED)) ||
 325                         (m_enable_fusion == GEOMAGNETIC_RV_ENABLED && sensor_base::is_supported(FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED))) {
 326                 sensor_event_t fusion_event;
 327                 m_enable_fusion = 0;
 328
 329                 m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
 330
 331                 m_orientation_filter.get_device_orientation(m_accel_ptr, m_gyro_ptr, m_magnetic_ptr);
 332
 333                 m_time = get_timestamp();
 334                 fusion_event.sensor_id = get_id();
 335                 fusion_event.event_type = FUSION_EVENT;
 336                 fusion_event.data.accuracy = SENSOR_ACCURACY_GOOD;
 337                 fusion_event.data.timestamp = m_time;
 338                 fusion_event.data.value_count = 4;
 339                 fusion_event.data.values[0] = m_orientation_filter.m_quaternion.m_quat.m_vec[0];
 340                 fusion_event.data.values[1] = m_orientation_filter.m_quaternion.m_quat.m_vec[1];
 341                 fusion_event.data.values[2] = m_orientation_filter.m_quaternion.m_quat.m_vec[2];
 342                 fusion_event.data.values[3] = m_orientation_filter.m_quaternion.m_quat.m_vec[3];
 343
 344                 m_accel_ptr = m_gyro_ptr = m_magnetic_ptr = NULL;
 345
 346                 push(fusion_event);
 347         }
 348
 349         return;
 350 }
 351
 352 int fusion_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
 353 {
 354         sensor_data<float> accel;
 355         sensor_data<float> gyro;
 356         sensor_data<float> magnetic;
 357
 358         sensor_data_t accel_data;
 359         sensor_data_t gyro_data;
 360         sensor_data_t magnetic_data;
 361
 362         euler_angles<float> euler_orientation;
 363
 364         if (event_type != FUSION_ORIENTATION_ENABLED ||
 365                         event_type != FUSION_ROTATION_VECTOR_ENABLED ||
 366                         event_type != FUSION_GAMING_ROTATION_VECTOR_ENABLED ||
 367                         event_type != FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED)
 368                 return -1;
 369
 370         m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data);
 371         pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale);
 372         accel.m_time_stamp = accel_data.timestamp;
 373
 374         if (event_type == FUSION_ORIENTATION_ENABLED ||
 375                         event_type == FUSION_ROTATION_VECTOR_ENABLED ||
 376                         event_type == FUSION_GAMING_ROTATION_VECTOR_ENABLED)
 377         {
 378                 m_gyro_sensor->get_sensor_data(GYROSCOPE_RAW_DATA_EVENT, gyro_data);
 379                 pre_process_data(gyro, gyro_data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale);
 380                 gyro.m_time_stamp = gyro_data.timestamp;
 381         }
 382
 383         if (event_type == FUSION_ORIENTATION_ENABLED ||
 384                         event_type == FUSION_ROTATION_VECTOR_ENABLED ||
 385                         event_type == FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED)
 386         {
 387                 m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_RAW_DATA_EVENT, magnetic_data);
 388                 pre_process_data(magnetic, magnetic_data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale);
 389                 magnetic.m_time_stamp = magnetic_data.timestamp;
 390         }
 391
 392         m_orientation_filter_poll.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
 393
 394         if (event_type == FUSION_ORIENTATION_ENABLED || event_type == FUSION_ROTATION_VECTOR_ENABLED)
 395                 m_orientation_filter_poll.get_device_orientation(&accel, &gyro, &magnetic);
 396         else if (event_type == FUSION_GAMING_ROTATION_VECTOR_ENABLED)
 397                 m_orientation_filter_poll.get_device_orientation(&accel, &gyro, NULL);
 398         else if (event_type == FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED)
 399                 m_orientation_filter_poll.get_device_orientation(&accel, NULL, &magnetic);
 400
 401         data.accuracy = SENSOR_ACCURACY_GOOD;
 402         data.timestamp = get_timestamp();
 403         data.value_count = 4;
 404         data.values[0] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[0];
 405         data.values[1] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[1];
 406         data.values[2] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[2];
 407         data.values[3] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[3];
 408
 409         return 0;
 410 }
 411
 412 bool fusion_sensor::get_properties(sensor_properties_s &properties)
 413 {
 414         properties.min_range = 0;
 415         properties.max_range = 0;
 416         properties.resolution = 0;
 417         properties.vendor = m_vendor;
 418         properties.name = SENSOR_NAME;
 419         properties.min_interval = 0;
 420         properties.fifo_count = 0;
 421         properties.max_batch_count = 0;
 422
 423         return true;
 424 }
 425
 426 extern "C" sensor_module* create(void)
 427 {
 428         fusion_sensor *sensor;
 429
 430         try {
 431                 sensor = new(std::nothrow) fusion_sensor;
 432         } catch (int err) {
 433                 ERR("Failed to create module, err: %d, cause: %s", err, strerror(err));
 434                 return NULL;
 435         }
 436
 437         sensor_module *module = new(std::nothrow) sensor_module;
 438         retvm_if(!module || !sensor, NULL, "Failed to allocate memory");
 439
 440         module->sensors.push_back(sensor);
 441         return module;
 442 }