fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 06:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Use isAllFinite() in all places that check finiteness on entire vectors or matrices

Browse Source
This commit is contained in:
Matthias Grob
2022-10-19 19:09:20 +02:00
parent 93de9567a5
commit 5ca28dd6dc
38 changed files with 128 additions and 252 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+2 -6
View File
@@ -441,9 +441,7 @@ bool AttitudeEstimatorQ::init_attitude_q()
_q.normalize();
if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
_q.length() > 0.95f && _q.length() < 1.05f) {
if (_q.isAllFinite() && _q.length() > 0.95f && _q.length() < 1.05f) {
_initialized = true;
} else {
@@ -552,9 +550,7 @@ bool AttitudeEstimatorQ::update(float dt)
// Normalize quaternion
_q.normalize();
if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
if (!_q.isAllFinite()) {
// Reset quaternion to last good state
_q = q_last;
_rates.zero();