diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-18 18:28:54 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-18 18:28:54 +0100 |
commit | bce67c6b036cddbe1542f910c6e605256283768f (patch) | |
tree | 0fc9487ce26dc97aa8d9aee56e43cb1d6ee3fa4a /src/modules/fw_att_pos_estimator/estimator.cpp | |
parent | 7cdb7291af52f2b60aa53607e7ec229fecf7497f (diff) | |
download | px4-firmware-bce67c6b036cddbe1542f910c6e605256283768f.tar.gz px4-firmware-bce67c6b036cddbe1542f910c6e605256283768f.tar.bz2 px4-firmware-bce67c6b036cddbe1542f910c6e605256283768f.zip |
Init / reinit improvements
Diffstat (limited to 'src/modules/fw_att_pos_estimator/estimator.cpp')
-rw-r--r-- | src/modules/fw_att_pos_estimator/estimator.cpp | 41 |
1 files changed, 40 insertions, 1 deletions
diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index fa81d4dfa..36b9f4491 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1651,7 +1651,7 @@ void StoreStates(uint64_t timestamp_ms) storeIndex = 0; } -void ResetStates() +void ResetStoredStates() { // reset all stored states memset(&storedStates[0][0], 0, sizeof(storedStates)); @@ -2032,6 +2032,45 @@ void ResetVelocity(void) } } + +/** + * Check the filter inputs and bound its operational state + * + * This check will reset the filter states if required + * due to a failure of consistency or timeout checks. + * it should be run after the measurement data has been + * updated, but before any of the fusion steps are + * executed. + */ +void CheckAndBound() +{ + + // Store the old filter state + bool currStaticMode = staticMode; + + // Reset the filter if the IMU data is too old + if (dtIMU > 0.2f) { + ResetVelocity(); + ResetPosition(); + ResetHeight(); + ResetStoredStates(); + + // that's all we can do here, return + return; + } + + // Check if we're on ground - this also sets static mode. + OnGroundCheck(); + + // Check if we switched between states + if (currStaticMode != staticMode) { + ResetVelocity(); + ResetPosition(); + ResetHeight(); + ResetStoredStates(); + } +} + void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) { float initialRoll, initialPitch; |