src/sensor/rotation_vector/fusion_base.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 #include <stdio.h>
  20 #include <stdlib.h>
  21 #include <unistd.h>
  22 #include <errno.h>
  23 #include <sys/types.h>
  24 #include <sensor_log.h>
  25 #include <sensor_loader.h>
  26 #include <sensor_base.h>
  27 #include <cmath>
  28 #include "fusion_base.h"
  29
  30 #define ACCEL_COMPENSATION -1
  31 #define GYRO_COMPENSATION 1
  32 #define MAG_COMPENSATION -1
  33
  34 fusion_base::fusion_base()
  35 : m_enable_accel(false)
  36 , m_enable_gyro(false)
  37 , m_enable_magnetic(false)
  38 {
  39         _I("fusion_base is created!");
  40 }
  41
  42 fusion_base::~fusion_base()
  43 {
  44         _I("fusion_sensor is destroyed!");
  45 }
  46
  47 void fusion_base::clear(void)
  48 {
  49         m_enable_accel = false;
  50         m_enable_gyro = false;
  51         m_enable_magnetic = false;
  52 }
  53
  54 void fusion_base::push_accel(sensor_data_t &data)
  55 {
  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())
  61                 store_orientation();
  62 }
  63
  64 void fusion_base::push_gyro(sensor_data_t &data)
  65 {
  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;
  69         m_enable_gyro = true;
  70         if (get_orientation())
  71                 store_orientation();
  72 }
  73
  74 void fusion_base::push_mag(sensor_data_t &data)
  75 {
  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())
  81                 store_orientation();
  82
  83 }
  84
  85 bool fusion_base::get_rv(unsigned long long &timestamp, float &x, float &y, float &z, float &w)
  86 {
  87         if (m_timestamp == 0)
  88                 return false;
  89         timestamp = m_timestamp;
  90         x = m_x;
  91         y = m_y;
  92         z = m_z;
  93         w = m_w;
  94         return true;
  95 }
  96
  97 void fusion_base::store_orientation(void)
  98 {
  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];
 103         clear();
 104 }