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