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         if (!rotation_to_gravity(rotation, gravity)) {
 165                 _D("Invalid rotation_vector: [%10f] [%10f] [%10f] [%10f]", x, y, z, w);
 166                 return;
 167         }
 168
 169         gravity_event = (sensor_event_t *)malloc(sizeof(sensor_event_t));
 170         if (!gravity_event) {
 171                 _E("Failed to allocate memory");
 172                 return;
 173         }
 174         gravity_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 175         if (!gravity_event->data) {
 176                 _E("Failed to allocate memory");
 177                 free(gravity_event);
 178                 return;
 179         }
 180
 181         gravity_event->sensor_id = get_id();
 182         gravity_event->event_type = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME;
 183         gravity_event->data_length = sizeof(sensor_data_t);
 184         gravity_event->data->accuracy = accuracy;
 185         gravity_event->data->timestamp = m_fusion->get_data_timestamp();
 186         gravity_event->data->value_count = 3;
 187         gravity_event->data->values[0] = gravity[0];
 188         gravity_event->data->values[1] = gravity[1];
 189         gravity_event->data->values[2] = gravity[2];
 190         push(gravity_event);
 191
 192         m_time = event.data->timestamp;
 193         m_x = gravity[0];
 194         m_y = gravity[1];
 195         m_z = gravity[2];
 196         m_accuracy = accuracy;
 197 }
 198
 199 void gravity_sensor::synthesize_lowpass(const sensor_event_t& event)
 200 {
 201         if (event.event_type != ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME)
 202                 return;
 203
 204         sensor_event_t *gravity_event;
 205         float x, y, z, norm, alpha, tau, err;
 206
 207         norm = NORM(event.data->values[0], event.data->values[1], event.data->values[2]);
 208         x = event.data->values[0] / norm * GRAVITY;
 209         y = event.data->values[1] / norm * GRAVITY;
 210         z = event.data->values[2] / norm * GRAVITY;
 211
 212         if (m_time > 0) {
 213                 err = fabs(norm - GRAVITY) / GRAVITY;
 214                 tau = (err < 0.1 ? TAU_LOW : err > 0.9 ? TAU_HIGH : TAU_MID);
 215                 alpha = tau / (tau + (float)(event.data->timestamp - m_time) / US_PER_SEC);
 216                 x = alpha * m_x + (1 - alpha) * x;
 217                 y = alpha * m_y + (1 - alpha) * y;
 218                 z = alpha * m_z + (1 - alpha) * z;
 219                 norm = NORM(x, y, z);
 220                 x = x / norm * GRAVITY;
 221                 y = y / norm * GRAVITY;
 222                 z = z / norm * GRAVITY;
 223         }
 224
 225         gravity_event = (sensor_event_t *)malloc(sizeof(sensor_event_t));
 226         if (!gravity_event) {
 227                 _E("Failed to allocate memory");
 228                 return;
 229         }
 230         gravity_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 231         if (!gravity_event->data) {
 232                 _E("Failed to allocate memory");
 233                 free(gravity_event);
 234                 return;
 235         }
 236
 237         gravity_event->sensor_id = get_id();
 238         gravity_event->event_type = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME;
 239         gravity_event->data_length = sizeof(sensor_data_t);
 240         gravity_event->data->accuracy = event.data->accuracy;
 241         gravity_event->data->timestamp = event.data->timestamp;
 242         gravity_event->data->value_count = 3;
 243         gravity_event->data->values[0] = x;
 244         gravity_event->data->values[1] = y;
 245         gravity_event->data->values[2] = z;
 246         push(gravity_event);
 247
 248         m_time = event.data->timestamp;
 249         m_x = x;
 250         m_y = y;
 251         m_z = z;
 252         m_accuracy = event.data->accuracy;
 253 }
 254
 255 void gravity_sensor::synthesize_fusion(const sensor_event_t& event)
 256 {
 257         sensor_event_t *gravity_event;
 258
 259         if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) {
 260                 fusion_set_accel(event);
 261                 m_fusion_phase |= PHASE_ACCEL_READY;
 262         } else if (event.event_type == GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME) {
 263                 fusion_set_gyro(event);
 264                 m_fusion_phase |= PHASE_GYRO_READY;
 265         }
 266
 267         if (m_fusion_phase != PHASE_FUSION_READY)
 268                 return;
 269
 270         m_fusion_phase = 0;
 271
 272         fusion_update_angle();
 273         fusion_get_gravity();
 274
 275         gravity_event = (sensor_event_t *)malloc(sizeof(sensor_event_t));
 276         if (!gravity_event) {
 277                 _E("Failed to allocate memory");
 278                 return;
 279         }
 280         gravity_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 281         if (!gravity_event->data) {
 282                 _E("Failed to allocate memory");
 283                 free(gravity_event);
 284                 return;
 285         }
 286
 287         gravity_event->sensor_id = get_id();
 288         gravity_event->event_type = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME;
 289         gravity_event->data_length = sizeof(sensor_data_t);
 290         gravity_event->data->accuracy = m_accuracy;
 291         gravity_event->data->timestamp = m_time;
 292         gravity_event->data->value_count = 3;
 293         gravity_event->data->values[0] = m_x;
 294         gravity_event->data->values[1] = m_y;
 295         gravity_event->data->values[2] = m_z;
 296         push(gravity_event);
 297 }
 298
 299 void gravity_sensor::fusion_set_accel(const sensor_event_t& event)
 300 {
 301         double x = event.data->values[0];
 302         double y = event.data->values[1];
 303         double z = event.data->values[2];
 304
 305         m_accel_mag = NORM(x, y, z);
 306
 307         m_angle_n[0] = ARCTAN(z, y);
 308         m_angle_n[1] = ARCTAN(x, z);
 309         m_angle_n[2] = ARCTAN(y, x);
 310
 311         m_accuracy = event.data->accuracy;
 312         m_time_new = event.data->timestamp;
 313
 314         _D("AccIn: (%f, %f, %f)", x/m_accel_mag, y/m_accel_mag, z/m_accel_mag);
 315 }
 316
 317 void gravity_sensor::fusion_set_gyro(const sensor_event_t& event)
 318 {
 319         m_velocity[0] = -DEG2RAD(event.data->values[0]);
 320         m_velocity[1] = -DEG2RAD(event.data->values[1]);
 321         m_velocity[2] = -DEG2RAD(event.data->values[2]);
 322
 323         m_time_new = event.data->timestamp;
 324 }
 325
 326 void gravity_sensor::fusion_update_angle()
 327 {
 328         _D("AngleIn: (%f, %f, %f)", m_angle_n[0], m_angle_n[1], m_angle_n[2]);
 329         _D("AngAccl: (%f, %f, %f)", m_velocity[0], m_velocity[1], m_velocity[2]);
 330         _D("Angle  : (%f, %f, %f)", m_angle[0], m_angle[1], m_angle[2]);
 331
 332         if (m_angle[0] == INV_ANGLE) {
 333                 /* 1st iteration */
 334                 m_angle[0] = m_angle_n[0];
 335                 m_angle[1] = m_angle_n[1];
 336                 m_angle[2] = m_angle_n[2];
 337         } else {
 338                 complementary(m_time_new - m_time);
 339         }
 340
 341         _D("Angle' : (%f, %f, %f)", m_angle[0], m_angle[1], m_angle[2]);
 342 }
 343
 344 void gravity_sensor::fusion_get_gravity()
 345 {
 346         double x = 0, y = 0, z = 0;
 347         double norm;
 348         double vec[3][3];
 349
 350         /* Rotating along y-axis then z-axis */
 351         vec[0][2] = cos(m_angle[1]);
 352         vec[0][0] = sin(m_angle[1]);
 353         vec[0][1] = vec[0][0] * tan(m_angle[2]);
 354
 355         /* Rotating along z-axis then x-axis */
 356         vec[1][0] = cos(m_angle[2]);
 357         vec[1][1] = sin(m_angle[2]);
 358         vec[1][2] = vec[1][1] * tan(m_angle[0]);
 359
 360         /* Rotating along x-axis then y-axis */
 361         vec[2][1] = cos(m_angle[0]);
 362         vec[2][2] = sin(m_angle[0]);
 363         vec[2][0] = vec[2][2] * tan(m_angle[1]);
 364
 365         /* Normalize */
 366         for (int i = 0; i < 3; ++i) {
 367                 norm = NORM(vec[i][0], vec[i][1], vec[i][2]);
 368                 vec[i][0] /= norm;
 369                 vec[i][1] /= norm;
 370                 vec[i][2] /= norm;
 371                 x += vec[i][0];
 372                 y += vec[i][1];
 373                 z += vec[i][2];
 374         }
 375
 376         norm = NORM(x, y, z);
 377
 378         m_x = x / norm * GRAVITY;
 379         m_y = y / norm * GRAVITY;
 380         m_z = z / norm * GRAVITY;
 381         m_time = m_time_new;
 382 }
 383
 384 void gravity_sensor::complementary(unsigned long long time_diff)
 385 {
 386         double err = fabs(m_accel_mag - GRAVITY) / GRAVITY;
 387         double tau = (err < 0.1 ? TAU_LOW : err > 0.9 ? TAU_HIGH : TAU_MID);
 388         double delta_t = (double)time_diff/ US_PER_SEC;
 389         double alpha = tau / (tau + delta_t);
 390
 391         _D("mag, err, tau, dt, alpha = %f, %f, %f, %f, %f", m_accel_mag, err, tau, delta_t, alpha);
 392
 393         m_angle[0] = complementary(m_angle[0], m_angle_n[0], m_velocity[0], delta_t, alpha);
 394         m_angle[1] = complementary(m_angle[1], m_angle_n[1], m_velocity[1], delta_t, alpha);
 395         m_angle[2] = complementary(m_angle[2], m_angle_n[2], m_velocity[2], delta_t, alpha);
 396 }
 397
 398 double gravity_sensor::complementary(double angle, double angle_in, double vel, double delta_t, double alpha)
 399 {
 400         double s, c;
 401         angle = angle + vel * delta_t;
 402         s = alpha * sin(angle) + (1 - alpha) * sin(angle_in);
 403         c = alpha * cos(angle) + (1 - alpha) * cos(angle_in);
 404         return ARCTAN(s, c);
 405 }
 406
 407 int gravity_sensor::get_data(sensor_data_t **data, int *length)
 408 {
 409         /* if It is batch sensor, remains can be 2+ */
 410         int remains = 1;
 411
 412         sensor_data_t *sensor_data;
 413         sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 414
 415         sensor_data->accuracy = m_accuracy;
 416         sensor_data->timestamp = m_time;
 417         sensor_data->value_count = 3;
 418         sensor_data->values[0] = m_x;
 419         sensor_data->values[1] = m_y;
 420         sensor_data->values[2] = m_z;
 421
 422         *data = sensor_data;
 423         *length = sizeof(sensor_data_t);
 424
 425         return --remains;
 426 }
 427
 428 bool gravity_sensor::set_batch_latency(unsigned long latency)
 429 {
 430         return false;
 431 }
 432
 433 bool gravity_sensor::set_wakeup(int wakeup)
 434 {
 435         return false;
 436 }
 437
 438 bool gravity_sensor::on_start(void)
 439 {
 440         if (m_fusion)
 441                 m_fusion->start();
 442
 443         if (m_accel_sensor)
 444                 m_accel_sensor->start();
 445
 446         if (m_gyro_sensor)
 447                 m_gyro_sensor->start();
 448
 449         m_time = 0;
 450         m_fusion_phase = 0;
 451         m_angle[0] = INV_ANGLE;
 452
 453         return activate();
 454 }
 455
 456 bool gravity_sensor::on_stop(void)
 457 {
 458         if (m_fusion)
 459                 m_fusion->stop();
 460
 461         if (m_accel_sensor)
 462                 m_accel_sensor->stop();
 463
 464         if (m_gyro_sensor)
 465                 m_gyro_sensor->stop();
 466
 467         m_time = 0;
 468
 469         return deactivate();
 470 }
 471
 472 bool gravity_sensor::add_interval(int client_id, unsigned int interval, bool is_processor)
 473 {
 474         if (m_fusion)
 475                 m_fusion->set_interval(FUSION_EVENT_AGM, client_id, interval);
 476
 477         if (m_accel_sensor)
 478                 m_accel_sensor->add_interval(client_id, interval, true);
 479
 480         if (m_gyro_sensor)
 481                 m_gyro_sensor->add_interval(client_id, interval, true);
 482
 483         return sensor_base::add_interval(client_id, interval, is_processor);
 484 }
 485
 486 bool gravity_sensor::delete_interval(int client_id, bool is_processor)
 487 {
 488         if (m_fusion)
 489                 m_fusion->unset_interval(FUSION_EVENT_AGM, client_id);
 490
 491         if (m_accel_sensor)
 492                 m_accel_sensor->delete_interval(client_id, true);
 493
 494         if (m_gyro_sensor)
 495                 m_gyro_sensor->delete_interval(client_id, true);
 496
 497         return sensor_base::delete_interval(client_id, is_processor);
 498 }
 499
 500 bool gravity_sensor::set_interval(unsigned long interval)
 501 {
 502         m_interval = interval;
 503         return true;
 504 }
 505
 506 bool gravity_sensor::rotation_to_gravity(const float *rotation, float *gravity)
 507 {
 508         float R[9];
 509
 510         if (quat_to_matrix(rotation, R) < 0)
 511                 return false;
 512
 513         gravity[0] = R[6] * GRAVITY;
 514         gravity[1] = R[7] * GRAVITY;
 515         gravity[2] = R[8] * GRAVITY;
 516
 517         return true;
 518 }
 519
 520 bool gravity_sensor::check_sampling_time(unsigned long long timestamp)
 521 {
 522         const float MIN_DELIVERY_DIFF_FACTOR = 0.75f;
 523         const int MS_TO_US = 1000;
 524         long long diff_time;
 525
 526         diff_time = timestamp - m_time;
 527
 528         if (m_time && (diff_time < m_interval * MS_TO_US * MIN_DELIVERY_DIFF_FACTOR))
 529                 return false;
 530
 531         return true;
 532 }