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 <cmath>
  26 #include "fusion_base.h"
  27
  28 #define ACCEL_COMPENSATION -1
  29 #define GYRO_COMPENSATION 1
  30 #define MAG_COMPENSATION -1
  31
  32 fusion_base::fusion_base()
  33 : m_enable_accel(false)
  34 , m_enable_gyro(false)
  35 , m_enable_magnetic(false)
  36 , m_x(0)
  37 , m_y(0)
  38 , m_z(0)
  39 , m_w(0)
  40 , m_timestamp(0)
  41 {
  42 }
  43
  44 fusion_base::~fusion_base()
  45 {
  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 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
 104         if (std::isnan(m_x) || std::isnan(m_y) || std::isnan(m_z) || std::isnan(m_w)) {
 105                 m_timestamp = 0;
 106                 m_orientation_filter = orientation_filter<float>();
 107         }
 108         clear();
 109 }