src/rotation_vector/rv/rv_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 <rv_sensor.h>
  31 #include <sensor_plugin_loader.h>
  32 #include <orientation_filter.h>
  33 #include <cvirtual_sensor_config.h>
  34
  35 #define SENSOR_NAME "RV_SENSOR"
  36 #define SENSOR_TYPE_RV          "ROTATION_VECTOR"
  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
  47 #define ELEMENT_NAME                                                                                    "NAME"
  48 #define ELEMENT_VENDOR                                                                                  "VENDOR"
  49 #define ELEMENT_RAW_DATA_UNIT                                                                   "RAW_DATA_UNIT"
  50 #define ELEMENT_DEFAULT_SAMPLING_TIME                                                   "DEFAULT_SAMPLING_TIME"
  51 #define ELEMENT_ACCEL_STATIC_BIAS                                                               "ACCEL_STATIC_BIAS"
  52 #define ELEMENT_GYRO_STATIC_BIAS                                                                "GYRO_STATIC_BIAS"
  53 #define ELEMENT_GEOMAGNETIC_STATIC_BIAS                                                 "GEOMAGNETIC_STATIC_BIAS"
  54 #define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION                   "ACCEL_ROTATION_DIRECTION_COMPENSATION"
  55 #define ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION                    "GYRO_ROTATION_DIRECTION_COMPENSATION"
  56 #define ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION             "GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION"
  57 #define ELEMENT_ACCEL_SCALE                                                                             "ACCEL_SCALE"
  58 #define ELEMENT_GYRO_SCALE                                                                              "GYRO_SCALE"
  59 #define ELEMENT_GEOMAGNETIC_SCALE                                                               "GEOMAGNETIC_SCALE"
  60 #define ELEMENT_MAGNETIC_ALIGNMENT_FACTOR                                               "MAGNETIC_ALIGNMENT_FACTOR"
  61
  62 void pre_process_data(sensor_data<float> &data_out, const float *data_in, float *bias, int *sign, float scale)
  63 {
  64         data_out.m_data.m_vec[0] = sign[0] * (data_in[0] - bias[0]) / scale;
  65         data_out.m_data.m_vec[1] = sign[1] * (data_in[1] - bias[1]) / scale;
  66         data_out.m_data.m_vec[2] = sign[2] * (data_in[2] - bias[2]) / scale;
  67 }
  68
  69 rv_sensor::rv_sensor()
  70 : m_accel_sensor(NULL)
  71 , m_gyro_sensor(NULL)
  72 , m_magnetic_sensor(NULL)
  73 , m_accuracy(-1)
  74 , m_time(0)
  75 {
  76         cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance();
  77
  78         m_name = string(SENSOR_NAME);
  79         register_supported_event(ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME);
  80         m_enable_orientation = 0;
  81
  82         if (!config.get(SENSOR_TYPE_RV, ELEMENT_VENDOR, m_vendor)) {
  83                 ERR("[VENDOR] is empty\n");
  84                 throw ENXIO;
  85         }
  86
  87         INFO("m_vendor = %s", m_vendor.c_str());
  88
  89         if (!config.get(SENSOR_TYPE_RV, 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_RV, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) {
  97                 ERR("[ACCEL_STATIC_BIAS] is empty\n");
  98                 throw ENXIO;
  99         }
 100
 101         INFO("m_accel_static_bias = (%f, %f, %f)", m_accel_static_bias[0], m_accel_static_bias[1], m_accel_static_bias[2]);
 102
 103         if (!config.get(SENSOR_TYPE_RV, ELEMENT_GYRO_STATIC_BIAS, m_gyro_static_bias,3)) {
 104                 ERR("[GYRO_STATIC_BIAS] is empty\n");
 105                 throw ENXIO;
 106         }
 107
 108         INFO("m_gyro_static_bias = (%f, %f, %f)", m_gyro_static_bias[0], m_gyro_static_bias[1], m_gyro_static_bias[2]);
 109
 110         if (!config.get(SENSOR_TYPE_RV, ELEMENT_GEOMAGNETIC_STATIC_BIAS, m_geomagnetic_static_bias, 3)) {
 111                 ERR("[GEOMAGNETIC_STATIC_BIAS] is empty\n");
 112                 throw ENXIO;
 113         }
 114
 115         INFO("m_geomagnetic_static_bias = (%f, %f, %f)", m_geomagnetic_static_bias[0], m_geomagnetic_static_bias[1], m_geomagnetic_static_bias[2]);
 116
 117         if (!config.get(SENSOR_TYPE_RV, ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION, m_accel_rotation_direction_compensation, 3)) {
 118                 ERR("[ACCEL_ROTATION_DIRECTION_COMPENSATION] is empty\n");
 119                 throw ENXIO;
 120         }
 121
 122         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]);
 123
 124         if (!config.get(SENSOR_TYPE_RV, ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION, m_gyro_rotation_direction_compensation, 3)) {
 125                 ERR("[GYRO_ROTATION_DIRECTION_COMPENSATION] is empty\n");
 126                 throw ENXIO;
 127         }
 128
 129         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]);
 130
 131         if (!config.get(SENSOR_TYPE_RV, ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION, m_geomagnetic_rotation_direction_compensation, 3)) {
 132                 ERR("[GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION] is empty\n");
 133                 throw ENXIO;
 134         }
 135
 136         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]);
 137
 138         if (!config.get(SENSOR_TYPE_RV, ELEMENT_ACCEL_SCALE, &m_accel_scale)) {
 139                 ERR("[ACCEL_SCALE] is empty\n");
 140                 throw ENXIO;
 141         }
 142
 143         INFO("m_accel_scale = %f", m_accel_scale);
 144
 145         if (!config.get(SENSOR_TYPE_RV, ELEMENT_GYRO_SCALE, &m_gyro_scale)) {
 146                 ERR("[GYRO_SCALE] is empty\n");
 147                 throw ENXIO;
 148         }
 149
 150         INFO("m_gyro_scale = %f", m_gyro_scale);
 151
 152         if (!config.get(SENSOR_TYPE_RV, ELEMENT_GEOMAGNETIC_SCALE, &m_geomagnetic_scale)) {
 153                 ERR("[GEOMAGNETIC_SCALE] is empty\n");
 154                 throw ENXIO;
 155         }
 156
 157         INFO("m_geomagnetic_scale = %f", m_geomagnetic_scale);
 158
 159         if (!config.get(SENSOR_TYPE_RV, ELEMENT_MAGNETIC_ALIGNMENT_FACTOR, &m_magnetic_alignment_factor)) {
 160                 ERR("[MAGNETIC_ALIGNMENT_FACTOR] is empty\n");
 161                 throw ENXIO;
 162         }
 163
 164         INFO("m_magnetic_alignment_factor = %d", m_magnetic_alignment_factor);
 165
 166         m_interval = m_default_sampling_time * MS_TO_US;
 167
 168 }
 169
 170 rv_sensor::~rv_sensor()
 171 {
 172         INFO("rv_sensor is destroyed!\n");
 173 }
 174
 175 bool rv_sensor::init()
 176 {
 177         m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
 178         m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR);
 179         m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR);
 180
 181         if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor) {
 182                 ERR("Failed to load sensors,  accel: 0x%x, gyro: 0x%x, mag: 0x%x",
 183                         m_accel_sensor, m_gyro_sensor, m_magnetic_sensor);
 184                 return false;
 185         }
 186
 187         INFO("%s is created!\n", sensor_base::get_name());
 188
 189         return true;
 190 }
 191
 192 sensor_type_t rv_sensor::get_type(void)
 193 {
 194         return ROTATION_VECTOR_SENSOR;
 195 }
 196
 197 bool rv_sensor::on_start(void)
 198 {
 199         AUTOLOCK(m_mutex);
 200
 201         m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT);
 202         m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
 203         m_accel_sensor->start();
 204         m_gyro_sensor->add_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME);
 205         m_gyro_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
 206         m_gyro_sensor->start();
 207         m_magnetic_sensor->add_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME);
 208         m_magnetic_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
 209         m_magnetic_sensor->start();
 210
 211         activate();
 212         return true;
 213 }
 214
 215 bool rv_sensor::on_stop(void)
 216 {
 217         AUTOLOCK(m_mutex);
 218
 219         m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT);
 220         m_accel_sensor->delete_interval((intptr_t)this, false);
 221         m_accel_sensor->stop();
 222         m_gyro_sensor->delete_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME);
 223         m_gyro_sensor->delete_interval((intptr_t)this, false);
 224         m_gyro_sensor->stop();
 225         m_magnetic_sensor->delete_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME);
 226         m_magnetic_sensor->delete_interval((intptr_t)this, false);
 227         m_magnetic_sensor->stop();
 228
 229         deactivate();
 230         return true;
 231 }
 232
 233 bool rv_sensor::add_interval(int client_id, unsigned int interval)
 234 {
 235         AUTOLOCK(m_mutex);
 236
 237         m_accel_sensor->add_interval(client_id, interval, false);
 238         m_gyro_sensor->add_interval(client_id, interval, false);
 239         m_magnetic_sensor->add_interval(client_id, interval, false);
 240
 241         return sensor_base::add_interval(client_id, interval, false);
 242 }
 243
 244 bool rv_sensor::delete_interval(int client_id)
 245 {
 246         AUTOLOCK(m_mutex);
 247
 248         m_accel_sensor->delete_interval(client_id, false);
 249         m_gyro_sensor->delete_interval(client_id, false);
 250         m_magnetic_sensor->delete_interval(client_id, false);
 251
 252         return sensor_base::delete_interval(client_id, false);
 253 }
 254
 255 void rv_sensor::synthesize(const sensor_event_t& event, vector<sensor_event_t> &outs)
 256 {
 257         const float MIN_DELIVERY_DIFF_FACTOR = 0.75f;
 258         unsigned long long diff_time;
 259
 260         sensor_event_t rv_event;
 261         quaternion<float> quaternion_orientation;
 262
 263         if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) {
 264                 diff_time = event.data.timestamp - m_time;
 265
 266                 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 267                         return;
 268
 269                 pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale);
 270
 271                 m_accel.m_time_stamp = event.data.timestamp;
 272
 273                 m_enable_orientation |= ACCELEROMETER_ENABLED;
 274         }
 275         else if (event.event_type == GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME) {
 276                 diff_time = event.data.timestamp - m_time;
 277
 278                 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 279                         return;
 280
 281                 pre_process_data(m_gyro, event.data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale);
 282
 283                 m_gyro.m_time_stamp = event.data.timestamp;
 284
 285                 m_enable_orientation |= GYROSCOPE_ENABLED;
 286         }
 287         else if (event.event_type == GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME) {
 288                 diff_time = event.data.timestamp - m_time;
 289
 290                 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 291                         return;
 292
 293                 pre_process_data(m_magnetic, event.data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale);
 294
 295                 m_magnetic.m_time_stamp = event.data.timestamp;
 296
 297                 m_enable_orientation |= GEOMAGNETIC_ENABLED;
 298         }
 299
 300         if (m_enable_orientation == ORIENTATION_ENABLED) {
 301                 m_enable_orientation = 0;
 302
 303                 m_orientation.m_pitch_phase_compensation = m_pitch_rotation_compensation;
 304                 m_orientation.m_roll_phase_compensation = m_roll_rotation_compensation;
 305                 m_orientation.m_azimuth_phase_compensation = m_azimuth_rotation_compensation;
 306                 m_orientation.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
 307
 308                 {
 309                         AUTOLOCK(m_fusion_mutex);
 310                         quaternion_orientation = m_orientation.get_9axis_quaternion(m_accel, m_gyro, m_magnetic);
 311                 }
 312
 313                 m_time = get_timestamp();
 314
 315                 rv_event.sensor_id = get_id();
 316                 rv_event.event_type = ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME;
 317                 rv_event.data.accuracy = SENSOR_ACCURACY_GOOD;
 318                 rv_event.data.timestamp = m_time;
 319                 rv_event.data.value_count = 4;
 320                 rv_event.data.values[0] = quaternion_orientation.m_quat.m_vec[1];
 321                 rv_event.data.values[1] = quaternion_orientation.m_quat.m_vec[2];
 322                 rv_event.data.values[2] = quaternion_orientation.m_quat.m_vec[3];
 323                 rv_event.data.values[3] = quaternion_orientation.m_quat.m_vec[0];
 324
 325                 push(rv_event);
 326         }
 327
 328         return;
 329 }
 330
 331 int rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &data)
 332 {
 333         sensor_data<float> accel;
 334         sensor_data<float> gyro;
 335         sensor_data<float> magnetic;
 336
 337         sensor_data_t accel_data;
 338         sensor_data_t gyro_data;
 339         sensor_data_t magnetic_data;
 340
 341         quaternion<float> quaternion_orientation;
 342
 343         if (event_type != ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME)
 344                 return -1;
 345
 346         m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data);
 347         m_gyro_sensor->get_sensor_data(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME, gyro_data);
 348         m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, magnetic_data);
 349
 350         pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale);
 351         pre_process_data(gyro, gyro_data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale);
 352         pre_process_data(magnetic, magnetic_data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale);
 353         accel.m_time_stamp = accel_data.timestamp;
 354         gyro.m_time_stamp = gyro_data.timestamp;
 355         magnetic.m_time_stamp = magnetic_data.timestamp;
 356
 357         m_orientation.m_pitch_phase_compensation = m_pitch_rotation_compensation;
 358         m_orientation.m_roll_phase_compensation = m_roll_rotation_compensation;
 359         m_orientation.m_azimuth_phase_compensation = m_azimuth_rotation_compensation;
 360         m_orientation.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
 361
 362         {
 363                 AUTOLOCK(m_fusion_mutex);
 364                 quaternion_orientation = m_orientation.get_9axis_quaternion(m_accel, m_gyro, m_magnetic);
 365         }
 366
 367         data.accuracy = SENSOR_ACCURACY_GOOD;
 368         data.timestamp = m_time;
 369         data.value_count = 4;
 370         data.values[0] = quaternion_orientation.m_quat.m_vec[1];
 371         data.values[1] = quaternion_orientation.m_quat.m_vec[2];
 372         data.values[2] = quaternion_orientation.m_quat.m_vec[3];
 373         data.values[3] = quaternion_orientation.m_quat.m_vec[0];
 374
 375         return 0;
 376 }
 377
 378 bool rv_sensor::get_properties(sensor_properties_s &properties)
 379 {
 380         properties.vendor = m_vendor;
 381         properties.name = SENSOR_NAME;
 382         properties.min_range = -1;
 383         properties.max_range = 1;
 384         properties.resolution = 0.000001;
 385         properties.fifo_count = 0;
 386         properties.max_batch_count = 0;
 387         properties.min_interval = 1;
 388
 389         return true;
 390 }
 391
 392 extern "C" sensor_module* create(void)
 393 {
 394         rv_sensor *sensor;
 395
 396         try {
 397                 sensor = new(std::nothrow) rv_sensor;
 398         } catch (int err) {
 399                 ERR("Failed to create module, err: %d, cause: %s", err, strerror(err));
 400                 return NULL;
 401         }
 402
 403         sensor_module *module = new(std::nothrow) sensor_module;
 404         retvm_if(!module || !sensor, NULL, "Failed to allocate memory");
 405
 406         module->sensors.push_back(sensor);
 407         return module;
 408 }