src/server/plugins/linear_accel/linear_accel_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 <sensor_logs.h>
  29 #include <linear_accel_sensor.h>
  30 #include <sensor_loader.h>
  31 #include <virtual_sensor_config.h>
  32
  33 using std::string;
  34 using std::vector;
  35
  36 #define SENSOR_NAME "LINEAR_ACCEL_SENSOR"
  37 #define SENSOR_TYPE_LINEAR_ACCEL        "LINEAR_ACCEL"
  38 #define SENSOR_TYPE_GRAVITY             "GRAVITY"
  39 #define SENSOR_TYPE_ORIENTATION         "ORIENTATION"
  40
  41 #define ELEMENT_NAME                                                                                    "NAME"
  42 #define ELEMENT_VENDOR                                                                                  "VENDOR"
  43 #define ELEMENT_RAW_DATA_UNIT                                                                   "RAW_DATA_UNIT"
  44 #define ELEMENT_DEFAULT_SAMPLING_TIME                                                   "DEFAULT_SAMPLING_TIME"
  45 #define ELEMENT_ACCEL_STATIC_BIAS                                                               "ACCEL_STATIC_BIAS"
  46 #define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION                   "ACCEL_ROTATION_DIRECTION_COMPENSATION"
  47 #define ELEMENT_ACCEL_SCALE                                                                             "ACCEL_SCALE"
  48 #define ELEMENT_LINEAR_ACCEL_SIGN_COMPENSATION                                  "LINEAR_ACCEL_SIGN_COMPENSATION"
  49 #define ELEMENT_ORIENTATION_DATA_UNIT                                                   "RAW_DATA_UNIT"
  50 #define ELEMENT_GRAVITY_SIGN_COMPENSATION                                               "GRAVITY_SIGN_COMPENSATION"
  51 #define ELEMENT_PITCH_ROTATION_COMPENSATION                                             "PITCH_ROTATION_COMPENSATION"
  52 #define ELEMENT_ROLL_ROTATION_COMPENSATION                                              "ROLL_ROTATION_COMPENSATION"
  53 #define ELEMENT_AZIMUTH_ROTATION_COMPENSATION                                   "AZIMUTH_ROTATION_COMPENSATION"
  54
  55 #define INITIAL_VALUE -1
  56 #define GRAVITY 9.80665
  57 #define DEVIATION 0.1
  58
  59 #define PI 3.141593
  60 #define AZIMUTH_OFFSET_DEGREES 360
  61 #define AZIMUTH_OFFSET_RADIANS (2 * PI)
  62
  63 #define MS_TO_US 1000
  64 #define MIN_DELIVERY_DIFF_FACTOR 0.75f
  65
  66 #define ACCELEROMETER_ENABLED 0x01
  67 #define GRAVITY_ENABLED 0x02
  68 #define LINEAR_ACCEL_ENABLED 3
  69
  70 linear_accel_sensor::linear_accel_sensor()
  71 : m_accel_sensor(NULL)
  72 , m_gyro_sensor(NULL)
  73 , m_magnetic_sensor(NULL)
  74 , m_fusion_sensor(NULL)
  75 , m_time(0)
  76 {
  77         virtual_sensor_config &config = virtual_sensor_config::get_instance();
  78
  79         m_name = string(SENSOR_NAME);
  80         m_enable_linear_accel = 0;
  81         register_supported_event(LINEAR_ACCEL_RAW_DATA_EVENT);
  82
  83         sensor_hal *fusion_sensor_hal = sensor_loader::get_instance().get_sensor_hal(SENSOR_HAL_TYPE_FUSION);
  84         if (!fusion_sensor_hal)
  85                 m_hardware_fusion = false;
  86         else
  87                 m_hardware_fusion = true;
  88
  89
  90         if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_VENDOR, m_vendor)) {
  91                 ERR("[VENDOR] is empty\n");
  92                 throw ENXIO;
  93         }
  94
  95         INFO("m_vendor = %s", m_vendor.c_str());
  96
  97         if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) {
  98                 ERR("[RAW_DATA_UNIT] is empty\n");
  99                 throw ENXIO;
 100         }
 101
 102         INFO("m_raw_data_unit = %s", m_raw_data_unit.c_str());
 103
 104         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ORIENTATION_DATA_UNIT, m_orientation_data_unit)) {
 105                 ERR("[ORIENTATION_DATA_UNIT] is empty\n");
 106                 throw ENXIO;
 107         }
 108
 109         INFO("m_orientation_data_unit = %s", m_orientation_data_unit.c_str());
 110
 111         if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) {
 112                 ERR("[DEFAULT_SAMPLING_TIME] is empty\n");
 113                 throw ENXIO;
 114         }
 115
 116         INFO("m_default_sampling_time = %d", m_default_sampling_time);
 117
 118         if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) {
 119                 ERR("[ACCEL_STATIC_BIAS] is empty\n");
 120                 throw ENXIO;
 121         }
 122
 123         if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION, m_accel_rotation_direction_compensation, 3)) {
 124                 ERR("[ACCEL_ROTATION_DIRECTION_COMPENSATION] is empty\n");
 125                 throw ENXIO;
 126         }
 127
 128         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]);
 129
 130         if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_GRAVITY_SIGN_COMPENSATION, m_gravity_sign_compensation, 3)) {
 131                 ERR("[GRAVITY_SIGN_COMPENSATION] is empty\n");
 132                 throw ENXIO;
 133         }
 134
 135         INFO("m_gravity_sign_compensation = (%d, %d, %d)", m_gravity_sign_compensation[0], m_gravity_sign_compensation[1], m_gravity_sign_compensation[2]);
 136
 137         if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_LINEAR_ACCEL_SIGN_COMPENSATION, m_linear_accel_sign_compensation, 3)) {
 138                 ERR("[LINEAR_ACCEL_SIGN_COMPENSATION] is empty\n");
 139                 throw ENXIO;
 140         }
 141
 142         INFO("m_linear_accel_sign_compensation = (%d, %d, %d)", m_linear_accel_sign_compensation[0], m_linear_accel_sign_compensation[1], m_linear_accel_sign_compensation[2]);
 143
 144         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_AZIMUTH_ROTATION_COMPENSATION, &m_azimuth_rotation_compensation)) {
 145                 ERR("[AZIMUTH_ROTATION_COMPENSATION] is empty\n");
 146                 throw ENXIO;
 147         }
 148
 149         INFO("m_azimuth_rotation_compensation = %d", m_azimuth_rotation_compensation);
 150
 151         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) {
 152                 ERR("[PITCH_ROTATION_COMPENSATION] is empty\n");
 153                 throw ENXIO;
 154         }
 155
 156         INFO("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation);
 157
 158         if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) {
 159                 ERR("[ROLL_ROTATION_COMPENSATION] is empty\n");
 160                 throw ENXIO;
 161         }
 162
 163         INFO("m_roll_rotation_compensation = %d", m_roll_rotation_compensation);
 164
 165         m_interval = m_default_sampling_time * MS_TO_US;
 166
 167 }
 168
 169 linear_accel_sensor::~linear_accel_sensor()
 170 {
 171         INFO("linear_accel_sensor is destroyed!\n");
 172 }
 173
 174 bool linear_accel_sensor::init()
 175 {
 176         m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
 177         m_gyro_sensor = sensor_loader::get_instance().get_sensor(GYROSCOPE_SENSOR);
 178         m_magnetic_sensor = sensor_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR);
 179
 180         m_fusion_sensor = sensor_loader::get_instance().get_sensor(FUSION_SENSOR);
 181
 182         if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor || !m_fusion_sensor) {
 183                 ERR("Failed to load sensors,  accel: 0x%x, gyro: 0x%x, mag: 0x%x, fusion: 0x%x",
 184                         m_accel_sensor, m_gyro_sensor, m_magnetic_sensor, m_fusion_sensor);
 185                 return false;
 186         }
 187
 188         INFO("%s is created!", sensor_base::get_name());
 189         return true;
 190 }
 191
 192 void linear_accel_sensor::get_types(vector<sensor_type_t> &types)
 193 {
 194         types.push_back(LINEAR_ACCEL_SENSOR);
 195 }
 196
 197 bool linear_accel_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
 205         if (!m_hardware_fusion) {
 206                 m_gyro_sensor->add_client(GYROSCOPE_RAW_DATA_EVENT);
 207                 m_gyro_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
 208                 m_gyro_sensor->start();
 209                 m_magnetic_sensor->add_client(GEOMAGNETIC_RAW_DATA_EVENT);
 210                 m_magnetic_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
 211                 m_magnetic_sensor->start();
 212         }
 213
 214         m_fusion_sensor->register_supported_event(FUSION_EVENT);
 215         m_fusion_sensor->register_supported_event(FUSION_ORIENTATION_ENABLED);
 216         m_fusion_sensor->add_client(FUSION_EVENT);
 217         m_fusion_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
 218         m_fusion_sensor->start();
 219
 220         activate();
 221         return true;
 222 }
 223
 224 bool linear_accel_sensor::on_stop(void)
 225 {
 226         AUTOLOCK(m_mutex);
 227         m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT);
 228         m_accel_sensor->delete_interval((intptr_t)this, false);
 229         m_accel_sensor->stop();
 230
 231         if (!m_hardware_fusion) {
 232                 m_gyro_sensor->delete_client(GYROSCOPE_RAW_DATA_EVENT);
 233                 m_gyro_sensor->delete_interval((intptr_t)this, false);
 234                 m_gyro_sensor->stop();
 235                 m_magnetic_sensor->delete_client(GEOMAGNETIC_RAW_DATA_EVENT);
 236                 m_magnetic_sensor->delete_interval((intptr_t)this, false);
 237                 m_magnetic_sensor->stop();
 238         }
 239
 240         m_fusion_sensor->delete_client(FUSION_EVENT);
 241         m_fusion_sensor->delete_interval((intptr_t)this, false);
 242         m_fusion_sensor->unregister_supported_event(FUSION_EVENT);
 243         m_fusion_sensor->unregister_supported_event(FUSION_ORIENTATION_ENABLED);
 244         m_fusion_sensor->stop();
 245
 246         deactivate();
 247         return true;
 248 }
 249
 250 bool linear_accel_sensor::add_interval(int client_id, unsigned int interval)
 251 {
 252         AUTOLOCK(m_mutex);
 253         m_accel_sensor->add_interval(client_id, interval, false);
 254
 255         if (!m_hardware_fusion) {
 256                 m_gyro_sensor->add_interval(client_id, interval, false);
 257                 m_magnetic_sensor->add_interval(client_id, interval, false);
 258         }
 259
 260         m_fusion_sensor->add_interval(client_id, interval, false);
 261
 262         return sensor_base::add_interval(client_id, interval, false);
 263 }
 264
 265 bool linear_accel_sensor::delete_interval(int client_id)
 266 {
 267         AUTOLOCK(m_mutex);
 268         m_accel_sensor->delete_interval(client_id, false);
 269
 270         if (!m_hardware_fusion) {
 271                 m_gyro_sensor->delete_interval(client_id, false);
 272                 m_magnetic_sensor->delete_interval(client_id, false);
 273         }
 274
 275         m_fusion_sensor->delete_interval(client_id, false);
 276
 277         return sensor_base::delete_interval(client_id, false);
 278 }
 279
 280 sensor_data_t linear_accel_sensor::calculate_gravity(sensor_data_t data)
 281 {
 282         sensor_data_t gravity_data;
 283         float pitch, roll, azimuth;
 284         float azimuth_offset;
 285
 286         quaternion<float> quat(data.values[0], data.values[1],
 287                         data.values[2], data.values[3]);
 288
 289         euler_angles<float> euler = quat2euler(quat);
 290
 291         if(m_orientation_data_unit == "DEGREES") {
 292                 euler = rad2deg(euler);
 293                 azimuth_offset = AZIMUTH_OFFSET_DEGREES;
 294         }
 295         else {
 296                 azimuth_offset = AZIMUTH_OFFSET_RADIANS;
 297         }
 298
 299         euler.m_ang.m_vec[0] *= m_pitch_rotation_compensation;
 300         euler.m_ang.m_vec[1] *= m_roll_rotation_compensation;
 301         euler.m_ang.m_vec[2] *= m_azimuth_rotation_compensation;
 302
 303         pitch = euler.m_ang.m_vec[0];
 304         roll = euler.m_ang.m_vec[1];
 305         if (euler.m_ang.m_vec[2] >= 0)
 306                 azimuth = euler.m_ang.m_vec[2];
 307         else
 308                 azimuth = euler.m_ang.m_vec[2] + azimuth_offset;
 309
 310         if(m_orientation_data_unit == "DEGREES") {
 311                 azimuth *= DEG2RAD;
 312                 pitch *= DEG2RAD;
 313                 roll *= DEG2RAD;
 314         }
 315
 316
 317         if ((roll >= (M_PI/2)-DEVIATION && roll <= (M_PI/2)+DEVIATION) ||
 318                         (roll >= -(M_PI/2)-DEVIATION && roll <= -(M_PI/2)+DEVIATION)) {
 319                 gravity_data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll) * cos(azimuth);
 320                 gravity_data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(azimuth);
 321                 gravity_data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll);
 322         } else if ((pitch >= (M_PI/2)-DEVIATION && pitch <= (M_PI/2)+DEVIATION) ||
 323                         (pitch >= -(M_PI/2)-DEVIATION && pitch <= -(M_PI/2)+DEVIATION)) {
 324                 gravity_data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(azimuth);
 325                 gravity_data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch) * cos(azimuth);
 326                 gravity_data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(pitch);
 327         } else {
 328                 gravity_data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll);
 329                 gravity_data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch);
 330                 gravity_data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll) * cos(pitch);
 331         }
 332         gravity_data.value_count = 3;
 333         gravity_data.timestamp = m_time;
 334         gravity_data.accuracy = SENSOR_ACCURACY_GOOD;
 335
 336         return gravity_data;
 337 }
 338
 339 void linear_accel_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
 340 {
 341         sensor_event_t lin_accel_event;
 342         sensor_data_t gravity_data;
 343
 344         unsigned long long diff_time;
 345
 346         if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) {
 347                 diff_time = event.data.timestamp - m_time;
 348
 349                 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 350                         return;
 351
 352                 m_accel.m_data.m_vec[0] = m_accel_rotation_direction_compensation[0] * (event.data.values[0] - m_accel_static_bias[0]) / ACCEL_SCALE;
 353                 m_accel.m_data.m_vec[1] = m_accel_rotation_direction_compensation[1] * (event.data.values[1] - m_accel_static_bias[1]) / ACCEL_SCALE;
 354                 m_accel.m_data.m_vec[2] = m_accel_rotation_direction_compensation[2] * (event.data.values[2] - m_accel_static_bias[2]) / ACCEL_SCALE;
 355
 356                 m_accel.m_time_stamp = event.data.timestamp;
 357
 358                 m_enable_linear_accel |= ACCELEROMETER_ENABLED;
 359         }
 360         else if (event.event_type == FUSION_EVENT) {
 361                 diff_time = event.data.timestamp - m_time;
 362
 363                 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 364                         return;
 365
 366                 gravity_data = calculate_gravity(event.data);
 367
 368                 m_enable_linear_accel |= GRAVITY_ENABLED;
 369         }
 370
 371         if (m_enable_linear_accel == LINEAR_ACCEL_ENABLED) {
 372                 m_enable_linear_accel = 0;
 373
 374                 m_time = get_timestamp();
 375                 lin_accel_event.sensor_id = get_id();
 376                 lin_accel_event.event_type = LINEAR_ACCEL_RAW_DATA_EVENT;
 377                 lin_accel_event.data.value_count = 3;
 378                 lin_accel_event.data.timestamp = m_time;
 379                 lin_accel_event.data.accuracy = SENSOR_ACCURACY_GOOD;
 380                 lin_accel_event.data.values[0] = m_linear_accel_sign_compensation[0] * (m_accel.m_data.m_vec[0] - gravity_data.values[0]);
 381                 lin_accel_event.data.values[1] = m_linear_accel_sign_compensation[1] * (m_accel.m_data.m_vec[1] - gravity_data.values[1]);
 382                 lin_accel_event.data.values[2] = m_linear_accel_sign_compensation[2] * (m_accel.m_data.m_vec[2] - gravity_data.values[2]);
 383                 push(lin_accel_event);
 384         }
 385
 386         return;
 387 }
 388
 389 int linear_accel_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
 390 {
 391         sensor_data_t gravity_data, accel_data, fusion_data;
 392         m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data);
 393         m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data);
 394
 395         gravity_data = calculate_gravity(fusion_data);
 396
 397         accel_data.values[0] = m_accel_rotation_direction_compensation[0] * (accel_data.values[0] - m_accel_static_bias[0]) / ACCEL_SCALE;
 398         accel_data.values[1] = m_accel_rotation_direction_compensation[1] * (accel_data.values[1] - m_accel_static_bias[1]) / ACCEL_SCALE;
 399         accel_data.values[2] = m_accel_rotation_direction_compensation[2] * (accel_data.values[2] - m_accel_static_bias[2]) / ACCEL_SCALE;
 400
 401         if (event_type != LINEAR_ACCEL_RAW_DATA_EVENT)
 402                 return -1;
 403
 404         data.accuracy = SENSOR_ACCURACY_GOOD;
 405         data.timestamp = get_timestamp();
 406         data.values[0] = m_linear_accel_sign_compensation[0] * (accel_data.values[0] - gravity_data.values[0]);
 407         data.values[1] = m_linear_accel_sign_compensation[1] * (accel_data.values[1] - gravity_data.values[1]);
 408         data.values[2] = m_linear_accel_sign_compensation[2] * (accel_data.values[2] - gravity_data.values[2]);
 409         data.value_count = 3;
 410         return 0;
 411 }
 412
 413 bool linear_accel_sensor::get_properties(sensor_type_t sensor_type, sensor_properties_s &properties)
 414 {
 415         m_accel_sensor->get_properties(ACCELEROMETER_SENSOR, properties);
 416         properties.name = "Linear Acceleration Sensor";
 417         properties.vendor = m_vendor;
 418         properties.resolution = 0.000001;
 419
 420         return true;
 421 }
 422