src/sensor/gravity/gravity_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
  29 #include <sensor_log.h>
  30 #include <sensor_types.h>
  31
  32 #include <sensor_common.h>
  33 #include <virtual_sensor.h>
  34 #include <gravity_sensor.h>
  35 #include <sensor_loader.h>
  36 #include <fusion_util.h>
  37
  38 #define SENSOR_NAME "SENSOR_GRAVITY"
  39
  40 #define GRAVITY 9.80665
  41
  42 #define PHASE_ACCEL_READY 0x01
  43 #define PHASE_GYRO_READY 0x02
  44 #define PHASE_FUSION_READY 0x03
  45 #define US_PER_SEC 1000000
  46 #define MS_PER_SEC 1000
  47 #define INV_ANGLE -1000
  48 #define TAU_LOW 0.4
  49 #define TAU_MID 0.75
  50 #define TAU_HIGH 0.99
  51
  52 #define DEG2RAD(x) ((x) * M_PI / 180.0)
  53 #define NORM(x, y, z) sqrt((x)*(x) + (y)*(y) + (z)*(z))
  54 #define ARCTAN(x, y) ((x) == 0 ? 0 : (y) != 0 ? atan2((x), (y)) : (x) > 0 ? M_PI/2.0 : -M_PI/2.0)
  55
  56 gravity_sensor::gravity_sensor()
  57 : m_fusion(NULL)
  58 , m_accel_sensor(NULL)
  59 , m_gyro_sensor(NULL)
  60 , m_fusion_phase(0)
  61 , m_x(-1)
  62 , m_y(-1)
  63 , m_z(-1)
  64 , m_accuracy(-1)
  65 , m_time(0)
  66 {
  67 }
  68
  69 gravity_sensor::~gravity_sensor()
  70 {
  71         _I("gravity_sensor is destroyed!\n");
  72 }
  73
  74 bool gravity_sensor::init()
  75 {
  76         /* Acc (+ Gyro) fusion */
  77         m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
  78
  79         if (!m_accel_sensor) {
  80                 _E("cannot load accelerometer sensor_hal[%s]", get_name());
  81                 return false;
  82         }
  83
  84         m_gyro_sensor = sensor_loader::get_instance().get_sensor(GYROSCOPE_SENSOR);
  85
  86         _I("%s (%s) is created!\n", get_name(), m_gyro_sensor ? "Acc+Gyro Fusion" : "LowPass Acc");
  87         return true;
  88 }
  89
  90 sensor_type_t gravity_sensor::get_type(void)
  91 {
  92         return GRAVITY_SENSOR;
  93 }
  94
  95 unsigned int gravity_sensor::get_event_type(void)
  96 {
  97         return GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME;
  98 }
  99
 100 const char* gravity_sensor::get_name(void)
 101 {
 102         return SENSOR_NAME;
 103 }
 104
 105 bool gravity_sensor::get_sensor_info(sensor_info &info)
 106 {
 107         info.set_type(get_type());
 108         info.set_id(get_id());
 109         info.set_privilege(SENSOR_PRIVILEGE_PUBLIC); // FIXME
 110         info.set_name("Gravity Sensor");
 111         info.set_vendor("Samsung Electronics");
 112         info.set_min_range(-19.6);
 113         info.set_max_range(19.6);
 114         info.set_resolution(0.01);
 115         info.set_min_interval(1);
 116         info.set_fifo_count(0);
 117         info.set_max_batch_count(0);
 118         info.set_supported_event(get_event_type());
 119         info.set_wakeup_supported(false);
 120
 121         return true;
 122 }
 123
 124 void gravity_sensor::synthesize(const sensor_event_t& event)
 125 {
 126         /* If the rotation vector sensor is available */
 127         if (m_fusion) {
 128                 synthesize_rv(event);
 129                 return;
 130         }
 131
 132         /* If both Acc & Gyro are available */
 133         if (m_gyro_sensor) {
 134                 synthesize_fusion(event);
 135                 return;
 136         }
 137
 138         /* If only Acc is available */
 139         synthesize_lowpass(event);
 140 }
 141
 142 void gravity_sensor::synthesize_rv(const sensor_event_t& event)
 143 {
 144         if (!m_fusion->is_data_ready())
 145                 return;
 146
 147         sensor_event_t *gravity_event;
 148         float gravity[3];
 149         float x, y, z, w, heading_accuracy;
 150         int accuracy;
 151
 152         if (!m_fusion->get_rotation_vector(x, y, z, w, heading_accuracy, accuracy)) {
 153                 _E("Failed to get rotation vector");
 154                 return;
 155         }
 156
 157         unsigned long long timestamp = m_fusion->get_data_timestamp();
 158
 159         if (!check_sampling_time(timestamp))
 160                 return;
 161
 162         float rotation[4] = {x, y, z, w};
 163
 164         rotation_to_gravity(rotation, gravity);
 165
 166         gravity_event = (sensor_event_t *)malloc(sizeof(sensor_event_t));
 167         if (!gravity_event) {
 168                 _E("Failed to allocate memory");
 169                 return;
 170         }
 171         gravity_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 172         if (!gravity_event->data) {
 173                 _E("Failed to allocate memory");
 174                 free(gravity_event);
 175                 return;
 176         }
 177
 178         gravity_event->sensor_id = get_id();
 179         gravity_event->event_type = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME;
 180         gravity_event->data_length = sizeof(sensor_data_t);
 181         gravity_event->data->accuracy = accuracy;
 182         gravity_event->data->timestamp = m_fusion->get_data_timestamp();
 183         gravity_event->data->value_count = 3;
 184         gravity_event->data->values[0] = gravity[0];
 185         gravity_event->data->values[1] = gravity[1];
 186         gravity_event->data->values[2] = gravity[2];
 187         push(gravity_event);
 188
 189         m_time = event.data->timestamp;
 190         m_x = gravity[0];
 191         m_y = gravity[1];
 192         m_z = gravity[2];
 193         m_accuracy = accuracy;
 194 }
 195
 196 void gravity_sensor::synthesize_lowpass(const sensor_event_t& event)
 197 {
 198         if (event.event_type != ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME)
 199                 return;
 200
 201         sensor_event_t *gravity_event;
 202         float x, y, z, norm, alpha, tau, err;
 203
 204         norm = NORM(event.data->values[0], event.data->values[1], event.data->values[2]);
 205         x = event.data->values[0] / norm * GRAVITY;
 206         y = event.data->values[1] / norm * GRAVITY;
 207         z = event.data->values[2] / norm * GRAVITY;
 208
 209         if (m_time > 0) {
 210                 err = fabs(norm - GRAVITY) / GRAVITY;
 211                 tau = (err < 0.1 ? TAU_LOW : err > 0.9 ? TAU_HIGH : TAU_MID);
 212                 alpha = tau / (tau + (float)(event.data->timestamp - m_time) / US_PER_SEC);
 213                 x = alpha * m_x + (1 - alpha) * x;
 214                 y = alpha * m_y + (1 - alpha) * y;
 215                 z = alpha * m_z + (1 - alpha) * z;
 216                 norm = NORM(x, y, z);
 217                 x = x / norm * GRAVITY;
 218                 y = y / norm * GRAVITY;
 219                 z = z / norm * GRAVITY;
 220         }
 221
 222         gravity_event = (sensor_event_t *)malloc(sizeof(sensor_event_t));
 223         if (!gravity_event) {
 224                 _E("Failed to allocate memory");
 225                 return;
 226         }
 227         gravity_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 228         if (!gravity_event->data) {
 229                 _E("Failed to allocate memory");
 230                 free(gravity_event);
 231                 return;
 232         }
 233
 234         gravity_event->sensor_id = get_id();
 235         gravity_event->event_type = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME;
 236         gravity_event->data_length = sizeof(sensor_data_t);
 237         gravity_event->data->accuracy = event.data->accuracy;
 238         gravity_event->data->timestamp = event.data->timestamp;
 239         gravity_event->data->value_count = 3;
 240         gravity_event->data->values[0] = x;
 241         gravity_event->data->values[1] = y;
 242         gravity_event->data->values[2] = z;
 243         push(gravity_event);
 244
 245         m_time = event.data->timestamp;
 246         m_x = x;
 247         m_y = y;
 248         m_z = z;
 249         m_accuracy = event.data->accuracy;
 250 }
 251
 252 void gravity_sensor::synthesize_fusion(const sensor_event_t& event)
 253 {
 254         sensor_event_t *gravity_event;
 255
 256         if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) {
 257                 fusion_set_accel(event);
 258                 m_fusion_phase |= PHASE_ACCEL_READY;
 259         } else if (event.event_type == GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME) {
 260                 fusion_set_gyro(event);
 261                 m_fusion_phase |= PHASE_GYRO_READY;
 262         }
 263
 264         if (m_fusion_phase != PHASE_FUSION_READY)
 265                 return;
 266
 267         m_fusion_phase = 0;
 268
 269         fusion_update_angle();
 270         fusion_get_gravity();
 271
 272         gravity_event = (sensor_event_t *)malloc(sizeof(sensor_event_t));
 273         if (!gravity_event) {
 274                 _E("Failed to allocate memory");
 275                 return;
 276         }
 277         gravity_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 278         if (!gravity_event->data) {
 279                 _E("Failed to allocate memory");
 280                 free(gravity_event);
 281                 return;
 282         }
 283
 284         gravity_event->sensor_id = get_id();
 285         gravity_event->event_type = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME;
 286         gravity_event->data_length = sizeof(sensor_data_t);
 287         gravity_event->data->accuracy = m_accuracy;
 288         gravity_event->data->timestamp = m_time;
 289         gravity_event->data->value_count = 3;
 290         gravity_event->data->values[0] = m_x;
 291         gravity_event->data->values[1] = m_y;
 292         gravity_event->data->values[2] = m_z;
 293         push(gravity_event);
 294 }
 295
 296 void gravity_sensor::fusion_set_accel(const sensor_event_t& event)
 297 {
 298         double x = event.data->values[0];
 299         double y = event.data->values[1];
 300         double z = event.data->values[2];
 301
 302         m_accel_mag = NORM(x, y, z);
 303
 304         m_angle_n[0] = ARCTAN(z, y);
 305         m_angle_n[1] = ARCTAN(x, z);
 306         m_angle_n[2] = ARCTAN(y, x);
 307
 308         m_accuracy = event.data->accuracy;
 309         m_time_new = event.data->timestamp;
 310
 311         _D("AccIn: (%f, %f, %f)", x/m_accel_mag, y/m_accel_mag, z/m_accel_mag);
 312 }
 313
 314 void gravity_sensor::fusion_set_gyro(const sensor_event_t& event)
 315 {
 316         m_velocity[0] = -DEG2RAD(event.data->values[0]);
 317         m_velocity[1] = -DEG2RAD(event.data->values[1]);
 318         m_velocity[2] = -DEG2RAD(event.data->values[2]);
 319
 320         m_time_new = event.data->timestamp;
 321 }
 322
 323 void gravity_sensor::fusion_update_angle()
 324 {
 325         _D("AngleIn: (%f, %f, %f)", m_angle_n[0], m_angle_n[1], m_angle_n[2]);
 326         _D("AngAccl: (%f, %f, %f)", m_velocity[0], m_velocity[1], m_velocity[2]);
 327         _D("Angle  : (%f, %f, %f)", m_angle[0], m_angle[1], m_angle[2]);
 328
 329         if (m_angle[0] == INV_ANGLE) {
 330                 /* 1st iteration */
 331                 m_angle[0] = m_angle_n[0];
 332                 m_angle[1] = m_angle_n[1];
 333                 m_angle[2] = m_angle_n[2];
 334         } else {
 335                 complementary(m_time_new - m_time);
 336         }
 337
 338         _D("Angle' : (%f, %f, %f)", m_angle[0], m_angle[1], m_angle[2]);
 339 }
 340
 341 void gravity_sensor::fusion_get_gravity()
 342 {
 343         double x = 0, y = 0, z = 0;
 344         double norm;
 345         double vec[3][3];
 346
 347         /* Rotating along y-axis then z-axis */
 348         vec[0][2] = cos(m_angle[1]);
 349         vec[0][0] = sin(m_angle[1]);
 350         vec[0][1] = vec[0][0] * tan(m_angle[2]);
 351
 352         /* Rotating along z-axis then x-axis */
 353         vec[1][0] = cos(m_angle[2]);
 354         vec[1][1] = sin(m_angle[2]);
 355         vec[1][2] = vec[1][1] * tan(m_angle[0]);
 356
 357         /* Rotating along x-axis then y-axis */
 358         vec[2][1] = cos(m_angle[0]);
 359         vec[2][2] = sin(m_angle[0]);
 360         vec[2][0] = vec[2][2] * tan(m_angle[1]);
 361
 362         /* Normalize */
 363         for (int i = 0; i < 3; ++i) {
 364                 norm = NORM(vec[i][0], vec[i][1], vec[i][2]);
 365                 vec[i][0] /= norm;
 366                 vec[i][1] /= norm;
 367                 vec[i][2] /= norm;
 368                 x += vec[i][0];
 369                 y += vec[i][1];
 370                 z += vec[i][2];
 371         }
 372
 373         norm = NORM(x, y, z);
 374
 375         m_x = x / norm * GRAVITY;
 376         m_y = y / norm * GRAVITY;
 377         m_z = z / norm * GRAVITY;
 378         m_time = m_time_new;
 379 }
 380
 381 void gravity_sensor::complementary(unsigned long long time_diff)
 382 {
 383         double err = fabs(m_accel_mag - GRAVITY) / GRAVITY;
 384         double tau = (err < 0.1 ? TAU_LOW : err > 0.9 ? TAU_HIGH : TAU_MID);
 385         double delta_t = (double)time_diff/ US_PER_SEC;
 386         double alpha = tau / (tau + delta_t);
 387
 388         _D("mag, err, tau, dt, alpha = %f, %f, %f, %f, %f", m_accel_mag, err, tau, delta_t, alpha);
 389
 390         m_angle[0] = complementary(m_angle[0], m_angle_n[0], m_velocity[0], delta_t, alpha);
 391         m_angle[1] = complementary(m_angle[1], m_angle_n[1], m_velocity[1], delta_t, alpha);
 392         m_angle[2] = complementary(m_angle[2], m_angle_n[2], m_velocity[2], delta_t, alpha);
 393 }
 394
 395 double gravity_sensor::complementary(double angle, double angle_in, double vel, double delta_t, double alpha)
 396 {
 397         double s, c;
 398         angle = angle + vel * delta_t;
 399         s = alpha * sin(angle) + (1 - alpha) * sin(angle_in);
 400         c = alpha * cos(angle) + (1 - alpha) * cos(angle_in);
 401         return ARCTAN(s, c);
 402 }
 403
 404 int gravity_sensor::get_data(sensor_data_t **data, int *length)
 405 {
 406         /* if It is batch sensor, remains can be 2+ */
 407         int remains = 1;
 408
 409         sensor_data_t *sensor_data;
 410         sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 411
 412         sensor_data->accuracy = m_accuracy;
 413         sensor_data->timestamp = m_time;
 414         sensor_data->value_count = 3;
 415         sensor_data->values[0] = m_x;
 416         sensor_data->values[1] = m_y;
 417         sensor_data->values[2] = m_z;
 418
 419         *data = sensor_data;
 420         *length = sizeof(sensor_data_t);
 421
 422         return --remains;
 423 }
 424
 425 bool gravity_sensor::set_batch_latency(unsigned long latency)
 426 {
 427         return false;
 428 }
 429
 430 bool gravity_sensor::set_wakeup(int wakeup)
 431 {
 432         return false;
 433 }
 434
 435 bool gravity_sensor::on_start(void)
 436 {
 437         if (m_fusion)
 438                 m_fusion->start();
 439
 440         if (m_accel_sensor)
 441                 m_accel_sensor->start();
 442
 443         if (m_gyro_sensor)
 444                 m_gyro_sensor->start();
 445
 446         m_time = 0;
 447         m_fusion_phase = 0;
 448         m_angle[0] = INV_ANGLE;
 449
 450         return activate();
 451 }
 452
 453 bool gravity_sensor::on_stop(void)
 454 {
 455         if (m_fusion)
 456                 m_fusion->stop();
 457
 458         if (m_accel_sensor)
 459                 m_accel_sensor->stop();
 460
 461         if (m_gyro_sensor)
 462                 m_gyro_sensor->stop();
 463
 464         m_time = 0;
 465
 466         return deactivate();
 467 }
 468
 469 bool gravity_sensor::add_interval(int client_id, unsigned int interval, bool is_processor)
 470 {
 471         if (m_fusion)
 472                 m_fusion->set_interval(FUSION_EVENT_AGM, client_id, interval);
 473
 474         if (m_accel_sensor)
 475                 m_accel_sensor->add_interval(client_id, interval, true);
 476
 477         if (m_gyro_sensor)
 478                 m_gyro_sensor->add_interval(client_id, interval, true);
 479
 480         return sensor_base::add_interval(client_id, interval, is_processor);
 481 }
 482
 483 bool gravity_sensor::delete_interval(int client_id, bool is_processor)
 484 {
 485         if (m_fusion)
 486                 m_fusion->unset_interval(FUSION_EVENT_AGM, client_id);
 487
 488         if (m_accel_sensor)
 489                 m_accel_sensor->delete_interval(client_id, true);
 490
 491         if (m_gyro_sensor)
 492                 m_gyro_sensor->delete_interval(client_id, true);
 493
 494         return sensor_base::delete_interval(client_id, is_processor);
 495 }
 496
 497 bool gravity_sensor::set_interval(unsigned long interval)
 498 {
 499         m_interval = interval;
 500         return true;
 501 }
 502
 503 bool gravity_sensor::rotation_to_gravity(const float *rotation, float *gravity)
 504 {
 505         float R[9];
 506
 507         if (quat_to_matrix(rotation, R) < 0)
 508                 return false;
 509
 510         gravity[0] = R[6] * GRAVITY;
 511         gravity[1] = R[7] * GRAVITY;
 512         gravity[2] = R[8] * GRAVITY;
 513
 514         return true;
 515 }
 516
 517 bool gravity_sensor::check_sampling_time(unsigned long long timestamp)
 518 {
 519         const float MIN_DELIVERY_DIFF_FACTOR = 0.75f;
 520         const int MS_TO_US = 1000;
 521         long long diff_time;
 522
 523         diff_time = timestamp - m_time;
 524
 525         if (m_time && (diff_time < m_interval * MS_TO_US * MIN_DELIVERY_DIFF_FACTOR))
 526                 return false;
 527
 528         return true;
 529 }