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)) {
auxMagError = getMagError(this, state->auxMag);
// Handles alarms only if it will rely on aux mag only
if (checkMagValidity(this, auxMagError,
(this->auxMagUsage == AUXMAGSETTINGS_USAGE_AUXONLY))) {
bool auxMagValid = checkMagValidity(this, auxMagError, (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[1] = state->auxMag[1];
temp_mag[2] = state->auxMag[2];
@ -123,7 +124,10 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
}*/
boardMagError = getMagError(this, state->boardMag);
// 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[1] += state->boardMag[1];
temp_mag[2] += state->boardMag[2];