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"
29 #include "orientation_filter.h"
31 #define ACCEL_COMPENSATION -1
32 #define GYRO_COMPENSATION 1
33 #define MAG_COMPENSATION -1
35 fusion_base::fusion_base()
36 : m_enable_accel(false)
37 , m_enable_gyro(false)
38 , m_enable_magnetic(false)
40 _I("fusion_base is created!");
43 fusion_base::~fusion_base()
45 _I("fusion_sensor is destroyed!");
48 void fusion_base::clear(void)
50 m_enable_accel = false;
51 m_enable_gyro = false;
52 m_enable_magnetic = false;
55 void fusion_base::push_accel(sensor_data_t &data)
57 //_I("[fusion_sensor] : Pushing accel");
58 pre_process_data(m_accel, data.values, ACCEL_COMPENSATION, ACCEL_SCALE);
59 m_accel.m_time_stamp = data.timestamp;
60 m_enable_accel = true;
61 if (get_orientation())
65 void fusion_base::push_gyro(sensor_data_t &data)
67 //_I("[fusion_sensor] : Pushing mag");
68 pre_process_data(m_gyro, data.values, GYRO_COMPENSATION, GYRO_SCALE);
69 m_gyro.m_time_stamp = data.timestamp;
71 if (get_orientation())
75 void fusion_base::push_mag(sensor_data_t &data)
77 //_I("[fusion_sensor] : Pushing gyro");
78 pre_process_data(m_magnetic, data.values, MAG_COMPENSATION, GEOMAGNETIC_SCALE);
79 m_magnetic.m_time_stamp = data.timestamp;
80 m_enable_magnetic = true;
81 if (get_orientation())
86 bool fusion_base::get_rv(unsigned long long ×tamp, float &x, float &y, float &z, float &w)
90 timestamp = m_timestamp;
98 void fusion_base::store_orientation(void)
100 m_x = m_orientation_filter.m_quaternion.m_quat.m_vec[0];
101 m_y = m_orientation_filter.m_quaternion.m_quat.m_vec[1];
102 m_z = m_orientation_filter.m_quaternion.m_quat.m_vec[2];
103 m_w = m_orientation_filter.m_quaternion.m_quat.m_vec[3];