From fcc3f519d87207638c33aff4aa97f7fa32c224b5 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 6 Jun 2013 17:59:27 +0200 Subject: [PATCH] filterekf: Remember critical divergence and set error flag to prevent (re)arming so the user will notice --- flight/modules/StateEstimation/filterekf.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index e0c9ed36a..8003c1f13 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -427,6 +427,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) { if (invalid_var(vardata.P[t])) { INSResetP(this->ekfConfiguration.P); + this->init_stage = -1; break; } } @@ -434,7 +435,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // all sensor data has been used, reset! this->work.updated = 0; - return 0; + if (this->init_stage < 0) { + return 2; + } else { + return 0; + } } // check for invalid variance values