src/rotation_vector/geomagnetic_rv/geomagnetic_rv_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 <geomagnetic_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 "GEOMAGNETIC_RV_SENSOR"
  36 #define SENSOR_TYPE_GEOMAGNETIC_RV              "GEOMAGNETIC_ROTATION_VECTOR"
  37
  38 #define ACCELEROMETER_ENABLED 0x01
  39 #define GEOMAGNETIC_ENABLED 0x02
  40 #define GEOMAGNETIC_RV_ENABLED 3
  41
  42 #define INITIAL_VALUE -1
  43
  44 #define MS_TO_US 1000
  45
  46 #define ELEMENT_NAME                                                                                    "NAME"
  47 #define ELEMENT_VENDOR                                                                                  "VENDOR"
  48 #define ELEMENT_RAW_DATA_UNIT                                                                   "RAW_DATA_UNIT"
  49 #define ELEMENT_DEFAULT_SAMPLING_TIME                                                   "DEFAULT_SAMPLING_TIME"
  50 #define ELEMENT_ACCEL_STATIC_BIAS                                                               "ACCEL_STATIC_BIAS"
  51 #define ELEMENT_GEOMAGNETIC_STATIC_BIAS                                                 "GEOMAGNETIC_STATIC_BIAS"
  52 #define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION                   "ACCEL_ROTATION_DIRECTION_COMPENSATION"
  53 #define ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION             "GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION"
  54 #define ELEMENT_ACCEL_SCALE                                                                             "ACCEL_SCALE"
  55 #define ELEMENT_GEOMAGNETIC_SCALE                                                               "GEOMAGNETIC_SCALE"
  56 #define ELEMENT_MAGNETIC_ALIGNMENT_FACTOR                                               "MAGNETIC_ALIGNMENT_FACTOR"
  57
  58 void pre_process_data(sensor_data<float> &data_out, const float *data_in, float *bias, int *sign, float scale)
  59 {
  60         data_out.m_data.m_vec[0] = sign[0] * (data_in[0] - bias[0]) / scale;
  61         data_out.m_data.m_vec[1] = sign[1] * (data_in[1] - bias[1]) / scale;
  62         data_out.m_data.m_vec[2] = sign[2] * (data_in[2] - bias[2]) / scale;
  63 }
  64
  65 geomagnetic_rv_sensor::geomagnetic_rv_sensor()
  66 : m_accel_sensor(NULL)
  67 , m_magnetic_sensor(NULL)
  68 , m_accuracy(-1)
  69 , m_time(0)
  70 {
  71         cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance();
  72
  73         m_name = string(SENSOR_NAME);
  74         register_supported_event(GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME);
  75         m_enable_geomagnetic_rv = 0;
  76
  77         if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_VENDOR, m_vendor)) {
  78                 ERR("[VENDOR] is empty\n");
  79                 throw ENXIO;
  80         }
  81
  82         INFO("m_vendor = %s", m_vendor.c_str());
  83
  84         if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) {
  85                 ERR("[DEFAULT_SAMPLING_TIME] is empty\n");
  86                 throw ENXIO;
  87         }
  88
  89         INFO("m_default_sampling_time = %d", m_default_sampling_time);
  90
  91         if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) {
  92                 ERR("[ACCEL_STATIC_BIAS] is empty\n");
  93                 throw ENXIO;
  94         }
  95
  96         INFO("m_accel_static_bias = (%f, %f, %f)", m_accel_static_bias[0], m_accel_static_bias[1], m_accel_static_bias[2]);
  97
  98         if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_GEOMAGNETIC_STATIC_BIAS, m_geomagnetic_static_bias, 3)) {
  99                 ERR("[GEOMAGNETIC_STATIC_BIAS] is empty\n");
 100                 throw ENXIO;
 101         }
 102
 103         INFO("m_geomagnetic_static_bias = (%f, %f, %f)", m_geomagnetic_static_bias[0], m_geomagnetic_static_bias[1], m_geomagnetic_static_bias[2]);
 104
 105         if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION, m_accel_rotation_direction_compensation, 3)) {
 106                 ERR("[ACCEL_ROTATION_DIRECTION_COMPENSATION] is empty\n");
 107                 throw ENXIO;
 108         }
 109
 110         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]);
 111
 112         if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION, m_geomagnetic_rotation_direction_compensation, 3)) {
 113                 ERR("[GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION] is empty\n");
 114                 throw ENXIO;
 115         }
 116
 117         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]);
 118
 119         if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_ACCEL_SCALE, &m_accel_scale)) {
 120                 ERR("[ACCEL_SCALE] is empty\n");
 121                 throw ENXIO;
 122         }
 123
 124         INFO("m_accel_scale = %f", m_accel_scale);
 125
 126         if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_GEOMAGNETIC_SCALE, &m_geomagnetic_scale)) {
 127                 ERR("[GEOMAGNETIC_SCALE] is empty\n");
 128                 throw ENXIO;
 129         }
 130
 131         INFO("m_geomagnetic_scale = %f", m_geomagnetic_scale);
 132
 133         if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_MAGNETIC_ALIGNMENT_FACTOR, &m_magnetic_alignment_factor)) {
 134                 ERR("[MAGNETIC_ALIGNMENT_FACTOR] is empty\n");
 135                 throw ENXIO;
 136         }
 137
 138         INFO("m_magnetic_alignment_factor = %d", m_magnetic_alignment_factor);
 139
 140         m_interval = m_default_sampling_time * MS_TO_US;
 141
 142 }
 143
 144 geomagnetic_rv_sensor::~geomagnetic_rv_sensor()
 145 {
 146         INFO("geomagnetic_rv_sensor is destroyed!\n");
 147 }
 148
 149 bool geomagnetic_rv_sensor::init()
 150 {
 151         m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
 152         m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR);
 153
 154         if (!m_accel_sensor || !m_magnetic_sensor) {
 155                 ERR("Failed to load sensors,  accel: 0x%x, mag: 0x%x",
 156                         m_accel_sensor, m_magnetic_sensor);
 157                 return false;
 158         }
 159
 160         INFO("%s is created!\n", sensor_base::get_name());
 161
 162         return true;
 163 }
 164
 165 sensor_type_t geomagnetic_rv_sensor::get_type(void)
 166 {
 167         return GEOMAGNETIC_RV_SENSOR;
 168 }
 169
 170 bool geomagnetic_rv_sensor::on_start(void)
 171 {
 172         AUTOLOCK(m_mutex);
 173
 174         m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT);
 175         m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
 176         m_accel_sensor->start();
 177         m_magnetic_sensor->add_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME);
 178         m_magnetic_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
 179         m_magnetic_sensor->start();
 180
 181         activate();
 182         return true;
 183 }
 184
 185 bool geomagnetic_rv_sensor::on_stop(void)
 186 {
 187         AUTOLOCK(m_mutex);
 188
 189         m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT);
 190         m_accel_sensor->delete_interval((intptr_t)this, false);
 191         m_accel_sensor->stop();
 192         m_magnetic_sensor->delete_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME);
 193         m_magnetic_sensor->delete_interval((intptr_t)this, false);
 194         m_magnetic_sensor->stop();
 195
 196         deactivate();
 197         return true;
 198 }
 199
 200 bool geomagnetic_rv_sensor::add_interval(int client_id, unsigned int interval)
 201 {
 202         AUTOLOCK(m_mutex);
 203
 204         m_accel_sensor->add_interval(client_id, interval, false);
 205         m_magnetic_sensor->add_interval(client_id, interval, false);
 206
 207         return sensor_base::add_interval(client_id, interval, false);
 208 }
 209
 210 bool geomagnetic_rv_sensor::delete_interval(int client_id)
 211 {
 212         AUTOLOCK(m_mutex);
 213
 214         m_accel_sensor->delete_interval(client_id, false);
 215         m_magnetic_sensor->delete_interval(client_id, false);
 216
 217         return sensor_base::delete_interval(client_id, false);
 218 }
 219
 220 void geomagnetic_rv_sensor::synthesize(const sensor_event_t& event, vector<sensor_event_t> &outs)
 221 {
 222         const float MIN_DELIVERY_DIFF_FACTOR = 0.75f;
 223         unsigned long long diff_time;
 224
 225         sensor_event_t rv_event;
 226         quaternion<float> quaternion_geo_rv;
 227
 228         if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) {
 229                 diff_time = event.data.timestamp - m_time;
 230
 231                 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 232                         return;
 233
 234                 pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale);
 235
 236                 m_accel.m_time_stamp = event.data.timestamp;
 237
 238                 m_enable_geomagnetic_rv |= ACCELEROMETER_ENABLED;
 239         }
 240         else if (event.event_type == GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME) {
 241                 diff_time = event.data.timestamp - m_time;
 242
 243                 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 244                         return;
 245
 246                 pre_process_data(m_magnetic, event.data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale);
 247
 248                 m_magnetic.m_time_stamp = event.data.timestamp;
 249
 250                 m_enable_geomagnetic_rv |= GEOMAGNETIC_ENABLED;
 251         }
 252
 253         if (m_enable_geomagnetic_rv == GEOMAGNETIC_RV_ENABLED) {
 254                 m_enable_geomagnetic_rv = 0;
 255
 256                 m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
 257
 258                 {
 259                         AUTOLOCK(m_fusion_mutex);
 260                         quaternion_geo_rv = m_orientation_filter.get_geomagnetic_quaternion(m_accel, m_magnetic);
 261                 }
 262
 263                 m_time = get_timestamp();
 264
 265                 rv_event.sensor_id = get_id();
 266                 rv_event.event_type = GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME;
 267                 rv_event.data.accuracy = SENSOR_ACCURACY_GOOD;
 268                 rv_event.data.timestamp = m_time;
 269                 rv_event.data.value_count = 4;
 270                 rv_event.data.values[0] = quaternion_geo_rv.m_quat.m_vec[1];
 271                 rv_event.data.values[1] = quaternion_geo_rv.m_quat.m_vec[2];
 272                 rv_event.data.values[2] = quaternion_geo_rv.m_quat.m_vec[3];
 273                 rv_event.data.values[3] = quaternion_geo_rv.m_quat.m_vec[0];
 274
 275                 push(rv_event);
 276         }
 277
 278         return;
 279 }
 280
 281 int geomagnetic_rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &data)
 282 {
 283         sensor_data<float> accel;
 284         sensor_data<float> magnetic;
 285
 286         sensor_data_t accel_data;
 287         sensor_data_t magnetic_data;
 288
 289         quaternion<float> quaternion_geo_rv;
 290
 291         if (event_type != GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME)
 292                 return -1;
 293
 294         m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data);
 295         m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, magnetic_data);
 296
 297         pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale);
 298         pre_process_data(magnetic, magnetic_data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale);
 299         accel.m_time_stamp = accel_data.timestamp;
 300         magnetic.m_time_stamp = magnetic_data.timestamp;
 301
 302         m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
 303
 304         {
 305                 AUTOLOCK(m_fusion_mutex);
 306                 quaternion_geo_rv = m_orientation_filter.get_geomagnetic_quaternion(m_accel, m_magnetic);
 307         }
 308
 309         data.accuracy = SENSOR_ACCURACY_GOOD;
 310         data.timestamp = m_time;
 311         data.value_count = 4;
 312         data.values[0] = quaternion_geo_rv.m_quat.m_vec[1];
 313         data.values[1] = quaternion_geo_rv.m_quat.m_vec[2];
 314         data.values[2] = quaternion_geo_rv.m_quat.m_vec[3];
 315         data.values[3] = quaternion_geo_rv.m_quat.m_vec[0];
 316
 317         return 0;
 318 }
 319
 320 bool geomagnetic_rv_sensor::get_properties(sensor_properties_s &properties)
 321 {
 322         properties.vendor = m_vendor;
 323         properties.name = SENSOR_NAME;
 324         properties.min_range = -1;
 325         properties.max_range = 1;
 326         properties.resolution = 0.000001;
 327         properties.fifo_count = 0;
 328         properties.max_batch_count = 0;
 329         properties.min_interval = 1;
 330
 331         return true;
 332 }
 333
 334 extern "C" sensor_module* create(void)
 335 {
 336         geomagnetic_rv_sensor *sensor;
 337
 338         try {
 339                 sensor = new(std::nothrow) geomagnetic_rv_sensor;
 340         } catch (int err) {
 341                 ERR("Failed to create module, err: %d, cause: %s", err, strerror(err));
 342                 return NULL;
 343         }
 344
 345         sensor_module *module = new(std::nothrow) sensor_module;
 346         retvm_if(!module || !sensor, NULL, "Failed to allocate memory");
 347
 348         module->sensors.push_back(sensor);
 349         return module;
 350 }