rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

OP-1406 - Always feeds mag data even if invalid

Browse Source
This commit is contained in:
Alessio Morale 2014-08-03 11:40:55 +02:00
parent 869515b349
commit 84758a6731
1 changed files with 7 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
flight/modules/StateEstimation/filtermag.c
View File

@ -105,8 +105,9 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
IS_SET(state->updated, SENSORUPDATES_auxMag)) { IS_SET(state->updated, SENSORUPDATES_auxMag)) {
auxMagError = getMagError(this, state->auxMag); auxMagError = getMagError(this, state->auxMag);
// Handles alarms only if it will rely on aux mag only // Handles alarms only if it will rely on aux mag only
if (checkMagValidity(this, auxMagError, bool auxMagValid = checkMagValidity(this, auxMagError, (this->auxMagUsage == AUXMAGSETTINGS_USAGE_AUXONLY));
(this->auxMagUsage == AUXMAGSETTINGS_USAGE_AUXONLY))) { // if we are going to use Aux only, force the update even if mag is invalid
if (auxMagValid || (this->auxMagUsage == AUXMAGSETTINGS_USAGE_AUXONLY)) {
temp_mag[0] = state->auxMag[0]; temp_mag[0] = state->auxMag[0];
temp_mag[1] = state->auxMag[1]; temp_mag[1] = state->auxMag[1];
temp_mag[2] = state->auxMag[2]; temp_mag[2] = state->auxMag[2];
@ -123,7 +124,10 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
}*/ }*/
boardMagError = getMagError(this, state->boardMag); boardMagError = getMagError(this, state->boardMag);
// sets warning only if no mag data are available (aux is invalid or missing) // sets warning only if no mag data are available (aux is invalid or missing)
if (checkMagValidity(this, boardMagError, temp_status == MAGSTATUS_INVALID)) { bool boardMagValid = checkMagValidity(this, boardMagError, (temp_status == MAGSTATUS_INVALID));
// force it to be set to board mag value if no data has been feed to temp_mag yet.
// this works also as a failsafe in case aux mag stops feeding data.
if (boardMagValid || (temp_status == MAGSTATUS_INVALID)) {
temp_mag[0] += state->boardMag[0]; temp_mag[0] += state->boardMag[0];
temp_mag[1] += state->boardMag[1]; temp_mag[1] += state->boardMag[1];
temp_mag[2] += state->boardMag[2]; temp_mag[2] += state->boardMag[2];