From b1ceb13a2aa80b121e81ab0d406ff45a90514f99 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 11 Mar 2015 17:36:41 +0530 Subject: [PATCH] Updating orientation filter to support new configuration handling - Updated/cleanup of orientation filter to support new configuration handling Change-Id: I5a8b7001db7edbf48d88945937b7eede1f31fa37 --- src/sensor_fusion/orientation_filter.cpp | 16 +++++++++------- src/sensor_fusion/orientation_filter.h | 4 +--- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index ae43948..47c7b34 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -61,9 +61,6 @@ orientation_filter::orientation_filter() m_var_pitch = vec; m_var_azimuth = vec; - m_pitch_phase_compensation = 1; - m_roll_phase_compensation = 1; - m_azimuth_phase_compensation = 1; m_magnetic_alignment_factor = 1; m_gyro.m_time_stamp = 0; @@ -231,10 +228,6 @@ inline void orientation_filter::time_update() orientation = quat2euler(quat_output); - m_orientation.m_ang.m_vec[0] = orientation.m_ang.m_vec[0] * m_pitch_phase_compensation; - m_orientation.m_ang.m_vec[1] = orientation.m_ang.m_vec[1] * m_roll_phase_compensation; - m_orientation.m_ang.m_vec[2] = orientation.m_ang.m_vec[2] * m_azimuth_phase_compensation; - m_rot_matrix = quat2rot_mat(m_quat_driv); quat_error = m_quat_aid * m_quat_driv; @@ -378,6 +371,15 @@ void orientation_filter::get_device_orientation(const sensor_data *a time_update_gaming_rv(); measurement_update(); + + if (magnetic == NULL) { + m_quaternion = m_quat_gaming_rv; + } else { + m_quaternion = m_quat_9axis; + } + + } else { + m_quaternion = m_quat_aid; } } diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h index 8a1351b..c89a6b8 100644 --- a/src/sensor_fusion/orientation_filter.h +++ b/src/sensor_fusion/orientation_filter.h @@ -64,11 +64,9 @@ public: euler_angles m_orientation; quaternion m_quat_9axis; quaternion m_quat_gaming_rv; + quaternion m_quaternion; TYPE m_gyro_dt; - int m_pitch_phase_compensation; - int m_roll_phase_compensation; - int m_azimuth_phase_compensation; int m_magnetic_alignment_factor; orientation_filter(); -- 2.7.4