diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-23 16:03:24 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-23 16:03:24 +0200 |
| commit | bcca3cae748ffea2df51907992e4a3c7ca673fd2 (patch) | |
| tree | 4c2a12a9db1590b85d33180d31a7fda4824645ac /src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | |
| parent | 1143cdbadfdf5a00765373cfa2ad84f6d23f2c51 (diff) | |
| download | px4-firmware-bcca3cae748ffea2df51907992e4a3c7ca673fd2.tar.gz px4-firmware-bcca3cae748ffea2df51907992e4a3c7ca673fd2.tar.bz2 px4-firmware-bcca3cae748ffea2df51907992e4a3c7ca673fd2.zip | |
Run full update straight after reset, filter wind speed dynamically
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp')
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 22 |
1 files changed, 2 insertions, 20 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 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 |