rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

filterekf: Remember critical divergence and set error flag to prevent (re)arming so the user will notice

Browse Source
This commit is contained in:
Corvus Corax 2013-06-06 17:59:27 +02:00
parent b0db191703
commit fcc3f519d8
1 changed files with 6 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
flight/modules/StateEstimation/filterekf.c
View File

@ -427,6 +427,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) { for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) {
if (invalid_var(vardata.P[t])) { if (invalid_var(vardata.P[t])) {
INSResetP(this->ekfConfiguration.P); INSResetP(this->ekfConfiguration.P);
this->init_stage = -1;
break; break;
} }
} }
@ -434,7 +435,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
// all sensor data has been used, reset! // all sensor data has been used, reset!
this->work.updated = 0; this->work.updated = 0;
return 0; if (this->init_stage < 0) {
return 2;
} else {
return 0;
}
} }
// check for invalid variance values // check for invalid variance values