fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 22:33:34 +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
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+5
View File
@@ -307,6 +307,11 @@ bool Ekf::update()
// the output observer always runs // the output observer always runs
calculateOutputStates(); 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 // 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) { if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) {
return true; return true;