diff options
| -rw-r--r-- | src/modules/fw_att_pos_estimator/estimator.cpp | 41 | ||||
| -rw-r--r-- | src/modules/fw_att_pos_estimator/estimator.h | 8 | ||||
| -rw-r--r-- | src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 21 |
3 files changed, 68 insertions, 2 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; diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index b8b218e39..256aff771 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -158,6 +158,8 @@ void StoreStates(uint64_t timestamp_ms); // recall stste vector stored at closest time to the one specified by msec void RecallStates(float statesForFusion[n_states], uint64_t msec); +void ResetStoredStates(); + void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); void calcEarthRateNED(Vector3f &omega, float latitude); @@ -186,5 +188,11 @@ void ConstrainStates(); void ForceSymmetry(); +void CheckAndBound(); + +void ResetPosition(); + +void ResetVelocity(); + uint32_t millis(); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index df2608f83..8b41e7479 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -604,6 +604,9 @@ FixedwingEstimator::task_main() orb_check(_gps_sub, &gps_updated); if (gps_updated) { + + uint64_t last_gps = _gps.timestamp_position; + orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); @@ -612,6 +615,14 @@ FixedwingEstimator::task_main() newDataGps = false; } else { + + /* check if we had a GPS outage for a long time */ + if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) { + ResetPosition(); + ResetVelocity(); + ResetStoredStates(); + } + /* fuse GPS updates */ //_gps.timestamp / 1e3; @@ -688,6 +699,12 @@ FixedwingEstimator::task_main() /** + * CHECK IF THE INPUT DATA IS SANE + */ + CheckAndBound(); + + + /** * PART TWO: EXECUTE THE FILTER **/ @@ -715,7 +732,9 @@ FixedwingEstimator::task_main() _local_pos.ref_timestamp = _gps.timestamp_position; // Store - _baro_ref = baroHgt; + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + _baro_ref = _baro.altitude; + baroHgt = _baro.altitude - _baro_ref; _baro_gps_offset = baroHgt - alt; // XXX this is not multithreading safe |