From cd5d6ad07f9a3fd3f51553ed5bb97bafe5835ded Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 18 Mar 2015 14:16:45 +0530 Subject: [PATCH] Fixing gyro data handling and cleanup - Fixing code related to input gyro data handling. - cleanup of orientation filter code. Change-Id: I9efd0c69ed7c2ab0dcc334e460723d9f8cdea668 --- src/sensor_fusion/orientation_filter.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index 47c7b34..5aa4968 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -88,9 +88,9 @@ inline void orientation_filter::initialize_sensor_data(const sensor_datam_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::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::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; -- 2.7.4