From 011c5987936f3a36ec5f164953e37b29ef0050af Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 8 Apr 2014 18:31:11 +0200 Subject: [PATCH] OP-1296 only use GPS in filterchains when explicitly requested disallow arming when GPS is needed for initialization until lock is acquired --- flight/modules/Attitude/revolution/attitude.c | 2 +- flight/modules/StateEstimation/filterbaro.c | 60 +++++++++++++++---- .../StateEstimation/inc/stateestimation.h | 1 + .../modules/StateEstimation/stateestimation.c | 45 +++++++++----- shared/uavobjectdefinition/revosettings.xml | 2 +- 5 files changed, 83 insertions(+), 27 deletions(-) diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index d502d1c8b..9621e41d7 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -278,7 +278,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters) case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: ret_val = updateAttitudeComplementary(first_run); break; - case REVOSETTINGS_FUSIONALGORITHM_INS13OUTDOOR: + case REVOSETTINGS_FUSIONALGORITHM_INS13GPSOUTDOOR: ret_val = updateAttitudeINSGPS(first_run, true); break; case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR: diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c index a44927392..4715d7762 100644 --- a/flight/modules/StateEstimation/filterbaro.c +++ b/flight/modules/StateEstimation/filterbaro.c @@ -36,7 +36,7 @@ // Private constants -#define STACK_REQUIRED 64 +#define STACK_REQUIRED 128 #define INIT_CYCLES 100 // Private types @@ -44,30 +44,59 @@ struct data { float baroOffset; float baroGPSOffsetCorrectionAlpha; float baroAlt; + float gpsAlt; int16_t first_run; + bool useGPS; }; // Private variables // Private functions -static int32_t init(stateFilter *self); +static int32_t initwithgps(stateFilter *self); +static int32_t initwithoutgps(stateFilter *self); +static int32_t maininit(stateFilter *self); static int32_t filter(stateFilter *self, stateEstimation *state); int32_t filterBaroInitialize(stateFilter *handle) { - handle->init = &init; + handle->init = &initwithgps; handle->filter = &filter; handle->localdata = pvPortMalloc(sizeof(struct data)); return STACK_REQUIRED; } -static int32_t init(stateFilter *self) +int32_t filterBaroiInitialize(stateFilter *handle) +{ + handle->init = &initwithoutgps; + handle->filter = &filter; + handle->localdata = pvPortMalloc(sizeof(struct data)); + return STACK_REQUIRED; +} + +static int32_t initwithgps(stateFilter *self) +{ + struct data *this = (struct data *)self->localdata; + + this->useGPS = 1; + return maininit(self); +} + +static int32_t initwithoutgps(stateFilter *self) +{ + struct data *this = (struct data *)self->localdata; + + this->useGPS = 0; + return maininit(self); +} + +static int32_t maininit(stateFilter *self) { struct data *this = (struct data *)self->localdata; this->baroOffset = 0.0f; + this->gpsAlt = 0.0f; this->first_run = INIT_CYCLES; RevoSettingsInitialize(); @@ -81,17 +110,29 @@ static int32_t filter(stateFilter *self, stateEstimation *state) struct data *this = (struct data *)self->localdata; if (this->first_run) { + // Make sure initial location is initialized properly before continuing + if (this->useGPS && IS_SET(state->updated, SENSORUPDATES_pos)) { + if (this->first_run == INIT_CYCLES) { + this->gpsAlt = state->pos[2]; + this->first_run--; + } + } // Initialize to current altitude reading at initial location if (IS_SET(state->updated, SENSORUPDATES_baro)) { - this->baroOffset = ((float)(INIT_CYCLES)-this->first_run) / (float)(INIT_CYCLES)*this->baroOffset + (this->first_run / (float)(INIT_CYCLES)) * state->baro[0]; - this->baroAlt = 0; - this->first_run--; + if (this->first_run < INIT_CYCLES || !this->useGPS) { + this->baroOffset = (((float)(INIT_CYCLES)-this->first_run) / (float)(INIT_CYCLES)) * this->baroOffset + (this->first_run / (float)(INIT_CYCLES)) * (state->baro[0] + this->gpsAlt); + this->baroAlt = state->baro[0]; + this->first_run--; + } UNSET_MASK(state->updated, SENSORUPDATES_baro); } + // make sure we raise an error until properly initialized - would not be good if people arm and + // use altitudehold without initialized barometer filter + return 2; } else { // Track barometric altitude offset with a low pass filter // based on GPS altitude if available - if (IS_SET(state->updated, SENSORUPDATES_pos)) { + if (this->useGPS && IS_SET(state->updated, SENSORUPDATES_pos)) { this->baroOffset = this->baroOffset * this->baroGPSOffsetCorrectionAlpha + (1.0f - this->baroGPSOffsetCorrectionAlpha) * (this->baroAlt + state->pos[2]); } @@ -100,9 +141,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state) this->baroAlt = state->baro[0]; state->baro[0] -= this->baroOffset; } + return 0; } - - return 0; } diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h index e0da4bf27..298edb31e 100644 --- a/flight/modules/StateEstimation/inc/stateestimation.h +++ b/flight/modules/StateEstimation/inc/stateestimation.h @@ -64,6 +64,7 @@ typedef struct stateFilterStruct { int32_t filterMagInitialize(stateFilter *handle); +int32_t filterBaroiInitialize(stateFilter *handle); int32_t filterBaroInitialize(stateFilter *handle); int32_t filterAltitudeInitialize(stateFilter *handle); int32_t filterAirInitialize(stateFilter *handle); diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 3c142cdf5..901dcabb6 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -123,6 +123,7 @@ static filterPipeline *filterChain = NULL; // different filters available to state estimation static stateFilter magFilter; static stateFilter baroFilter; +static stateFilter baroiFilter; static stateFilter altitudeFilter; static stateFilter airFilter; static stateFilter stationaryFilter; @@ -138,15 +139,28 @@ static filterPipeline *cfQueue = &(filterPipeline) { .next = &(filterPipeline) { .filter = &airFilter, .next = &(filterPipeline) { - .filter = &llaFilter, + .filter = &baroiFilter, .next = &(filterPipeline) { - .filter = &baroFilter, + .filter = &altitudeFilter, .next = &(filterPipeline) { - .filter = &altitudeFilter, - .next = &(filterPipeline) { - .filter = &cfFilter, - .next = NULL, - } + .filter = &cfFilter, + .next = NULL, + } + } + } + } +}; +static const filterPipeline *cfmiQueue = &(filterPipeline) { + .filter = &magFilter, + .next = &(filterPipeline) { + .filter = &airFilter, + .next = &(filterPipeline) { + .filter = &baroiFilter, + .next = &(filterPipeline) { + .filter = &altitudeFilter, + .next = &(filterPipeline) { + .filter = &cfmFilter, + .next = NULL, } } } @@ -176,15 +190,12 @@ static const filterPipeline *ekf13iQueue = &(filterPipeline) { .next = &(filterPipeline) { .filter = &airFilter, .next = &(filterPipeline) { - .filter = &llaFilter, + .filter = &baroiFilter, .next = &(filterPipeline) { - .filter = &baroFilter, + .filter = &stationaryFilter, .next = &(filterPipeline) { - .filter = &stationaryFilter, - .next = &(filterPipeline) { - .filter = &ekf13iFilter, - .next = NULL, - } + .filter = &ekf13iFilter, + .next = NULL, } } } @@ -256,6 +267,7 @@ int32_t StateEstimationInitialize(void) uint32_t stack_required = STACK_SIZE_BYTES; // Initialize Filters stack_required = maxint32_t(stack_required, filterMagInitialize(&magFilter)); + stack_required = maxint32_t(stack_required, filterBaroiInitialize(&baroiFilter)); stack_required = maxint32_t(stack_required, filterBaroInitialize(&baroFilter)); stack_required = maxint32_t(stack_required, filterAltitudeInitialize(&altitudeFilter)); stack_required = maxint32_t(stack_required, filterAirInitialize(&airFilter)); @@ -334,12 +346,15 @@ static void StateEstimationCb(void) newFilterChain = cfQueue; break; case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG: + newFilterChain = cfmiQueue; + break; + case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR: newFilterChain = cfmQueue; break; case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR: newFilterChain = ekf13iQueue; break; - case REVOSETTINGS_FUSIONALGORITHM_INS13OUTDOOR: + case REVOSETTINGS_FUSIONALGORITHM_INS13GPSOUTDOOR: newFilterChain = ekf13Queue; break; default: diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml index 8d634d1cd..0dd5e4d82 100644 --- a/shared/uavobjectdefinition/revosettings.xml +++ b/shared/uavobjectdefinition/revosettings.xml @@ -1,7 +1,7 @@ Settings for the revo to control the algorithm and what is updated - +