diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-22 02:24:29 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-22 02:24:29 +0200 |
commit | 39a0d4e54db1252678eb8f4ebc872c589da2d9e7 (patch) | |
tree | c8a3ddff03c1ce6a195a20b3310b3608142e39d1 /src/modules | |
parent | 904ada124baea8ef744535053a0c3b40871565e3 (diff) | |
download | px4-firmware-39a0d4e54db1252678eb8f4ebc872c589da2d9e7.tar.gz px4-firmware-39a0d4e54db1252678eb8f4ebc872c589da2d9e7.tar.bz2 px4-firmware-39a0d4e54db1252678eb8f4ebc872c589da2d9e7.zip |
Better error handling / reporting in filter
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator.cpp | 20 |
1 files changed, 11 insertions, 9 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 99de161aa..14761831c 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2243,18 +2243,21 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { err_report->statesNaN = true; ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", summedDelAng.x, summedDelAng.y, summedDelAng.z); err = true; + goto out; } // delta angles if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { err_report->statesNaN = true; ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", correctedDelAng.x, correctedDelAng.y, correctedDelAng.z); err = true; + goto out; } // delta angles if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { err_report->statesNaN = true; ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", summedDelVel.x, summedDelVel.y, summedDelVel.z); err = true; + goto out; } // delta velocities // check all states and covariance matrices @@ -2264,28 +2267,24 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { err_report->covarianceNaN = true; err = true; - } // intermediate result used for covariance updates - if (err) { ekf_debug("KH NaN"); - } + goto out; + } // intermediate result used for covariance updates if (!isfinite(KHP[i][j])) { err_report->covarianceNaN = true; err = true; - } // intermediate result used for covariance updates - if (err) { ekf_debug("KHP NaN"); - } + goto out; + } // intermediate result used for covariance updates if (!isfinite(P[i][j])) { err_report->covarianceNaN = true; err = true; - } // covariance matrix - if (err) { ekf_debug("P NaN"); - } + } // covariance matrix } if (!isfinite(Kfusion[i])) { @@ -2293,6 +2292,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { err_report->kalmanGainsNaN = true; ekf_debug("Kfusion NaN"); err = true; + goto out; } // Kalman gains if (!isfinite(states[i])) { @@ -2300,9 +2300,11 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { err_report->statesNaN = true; ekf_debug("states NaN: i: %u val: %f", i, states[i]); err = true; + goto out; } // state matrix } +out: if (err) { FillErrorReport(err_report); } |