src/sensor_fusion/standalone/gravity_sensor.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 #ifdef _GRAVITY_SENSOR_H
  21
  22 #define GRAVITY         9.80665
  23
  24 sensor_data<float> gravity_sensor::get_gravity(const sensor_data<float> accel,
  25                                 const sensor_data<float> gyro, const sensor_data<float> magnetic)
  26 {
  27         euler_angles<float> orientation;
  28         sensor_data<float> gravity;
  29
  30         orientation = orien_sensor.get_orientation(accel, gyro, magnetic);
  31
  32         gravity.m_data.m_vec[0] = GRAVITY * sin(orientation.m_ang.m_vec[0]);
  33         gravity.m_data.m_vec[1] = GRAVITY * sin(orientation.m_ang.m_vec[1]);
  34         gravity.m_data.m_vec[2] = GRAVITY * cos(orientation.m_ang.m_vec[0]) *
  35                                                                         cos(orientation.m_ang.m_vec[1]);
  36
  37         return gravity;
  38 }
  39
  40 #endif