From 17ad7071c35d9e6a19c2047a6700ad2a2f0f2c92 Mon Sep 17 00:00:00 2001 From: kevindsp Date: Fri, 12 Nov 2021 01:41:37 +0800 Subject: [PATCH] fix the quaternion normalization issue Be course of the numerical computing error . The normalization of the quaternion can't always equal to 1 precisely. It could occasionally trigger the error"attitude estimate no longer valid". So enlarge the threshold to 1e-6f. That keeps it silence. --- src/modules/commander/Commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index e3ddd00890..2fe39d4aed 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3778,7 +3778,7 @@ void Commander::estimator_check() && (fabsf(q(1)) <= 1.f) && (fabsf(q(2)) <= 1.f) && (fabsf(q(3)) <= 1.f); - const bool norm_in_tolerance = (fabsf(1.f - q.norm()) <= FLT_EPSILON); + const bool norm_in_tolerance = (fabsf(1.f - q.norm()) <= 1e-6f); const bool condition_attitude_valid = (hrt_elapsed_time(&attitude.timestamp) < 1_s) && norm_in_tolerance && no_element_larger_than_one;