src/sensor.cpp

   1 /*
   2  * Copyright (c) 2014 Samsung Electronics Co., Ltd All Rights Reserved
   3  *
   4  * Licensed under the Apache License, Version 2.0 (the "License");
   5  * you may not use this file except in compliance with the License.
   6  * You may obtain a copy of the License at
   7  *
   8  * http://www.apache.org/licenses/LICENSE-2.0
   9  *
  10  * Unless required by applicable law or agreed to in writing, software
  11  * distributed under the License is distributed on an "AS IS" BASIS,
  12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13  * See the License for the specific language governing permissions and
  14  * limitations under the License.
  15  */
  16
  17 #include <math.h>
  18 #include <stdlib.h>
  19 #include <string.h>
  20 #include <sys/time.h>
  21
  22 #include <sensor_internal.h>
  23 #include <sensor.h>
  24 #include <sensor_private.h>
  25 #include <libgen.h>
  26 #include <memory>
  27 #include <sensor_log.h>
  28
  29 #define RETURN_VAL_IF(expr, err) \
  30         do { \
  31                 if (expr) { \
  32                         _E_MSG(err); \
  33                         return (err); \
  34                 } \
  35         } while (0)
  36
  37 #define RETURN_ERROR(err) \
  38         do { \
  39                 _E_MSG(err); \
  40                 return (err); \
  41         } while (0)
  42
  43 #define SENSOR_SHIFT_TYPE 16
  44 #define SENSOR_UNDEFINED_ID -1
  45
  46 #define SENSOR_LISTENER_MAGIC 0xCAFECAFE
  47
  48 static int sensor_connect (sensor_h sensor, sensor_listener_h listener)
  49 {
  50         int id = SENSOR_UNDEFINED_ID;
  51         int event_type;
  52         sensor_type_t type;
  53         bool support = false;
  54
  55         if (!listener)
  56                 return SENSOR_ERROR_INVALID_PARAMETER;
  57
  58         _D("called sensor_connect : listener[0x%x], sensor[0x%x]", listener, sensor);
  59
  60         sensord_get_type(sensor, &type);
  61         event_type = type << SENSOR_SHIFT_TYPE | 0x1;
  62
  63         if (!sensord_is_supported_event_type(sensor, event_type, &support))
  64                 return SENSOR_ERROR_INVALID_PARAMETER;
  65
  66         if (!support)
  67                 return SENSOR_ERROR_NOT_SUPPORTED;
  68
  69         id = sensord_connect(sensor);
  70
  71         if (id < 0)
  72                 return SENSOR_ERROR_OPERATION_FAILED;
  73
  74         _D("success sensor_connect: id[%d]", id);
  75
  76         listener->id = id;
  77         listener->type = type;
  78
  79         return id;
  80 }
  81
  82 int sensor_is_supported(sensor_type_e type, bool *supported)
  83 {
  84         sensor_t sensor;
  85
  86         if (type < SENSOR_ALL)
  87                 return SENSOR_ERROR_INVALID_PARAMETER;
  88
  89         if (!supported)
  90                 return SENSOR_ERROR_INVALID_PARAMETER;
  91
  92         _D("called sensor_is_supported : type[%d]", type);
  93
  94         sensor = sensord_get_sensor((sensor_type_t)type);
  95
  96         *supported = false;
  97
  98         if (sensor)
  99                 *supported = true;
 100
 101         _D("success sensor(%d) is supported[%d] : sensor[0x%x]",
 102                 type, *supported, sensor);
 103
 104         return SENSOR_ERROR_NONE;
 105 }
 106
 107 int sensor_get_default_sensor(sensor_type_e type, sensor_h *sensor)
 108 {
 109         sensor_t _sensor;
 110         sensor_privilege_t privilege;
 111
 112         _D("called sensor_get_default_sensor : type[%d], sensor[0x%x]", type, sensor);
 113
 114         if (type < SENSOR_ALL)
 115                 return SENSOR_ERROR_INVALID_PARAMETER;
 116
 117         if (!sensor)
 118                 return SENSOR_ERROR_INVALID_PARAMETER;
 119
 120         _sensor = sensord_get_sensor((sensor_type_t)type);
 121
 122         if (!_sensor)
 123                 return SENSOR_ERROR_NOT_SUPPORTED;
 124
 125         sensord_get_privilege(_sensor, &privilege);
 126
 127         if (privilege != SENSOR_PRIVILEGE_PUBLIC)
 128                 return SENSOR_ERROR_PERMISSION_DENIED;
 129
 130         *sensor = _sensor;
 131
 132         _D("success sensor_get_default_sensor sensor[0x%x]", _sensor);
 133
 134         return SENSOR_ERROR_NONE;
 135 }
 136
 137 int sensor_get_sensor_list(sensor_type_e type, sensor_h **list, int *sensor_count)
 138 {
 139         sensor_h *_list = NULL;
 140         int count;
 141
 142         _D("called sensor_get_list : type[%d]");
 143
 144         if (type < SENSOR_ALL)
 145                 return SENSOR_ERROR_INVALID_PARAMETER;
 146
 147         if (!sensor_count || !list)
 148                 return SENSOR_ERROR_INVALID_PARAMETER;
 149
 150         sensord_get_sensor_list((sensor_type_t)type, &_list, &count);
 151
 152         if (count == 0)
 153                 return SENSOR_ERROR_NOT_SUPPORTED;
 154
 155         int i, j;
 156         int count_public = 0;
 157
 158         for (i = 0; i < count; ++i) {
 159                 sensor_privilege_t privilege;
 160
 161                 sensord_get_privilege(_list[i], &privilege);
 162                 if (privilege != SENSOR_PRIVILEGE_PUBLIC)
 163                         continue;
 164
 165                 count_public++;
 166         }
 167
 168         if (count_public == 0) {
 169                 free(_list);
 170                 return SENSOR_ERROR_PERMISSION_DENIED;
 171         }
 172
 173         *list = (sensor_h *) malloc((sizeof(int *)) * count_public);
 174
 175         if (!*list) {
 176                 free(_list);
 177                 return SENSOR_ERROR_OUT_OF_MEMORY;
 178         }
 179
 180         for (i = 0, j = 0; i < count; ++i) {
 181                 sensor_privilege_t privilege;
 182
 183                 sensord_get_privilege(_list[i], &privilege);
 184                 if (privilege != SENSOR_PRIVILEGE_PUBLIC)
 185                         continue;
 186
 187                 *(*list + j) = _list[i];
 188                 j++;
 189         }
 190
 191         free(_list);
 192
 193         *sensor_count = count_public;
 194
 195         _D("success sensor_get_list");
 196
 197         return SENSOR_ERROR_NONE;
 198 }
 199
 200 int sensor_is_wake_up(sensor_h sensor, bool *wakeup)
 201 {
 202         return SENSOR_ERROR_NOT_SUPPORTED;
 203 }
 204
 205 int sensor_create_listener(sensor_h sensor, sensor_listener_h *listener)
 206 {
 207         struct sensor_listener_s *_listener;
 208         int error;
 209
 210         _D("called sensor_create_listener : listener[0x%x]", listener);
 211
 212         if (!sensor || !listener)
 213                 return SENSOR_ERROR_INVALID_PARAMETER;
 214
 215         _listener = new(std::nothrow) struct sensor_listener_s;
 216
 217         if (!_listener)
 218                 return SENSOR_ERROR_OUT_OF_MEMORY;
 219
 220         error = sensor_connect(sensor, _listener);
 221
 222         if (error < 0) {
 223                 delete (struct sensor_listener_s *)_listener;
 224                 return SENSOR_ERROR_OPERATION_FAILED;
 225         }
 226
 227         _listener->sensor = sensor;
 228         _listener->option = SENSOR_OPTION_DEFAULT;
 229         _listener->magic = SENSOR_LISTENER_MAGIC;
 230
 231         *listener = (sensor_listener_h) _listener;
 232
 233         _D("success sensor_create_listener");
 234
 235         return SENSOR_ERROR_NONE;
 236 }
 237
 238 int sensor_destroy_listener(sensor_listener_h listener)
 239 {
 240         _D("called sensor_destroy : listener[0x%x]", listener);
 241
 242         if (!listener)
 243                 return SENSOR_ERROR_INVALID_PARAMETER;
 244
 245         if (listener->magic != SENSOR_LISTENER_MAGIC)
 246                 return SENSOR_ERROR_INVALID_PARAMETER;
 247
 248         sensord_disconnect(listener->id);
 249         listener->magic = 0;
 250
 251         delete (sensor_listener_s *)listener;
 252
 253         _D("success sensor_destroy");
 254
 255         return SENSOR_ERROR_NONE;
 256 }
 257
 258 int sensor_listener_start(sensor_listener_h listener)
 259 {
 260         int id;
 261         unsigned int option = 0;
 262
 263         _D("called sensor_listener_start : listener[0x%x]", listener);
 264
 265         if (!listener)
 266                 return SENSOR_ERROR_INVALID_PARAMETER;
 267
 268         if (listener->magic != SENSOR_LISTENER_MAGIC)
 269                 return SENSOR_ERROR_INVALID_PARAMETER;
 270
 271         id = listener->id;
 272         option = listener->option;
 273
 274         if (!sensord_start(id, option))
 275                 return SENSOR_ERROR_OPERATION_FAILED;
 276
 277         _D("success sensor_listener_start : id[%d]", id);
 278
 279         return SENSOR_ERROR_NONE;
 280 }
 281
 282 int sensor_listener_stop(sensor_listener_h listener)
 283 {
 284         int id;
 285
 286         _D("called sensor_listener_stop : listener[0x%x]", listener);
 287
 288         if (!listener)
 289                 return SENSOR_ERROR_INVALID_PARAMETER;
 290
 291         if (listener->magic != SENSOR_LISTENER_MAGIC)
 292                 return SENSOR_ERROR_INVALID_PARAMETER;
 293
 294         id = listener->id;
 295
 296         if (!sensord_stop(id))
 297                 return SENSOR_ERROR_OPERATION_FAILED;
 298
 299         _D("success sensor_listener_stop");
 300
 301         return SENSOR_ERROR_NONE;
 302 }
 303
 304 static void sensor_callback(sensor_t sensor, unsigned int event_type, sensor_data_t *data, void *user_data)
 305 {
 306         sensor_event_s *event;
 307         sensor_listener_h listener;
 308         listener = (sensor_listener_h)user_data;
 309
 310         _D("success sensor_callback, sensor[%p] listener[%p] listener->callback[%p]", sensor, listener, listener->callback);
 311         if (!sensor || !listener->callback)
 312                 return;
 313
 314         event = (sensor_event_s *)data;
 315
 316         ((sensor_event_cb) listener->callback)(sensor, event, listener->user_data);
 317         return;
 318 }
 319
 320 int sensor_listener_set_event_cb(sensor_listener_h listener,
 321                 unsigned int interval, sensor_event_cb callback, void *user_data)
 322 {
 323         int id;
 324         unsigned int event_id;
 325
 326         if (!listener || !callback)
 327                 return SENSOR_ERROR_INVALID_PARAMETER;
 328
 329         if (listener->magic != SENSOR_LISTENER_MAGIC)
 330                 return SENSOR_ERROR_INVALID_PARAMETER;
 331
 332         _D("called sensor_listener_set_event : listener[0x%x], interval[%d], callback[0x%x], user_data[0x%x], id[%d]",
 333                         listener, interval, callback, user_data, listener->id);
 334
 335         id = listener->id;
 336         event_id = (listener->type) << SENSOR_SHIFT_TYPE | 0x1;
 337
 338         listener->callback = (void *)callback;
 339         listener->user_data = user_data;
 340
 341         if (!sensord_register_event(id, event_id, interval, 0,
 342                                 sensor_callback, listener)) {
 343                 listener->callback = NULL;
 344                 listener->user_data = NULL;
 345
 346                 return SENSOR_ERROR_OPERATION_FAILED;
 347         }
 348
 349         _D("success sensor_listener_set_event");
 350
 351         return SENSOR_ERROR_NONE;
 352 }
 353
 354 int sensor_listener_unset_event_cb(sensor_listener_h listener)
 355 {
 356         int id;
 357         int type;
 358         unsigned int event_id;
 359
 360         _D("called sensor_unregister_event : listener[0x%x]", listener);
 361
 362         if (!listener)
 363                 return SENSOR_ERROR_INVALID_PARAMETER;
 364
 365         if (listener->magic != SENSOR_LISTENER_MAGIC)
 366                 return SENSOR_ERROR_INVALID_PARAMETER;
 367
 368         id = listener->id;
 369         type = (int)listener->type;
 370         event_id = type << SENSOR_SHIFT_TYPE | 0x1;
 371
 372         if (!sensord_unregister_event(id, event_id))
 373                 return SENSOR_ERROR_OPERATION_FAILED;
 374
 375         listener->callback = NULL;
 376         listener->user_data = NULL;
 377
 378         _D("success sensor_unregister_event");
 379
 380         return SENSOR_ERROR_NONE;
 381 }
 382
 383 static void accuracy_changed_callback(sensor_t sensor,
 384                 unsigned long long timestamp, int accuracy, void *data)
 385 {
 386         sensor_listener_h listener = (sensor_listener_h)data;
 387
 388         if (!sensor || !listener->accu_callback)
 389                 return;
 390
 391         ((sensor_accuracy_changed_cb)listener->accu_callback)
 392                         (sensor, timestamp, (sensor_data_accuracy_e)accuracy, listener->accu_user_data);
 393
 394         return;
 395 }
 396
 397 int sensor_listener_set_accuracy_cb(sensor_listener_h listener,
 398                 sensor_accuracy_changed_cb callback, void *data)
 399 {
 400         int id;
 401
 402         _D("called sensor_register_accuracy_cb : listener[0x%x], callback[0x%x], user_data[0x%x] cb[%p]",
 403                         listener, callback, data, accuracy_changed_callback);
 404
 405         if (!listener || !callback)
 406                 return SENSOR_ERROR_INVALID_PARAMETER;
 407
 408         if (listener->magic != SENSOR_LISTENER_MAGIC)
 409                 return SENSOR_ERROR_INVALID_PARAMETER;
 410
 411         id = listener->id;
 412         listener->accu_callback = (void *)callback;
 413         listener->accu_user_data = data;
 414
 415         if (!sensord_register_accuracy_cb(id, accuracy_changed_callback, listener)) {
 416                 listener->accu_callback = NULL;
 417                 listener->accu_user_data = NULL;
 418
 419                 return SENSOR_ERROR_OPERATION_FAILED;
 420         }
 421
 422         _D("success sensor_register_accuracy_cb");
 423
 424         return SENSOR_ERROR_NONE;
 425 }
 426
 427 int sensor_listener_unset_accuracy_cb(sensor_listener_h listener)
 428 {
 429         int id;
 430
 431         _D("called sensor_unregister_accuracy_cb : listener[0x%x]", listener);
 432
 433         if (!listener)
 434                 return SENSOR_ERROR_INVALID_PARAMETER;
 435
 436         if (listener->magic != SENSOR_LISTENER_MAGIC)
 437                 return SENSOR_ERROR_INVALID_PARAMETER;
 438
 439         id = listener->id;
 440
 441         if (!sensord_unregister_accuracy_cb(id))
 442                 return SENSOR_ERROR_OPERATION_FAILED;
 443
 444         listener->accu_callback = NULL;
 445         listener->accu_user_data = NULL;
 446
 447         _D("success sensor_unregister_accuracy_cb");
 448
 449         return SENSOR_ERROR_NONE;
 450 }
 451
 452 int sensor_listener_set_interval(sensor_listener_h listener, unsigned int interval)
 453 {
 454         int id;
 455         int type;
 456         unsigned int event_id;
 457
 458         _D("called sensor_set_interval : listener[0x%x], interval[%d]", listener, interval);
 459
 460         if (!listener)
 461                 return SENSOR_ERROR_INVALID_PARAMETER;
 462
 463         if (listener->magic != SENSOR_LISTENER_MAGIC)
 464                 return SENSOR_ERROR_INVALID_PARAMETER;
 465
 466         id = listener->id;
 467         type = (int)listener->type;
 468         event_id = type << SENSOR_SHIFT_TYPE | 0x1;
 469
 470         if (!sensord_change_event_interval(id, event_id, interval))
 471                 return SENSOR_ERROR_OPERATION_FAILED;
 472
 473         _D("success sensor_set_interval");
 474
 475         return SENSOR_ERROR_NONE;
 476 }
 477
 478 int sensor_listener_set_max_batch_latency(sensor_listener_h listener, unsigned int max_batch_latency)
 479 {
 480         int id;
 481         int type;
 482         unsigned int event_id;
 483
 484         _D("called sensor_set_max_batch_latency : listener[0x%x], max_batch_latency[%d]", listener, max_batch_latency);
 485
 486         if (!listener)
 487                 return SENSOR_ERROR_INVALID_PARAMETER;
 488
 489         if (listener->magic != SENSOR_LISTENER_MAGIC)
 490                 return SENSOR_ERROR_INVALID_PARAMETER;
 491
 492         id = listener->id;
 493         type = (int)listener->type;
 494         event_id = type << SENSOR_SHIFT_TYPE | 0x1;
 495
 496         if (!sensord_change_event_max_batch_latency(id, event_id, max_batch_latency))
 497                 return SENSOR_ERROR_NOT_SUPPORTED;
 498
 499         _D("success sensor_set_max_batch_latency");
 500
 501         return SENSOR_ERROR_NONE;
 502 }
 503
 504 int sensor_listener_set_attribute_int(sensor_listener_h listener, sensor_attribute_e attribute, int value)
 505 {
 506         return SENSOR_ERROR_NOT_SUPPORTED;
 507 }
 508
 509 int sensor_listener_set_option(sensor_listener_h listener, sensor_option_e option)
 510 {
 511         int id;
 512
 513         _D("called sensor_set_option : listener[0x%x], option[%d]", listener, option);
 514
 515         if (!listener)
 516                 return SENSOR_ERROR_INVALID_PARAMETER;
 517
 518         if (listener->magic != SENSOR_LISTENER_MAGIC)
 519                 return SENSOR_ERROR_INVALID_PARAMETER;
 520
 521         id = listener->id;
 522
 523         if (!sensord_set_option(id, (int)option))
 524                 return SENSOR_ERROR_OPERATION_FAILED;
 525
 526         listener->option = option;
 527
 528         _D("success sensor_set_option");
 529
 530         return SENSOR_ERROR_NONE;
 531 }
 532
 533 int sensor_listener_read_data(sensor_listener_h listener, sensor_event_s *event)
 534 {
 535         int id;
 536         int type;
 537         sensor_data_t data;
 538         unsigned int data_id;
 539
 540         _D("called sensor_read_data : listener[0x%x]", listener);
 541
 542         if (!listener || !event)
 543                 return SENSOR_ERROR_INVALID_PARAMETER;
 544
 545         if (listener->magic != SENSOR_LISTENER_MAGIC)
 546                 return SENSOR_ERROR_INVALID_PARAMETER;
 547
 548         id = listener->id;
 549         type = (int)listener->type;
 550         data_id = type << SENSOR_SHIFT_TYPE | 0x1;
 551
 552         if (!sensord_get_data(id, data_id, &data))
 553                 return SENSOR_ERROR_OPERATION_FAILED;
 554
 555         event->accuracy = data.accuracy;
 556         event->timestamp = data.timestamp;
 557         event->value_count = data.value_count;
 558
 559         for (int i = 0; i < data.value_count; ++i)
 560                 event->values[i] = data.values[i];
 561
 562         _D("success sensor_read_data");
 563
 564         return SENSOR_ERROR_NONE;
 565 }
 566
 567 int sensor_get_name(sensor_h sensor, char** name)
 568 {
 569         _D("called sensor_get_name");
 570
 571         if (!sensor || !name)
 572                 return SENSOR_ERROR_INVALID_PARAMETER;
 573
 574         *name = strdup(sensord_get_name(sensor));
 575
 576         _D("success sensor_get_vendor : [%s]", *name);
 577
 578         return SENSOR_ERROR_NONE;
 579 }
 580
 581 int sensor_get_vendor(sensor_h sensor, char** vendor)
 582 {
 583         _D("called sensor_get_vendor");
 584
 585         if (!sensor || !vendor)
 586                 return SENSOR_ERROR_INVALID_PARAMETER;
 587
 588         *vendor = strdup(sensord_get_vendor(sensor));
 589
 590         _D("success sensor_vendor : [%s]", *vendor);
 591
 592         return SENSOR_ERROR_NONE;
 593 }
 594
 595 int sensor_get_type(sensor_h sensor, sensor_type_e *type)
 596 {
 597         sensor_type_t _type;
 598         _D("called sensor_get_type");
 599
 600         if (!sensor || !type)
 601                 return SENSOR_ERROR_INVALID_PARAMETER;
 602
 603         if (!sensord_get_type(sensor, &_type))
 604                 return SENSOR_ERROR_OPERATION_FAILED;
 605
 606         *type = (sensor_type_e) _type;
 607
 608         _D("success sensor_get_type : [%d]", *type);
 609
 610         return SENSOR_ERROR_NONE;
 611 }
 612
 613 int sensor_get_min_range(sensor_h sensor, float *min_range)
 614 {
 615         _D("called sensor_get_min_range");
 616
 617         if (!sensor || !min_range)
 618                 return SENSOR_ERROR_INVALID_PARAMETER;
 619
 620         if (!sensord_get_min_range(sensor, min_range))
 621                 return SENSOR_ERROR_OPERATION_FAILED;
 622
 623         _D("success sensor_get_min_range : [%d]", *min_range);
 624
 625         return SENSOR_ERROR_NONE;
 626 }
 627
 628 int sensor_get_max_range(sensor_h sensor, float *max_range)
 629 {
 630         _D("called sensor_get_max_range");
 631
 632         if (!sensor || !max_range)
 633                 return SENSOR_ERROR_INVALID_PARAMETER;
 634
 635         if (!sensord_get_max_range(sensor, max_range))
 636                 return SENSOR_ERROR_OPERATION_FAILED;
 637
 638         _D("success sensor_get_max_range : [%d]", *max_range);
 639
 640         return SENSOR_ERROR_NONE;
 641 }
 642
 643 int sensor_get_resolution(sensor_h sensor, float *resolution)
 644 {
 645         _D("called sensor_get_resolution");
 646
 647         if (!sensor || !resolution)
 648                 return SENSOR_ERROR_INVALID_PARAMETER;
 649
 650         if (!sensord_get_resolution(sensor, resolution))
 651                 return SENSOR_ERROR_OPERATION_FAILED;
 652
 653         _D("success sensor_get_resolution : [%d]", *resolution);
 654
 655         return SENSOR_ERROR_NONE;
 656 }
 657
 658 int sensor_get_min_interval(sensor_h sensor, int *min_interval)
 659 {
 660         _D("called sensor_get_min_interval");
 661
 662         if (!sensor || !min_interval)
 663                 return SENSOR_ERROR_INVALID_PARAMETER;
 664
 665         if (!sensord_get_min_interval(sensor, min_interval))
 666                 return SENSOR_ERROR_OPERATION_FAILED;
 667
 668         _D("success sensor_get_min_interval : [%d]", *min_interval);
 669
 670         return SENSOR_ERROR_NONE;
 671 }
 672
 673 int sensor_get_fifo_count(sensor_h sensor, int *fifo_count)
 674 {
 675         _D("called sensor_get_fifo_count");
 676
 677         if (!sensor || !fifo_count)
 678                 return SENSOR_ERROR_INVALID_PARAMETER;
 679
 680         if (!sensord_get_fifo_count(sensor, fifo_count))
 681                 return SENSOR_ERROR_OPERATION_FAILED;
 682
 683         _D("success sensor_get_fifo_count : [%d]", *fifo_count);
 684
 685         return SENSOR_ERROR_NONE;
 686 }
 687
 688 int sensor_get_max_batch_count(sensor_h sensor, int *max_batch_count)
 689 {
 690         _D("called sensor_get_max_batch_count");
 691
 692         if (!sensor || !max_batch_count)
 693                 return SENSOR_ERROR_INVALID_PARAMETER;
 694
 695         if (!sensord_get_max_batch_count(sensor, max_batch_count))
 696                 return SENSOR_ERROR_OPERATION_FAILED;
 697
 698         _D("success sensor_get_max_batch_count : [%d]", *max_batch_count);
 699
 700         return SENSOR_ERROR_NONE;
 701 }
 702
 703 /*
 704  *      FUNCTIONS : SENSOR_UTIL_*
 705  */
 706
 707 int sensor_util_get_declination (float latitude, float longitude, float altitude, float *declination)
 708 {
 709         if (!declination)
 710                 return SENSOR_ERROR_INVALID_PARAMETER;
 711
 712         setCoordinate (latitude, longitude, altitude, declination, NULL, 1);
 713
 714         return SENSOR_ERROR_NONE;
 715 }
 716
 717 int sensor_util_get_angle_change (float R[], float prevR[], float angleChange[])
 718 {
 719         if (getAngleChange (R, prevR, angleChange) < 0)
 720                 return SENSOR_ERROR_INVALID_PARAMETER;
 721
 722         return SENSOR_ERROR_NONE;
 723 }
 724
 725 int sensor_util_get_orientation (float R[], float values[])
 726 {
 727         if (!R || !values)
 728                 return SENSOR_ERROR_INVALID_PARAMETER;
 729
 730         values[0] = (float) atan2 (R[1], R[4]);
 731         values[1] = (float) asin (-R[7]);
 732         values[2] = (float) atan2 (-R[6], R[8]);
 733
 734         return SENSOR_ERROR_NONE;
 735 }
 736
 737 int sensor_util_get_inclination (float I[], float* inclination)
 738 {
 739         if (!I || !inclination)
 740                 return SENSOR_ERROR_INVALID_PARAMETER;
 741
 742         *inclination = atan2(I[5], I[4]);
 743
 744         return SENSOR_ERROR_NONE;
 745 }
 746
 747 int sensor_util_remap_coordinate_system (float inR[], sensor_util_axis_e x, sensor_util_axis_e y, float outR[])
 748 {
 749         if (remapCoordinateSystem (inR, x, y, outR) < 0)
 750                 return SENSOR_ERROR_INVALID_PARAMETER;
 751
 752         return SENSOR_ERROR_NONE;
 753 }
 754
 755 int sensor_util_get_rotation_matrix_from_vector (float Vx, float Vy, float Vz, float R[])
 756 {
 757         float RV[4] = {0, Vx, Vy, Vz};
 758
 759         RV[0] = 1 - Vx * Vx - Vy*Vy - Vz*Vz;
 760         RV[0] = (Vx > 0) ? (float) (sqrt (Vx)) : 0;
 761
 762         if (quatToMatrix(RV, R) < 0)
 763                 return SENSOR_ERROR_INVALID_PARAMETER;
 764
 765         return SENSOR_ERROR_NONE;
 766 }
 767
 768 int sensor_util_get_rotation_matrix (float Gx, float Gy, float Gz,float Mx, float My, float Mz,float R[], float I[])
 769 {
 770         float G[3] = {Gx, Gy, Gz};
 771         float M[3] = {Mx, My, Mz};
 772
 773         if (getRotationMatrix (G, M, R, I) < 0)
 774                 return SENSOR_ERROR_INVALID_PARAMETER;
 775
 776         return SENSOR_ERROR_NONE;
 777 }
 778