src/sensor_fusion/standalone/orientation_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 _ORIENTATION_SENSOR_H
  21
  22 float bias_accel[] = {0.098586, 0.18385, (10.084 - GRAVITY)};
  23 float bias_gyro[] = {-5.3539, 0.24325, 2.3391};
  24 float bias_magnetic[] = {0, 0, 0};
  25 int sign_accel[] = {+1, +1, +1};
  26 int sign_gyro[] = {+1, +1, +1};
  27 int sign_magnetic[] = {+1, +1, +1};
  28 float scale_accel = 1;
  29 float scale_gyro = 575;
  30 float scale_magnetic = 1;
  31
  32 int pitch_phase_compensation = -1;
  33 int roll_phase_compensation = -1;
  34 int yaw_phase_compensation = -1;
  35 int magnetic_alignment_factor = -1;
  36
  37 void pre_process_data(sensor_data<float> &data_out, sensor_data<float> &data_in, float *bias, int *sign, float scale)
  38 {
  39         data_out.m_data.m_vec[0] = sign[0] * (data_in.m_data.m_vec[0] - bias[0]) / scale;
  40         data_out.m_data.m_vec[1] = sign[1] * (data_in.m_data.m_vec[1] - bias[1]) / scale;
  41         data_out.m_data.m_vec[2] = sign[2] * (data_in.m_data.m_vec[2] - bias[2]) / scale;
  42
  43         data_out.m_time_stamp = data_in.m_time_stamp;
  44 }
  45
  46 euler_angles<float> orientation_sensor::get_orientation(sensor_data<float> accel_data,
  47                 sensor_data<float> gyro_data, sensor_data<float> magnetic_data)
  48 {
  49
  50         pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel);
  51         normalize(accel_data);
  52         pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro);
  53         pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
  54         normalize(magnetic_data);
  55
  56         orien_filter.m_pitch_phase_compensation = pitch_phase_compensation;
  57         orien_filter.m_roll_phase_compensation = roll_phase_compensation;
  58         orien_filter.m_yaw_phase_compensation = yaw_phase_compensation;
  59         orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor;
  60
  61         return orien_filter.get_orientation(accel_data, gyro_data, magnetic_data);
  62 }
  63
  64 rotation_matrix<float> orientation_sensor::get_rotation_matrix(sensor_data<float> accel_data,
  65                 sensor_data<float> gyro_data, sensor_data<float> magnetic_data)
  66 {
  67
  68         pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel);
  69         normalize(accel_data);
  70         pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro);
  71         pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
  72         normalize(magnetic_data);
  73
  74         orien_filter.m_pitch_phase_compensation = pitch_phase_compensation;
  75         orien_filter.m_roll_phase_compensation = roll_phase_compensation;
  76         orien_filter.m_yaw_phase_compensation = yaw_phase_compensation;
  77         orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor;
  78
  79         return orien_filter.get_rotation_matrix(accel_data, gyro_data, magnetic_data);
  80 }
  81
  82 #endif