sensord: change access permissions about all files
[platform/core/system/sensord.git] / src / server / plugins / fusion / fusion_sensor.cpp
1 /*
2  * sensord
3  *
4  * Copyright (c) 2015 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 #include <sensor_logs.h>
29 #include <sf_common.h>
30 #include <fusion_sensor.h>
31 #include <sensor_plugin_loader.h>
32 #include <orientation_filter.h>
33 #include <virtual_sensor_config.h>
34 #include <algorithm>
35
36 using std::string;
37 using std::vector;
38
39 #define SENSOR_NAME "FUSION_SENSOR"
40 #define SENSOR_TYPE_FUSION              "FUSION"
41
42 #define ACCELEROMETER_ENABLED 0x01
43 #define GYROSCOPE_ENABLED 0x02
44 #define GEOMAGNETIC_ENABLED 0x04
45 #define TILT_ENABLED 1
46 #define GAMING_RV_ENABLED 3
47 #define GEOMAGNETIC_RV_ENABLED 5
48 #define ORIENTATION_ENABLED 7
49 #define ROTATION_VECTOR_ENABLED 7
50 #define GYROSCOPE_UNCAL_ENABLED 7
51
52 #define INITIAL_VALUE -1
53
54 #define MS_TO_US 1000
55 #define MIN_DELIVERY_DIFF_FACTOR 0.75f
56
57 #define PI 3.141593
58 #define AZIMUTH_OFFSET_DEGREES 360
59 #define AZIMUTH_OFFSET_RADIANS (2 * PI)
60
61 #define ELEMENT_NAME                                                                                    "NAME"
62 #define ELEMENT_VENDOR                                                                                  "VENDOR"
63 #define ELEMENT_RAW_DATA_UNIT                                                                   "RAW_DATA_UNIT"
64 #define ELEMENT_DEFAULT_SAMPLING_TIME                                                   "DEFAULT_SAMPLING_TIME"
65 #define ELEMENT_ACCEL_STATIC_BIAS                                                               "ACCEL_STATIC_BIAS"
66 #define ELEMENT_GYRO_STATIC_BIAS                                                                "GYRO_STATIC_BIAS"
67 #define ELEMENT_GEOMAGNETIC_STATIC_BIAS                                                 "GEOMAGNETIC_STATIC_BIAS"
68 #define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION                   "ACCEL_ROTATION_DIRECTION_COMPENSATION"
69 #define ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION                    "GYRO_ROTATION_DIRECTION_COMPENSATION"
70 #define ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION             "GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION"
71 #define ELEMENT_MAGNETIC_ALIGNMENT_FACTOR                                               "MAGNETIC_ALIGNMENT_FACTOR"
72 #define ELEMENT_PITCH_ROTATION_COMPENSATION                                             "PITCH_ROTATION_COMPENSATION"
73 #define ELEMENT_ROLL_ROTATION_COMPENSATION                                              "ROLL_ROTATION_COMPENSATION"
74 #define ELEMENT_AZIMUTH_ROTATION_COMPENSATION                                   "AZIMUTH_ROTATION_COMPENSATION"
75
76 fusion_sensor::fusion_sensor()
77 : m_accel_sensor(NULL)
78 , m_gyro_sensor(NULL)
79 , m_magnetic_sensor(NULL)
80 , m_time(0)
81 {
82         virtual_sensor_config &config = virtual_sensor_config::get_instance();
83         m_name = string(SENSOR_NAME);
84         m_enable_fusion = 0;
85
86         if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_VENDOR, m_vendor)) {
87                 ERR("[VENDOR] is empty\n");
88                 throw ENXIO;
89         }
90
91         INFO("m_vendor = %s", m_vendor.c_str());
92
93         if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) {
94                 ERR("[RAW_DATA_UNIT] is empty\n");
95                 throw ENXIO;
96         }
97
98         INFO("m_raw_data_unit = %s", m_raw_data_unit.c_str());
99
100         if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) {
101                 ERR("[DEFAULT_SAMPLING_TIME] is empty\n");
102                 throw ENXIO;
103         }
104
105         INFO("m_default_sampling_time = %d", m_default_sampling_time);
106
107         if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) {
108                 ERR("[ACCEL_STATIC_BIAS] is empty\n");
109                 throw ENXIO;
110         }
111
112         INFO("m_accel_static_bias = (%f, %f, %f)", m_accel_static_bias[0], m_accel_static_bias[1], m_accel_static_bias[2]);
113
114         if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_GYRO_STATIC_BIAS, m_gyro_static_bias,3)) {
115                 ERR("[GYRO_STATIC_BIAS] is empty\n");
116                 throw ENXIO;
117         }
118
119         INFO("m_gyro_static_bias = (%f, %f, %f)", m_gyro_static_bias[0], m_gyro_static_bias[1], m_gyro_static_bias[2]);
120
121         if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_GEOMAGNETIC_STATIC_BIAS, m_geomagnetic_static_bias, 3)) {
122                 ERR("[GEOMAGNETIC_STATIC_BIAS] is empty\n");
123                 throw ENXIO;
124         }
125
126         INFO("m_geomagnetic_static_bias = (%f, %f, %f)", m_geomagnetic_static_bias[0], m_geomagnetic_static_bias[1], m_geomagnetic_static_bias[2]);
127
128         if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION, m_accel_rotation_direction_compensation, 3)) {
129                 ERR("[ACCEL_ROTATION_DIRECTION_COMPENSATION] is empty\n");
130                 throw ENXIO;
131         }
132
133         INFO("m_accel_rotation_direction_compensation = (%d, %d, %d)", m_accel_rotation_direction_compensation[0], m_accel_rotation_direction_compensation[1], m_accel_rotation_direction_compensation[2]);
134
135         if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION, m_gyro_rotation_direction_compensation, 3)) {
136                 ERR("[GYRO_ROTATION_DIRECTION_COMPENSATION] is empty\n");
137                 throw ENXIO;
138         }
139
140         INFO("m_gyro_rotation_direction_compensation = (%d, %d, %d)", m_gyro_rotation_direction_compensation[0], m_gyro_rotation_direction_compensation[1], m_gyro_rotation_direction_compensation[2]);
141
142         if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION, m_geomagnetic_rotation_direction_compensation, 3)) {
143                 ERR("[GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION] is empty\n");
144                 throw ENXIO;
145         }
146
147         INFO("m_geomagnetic_rotation_direction_compensation = (%d, %d, %d)", m_geomagnetic_rotation_direction_compensation[0], m_geomagnetic_rotation_direction_compensation[1], m_geomagnetic_rotation_direction_compensation[2]);
148
149         if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_MAGNETIC_ALIGNMENT_FACTOR, &m_magnetic_alignment_factor)) {
150                 ERR("[MAGNETIC_ALIGNMENT_FACTOR] is empty\n");
151                 throw ENXIO;
152         }
153
154         INFO("m_magnetic_alignment_factor = %d", m_magnetic_alignment_factor);
155
156         m_interval = m_default_sampling_time * MS_TO_US;
157
158         m_accel_ptr = m_gyro_ptr = m_magnetic_ptr = NULL;
159 }
160
161 fusion_sensor::~fusion_sensor()
162 {
163         INFO("fusion_sensor is destroyed!\n");
164 }
165
166 bool fusion_sensor::init(void)
167 {
168         m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
169         m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR);
170         m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR);
171
172         if (!m_accel_sensor) {
173                 ERR("Failed to load accel sensor: 0x%x", m_accel_sensor);
174                 return false;
175         }
176
177         if (!m_gyro_sensor)
178                 INFO("Failed to load gyro sensor: 0x%x", m_gyro_sensor);
179
180         if (!m_magnetic_sensor)
181                 INFO("Failed to load geomagnetic sensor: 0x%x", m_magnetic_sensor);
182
183         INFO("%s is created!", sensor_base::get_name());
184         return true;
185 }
186
187 void fusion_sensor::get_types(vector<sensor_type_t> &types)
188 {
189         types.push_back(FUSION_SENSOR);
190 }
191
192 bool fusion_sensor::on_start(void)
193 {
194         AUTOLOCK(m_mutex);
195         activate();
196         return true;
197 }
198
199 bool fusion_sensor::on_stop(void)
200 {
201         AUTOLOCK(m_mutex);
202         deactivate();
203         return true;
204 }
205
206 bool fusion_sensor::add_interval(int client_id, unsigned int interval)
207 {
208         bool retval;
209
210         AUTOLOCK(m_mutex);
211         retval = sensor_base::add_interval(client_id, interval, false);
212
213         m_interval = sensor_base::get_interval(client_id, false);
214
215         if (m_interval != 0)
216                 retval = true;
217
218         return retval;
219 }
220
221 bool fusion_sensor::delete_interval(int client_id)
222 {
223         bool retval;
224
225         AUTOLOCK(m_mutex);
226         retval = sensor_base::delete_interval(client_id, false);
227
228         m_interval = sensor_base::get_interval(client_id, false);
229
230         if (m_interval != 0)
231                 retval = true;
232
233         return retval;
234 }
235
236 void fusion_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
237 {
238         unsigned long long diff_time;
239         euler_angles<float> euler_orientation;
240
241         if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) {
242                 diff_time = event.data.timestamp - m_time;
243
244                 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
245                         return;
246
247                 pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, ACCEL_SCALE);
248
249                 m_accel.m_time_stamp = event.data.timestamp;
250
251                 m_accel_ptr = &m_accel;
252
253                 m_enable_fusion |= ACCELEROMETER_ENABLED;
254         }
255
256         if (sensor_base::is_supported(FUSION_ORIENTATION_ENABLED) ||
257                         sensor_base::is_supported(FUSION_ROTATION_VECTOR_ENABLED) ||
258                         sensor_base::is_supported(FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED) ||
259                         sensor_base::is_supported(FUSION_GYROSCOPE_UNCAL_ENABLED)) {
260                 if (event.event_type == GEOMAGNETIC_RAW_DATA_EVENT) {
261                         diff_time = event.data.timestamp - m_time;
262
263                         if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
264                                 return;
265
266                         pre_process_data(m_magnetic, event.data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, GEOMAGNETIC_SCALE);
267
268                         m_magnetic.m_time_stamp = event.data.timestamp;
269
270                         m_magnetic_ptr = &m_magnetic;
271
272                         m_enable_fusion |= GEOMAGNETIC_ENABLED;
273                 }
274         }
275
276         if (sensor_base::is_supported(FUSION_ORIENTATION_ENABLED) ||
277                         sensor_base::is_supported(FUSION_ROTATION_VECTOR_ENABLED) ||
278                         sensor_base::is_supported(FUSION_GAMING_ROTATION_VECTOR_ENABLED) ||
279                         sensor_base::is_supported(FUSION_GYROSCOPE_UNCAL_ENABLED)) {
280                 if (event.event_type == GYROSCOPE_RAW_DATA_EVENT) {
281                                 diff_time = event.data.timestamp - m_time;
282
283                                 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
284                                         return;
285
286                                 pre_process_data(m_gyro, event.data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, GYRO_SCALE);
287
288                                 m_gyro.m_time_stamp = event.data.timestamp;
289
290                                 m_gyro_ptr = &m_gyro;
291
292                                 m_enable_fusion |= GYROSCOPE_ENABLED;
293                 }
294         }
295
296         if ((m_enable_fusion == TILT_ENABLED && sensor_base::is_supported(FUSION_TILT_ENABLED)) ||
297                         (m_enable_fusion == ORIENTATION_ENABLED && sensor_base::is_supported(FUSION_ORIENTATION_ENABLED)) ||
298                         (m_enable_fusion == ROTATION_VECTOR_ENABLED && sensor_base::is_supported(FUSION_ROTATION_VECTOR_ENABLED)) ||
299                         (m_enable_fusion == GAMING_RV_ENABLED && sensor_base::is_supported(FUSION_GAMING_ROTATION_VECTOR_ENABLED)) ||
300                         (m_enable_fusion == GEOMAGNETIC_RV_ENABLED && sensor_base::is_supported(FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED)) ||
301                         (m_enable_fusion == GYROSCOPE_UNCAL_ENABLED && sensor_base::is_supported(FUSION_GYROSCOPE_UNCAL_ENABLED))) {
302                 sensor_event_t fusion_event;
303
304                 m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
305
306                 m_orientation_filter.get_device_orientation(m_accel_ptr, m_gyro_ptr, m_magnetic_ptr);
307
308
309
310                 if (m_enable_fusion == GYROSCOPE_UNCAL_ENABLED && sensor_base::is_supported(FUSION_GYROSCOPE_UNCAL_ENABLED)) {
311                         m_time = get_timestamp();
312                         fusion_event.sensor_id = get_id();
313                         fusion_event.data.timestamp = m_time;
314                         fusion_event.data.accuracy = SENSOR_ACCURACY_GOOD;
315                         fusion_event.event_type = FUSION_GYROSCOPE_UNCAL_EVENT;
316                         fusion_event.data.value_count = 3;
317                         fusion_event.data.values[0] = m_orientation_filter.m_gyro_bias.m_vec[0];
318                         fusion_event.data.values[1] = m_orientation_filter.m_gyro_bias.m_vec[1];
319                         fusion_event.data.values[2] = m_orientation_filter.m_gyro_bias.m_vec[2];
320
321                         push(fusion_event);
322                 }
323
324                 if ((m_enable_fusion == TILT_ENABLED && sensor_base::is_supported(FUSION_TILT_ENABLED)) ||
325                                 (m_enable_fusion == ORIENTATION_ENABLED && sensor_base::is_supported(FUSION_ORIENTATION_ENABLED)) ||
326                                 (m_enable_fusion == ROTATION_VECTOR_ENABLED && sensor_base::is_supported(FUSION_ROTATION_VECTOR_ENABLED)) ||
327                                 (m_enable_fusion == GAMING_RV_ENABLED && sensor_base::is_supported(FUSION_GAMING_ROTATION_VECTOR_ENABLED)) ||
328                                 (m_enable_fusion == GEOMAGNETIC_RV_ENABLED && sensor_base::is_supported(FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED))) {
329                         m_time = get_timestamp();
330                         fusion_event.sensor_id = get_id();
331                         fusion_event.data.timestamp = m_time;
332                         fusion_event.data.accuracy = SENSOR_ACCURACY_GOOD;
333                         fusion_event.event_type = FUSION_EVENT;
334                         fusion_event.data.value_count = 4;
335                         fusion_event.data.values[0] = m_orientation_filter.m_quaternion.m_quat.m_vec[0];
336                         fusion_event.data.values[1] = m_orientation_filter.m_quaternion.m_quat.m_vec[1];
337                         fusion_event.data.values[2] = m_orientation_filter.m_quaternion.m_quat.m_vec[2];
338                         fusion_event.data.values[3] = m_orientation_filter.m_quaternion.m_quat.m_vec[3];
339
340                         push(fusion_event);
341                 }
342
343                 m_enable_fusion = 0;
344
345                 m_accel_ptr = m_gyro_ptr = m_magnetic_ptr = NULL;
346         }
347
348         return;
349 }
350
351 int fusion_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
352 {
353         sensor_data<float> accel;
354         sensor_data<float> gyro;
355         sensor_data<float> magnetic;
356
357         sensor_data_t accel_data;
358         sensor_data_t gyro_data;
359         sensor_data_t magnetic_data;
360
361         euler_angles<float> euler_orientation;
362
363         if (event_type != FUSION_ORIENTATION_ENABLED &&
364                         event_type != FUSION_ROTATION_VECTOR_ENABLED &&
365                         event_type != FUSION_GAMING_ROTATION_VECTOR_ENABLED &&
366                         event_type != FUSION_TILT_ENABLED &&
367                         event_type != FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED &&
368                         event_type != FUSION_GYROSCOPE_UNCAL_ENABLED)
369                 return -1;
370
371         m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data);
372         pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, ACCEL_SCALE);
373         accel.m_time_stamp = accel_data.timestamp;
374
375         if (event_type == FUSION_ORIENTATION_ENABLED ||
376                         event_type == FUSION_ROTATION_VECTOR_ENABLED ||
377                         event_type == FUSION_GAMING_ROTATION_VECTOR_ENABLED ||
378                         event_type == FUSION_GYROSCOPE_UNCAL_ENABLED)
379         {
380                 m_gyro_sensor->get_sensor_data(GYROSCOPE_RAW_DATA_EVENT, gyro_data);
381                 pre_process_data(gyro, gyro_data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, GYRO_SCALE);
382                 gyro.m_time_stamp = gyro_data.timestamp;
383         }
384
385         if (event_type == FUSION_ORIENTATION_ENABLED ||
386                         event_type == FUSION_ROTATION_VECTOR_ENABLED ||
387                         event_type == FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED ||
388                         event_type == FUSION_GYROSCOPE_UNCAL_ENABLED)
389         {
390                 m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_RAW_DATA_EVENT, magnetic_data);
391                 pre_process_data(magnetic, magnetic_data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, GEOMAGNETIC_SCALE);
392                 magnetic.m_time_stamp = magnetic_data.timestamp;
393         }
394
395         m_orientation_filter_poll.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
396
397         if (event_type == FUSION_ORIENTATION_ENABLED ||
398                         event_type == FUSION_ROTATION_VECTOR_ENABLED ||
399                         event_type == FUSION_GYROSCOPE_UNCAL_ENABLED)
400                 m_orientation_filter_poll.get_device_orientation(&accel, &gyro, &magnetic);
401         else if (event_type == FUSION_GAMING_ROTATION_VECTOR_ENABLED)
402                 m_orientation_filter_poll.get_device_orientation(&accel, &gyro, NULL);
403         else if (event_type == FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED)
404                 m_orientation_filter_poll.get_device_orientation(&accel, NULL, &magnetic);
405         else if (event_type == FUSION_TILT_ENABLED)
406                         m_orientation_filter_poll.get_device_orientation(&accel, NULL, NULL);
407
408         if (event_type == FUSION_GYROSCOPE_UNCAL_ENABLED) {
409                 data.accuracy = SENSOR_ACCURACY_GOOD;
410                 data.timestamp = get_timestamp();
411                 data.value_count = 3;
412                 data.values[0] = m_orientation_filter_poll.m_gyro_bias.m_vec[0];
413                 data.values[1] = m_orientation_filter_poll.m_gyro_bias.m_vec[1];
414                 data.values[2] = m_orientation_filter_poll.m_gyro_bias.m_vec[2];
415         }
416         else if (event_type == FUSION_ORIENTATION_ENABLED ||
417                         event_type == FUSION_ROTATION_VECTOR_ENABLED ||
418                         event_type == FUSION_GAMING_ROTATION_VECTOR_ENABLED ||
419                         event_type == FUSION_TILT_ENABLED ||
420                         event_type == FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED) {
421                 data.accuracy = SENSOR_ACCURACY_GOOD;
422                 data.timestamp = get_timestamp();
423                 data.value_count = 4;
424                 data.values[0] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[0];
425                 data.values[1] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[1];
426                 data.values[2] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[2];
427                 data.values[3] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[3];
428         }
429
430         return 0;
431 }
432
433 bool fusion_sensor::get_properties(sensor_type_t sensor_type, sensor_properties_s &properties)
434 {
435         properties.min_range = 0;
436         properties.max_range = 0;
437         properties.resolution = 0;
438         properties.vendor = m_vendor;
439         properties.name = SENSOR_NAME;
440         properties.min_interval = 0;
441         properties.fifo_count = 0;
442         properties.max_batch_count = 0;
443
444         return true;
445 }