From bcca3cae748ffea2df51907992e4a3c7ca673fd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 Aug 2014 16:03:24 +0200 Subject: Run full update straight after reset, filter wind speed dynamically --- .../ekf_att_pos_estimator_main.cpp | 22 ++-------------------- 1 file changed, 2 insertions(+), 20 deletions(-) (limited to 'src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp') 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 c384b2566..b7e118d47 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 @@ -1220,30 +1220,12 @@ FixedwingEstimator::task_main() } 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; - } + // check (and reset the filter as needed) + (void)check_filter_state(); // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); - #if 0 - // debug code - could be tunred into a filter mnitoring/watchdog function - float tempQuat[4]; - - for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j]; - - quat2eul(eulerEst, tempQuat); - for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j]; - - if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi; - - if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi; - - #endif // store the predicted states for subsequent use by measurement fusion _ekf->StoreStates(IMUmsec); // Check if on ground - status is used by covariance prediction -- cgit v1.2.3