src/sensor/rotation_vector/test/test_projects/orientation_sensor_test/orientation_sensor_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_sensor.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 int pitch_phase_compensation = -1;
  30 int roll_phase_compensation = -1;
  31 int azimuth_phase_compensation = -1;
  32
  33 int main()
  34 {
  35         int data_available = ORIENTATION_DATA_SIZE;
  36         ifstream accel_in, gyro_in, mag_in;
  37         ofstream orien_file;
  38         string line_accel, line_gyro, line_magnetic;
  39         float sdata[3];
  40         unsigned long long time_stamp;
  41         euler_angles<float> orientation;
  42         rotation_matrix<float> orientation_mat;
  43         quaternion<float> orientation_9axis_quat;
  44         quaternion<float> orientation_geomagnetic_quat;
  45         quaternion<float> orientation_gaming_quat;
  46         orientation_sensor orien_sensor;
  47
  48         accel_in.open(((string)ORIENTATION_DATA_PATH + (string)"accel.txt").c_str());
  49         gyro_in.open(((string)ORIENTATION_DATA_PATH + (string)"gyro.txt").c_str());
  50         mag_in.open(((string)ORIENTATION_DATA_PATH + (string)"magnetic.txt").c_str());
  51
  52         orien_file.open(((string)"orientation.txt").c_str());
  53
  54         char *token = NULL;
  55
  56         while (data_available-- > 0)
  57         {
  58                 getline(accel_in, line_accel);
  59                 sdata[0] = strtof(line_accel.c_str(), &token);
  60                 sdata[1] = strtof(token, &token);
  61                 sdata[2] = strtof(token, &token);
  62                 time_stamp = strtoull(token, NULL, 10);
  63                 sensor_data<float> accel_data(sdata[0], sdata[1], sdata[2], time_stamp);
  64
  65                 cout << "Accel Data\t" << accel_data.m_data << "\t Time Stamp\t" << accel_data.m_time_stamp << "\n\n";
  66
  67                 getline(gyro_in, line_gyro);
  68                 sdata[0] = strtof(line_gyro.c_str(), &token);
  69                 sdata[1] = strtof(token, &token);
  70                 sdata[2] = strtof(token, &token);
  71                 time_stamp = strtoull(token, NULL, 10);
  72                 sensor_data<float> gyro_data(sdata[0], sdata[1], sdata[2], time_stamp);
  73
  74                 cout << "Gyro Data\t" << gyro_data.m_data << "\t Time Stamp\t" << gyro_data.m_time_stamp << "\n\n";
  75
  76                 getline(mag_in, line_magnetic);
  77                 sdata[0] = strtof(line_magnetic.c_str(), &token);
  78                 sdata[1] = strtof(token, &token);
  79                 sdata[2] = strtof(token, &token);
  80                 time_stamp = strtoull(token, NULL, 10);
  81                 sensor_data<float> magnetic_data(sdata[0], sdata[1], sdata[2], time_stamp);
  82
  83                 cout << "Magnetic Data\t" << magnetic_data.m_data << "\t Time Stamp\t" << magnetic_data.m_time_stamp << "\n\n";
  84
  85                 orien_sensor.get_device_orientation(&accel_data, &gyro_data, &magnetic_data);
  86
  87                 orientation = orien_sensor.orien_filter.m_orientation;
  88
  89                 cout << "Gyro Bias in radians\t" << orien_sensor.orien_filter.m_gyro_bias;
  90
  91                 orientation = rad2deg(orientation);
  92
  93                 orientation.m_ang.m_vec[0] *= pitch_phase_compensation;
  94                 orientation.m_ang.m_vec[1] *= roll_phase_compensation;
  95                 orientation.m_ang.m_vec[2] *= azimuth_phase_compensation;
  96
  97                 if (orientation.m_ang.m_vec[2] < 0)
  98                         orientation.m_ang.m_vec[2] += 360;
  99
 100                 orien_file << orientation.m_ang;
 101
 102                 orientation = orien_sensor.orien_filter.m_orientation;
 103
 104                 cout << "Orientation angles\t" << orientation.m_ang << "\n\n";
 105
 106                 cout << "Orientation matrix\t" << orien_sensor.orien_filter.m_rot_matrix.m_rot_mat << "\n\n";
 107
 108                 cout << "Orientation 9-axis quaternion\t" << orien_sensor.orien_filter.m_quat_9axis.m_quat << "\n\n";
 109
 110                 cout << "Orientation geomagnetic quaternion\t" << orien_sensor.orien_filter.m_quat_aid.m_quat << "\n\n";
 111
 112                 cout << "Orientation gaming quaternion\t" << orien_sensor.orien_filter.m_quat_gaming_rv.m_quat << "\n\n";
 113         }
 114
 115         accel_in.close();
 116         gyro_in.close();
 117         mag_in.close();
 118         orien_file.close();
 119
 120         return 0;
 121 }