From 9c63aba9a72083afe1e5f818c6559760c5b6b1cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 22:44:54 +0200 Subject: [PATCH] Ensure NaN check is executed before any parts of the filter are run. Fix GPS velocity reset for case of initialized GPS --- .../ekf_att_pos_estimator_main.cpp | 26 +++++++++++-------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 5d768b73dd..bc5f709e58 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1140,6 +1140,20 @@ FixedwingEstimator::task_main() float initVelNED[3]; + // Guard before running any parts of the filter + // of NaN / invalid values + if (_ekf->statesInitialised) { + + // We're apparently initialized in this case now + + int check = check_filter_state(); + + if (check) { + // Let the system re-initialize itself + continue; + } + } + /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { @@ -1204,16 +1218,6 @@ FixedwingEstimator::task_main() _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); } else if (_ekf->statesInitialised) { - // We're apparently initialized in this case now - - int check = check_filter_state(); - - if (check) { - // Let the system re-initialize itself - continue; - } - - // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); #if 0 @@ -1281,7 +1285,7 @@ FixedwingEstimator::task_main() // run the fusion step _ekf->FuseVelposNED(); - } else if (_ekf->statesInitialised) { + } else if (!_gps_initialized && _ekf->statesInitialised) { // Convert GPS measurements to Pos NE, hgt and Vel NED _ekf->velNED[0] = 0.0f; _ekf->velNED[1] = 0.0f;