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