Fixing gyro data handling and cleanup

- Fixing code related to input gyro data handling.
- cleanup of orientation filter code.

Change-Id: I9efd0c69ed7c2ab0dcc334e460723d9f8cdea668

diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp
index 47c7b34..5aa4968 100644 (file)
--- a/src/sensor_fusion/orientation_filter.cpp
+++ b/src/sensor_fusion/orientation_filter.cpp
@@ -88,9 +88,9 @@ inline void orientation_filter<TYPE>::initialize_sensor_data(const sensor_data<T
                m_gyro_dt = sample_interval_gyro * US2S;
                m_gyro.m_time_stamp = gyro->m_time_stamp;
 
-               m_gyro.m_data = m_gyro.m_data * (TYPE) PI;
+               m_gyro.m_data = gyro->m_data * (TYPE) PI;
 
-               m_gyro.m_data = gyro->m_data - m_bias_correction;
+               m_gyro.m_data = m_gyro.m_data - m_bias_correction;
        }
 
        if (magnetic != NULL) {
@@ -226,7 +226,7 @@ inline void orientation_filter<TYPE>::time_update()
        m_quat_9axis = quat_output;
        m_quat_gaming_rv = m_quat_9axis;
 
-       orientation = quat2euler(quat_output);
+       m_orientation = quat2euler(quat_output);
 
        m_rot_matrix = quat2rot_mat(m_quat_driv);
 
@@ -292,6 +292,8 @@ inline void orientation_filter<TYPE>::time_update_gaming_rv()
 
        quat_output = phase_correction(m_quat_driv, m_quat_aid);
 
+       m_orientation = quat2euler(quat_output);
+
        quat_error = m_quat_aid * m_quat_driv;
 
        euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI;