Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 22:58:10 +08:00
Code Issues Actions 15 Packages Projects Releases Wiki Activity

fix the quaternion normalization issue

Browse Source 
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.
This commit is contained in:
kevindsp
2021-11-12 01:41:37 +08:00
committed by Mathieu Bresciani
parent 6a60fba96d
commit 17ad7071c3
1 changed files with 1 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+1 -1
View File
@@ -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;