diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 5a08cf8cc..a727e215e 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -251,6 +251,29 @@ static const filterPipeline *ekf13Queue = &(filterPipeline) { } }; +static const filterPipeline *ekf13NavCFAttQueue = &(filterPipeline) { + .filter = &magFilter, + .next = &(filterPipeline) { + .filter = &airFilter, + .next = &(filterPipeline) { + .filter = &llaFilter, + .next = &(filterPipeline) { + .filter = &baroFilter, + .next = &(filterPipeline) { + .filter = &ekf13Filter, + .next = &(filterPipeline) { + .filter = &velocityFilter, + .next = &(filterPipeline) { + .filter = &cfmFilter, + .next = NULL, + } + } + } + } + } + } +}; + // Private functions static void settingsUpdatedCb(UAVObjEvent *objEv); @@ -398,6 +421,9 @@ static void StateEstimationCb(void) case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13: newFilterChain = ekf13Queue; break; + case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13CF: + newFilterChain = ekf13NavCFAttQueue; + break; default: newFilterChain = NULL; } diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml index 116b842cf..a99ad9c52 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