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 #include "orientation_filter.h"
  30
  31 #define ACCEL_COMPENSATION -1
  32 #define GYRO_COMPENSATION 1
  33 #define MAG_COMPENSATION -1
  34
  35 fusion_base::fusion_base()
  36 : m_enable_accel(false)
  37 , m_enable_gyro(false)
  38 , m_enable_magnetic(false)
  39 {
  40         _I("fusion_base is created!");
  41 }
  42
  43 fusion_base::~fusion_base()
  44 {
  45         _I("fusion_sensor is destroyed!");
  46 }
  47
  48 void fusion_base::clear(void)
  49 {
  50         m_enable_accel = false;
  51         m_enable_gyro = false;
  52         m_enable_magnetic = false;
  53 }
  54
  55 void fusion_base::push_accel(sensor_data_t &data)
  56 {
  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())
  62                 store_orientation();
  63 }
  64
  65 void fusion_base::push_gyro(sensor_data_t &data)
  66 {
  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;
  70         m_enable_gyro = true;
  71         if (get_orientation())
  72                 store_orientation();
  73 }
  74
  75 void fusion_base::push_mag(sensor_data_t &data)
  76 {
  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())
  82                 store_orientation();
  83
  84 }
  85
  86 bool fusion_base::get_rv(unsigned long long &timestamp, float &x, float &y, float &z, float &w)
  87 {
  88         if (m_timestamp == 0)
  89                 return false;
  90         timestamp = m_timestamp;
  91         x = m_x;
  92         y = m_y;
  93         z = m_z;
  94         w = m_w;
  95         return true;
  96 }
  97
  98 void fusion_base::store_orientation(void)
  99 {
 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];
 104         clear();
 105 }