[BugFix] Reset Sensor Fusion Backend if it returns NaN because of inconsitent input...

Change-Id: I29fe84898d32f710cb914748d3111edfb2da38b7
Signed-off-by: Akhil <akhil.kedia@samsung.com>

diff --git a/src/sensor/rotation_vector/fusion_base.cpp b/src/sensor/rotation_vector/fusion_base.cpp
index 51e45d3..89b5a44 100644 (file)
--- 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<float>();
+       }
        clear();
 }