Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

Commander: change attitude quaternion check to avoid numerical issues

Browse Source
This commit is contained in:
bresch
2021-08-04 16:21:09 +02:00
committed by Daniel Agar
parent fb4ac0f08c
commit 741f9c6d1a
1 changed files with 8 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+8 -2
View File
@@ -3880,9 +3880,15 @@ void Commander::estimator_check()
vehicle_attitude_s attitude{};
_vehicle_attitude_sub.copy(&attitude);
const matrix::Quatf q{attitude.q};
const matrix::Quatf q_norm{q.unit()};
const bool no_element_larger_than_one = (fabsf(q(0)) <= 1.f)
&& (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);
_status_flags.condition_attitude_valid = (hrt_elapsed_time(&attitude.timestamp) < 1_s)
&& (matrix::Quatf(q - q_norm).length() < FLT_EPSILON);
&& norm_in_tolerance
&& no_element_larger_than_one;
// angular velocity
vehicle_angular_velocity_s angular_velocity{};