src/sensor/rotation_vector/rv_sensor.cpp

   1 /*
   2  * sensord
   3  *
   4  * Copyright (c) 2016 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 "rv_sensor.h"
  21
  22 #include <sensor_log.h>
  23 #include <sensor_types.h>
  24 #include <fusion_util.h>
  25
  26 #define NAME_SENSOR  "http://tizen.org/sensor/general/rotation_vector/tizen_default"
  27 #define NAME_VENDOR  "tizen.org"
  28
  29 #define SRC_ID_ACC   0x1
  30 #define SRC_STR_ACC  "http://tizen.org/sensor/general/accelerometer"
  31
  32 #define SRC_ID_GYRO  0x2
  33 #define SRC_STR_GYRO "http://tizen.org/sensor/general/gyroscope"
  34
  35 #define SRC_ID_MAG   0x3
  36 #define SRC_STR_MAG  "http://tizen.org/sensor/general/magnetic"
  37
  38 static sensor_info2_t sensor_info = {
  39         id: 0x1,
  40         type: ROTATION_VECTOR_SENSOR,
  41         uri: NAME_SENSOR,
  42         vendor: NAME_VENDOR,
  43         min_range: 0,
  44         max_range: 1,
  45         resolution: 1,
  46         min_interval: 10,
  47         max_batch_count: 0,
  48         wakeup_supported: false,
  49         privilege:"",
  50 };
  51
  52 static required_sensor_s required_sensors[] = {
  53         {SRC_ID_ACC,     SRC_STR_ACC},
  54         {SRC_ID_GYRO,    SRC_STR_GYRO},
  55         {SRC_ID_MAG,     SRC_STR_MAG},
  56 };
  57
  58 rv_sensor::rv_sensor()
  59 : m_x(-1)
  60 , m_y(-1)
  61 , m_z(-1)
  62 , m_w(-1)
  63 , m_time(0)
  64 , m_interval(100)
  65 , m_accuracy(SENSOR_ACCURACY_UNDEFINED)
  66 {
  67 }
  68
  69 rv_sensor::~rv_sensor()
  70 {
  71 }
  72
  73 int rv_sensor::get_sensor_info(const sensor_info2_t **info)
  74 {
  75         *info = &sensor_info;
  76         return OP_SUCCESS;
  77 }
  78
  79 int rv_sensor::get_required_sensors(const required_sensor_s **sensors)
  80 {
  81         *sensors = required_sensors;
  82         return 3;
  83 }
  84
  85 int rv_sensor::update(uint32_t id, sensor_data_t *data, int len)
  86 {
  87         unsigned long long timestamp;
  88
  89         if (id == SRC_ID_ACC)
  90                 m_fusion.push_accel(*data);
  91         else if (id == SRC_ID_MAG)
  92                 m_fusion.push_mag(*data);
  93         else if (id == SRC_ID_GYRO)
  94                 m_fusion.push_gyro(*data);
  95
  96         if (!m_fusion.get_rv(timestamp, m_w, m_x, m_y, m_z))
  97                 return OP_ERROR;
  98
  99         if (timestamp == m_time)
 100                 return OP_ERROR;
 101
 102         m_time = timestamp;
 103         m_accuracy = data->accuracy;
 104
 105         _D("[rotation_vector] : [%10f] [%10f] [%10f] [%10f]", m_x, m_y, m_z, m_w);
 106         return OP_SUCCESS;
 107 }
 108
 109 int rv_sensor::get_data(sensor_data_t **data, int *length)
 110 {
 111         sensor_data_t *sensor_data;
 112         sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 113         retvm_if(!sensor_data, -ENOMEM, "Failed to allocate memory");
 114
 115         sensor_data->accuracy = m_accuracy;
 116         sensor_data->timestamp = m_time;
 117         sensor_data->value_count = 4;
 118         sensor_data->values[0] = m_w;
 119         sensor_data->values[1] = m_x;
 120         sensor_data->values[2] = m_y;
 121         sensor_data->values[3] = m_z;
 122
 123         *data = sensor_data;
 124         *length = sizeof(sensor_data_t);
 125
 126         return 0;
 127 }