src/sensor/orientation/orientation_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 <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 <orientation_sensor.h>
  35 #include <sensor_loader.h>
  36 #include <fusion_util.h>
  37
  38 #define SENSOR_NAME "SENSOR_ORIENTATION"
  39
  40 orientation_sensor::orientation_sensor()
  41 : m_rotation_vector_sensor(NULL)
  42 , m_azimuth(-1)
  43 , m_pitch(-1)
  44 , m_roll(-1)
  45 , m_accuracy(-1)
  46 , m_time(0)
  47 {
  48 }
  49
  50 orientation_sensor::~orientation_sensor()
  51 {
  52         _I("%s is destroyed!", SENSOR_NAME);
  53 }
  54
  55 bool orientation_sensor::init()
  56 {
  57         m_rotation_vector_sensor = sensor_loader::get_instance().get_sensor(ROTATION_VECTOR_SENSOR);
  58
  59         if (!m_rotation_vector_sensor) {
  60                 _E("cannot load sensor[%s]", SENSOR_NAME);
  61                 return false;
  62         }
  63         _I("%s is created!", SENSOR_NAME);
  64         return true;
  65 }
  66
  67 sensor_type_t orientation_sensor::get_type(void)
  68 {
  69         return ORIENTATION_SENSOR;
  70 }
  71
  72 unsigned int orientation_sensor::get_event_type(void)
  73 {
  74         return ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME;
  75 }
  76
  77 const char* orientation_sensor::get_name(void)
  78 {
  79         return SENSOR_NAME;
  80 }
  81
  82 bool orientation_sensor::get_sensor_info(sensor_info &info)
  83 {
  84         info.set_type(get_type());
  85         info.set_id(get_id());
  86         info.set_privilege(SENSOR_PRIVILEGE_PUBLIC);
  87         info.set_name(get_name());
  88         info.set_vendor("Samsung Electronics");
  89         info.set_min_range(-180);
  90         info.set_max_range(360);
  91         info.set_resolution(0.01);
  92         info.set_min_interval(1);
  93         info.set_fifo_count(0);
  94         info.set_max_batch_count(0);
  95         info.set_supported_event(get_event_type());
  96         info.set_wakeup_supported(false);
  97
  98         return true;
  99 }
 100
 101 void orientation_sensor::synthesize(const sensor_event_t& event)
 102 {
 103         int error;
 104         sensor_event_t *orientation_event;
 105         float azimuth, pitch, roll;
 106
 107         if (CONVERT_ID_TYPE(event.sensor_id) != ROTATION_VECTOR_SENSOR)
 108                 return;
 109
 110         error = quat_to_orientation(event.data->values, azimuth, pitch, roll);
 111         ret_if(error);
 112
 113         orientation_event = (sensor_event_t *)malloc(sizeof(sensor_event_t));
 114         if (!orientation_event) {
 115                 _E("Failed to allocate memory");
 116                 return;
 117         }
 118         orientation_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 119         if (!orientation_event->data) {
 120                 _E("Failed to allocate memory");
 121                 free(orientation_event);
 122                 return;
 123         }
 124
 125         orientation_event->sensor_id = get_id();
 126         orientation_event->event_type = CONVERT_TYPE_EVENT(ORIENTATION_SENSOR);
 127         orientation_event->data_length = sizeof(sensor_data_t);
 128         orientation_event->data->accuracy = event.data->accuracy;
 129         orientation_event->data->timestamp = event.data->timestamp;
 130         orientation_event->data->value_count = 3;
 131         orientation_event->data->values[0] = azimuth;
 132         orientation_event->data->values[1] = pitch;
 133         orientation_event->data->values[2] = roll;
 134         push(orientation_event);
 135
 136         m_azimuth = azimuth;
 137         m_pitch = pitch;
 138         m_roll = roll;
 139         m_time = event.data->timestamp;
 140         m_accuracy = event.data->accuracy;
 141
 142         _D("[orientation] : [%10f] [%10f] [%10f]", m_azimuth, m_pitch, m_roll);
 143 }
 144
 145 int orientation_sensor::get_data(sensor_data_t **data, int *length)
 146 {
 147         /* if It is batch sensor, remains can be 2+ */
 148         int remains = 1;
 149
 150         sensor_data_t *sensor_data;
 151         sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 152
 153         sensor_data->accuracy = m_accuracy;
 154         sensor_data->timestamp = m_time;
 155         sensor_data->value_count = 3;
 156         sensor_data->values[0] = m_azimuth;
 157         sensor_data->values[1] = m_pitch;
 158         sensor_data->values[2] = m_roll;
 159
 160         *data = sensor_data;
 161         *length = sizeof(sensor_data_t);
 162
 163         return --remains;
 164 }
 165
 166 bool orientation_sensor::set_interval(unsigned long interval)
 167 {
 168         m_interval = interval;
 169         return true;
 170 }
 171
 172 bool orientation_sensor::set_batch_latency(unsigned long latency)
 173 {
 174         return false;
 175 }
 176
 177 bool orientation_sensor::on_start(void)
 178 {
 179         if (m_rotation_vector_sensor)
 180                 m_rotation_vector_sensor->start();
 181
 182         m_time = 0;
 183
 184         return activate();
 185 }
 186
 187 bool orientation_sensor::on_stop(void)
 188 {
 189         if (m_rotation_vector_sensor)
 190                 m_rotation_vector_sensor->stop();
 191
 192         m_time = 0;
 193
 194         return deactivate();
 195 }
 196
 197 bool orientation_sensor::add_interval(int client_id, unsigned int interval, bool is_processor)
 198 {
 199         if (m_rotation_vector_sensor)
 200                 m_rotation_vector_sensor->add_interval(client_id, interval, true);
 201
 202         return sensor_base::add_interval(client_id, interval, is_processor);
 203 }
 204
 205 bool orientation_sensor::delete_interval(int client_id, bool is_processor)
 206 {
 207         if (m_rotation_vector_sensor)
 208                 m_rotation_vector_sensor->delete_interval(client_id, true);
 209
 210         return sensor_base::delete_interval(client_id, is_processor);
 211 }