4 * Copyright (c) 2015 Samsung Electronics Co., Ltd.
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
10 * http://www.apache.org/licenses/LICENSE-2.0
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.
26 #include <sys/types.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>
39 #define SENSOR_NAME "FUSION_SENSOR"
40 #define SENSOR_TYPE_FUSION "FUSION"
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
52 #define INITIAL_VALUE -1
55 #define MIN_DELIVERY_DIFF_FACTOR 0.75f
58 #define AZIMUTH_OFFSET_DEGREES 360
59 #define AZIMUTH_OFFSET_RADIANS (2 * PI)
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"
76 fusion_sensor::fusion_sensor()
77 : m_accel_sensor(NULL)
79 , m_magnetic_sensor(NULL)
82 virtual_sensor_config &config = virtual_sensor_config::get_instance();
83 m_name = string(SENSOR_NAME);
86 if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_VENDOR, m_vendor)) {
87 ERR("[VENDOR] is empty\n");
91 INFO("m_vendor = %s", m_vendor.c_str());
93 if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) {
94 ERR("[RAW_DATA_UNIT] is empty\n");
98 INFO("m_raw_data_unit = %s", m_raw_data_unit.c_str());
100 if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) {
101 ERR("[DEFAULT_SAMPLING_TIME] is empty\n");
105 INFO("m_default_sampling_time = %d", m_default_sampling_time);
107 if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) {
108 ERR("[ACCEL_STATIC_BIAS] is empty\n");
112 INFO("m_accel_static_bias = (%f, %f, %f)", m_accel_static_bias[0], m_accel_static_bias[1], m_accel_static_bias[2]);
114 if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_GYRO_STATIC_BIAS, m_gyro_static_bias,3)) {
115 ERR("[GYRO_STATIC_BIAS] is empty\n");
119 INFO("m_gyro_static_bias = (%f, %f, %f)", m_gyro_static_bias[0], m_gyro_static_bias[1], m_gyro_static_bias[2]);
121 if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_GEOMAGNETIC_STATIC_BIAS, m_geomagnetic_static_bias, 3)) {
122 ERR("[GEOMAGNETIC_STATIC_BIAS] is empty\n");
126 INFO("m_geomagnetic_static_bias = (%f, %f, %f)", m_geomagnetic_static_bias[0], m_geomagnetic_static_bias[1], m_geomagnetic_static_bias[2]);
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");
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]);
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");
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]);
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");
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]);
149 if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_MAGNETIC_ALIGNMENT_FACTOR, &m_magnetic_alignment_factor)) {
150 ERR("[MAGNETIC_ALIGNMENT_FACTOR] is empty\n");
154 INFO("m_magnetic_alignment_factor = %d", m_magnetic_alignment_factor);
156 m_interval = m_default_sampling_time * MS_TO_US;
158 m_accel_ptr = m_gyro_ptr = m_magnetic_ptr = NULL;
161 fusion_sensor::~fusion_sensor()
163 INFO("fusion_sensor is destroyed!\n");
166 bool fusion_sensor::init(void)
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);
172 if (!m_accel_sensor) {
173 ERR("Failed to load accel sensor: 0x%x", m_accel_sensor);
178 INFO("Failed to load gyro sensor: 0x%x", m_gyro_sensor);
180 if (!m_magnetic_sensor)
181 INFO("Failed to load geomagnetic sensor: 0x%x", m_magnetic_sensor);
183 INFO("%s is created!", sensor_base::get_name());
187 void fusion_sensor::get_types(vector<sensor_type_t> &types)
189 types.push_back(FUSION_SENSOR);
192 bool fusion_sensor::on_start(void)
199 bool fusion_sensor::on_stop(void)
206 bool fusion_sensor::add_interval(int client_id, unsigned int interval)
211 retval = sensor_base::add_interval(client_id, interval, false);
213 m_interval = sensor_base::get_interval(client_id, false);
221 bool fusion_sensor::delete_interval(int client_id)
226 retval = sensor_base::delete_interval(client_id, false);
228 m_interval = sensor_base::get_interval(client_id, false);
236 void fusion_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
238 unsigned long long diff_time;
239 euler_angles<float> euler_orientation;
241 if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) {
242 diff_time = event.data.timestamp - m_time;
244 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
247 pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, ACCEL_SCALE);
249 m_accel.m_time_stamp = event.data.timestamp;
251 m_accel_ptr = &m_accel;
253 m_enable_fusion |= ACCELEROMETER_ENABLED;
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;
263 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
266 pre_process_data(m_magnetic, event.data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, GEOMAGNETIC_SCALE);
268 m_magnetic.m_time_stamp = event.data.timestamp;
270 m_magnetic_ptr = &m_magnetic;
272 m_enable_fusion |= GEOMAGNETIC_ENABLED;
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;
283 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
286 pre_process_data(m_gyro, event.data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, GYRO_SCALE);
288 m_gyro.m_time_stamp = event.data.timestamp;
290 m_gyro_ptr = &m_gyro;
292 m_enable_fusion |= GYROSCOPE_ENABLED;
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;
304 m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
306 m_orientation_filter.get_device_orientation(m_accel_ptr, m_gyro_ptr, m_magnetic_ptr);
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];
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];
345 m_accel_ptr = m_gyro_ptr = m_magnetic_ptr = NULL;
351 int fusion_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
353 sensor_data<float> accel;
354 sensor_data<float> gyro;
355 sensor_data<float> magnetic;
357 sensor_data_t accel_data;
358 sensor_data_t gyro_data;
359 sensor_data_t magnetic_data;
361 euler_angles<float> euler_orientation;
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)
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;
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)
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;
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)
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;
395 m_orientation_filter_poll.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
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);
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];
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];
433 bool fusion_sensor::get_properties(sensor_type_t sensor_type, sensor_properties_s &properties)
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;