diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-04 09:52:21 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-04 09:52:21 +0200 |
| commit | cf67e810a4e21b97a12862fd55572b5d025156b5 (patch) | |
| tree | af6f17583e3c42e4262b0490bfb3c662a628d540 /src/modules | |
| parent | 77a4711ff992b4cbda03f539dd2e7bd927b76e95 (diff) | |
| download | px4-firmware-cf67e810a4e21b97a12862fd55572b5d025156b5.tar.gz px4-firmware-cf67e810a4e21b97a12862fd55572b5d025156b5.tar.bz2 px4-firmware-cf67e810a4e21b97a12862fd55572b5d025156b5.zip | |
More detailed estimator status feedback
Diffstat (limited to 'src/modules')
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator.cpp | 10 | ||||
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator.h | 7 |
2 files changed, 11 insertions, 6 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index bfb2007af..5ac6b079f 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2283,21 +2283,21 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { // check all integrators if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) { - err_report->statesNaN = true; + err_report->angNaN = true; ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z); err = true; goto out; } // delta angles if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { - err_report->statesNaN = true; + err_report->angNaN = true; ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); err = true; goto out; } // delta angles if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { - err_report->statesNaN = true; + err_report->summedDelVelNaN = true; ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z); err = true; goto out; @@ -2308,7 +2308,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { for (unsigned j = 0; j < n_states; j++) { if (!isfinite(KH[i][j])) { - err_report->covarianceNaN = true; + err_report->KHNaN = true; err = true; ekf_debug("KH NaN"); goto out; @@ -2316,7 +2316,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(KHP[i][j])) { - err_report->covarianceNaN = true; + err_report->KHPNaN = true; err = true; ekf_debug("KHP NaN"); goto out; diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index ec82896fb..401462923 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -66,9 +66,14 @@ struct ekf_status_report { uint32_t posFailTime; uint32_t hgtFailTime; float states[n_states]; - bool statesNaN; + bool angNaN; + bool summedDelVelNaN; + bool KHNaN; + bool KHPNaN; + bool PNaN; bool covarianceNaN; bool kalmanGainsNaN; + bool statesNaN; }; class AttPosEKF { |