Updating orientation sensor standalone test files

- Updating orientation sensor standalone test files after
restructuring or sensor fusion

Change-Id: I58519ba358290142d585f2d5aea9e667c61a4f33

diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp
index b9ff4bb..784ac3b 100644 (file)
--- a/src/sensor_fusion/test/orientation_sensor.cpp
+++ b/src/sensor_fusion/test/orientation_sensor.cpp
@@ -28,10 +28,6 @@ int sign_magnetic[] = {+1, +1, +1};
 float scale_accel = 1;
 float scale_gyro = 1150;
 float scale_magnetic = 1;
-
-int pitch_phase_compensation = -1;
-int roll_phase_compensation = -1;
-int azimuth_phase_compensation = -1;
 int magnetic_alignment_factor = -1;
 
 void pre_process_data(sensor_data<float> *data_out, sensor_data<float> *data_in, float *bias, int *sign, float scale)
@@ -53,9 +49,6 @@ void orientation_sensor::get_device_orientation(sensor_data<float> *accel_data,
        pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
        normalize(*magnetic_data);
 
-       orien_filter.m_pitch_phase_compensation = pitch_phase_compensation;
-       orien_filter.m_roll_phase_compensation = roll_phase_compensation;
-       orien_filter.m_azimuth_phase_compensation = azimuth_phase_compensation;
        orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor;
 
        orien_filter.get_device_orientation(accel_data, gyro_data, magnetic_data);

diff --git a/src/sensor_fusion/test/test_projects/orientation_sensor_test/orientation_sensor_main.cpp b/src/sensor_fusion/test/test_projects/orientation_sensor_test/orientation_sensor_main.cpp
index 53599d2..a35a6ce 100644 (file)
--- a/src/sensor_fusion/test/test_projects/orientation_sensor_test/orientation_sensor_main.cpp
+++ b/src/sensor_fusion/test/test_projects/orientation_sensor_test/orientation_sensor_main.cpp
@@ -26,6 +26,9 @@ using namespace std;
 
 #define ORIENTATION_DATA_PATH "../../../design/data/100ms/orientation/roll_pitch_yaw/"
 #define ORIENTATION_DATA_SIZE 1095
+int pitch_phase_compensation = -1;
+int roll_phase_compensation = -1;
+int azimuth_phase_compensation = -1;
 
 int main()
 {
@@ -81,9 +84,20 @@ int main()
 
                orien_sensor.get_device_orientation(&accel_data, &gyro_data, &magnetic_data);
 
-               orien_file << orien_sensor.orien_filter.m_orientation.m_ang;
+               orientation = orien_sensor.orien_filter.m_orientation;
 
-               cout << "Orientation angles\t" << orien_sensor.orien_filter.m_orientation.m_ang << "\n\n";
+               orientation = rad2deg(orientation);
+
+               orientation.m_ang.m_vec[0] *= pitch_phase_compensation;
+               orientation.m_ang.m_vec[1] *= roll_phase_compensation;
+               orientation.m_ang.m_vec[2] *= azimuth_phase_compensation; 
+
+               if (orientation.m_ang.m_vec[2] < 0)
+                       orientation.m_ang.m_vec[2] += 360;
+
+               orien_file << orientation.m_ang;
+
+               cout << "Orientation angles\t" << orientation.m_ang << "\n\n";
 
                cout << "Orientation matrix\t" << orien_sensor.orien_filter.m_rot_matrix.m_rot_mat << "\n\n";