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

EKF: Add check for NaN's on attitude states

Browse Source
This commit is contained in:
Paul Riseborough 2016-03-16 13:43:21 +11:00
parent 6b2e2dba90
commit e334a5dc57
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
EKF/ekf.cpp
View File

@ -307,6 +307,11 @@ bool Ekf::update()
// the output observer always runs
calculateOutputStates();
// check for NaN on attitude states
if (isnan(_state.quat_nominal(0)) || isnan(_output_new.quat_nominal(0))) {
return false;
}
// We don't have valid data to output until tilt and yaw alignment is complete
if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) {
return true;