diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-24 09:04:35 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-24 09:04:35 +0100 |
commit | 01f33df70718b7a7dba342fb5920d91b3cd83c09 (patch) | |
tree | 05fd21d12f0ab996eaf7499e012f0584be3e2650 /src/modules/fw_att_pos_estimator/estimator.h | |
parent | 43a672988dca59b9313482996773389cb67c5b1d (diff) | |
download | px4-firmware-01f33df70718b7a7dba342fb5920d91b3cd83c09.tar.gz px4-firmware-01f33df70718b7a7dba342fb5920d91b3cd83c09.tar.bz2 px4-firmware-01f33df70718b7a7dba342fb5920d91b3cd83c09.zip |
Added EKF filter health status reporting, added dynamic in-air reset.
Diffstat (limited to 'src/modules/fw_att_pos_estimator/estimator.h')
-rw-r--r-- | src/modules/fw_att_pos_estimator/estimator.h | 29 |
1 files changed, 28 insertions, 1 deletions
diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 256aff771..55e6eb12e 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -134,6 +134,22 @@ enum GPS_FIX { GPS_FIX_3D = 3 }; +struct ekf_status_report { + bool velHealth; + bool posHealth; + bool hgtHealth; + bool velTimeout; + bool posTimeout; + bool hgtTimeout; + uint32_t velFailTime; + uint32_t posFailTime; + uint32_t hgtFailTime; + float states[n_states]; + bool statesNaN; + bool covarianceNaN; + bool kalmanGainsNaN; +}; + void UpdateStrapdownEquationsNED(); void CovariancePrediction(float dt); @@ -188,11 +204,22 @@ void ConstrainStates(); void ForceSymmetry(); -void CheckAndBound(); +int CheckAndBound(); void ResetPosition(); void ResetVelocity(); +void ZeroVariables(); + +void GetFilterState(struct ekf_status_report *state); + +void GetLastErrorState(struct ekf_status_report *last_error); + +bool StatesNaN(struct ekf_status_report *err_report); +void FillErrorReport(struct ekf_status_report *err); + +void InitializeDynamic(float (&initvelNED)[3]); + uint32_t millis(); |