ekf_hgt: call specific height reset function instead of generic one

- Also use the delayed current data instead of newest that might not be
available (gps buffer is sometimes empty if the dt between samples is
larger than the delayed horizon).

- Separate "baro fault" from "baro intermittent": intermittent is a
  temporary failure and should prevent from switching to baro right now,
  but "fault" means that it should never be used anymore

- In case of height timeout, check for metrics but not for consistency
  as the filter is likely to have diverged already.
This commit is contained in:
bresch
2022-02-22 11:49:25 +01:00
committed by Mathieu Bresciani
parent cba73585e1
commit edabfd2f0e
4 changed files with 145 additions and 166 deletions
+10 -3
View File
@@ -542,8 +542,9 @@ private:
terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
// height sensor status
bool _baro_hgt_faulty{true}; ///< true if valid baro data is unavailable for use
bool _gps_intermittent{true}; ///< true if into the buffer is intermittent
bool _baro_hgt_faulty{false}; ///< true if baro data have been declared faulty TODO: move to fault flags
bool _baro_hgt_intermittent{true}; ///< true if data into the buffer is intermittent
bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent
// imu fault status
uint64_t _time_bad_vert_accel{0}; ///< last time a bad vertical accel was detected (uSec)
@@ -656,7 +657,13 @@ private:
void resetVerticalPositionTo(float new_vert_pos);
void resetHeight();
void resetHeightToBaro();
void resetHeightToGps();
void resetHeightToRng();
void resetHeightToEv();
void resetVerticalVelocityToGps();
void resetVerticalVelocityToZero();
// fuse optical flow line of sight rate measurements
void fuseOptFlow();