From 8df375810415c25e7833d1df0186cc3bac77f657 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 20 Sep 2016 02:15:17 +0200 Subject: [PATCH] LP-353 - Implement a specific Nav only option in ekf filter, add a debug NavYaw field for testing pourpose(it may be used for integrity check as well) --- flight/modules/StateEstimation/filterekf.c | 124 ++++++++---------- .../StateEstimation/inc/stateestimation.h | 3 + .../modules/StateEstimation/stateestimation.c | 34 ++++- shared/uavobjectdefinition/attitudestate.xml | 1 + shared/uavobjectdefinition/revosettings.xml | 2 +- 5 files changed, 96 insertions(+), 68 deletions(-) diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 8a3103e9c..199f984fd 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -71,87 +71,68 @@ struct data { bool inited; PiOSDeltatimeConfig dtconfig; + bool navOnly; }; -// Private variables -static bool initialized = 0; - // Private functions -static int32_t init13i(stateFilter *self); -static int32_t init13(stateFilter *self); -static int32_t maininit(stateFilter *self); +static int32_t init(stateFilter *self); static filterResult filter(stateFilter *self, stateEstimation *state); static inline bool invalid_var(float data); -static void globalInit(void); +static int32_t globalInit(stateFilter *handle, bool usePos, bool navOnly); -static void globalInit(void) +static int32_t globalInit(stateFilter *handle, bool usePos, bool navOnly) { - if (!initialized) { - initialized = 1; - EKFConfigurationInitialize(); - EKFStateVarianceInitialize(); - HomeLocationInitialize(); - } + handle->init = &init; + handle->filter = &filter; + handle->localdata = pios_malloc(sizeof(struct data)); + struct data *this = (struct data *)handle->localdata; + this->usePos = usePos; + this->navOnly = navOnly; + EKFConfigurationInitialize(); + EKFStateVarianceInitialize(); + HomeLocationInitialize(); + return STACK_REQUIRED; } int32_t filterEKF13iInitialize(stateFilter *handle) { - globalInit(); - handle->init = &init13i; - handle->filter = &filter; - handle->localdata = pios_malloc(sizeof(struct data)); - return STACK_REQUIRED; + return globalInit(handle, false, false); } + int32_t filterEKF13Initialize(stateFilter *handle) { - globalInit(); - handle->init = &init13; - handle->filter = &filter; - handle->localdata = pios_malloc(sizeof(struct data)); - return STACK_REQUIRED; + return globalInit(handle, true, false); } + +int32_t filterEKF13iNavOnlyInitialize(stateFilter *handle) +{ + return globalInit(handle, false, true); +} + +int32_t filterEKF13NavOnlyInitialize(stateFilter *handle) +{ + return globalInit(handle, true, true); +} + +#ifdef FALSE // XXX // TODO: Until the 16 state EKF is implemented, run 13 state, so compilation runs through // XXX int32_t filterEKF16iInitialize(stateFilter *handle) { - globalInit(); - handle->init = &init13i; - handle->filter = &filter; - handle->localdata = pios_malloc(sizeof(struct data)); - return STACK_REQUIRED; + return filterEKFi13Initialize(handle); } int32_t filterEKF16Initialize(stateFilter *handle) { - globalInit(); - handle->init = &init13; - handle->filter = &filter; - handle->localdata = pios_malloc(sizeof(struct data)); - return STACK_REQUIRED; + return filterEKF13Initialize(handle); } +#endif - -static int32_t init13i(stateFilter *self) -{ - struct data *this = (struct data *)self->localdata; - - this->usePos = 0; - return maininit(self); -} - -static int32_t init13(stateFilter *self) -{ - struct data *this = (struct data *)self->localdata; - - this->usePos = 1; - return maininit(self); -} - -static int32_t maininit(stateFilter *self) +static int32_t init(stateFilter *self) { struct data *this = (struct data *)self->localdata; @@ -303,13 +284,16 @@ static filterResult filter(stateFilter *self, stateEstimation *state) // Copy the attitude into the state // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true - state->attitude[0] = Nav.q[0]; - state->attitude[1] = Nav.q[1]; - state->attitude[2] = Nav.q[2]; - state->attitude[3] = Nav.q[3]; - state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]); - state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]); - state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]); + if(!this->navOnly){ + state->attitude[0] = Nav.q[0]; + state->attitude[1] = Nav.q[1]; + state->attitude[2] = Nav.q[2]; + state->attitude[3] = Nav.q[3]; + + state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]); + state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]); + state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]); + } state->pos[0] = Nav.Pos[0]; state->pos[1] = Nav.Pos[1]; state->pos[2] = Nav.Pos[2]; @@ -338,13 +322,21 @@ static filterResult filter(stateFilter *self, stateEstimation *state) // Copy the attitude into the state // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true - state->attitude[0] = Nav.q[0]; - state->attitude[1] = Nav.q[1]; - state->attitude[2] = Nav.q[2]; - state->attitude[3] = Nav.q[3]; - state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]); - state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]); - state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]); + if(!this->navOnly){ + + state->attitude[0] = Nav.q[0]; + state->attitude[1] = Nav.q[1]; + state->attitude[2] = Nav.q[2]; + state->attitude[3] = Nav.q[3]; + state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]); + state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]); + state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]); + } + { + float tmp[3]; + Quaternion2RPY(Nav.q, tmp); + state->debugNavYaw = tmp[2]; + } state->pos[0] = Nav.Pos[0]; state->pos[1] = Nav.Pos[1]; state->pos[2] = Nav.Pos[2]; diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h index c34ba9140..7de46bb10 100644 --- a/flight/modules/StateEstimation/inc/stateestimation.h +++ b/flight/modules/StateEstimation/inc/stateestimation.h @@ -71,6 +71,7 @@ typedef struct { float baro[1]; float auxMag[3]; float boardMag[3]; + float debugNavYaw; uint8_t magStatus; bool armed; sensorUpdates updated; @@ -95,6 +96,8 @@ int32_t filterCFInitialize(stateFilter *handle); int32_t filterCFMInitialize(stateFilter *handle); int32_t filterEKF13iInitialize(stateFilter *handle); int32_t filterEKF13Initialize(stateFilter *handle); +int32_t filterEKF13NavOnlyInitialize(stateFilter *handle); +int32_t filterEKF13iNavOnlyInitialize(stateFilter *handle); int32_t filterEKF16iInitialize(stateFilter *handle); int32_t filterEKF16Initialize(stateFilter *handle); diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index a727e215e..9d2afd09f 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -157,6 +157,8 @@ static stateFilter cfFilter; static stateFilter cfmFilter; static stateFilter ekf13iFilter; static stateFilter ekf13Filter; +static stateFilter ekf13iNavFilter; +static stateFilter ekf13NavFilter; // this is a hack to provide a computational shortcut for faster gyro state progression static float gyroRaw[3]; @@ -260,7 +262,30 @@ static const filterPipeline *ekf13NavCFAttQueue = &(filterPipeline) { .next = &(filterPipeline) { .filter = &baroFilter, .next = &(filterPipeline) { - .filter = &ekf13Filter, + .filter = &ekf13NavFilter, + .next = &(filterPipeline) { + .filter = &velocityFilter, + .next = &(filterPipeline) { + .filter = &cfmFilter, + .next = NULL, + } + } + } + } + } + } +}; + +static const filterPipeline *ekf13iNavCFAttQueue = &(filterPipeline) { + .filter = &magFilter, + .next = &(filterPipeline) { + .filter = &airFilter, + .next = &(filterPipeline) { + .filter = &baroiFilter, + .next = &(filterPipeline) { + .filter = &stationaryFilter, + .next = &(filterPipeline) { + .filter = &ekf13iNavFilter, .next = &(filterPipeline) { .filter = &velocityFilter, .next = &(filterPipeline) { @@ -343,6 +368,8 @@ int32_t StateEstimationInitialize(void) stack_required = maxint32_t(stack_required, filterCFMInitialize(&cfmFilter)); stack_required = maxint32_t(stack_required, filterEKF13iInitialize(&ekf13iFilter)); stack_required = maxint32_t(stack_required, filterEKF13Initialize(&ekf13Filter)); + stack_required = maxint32_t(stack_required, filterEKF13NavOnlyInitialize(&ekf13NavFilter)); + stack_required = maxint32_t(stack_required, filterEKF13iNavOnlyInitialize(&ekf13iNavFilter)); stateEstimationCallback = PIOS_CALLBACKSCHEDULER_Create(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, CALLBACKINFO_RUNNING_STATEESTIMATION, stack_required); @@ -424,12 +451,16 @@ static void StateEstimationCb(void) case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13CF: newFilterChain = ekf13NavCFAttQueue; break; + case REVOSETTINGS_FUSIONALGORITHM_TESTINGINSINDOORCF: + newFilterChain = ekf13iNavCFAttQueue; + break; default: newFilterChain = NULL; } // initialize filters in chain current = newFilterChain; bool error = 0; + states.debugNavYaw = 0; while (current != NULL) { int32_t result = current->filter->init((stateFilter *)current->filter); if (result != 0) { @@ -524,6 +555,7 @@ static void StateEstimationCb(void) s.q3 = states.attitude[2]; s.q4 = states.attitude[3]; Quaternion2RPY(&s.q1, &s.Roll); + s.NavYaw = states.debugNavYaw; AttitudeStateSet(&s); } // throttle alarms, raise alarm flags immediately diff --git a/shared/uavobjectdefinition/attitudestate.xml b/shared/uavobjectdefinition/attitudestate.xml index 0d5dc0c08..045f8bc22 100644 --- a/shared/uavobjectdefinition/attitudestate.xml +++ b/shared/uavobjectdefinition/attitudestate.xml @@ -8,6 +8,7 @@ + diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml index a99ad9c52..2cddb0f9d 100644 --- a/shared/uavobjectdefinition/revosettings.xml +++ b/shared/uavobjectdefinition/revosettings.xml @@ -2,7 +2,7 @@ Settings for the revo to control the algorithm and what is updated