src/sensor_fusion/standalone/test/orientation_filter_test/orientation_filter_main.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 #include "../../../orientation_filter.h"
  21 #include <stdlib.h>
  22 #include <iostream>
  23 #include <fstream>
  24 #include <string>
  25 using namespace std;
  26
  27 #define ORIENTATION_DATA_PATH "../../../design/data/100ms/orientation/roll_pitch_yaw/"
  28 #define ORIENTATION_DATA_SIZE 1095
  29
  30 float bias_accel[] = {0.098586, 0.18385, (10.084 - GRAVITY)};
  31 float bias_gyro[] = {-5.3539, 0.24325, 2.3391};
  32 float bias_magnetic[] = {0, 0, 0};
  33 int sign_accel[] = {+1, +1, +1};
  34 int sign_gyro[] = {+1, +1, +1};
  35 int sign_magnetic[] = {+1, +1, +1};
  36 float scale_accel = 1;
  37 float scale_gyro = 575;
  38 float scale_magnetic = 1;
  39
  40 int pitch_phase_compensation = -1;
  41 int roll_phase_compensation = -1;
  42 int yaw_phase_compensation = -1;
  43 int magnetic_alignment_factor = -1;
  44
  45 void pre_process_data(sensor_data<float> &data_out, sensor_data<float> &data_in, float *bias, int *sign, float scale)
  46 {
  47         data_out.m_data.m_vec[0] = sign[0] * (data_in.m_data.m_vec[0] - bias[0]) / scale;
  48         data_out.m_data.m_vec[1] = sign[1] * (data_in.m_data.m_vec[1] - bias[1]) / scale;
  49         data_out.m_data.m_vec[2] = sign[2] * (data_in.m_data.m_vec[2] - bias[2]) / scale;
  50
  51         data_out.m_time_stamp = data_in.m_time_stamp;
  52 }
  53
  54 int main()
  55 {
  56         int data_available = ORIENTATION_DATA_SIZE;
  57         ifstream accel_in, gyro_in, mag_in;
  58         ofstream orien_file;
  59         string line_accel, line_gyro, line_magnetic;
  60         float sdata[3];
  61         unsigned long long time_stamp;
  62         euler_angles<float> orientation;
  63         orientation_filter<float> orien_filter;
  64
  65         accel_in.open(((string)ORIENTATION_DATA_PATH + (string)"accel.txt").c_str());
  66         gyro_in.open(((string)ORIENTATION_DATA_PATH + (string)"gyro.txt").c_str());
  67         mag_in.open(((string)ORIENTATION_DATA_PATH + (string)"magnetic.txt").c_str());
  68
  69         orien_file.open(((string)"orientation.txt").c_str());
  70
  71         char *token = NULL;
  72
  73         while (data_available-- > 0)
  74         {
  75                 getline(accel_in, line_accel);
  76                 sdata[0] = strtof(line_accel.c_str(), &token);
  77                 sdata[1] = strtof(token, &token);
  78                 sdata[2] = strtof(token, &token);
  79                 time_stamp = strtoull (token, NULL, 10);
  80                 sensor_data<float> accel_data(sdata[0], sdata[1], sdata[2], time_stamp);
  81
  82                 getline(gyro_in, line_gyro);
  83                 sdata[0] = strtof(line_gyro.c_str(), &token);
  84                 sdata[1] = strtof(token, &token);
  85                 sdata[2] = strtof(token, &token);
  86                 time_stamp = strtoull (token, NULL, 10);
  87                 sensor_data<float> gyro_data(sdata[0], sdata[1], sdata[2], time_stamp);
  88
  89                 getline(mag_in, line_magnetic);
  90                 sdata[0] = strtof(line_magnetic.c_str(), &token);
  91                 sdata[1] = strtof(token, &token);
  92                 sdata[2] = strtof(token, &token);
  93                 time_stamp = strtoull (token, NULL, 10);
  94                 sensor_data<float> magnetic_data(sdata[0], sdata[1], sdata[2], time_stamp);
  95
  96                 pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel);
  97                 normalize(accel_data);
  98                 pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro);
  99                 pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
 100                 normalize(magnetic_data);
 101
 102                 cout << "Accel Data\t" << accel_data.m_data << "\t Time Stamp\t" << accel_data.m_time_stamp << "\n\n";
 103                 cout << "Gyro Data\t" << gyro_data.m_data << "\t Time Stamp\t" << gyro_data.m_time_stamp << "\n\n";
 104                 cout << "Magnetic Data\t" << magnetic_data.m_data << "\t Time Stamp\t" << magnetic_data.m_time_stamp << "\n\n";
 105
 106                 orien_filter.m_pitch_phase_compensation = pitch_phase_compensation;
 107                 orien_filter.m_roll_phase_compensation = roll_phase_compensation;
 108                 orien_filter.m_yaw_phase_compensation = yaw_phase_compensation;
 109                 orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor;
 110
 111                 orientation = orien_filter.get_orientation(accel_data, gyro_data, magnetic_data);
 112
 113                 orien_file << orientation.m_ang;
 114
 115                 cout << "Orientation Data\t" << orientation.m_ang << "\n\n";
 116         }
 117
 118         accel_in.close();
 119         gyro_in.close();
 120         mag_in.close();
 121         orien_file.close();
 122
 123         return 0;
 124 }