From 530996c4a54010f2217862ac891207cb796ffd7f Mon Sep 17 00:00:00 2001 From: Akhil Date: Wed, 30 Nov 2016 22:16:08 +0900 Subject: [PATCH] [BugFix] Reset Sensor Fusion Backend if it returns NaN because of inconsitent input and high error Change-Id: I29fe84898d32f710cb914748d3111edfb2da38b7 Signed-off-by: Akhil --- src/sensor/rotation_vector/fusion_base.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/sensor/rotation_vector/fusion_base.cpp b/src/sensor/rotation_vector/fusion_base.cpp index 51e45d3..89b5a44 100644 --- a/src/sensor/rotation_vector/fusion_base.cpp +++ b/src/sensor/rotation_vector/fusion_base.cpp @@ -100,5 +100,10 @@ void fusion_base::store_orientation(void) m_y = m_orientation_filter.m_quaternion.m_quat.m_vec[1]; m_z = m_orientation_filter.m_quaternion.m_quat.m_vec[2]; m_w = m_orientation_filter.m_quaternion.m_quat.m_vec[3]; + + if (std::isnan(m_x) || std::isnan(m_y) || std::isnan(m_z) || std::isnan(m_w)) { + m_timestamp = 0; + m_orientation_filter = orientation_filter(); + } clear(); } -- 2.7.4