diff options
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 7 |
1 files changed, 6 insertions, 1 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 b7e118d47..8fdc40d41 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 @@ -1221,7 +1221,12 @@ FixedwingEstimator::task_main() // We're apparently initialized in this case now // check (and reset the filter as needed) - (void)check_filter_state(); + int check = check_filter_state(); + + if (check) { + // Let the system re-initialize itself + continue; + } // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); |