4 * Copyright (c) 2016 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.
23 #include <sys/types.h>
24 #include <sensor_log.h>
25 #include <sensor_loader.h>
26 #include <sensor_base.h>
28 #include "fusion_base.h"
30 #define ACCEL_COMPENSATION -1
31 #define GYRO_COMPENSATION 1
32 #define MAG_COMPENSATION -1
34 fusion_base::fusion_base()
35 : m_enable_accel(false)
36 , m_enable_gyro(false)
37 , m_enable_magnetic(false)
39 _I("fusion_base is created!");
42 fusion_base::~fusion_base()
44 _I("fusion_sensor is destroyed!");
47 void fusion_base::clear(void)
49 m_enable_accel = false;
50 m_enable_gyro = false;
51 m_enable_magnetic = false;
54 void fusion_base::push_accel(sensor_data_t &data)
56 //_I("[fusion_sensor] : Pushing accel");
57 pre_process_data(m_accel, data.values, ACCEL_COMPENSATION, ACCEL_SCALE);
58 m_accel.m_time_stamp = data.timestamp;
59 m_enable_accel = true;
60 if (get_orientation())
64 void fusion_base::push_gyro(sensor_data_t &data)
66 //_I("[fusion_sensor] : Pushing mag");
67 pre_process_data(m_gyro, data.values, GYRO_COMPENSATION, GYRO_SCALE);
68 m_gyro.m_time_stamp = data.timestamp;
70 if (get_orientation())
74 void fusion_base::push_mag(sensor_data_t &data)
76 //_I("[fusion_sensor] : Pushing gyro");
77 pre_process_data(m_magnetic, data.values, MAG_COMPENSATION, GEOMAGNETIC_SCALE);
78 m_magnetic.m_time_stamp = data.timestamp;
79 m_enable_magnetic = true;
80 if (get_orientation())
85 bool fusion_base::get_rv(unsigned long long ×tamp, float &x, float &y, float &z, float &w)
89 timestamp = m_timestamp;
97 void fusion_base::store_orientation(void)
99 m_x = m_orientation_filter.m_quaternion.m_quat.m_vec[0];
100 m_y = m_orientation_filter.m_quaternion.m_quat.m_vec[1];
101 m_z = m_orientation_filter.m_quaternion.m_quat.m_vec[2];
102 m_w = m_orientation_filter.m_quaternion.m_quat.m_vec[3];