4 * Copyright (c) 2014 Samsung Electronics Co., Ltd.
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
10 * http://www.apache.org/licenses/LICENSE-2.0
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.
20 #include "../../../orientation_filter.h"
27 #define ORIENTATION_DATA_PATH "../../../design/data/100ms/orientation/roll_pitch_yaw/"
28 #define ORIENTATION_DATA_SIZE 1095
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;
40 int pitch_phase_compensation = -1;
41 int roll_phase_compensation = -1;
42 int yaw_phase_compensation = -1;
43 int magnetic_alignment_factor = -1;
45 void pre_process_data(sensor_data<float> &data_out, sensor_data<float> &data_in, float *bias, int *sign, float scale)
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;
51 data_out.m_time_stamp = data_in.m_time_stamp;
56 int data_available = ORIENTATION_DATA_SIZE;
57 ifstream accel_in, gyro_in, mag_in;
59 string line_accel, line_gyro, line_magnetic;
61 unsigned long long time_stamp;
62 euler_angles<float> orientation;
63 orientation_filter<float> orien_filter;
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());
69 orien_file.open(((string)"orientation.txt").c_str());
73 while (data_available-- > 0)
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);
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);
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);
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);
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";
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;
111 orientation = orien_filter.get_orientation(accel_data, gyro_data, magnetic_data);
113 orien_file << orientation.m_ang;
115 cout << "Orientation Data\t" << orientation.m_ang << "\n\n";