src/sensor/rotation_vector/rotation_vector_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 <rotation_vector_sensor.h>
  35 #include <sensor_loader.h>
  36 #include <fusion_util.h>
  37
  38 #define SENSOR_NAME "SENSOR_ROTATION_VECTOR"
  39
  40 #define NORM(x, y, z) sqrt((x)*(x) + (y)*(y) + (z)*(z))
  41
  42 #define STATE_ACCEL 0x1
  43 #define STATE_MAGNETIC 0x2
  44
  45 rotation_vector_sensor::rotation_vector_sensor()
  46 : m_accel_sensor(NULL)
  47 , m_mag_sensor(NULL)
  48 , m_x(-1)
  49 , m_y(-1)
  50 , m_z(-1)
  51 , m_w(-1)
  52 , m_time(0)
  53 , m_state(0)
  54 {
  55 }
  56
  57 rotation_vector_sensor::~rotation_vector_sensor()
  58 {
  59         _I("%s is destroyed!", SENSOR_NAME);
  60 }
  61
  62 bool rotation_vector_sensor::init()
  63 {
  64         m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
  65         m_mag_sensor = sensor_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR);
  66
  67         if (!m_accel_sensor || !m_mag_sensor) {
  68                 _E("cannot load sensors[%s]", SENSOR_NAME);
  69                 return false;
  70         }
  71
  72         _I("%s is created!", SENSOR_NAME);
  73         return true;
  74 }
  75
  76 sensor_type_t rotation_vector_sensor::get_type(void)
  77 {
  78         return ROTATION_VECTOR_SENSOR;
  79 }
  80
  81 unsigned int rotation_vector_sensor::get_event_type(void)
  82 {
  83         return CONVERT_TYPE_EVENT(ROTATION_VECTOR_SENSOR);
  84 }
  85
  86 const char* rotation_vector_sensor::get_name(void)
  87 {
  88         return SENSOR_NAME;
  89 }
  90
  91 bool rotation_vector_sensor::get_sensor_info(sensor_info &info)
  92 {
  93         info.set_type(get_type());
  94         info.set_id(get_id());
  95         info.set_privilege(SENSOR_PRIVILEGE_PUBLIC);
  96         info.set_name(get_name());
  97         info.set_vendor("Samsung Electronics");
  98         info.set_min_range(0);
  99         info.set_max_range(1);
 100         info.set_resolution(1);
 101         info.set_min_interval(1);
 102         info.set_fifo_count(0);
 103         info.set_max_batch_count(0);
 104         info.set_supported_event(get_event_type());
 105         info.set_wakeup_supported(false);
 106
 107         return true;
 108 }
 109
 110 void rotation_vector_sensor::synthesize(const sensor_event_t& event)
 111 {
 112         sensor_event_t *rotation_vector_event;
 113         float R[9];
 114         float I[9];
 115         float quat[4];
 116         int error;
 117
 118         if (event.event_type != GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME &&
 119                 event.event_type != ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME)
 120                 return;
 121
 122         if (event.event_type == GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME) {
 123                 m_mag[0] = event.data->values[0];
 124                 m_mag[1] = event.data->values[1];
 125                 m_mag[2] = event.data->values[2];
 126                 m_accuracy = event.data->accuracy;
 127
 128                 m_state |= STATE_MAGNETIC;
 129         }
 130
 131         if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) {
 132                 m_acc[0] = event.data->values[0];
 133                 m_acc[1] = event.data->values[1];
 134                 m_acc[2] = event.data->values[2];
 135
 136                 m_state |= STATE_ACCEL;
 137         }
 138
 139         if (m_state != (STATE_ACCEL | STATE_MAGNETIC))
 140                 return;
 141
 142         m_state = 0;
 143
 144         unsigned long long timestamp = event.data->timestamp;
 145
 146         error = calculate_rotation_matrix(m_acc, m_mag, R, I);
 147         ret_if(error < 0);
 148
 149         error = matrix_to_quat(R, quat);
 150         ret_if(error < 0);
 151
 152         rotation_vector_event = (sensor_event_t *)malloc(sizeof(sensor_event_t));
 153         if (!rotation_vector_event) {
 154                 _E("Failed to allocate memory");
 155                 return;
 156         }
 157         rotation_vector_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 158         if (!rotation_vector_event->data) {
 159                 _E("Failed to allocate memory");
 160                 free(rotation_vector_event);
 161                 return;
 162         }
 163
 164         rotation_vector_event->sensor_id = get_id();
 165         rotation_vector_event->event_type = CONVERT_TYPE_EVENT(ROTATION_VECTOR_SENSOR);
 166         rotation_vector_event->data_length = sizeof(sensor_data_t);
 167         rotation_vector_event->data->accuracy = m_accuracy;
 168         rotation_vector_event->data->timestamp = timestamp;
 169         rotation_vector_event->data->value_count = 4;
 170         rotation_vector_event->data->values[0] = quat[0];
 171         rotation_vector_event->data->values[1] = quat[1];
 172         rotation_vector_event->data->values[2] = quat[2];
 173         rotation_vector_event->data->values[3] = quat[3];
 174         push(rotation_vector_event);
 175
 176         m_time = timestamp;
 177         m_x = quat[0];
 178         m_y = quat[1];
 179         m_z = quat[2];
 180         m_w = quat[3];
 181         m_accuracy = event.data->accuracy;
 182
 183         _D("[rotation_vector] : [%10f] [%10f] [%10f] [%10f]", m_x, m_y, m_z, m_w);
 184 }
 185
 186 int rotation_vector_sensor::get_data(sensor_data_t **data, int *length)
 187 {
 188         sensor_data_t *sensor_data;
 189         sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 190
 191         sensor_data->accuracy = m_accuracy;
 192         sensor_data->timestamp = m_time;
 193         sensor_data->value_count = 3;
 194         sensor_data->values[0] = m_x;
 195         sensor_data->values[1] = m_y;
 196         sensor_data->values[2] = m_z;
 197
 198         *data = sensor_data;
 199         *length = sizeof(sensor_data_t);
 200
 201         return 0;
 202 }
 203
 204 bool rotation_vector_sensor::set_interval(unsigned long interval)
 205 {
 206         m_interval = interval;
 207         return true;
 208 }
 209
 210 bool rotation_vector_sensor::set_batch_latency(unsigned long latency)
 211 {
 212         return false;
 213 }
 214
 215 bool rotation_vector_sensor::on_start(void)
 216 {
 217         if (m_accel_sensor)
 218                 m_accel_sensor->start();
 219
 220         if (m_mag_sensor)
 221                 m_mag_sensor->start();
 222
 223         m_time = 0;
 224         return activate();
 225 }
 226
 227 bool rotation_vector_sensor::on_stop(void)
 228 {
 229         if (m_accel_sensor)
 230                 m_accel_sensor->stop();
 231
 232         if (m_mag_sensor)
 233                 m_mag_sensor->stop();
 234
 235         m_time = 0;
 236         m_state = 0;
 237
 238         return deactivate();
 239 }
 240
 241 bool rotation_vector_sensor::add_interval(int client_id, unsigned int interval, bool is_processor)
 242 {
 243         if (m_accel_sensor)
 244                 m_accel_sensor->add_interval(client_id, interval, true);
 245
 246         if (m_mag_sensor)
 247                 m_mag_sensor->add_interval(client_id, interval, true);
 248
 249         return sensor_base::add_interval(client_id, interval, is_processor);
 250 }
 251
 252 bool rotation_vector_sensor::delete_interval(int client_id, bool is_processor)
 253 {
 254         if (m_accel_sensor)
 255                 m_accel_sensor->delete_interval(client_id, true);
 256
 257         if (m_mag_sensor)
 258                 m_mag_sensor->delete_interval(client_id, true);
 259
 260         return sensor_base::delete_interval(client_id, is_processor);
 261 }