From 89abb7c085ffa644634a172286700570433cd09e Mon Sep 17 00:00:00 2001
From: Laurent Lalanne <f5soh@free.fr>
Date: Tue, 16 Apr 2019 21:36:53 +0200
Subject: [PATCH] LP-613 Add new algorithm with Complementary+GPS

---
 .../modules/StateEstimation/stateestimation.c | 21 +++++++++++++++++++
 shared/uavobjectdefinition/revosettings.xml   |  2 +-
 2 files changed, 22 insertions(+), 1 deletion(-)

diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c
index a8fbd8665..dd3315bfe 100644
--- a/flight/modules/StateEstimation/stateestimation.c
+++ b/flight/modules/StateEstimation/stateestimation.c
@@ -184,6 +184,22 @@ static const filterPipeline *cfQueue = &(filterPipeline) {
         }
     }
 };
+static const filterPipeline *cfgpsQueue = &(filterPipeline) {
+    .filter = &airFilter,
+    .next   = &(filterPipeline) {
+        .filter = &llaFilter,
+        .next   = &(filterPipeline) {
+            .filter = &baroiFilter,
+            .next   = &(filterPipeline) {
+                .filter = &altitudeFilter,
+                .next   = &(filterPipeline) {
+                    .filter = &cfFilter,
+                    .next   = NULL,
+                }
+            }
+        }
+    }
+};
 static const filterPipeline *cfmiQueue = &(filterPipeline) {
     .filter = &magFilter,
     .next   = &(filterPipeline) {
@@ -443,6 +459,11 @@ static void StateEstimationCb(void)
                 // reinit Mag alarm
                 AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_UNINITIALISED);
                 break;
+            case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYGPSOUTDOOR:
+                newFilterChain = cfgpsQueue;
+                // reinit Mag alarm
+                AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_UNINITIALISED);
+                break;
             case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG:
                 newFilterChain = cfmiQueue;
                 break;
diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml
index bb258e20b..c42aab1cf 100644
--- a/shared/uavobjectdefinition/revosettings.xml
+++ b/shared/uavobjectdefinition/revosettings.xml
@@ -2,7 +2,7 @@
     <object name="RevoSettings" singleinstance="true" settings="true" category="State">
         <description>Settings for the revo to control the algorithm and what is updated</description>
         <field name="FusionAlgorithm" units="" type="enum" elements="1" 
-        options="None,Basic (Complementary),Complementary+Mag,Complementary+Mag+GPSOutdoor,INS13Indoor,GPS Navigation (INS13),GPS Navigation (INS13+CF),Testing (INS Indoor+CF),Acro (No Sensors)" 
+        options="None,Basic (Complementary),Complementary+GPSOutdoor,Complementary+Mag,Complementary+Mag+GPSOutdoor,INS13Indoor,GPS Navigation (INS13),GPS Navigation (INS13+CF),Testing (INS Indoor+CF),Acro (No Sensors)" 
         defaultvalue="Basic (Complementary)"/>
 
         <!-- Low pass filter configuration to calculate offset of barometric altitude sensor.