diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-11 22:44:54 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-11 22:44:54 +0200 |
commit | 9c63aba9a72083afe1e5f818c6559760c5b6b1cc (patch) | |
tree | ddb17cdfc924b18e2bf529b607bdaaa14ba98a42 /src/modules | |
parent | 66e840ebd784c376aeb8c447541d17ab3fa9cf0f (diff) | |
download | px4-firmware-9c63aba9a72083afe1e5f818c6559760c5b6b1cc.tar.gz px4-firmware-9c63aba9a72083afe1e5f818c6559760c5b6b1cc.tar.bz2 px4-firmware-9c63aba9a72083afe1e5f818c6559760c5b6b1cc.zip |
Ensure NaN check is executed before any parts of the filter are run. Fix GPS velocity reset for case of initialized GPS
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 26 |
1 files 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 5d768b73d..bc5f709e5 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; |