dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #115 from PX4/fix_isfinite_again 

ekf: another stab at fixing isfinite for all
This commit is contained in:
Paul Riseborough 2016-04-29 09:12:59 +10:00
parent 22ad87599c f8a48f9cc5
commit 4c9c102597
1 changed files with 16 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
EKF/ekf.cpp
View File

@ -42,6 +42,21 @@
#include "ekf.h"
#include "mathlib.h"
#ifndef __PX4_QURT
#if defined(__cplusplus) && !defined(__PX4_NUTTX)
#include <cmath>
#define ISFINITE(x) std::isfinite(x)
#else
#define ISFINITE(x) isfinite(x)
#endif
#endif
#if defined(__PX4_QURT)
// Missing math.h defines
#define ISFINITE(x) __builtin_isfinite(x)
#endif
Ekf::Ekf():
_filter_initialised(false),
_earth_rate_initialised(false),
@ -343,7 +358,7 @@ bool Ekf::update()
calculateOutputStates();
// check for NaN or inf on attitude states
if (!std::isfinite(_state.quat_nominal(0)) || !std::isfinite(_output_new.quat_nominal(0))) {
if (!ISFINITE(_state.quat_nominal(0)) || !ISFINITE(_output_new.quat_nominal(0))) {
return false;
}