Restructuring get_device_orientation flow

- restructuring orientation_filter::get_device_orientation flow for easier maintenance
- cleanup

Change-Id: Ie1bf8025d462d71146db2466b67a3e9f8585e31c

diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp
index 5aa4968..ae88a46 100644 (file)
--- a/src/sensor_fusion/orientation_filter.cpp
+++ b/src/sensor_fusion/orientation_filter.cpp
@@ -359,29 +359,35 @@ void orientation_filter<TYPE>::get_device_orientation(const sensor_data<TYPE> *a
 {
        initialize_sensor_data(accel, gyro, magnetic);
 
-       if (magnetic != NULL)
+       if (gyro != NULL && magnetic != NULL) {
+
                orientation_triad_algorithm();
-       else if (gyro != NULL)
-               compute_accel_orientation();
 
-       if (gyro != NULL) {
                compute_covariance();
 
-               if (magnetic != NULL)
-                       time_update();
-               else
-                       time_update_gaming_rv();
+               time_update();
 
                measurement_update();
 
-               if (magnetic == NULL) {
-                       m_quaternion = m_quat_gaming_rv;
-               } else {
-                       m_quaternion = m_quat_9axis;
-               }
+               m_quaternion = m_quat_9axis;
+
+       } else if (!gyro) {
+
+               orientation_triad_algorithm();
 
-       } else {
                m_quaternion = m_quat_aid;
+
+       } else if (!magnetic) {
+
+               compute_accel_orientation();
+
+               compute_covariance();
+
+               time_update_gaming_rv();
+
+               measurement_update();
+
+               m_quaternion = m_quat_gaming_rv;
        }
 }
 

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 a35a6ce..2710332 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
@@ -90,7 +90,7 @@ int main()
 
                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; 
+               orientation.m_ang.m_vec[2] *= azimuth_phase_compensation;
 
                if (orientation.m_ang.m_vec[2] < 0)
                        orientation.m_ang.m_vec[2] += 360;

diff --git a/virtual_sensors.xml b/virtual_sensors.xml
index 8371d60..3210e4f 100755 (executable)
--- a/virtual_sensors.xml
+++ b/virtual_sensors.xml
@@ -131,7 +131,7 @@
                        <DEFAULT_SAMPLING_TIME value="100" />
                </GAMING_ROTATION_VECTOR>
        </DEVICE>
-       
+
        <DEVICE type="Mobile-RD-PQ_V2">
                <FUSION>
                        <NAME value="FUSION_SENSOR" />