src/sensor_fusion/standalone/util/compute_gravity.cpp

   1 /*
   2  * sensord
   3  *
   4  * Copyright (c) 2014 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
  20
  21 #ifdef _COMPUTE_GRAVITY_H
  22
  23 #include "compute_gravity.h"
  24
  25 #define GRAVITY         9.80665
  26
  27 template <typename TYPE>
  28 compute_gravity<TYPE>::compute_gravity()
  29 {
  30
  31 }
  32
  33 template <typename TYPE>
  34 compute_gravity<TYPE>::~compute_gravity()
  35 {
  36
  37 }
  38
  39 template <typename TYPE>
  40 sensor_data<TYPE> compute_gravity<TYPE>::orientation2gravity(const sensor_data<TYPE> accel,
  41                 const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
  42 {
  43         orientation_filter<TYPE> orien_filter;
  44         euler_angles<TYPE> orientation;
  45         sensor_data<TYPE> gravity;
  46
  47         orientation = orien_filter.get_orientation(accel, gyro, magnetic);
  48
  49         gravity.m_data.m_vec[0] = GRAVITY * sin(orien_filter.m_orientation.m_ang.m_vec[0]);
  50         gravity.m_data.m_vec[1] = GRAVITY * sin(orien_filter.m_orientation.m_ang.m_vec[1]);
  51         gravity.m_data.m_vec[2] = GRAVITY * cos(orien_filter.m_orientation.m_ang.m_vec[0]) *
  52                                                                         cos(orien_filter.m_orientation.m_ang.m_vec[1]);
  53
  54         return gravity;
  55 }
  56
  57 #endif