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

@@ -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 init(stateFilter *self);
-static int32_t init13(stateFilter *self);
-static int32_t maininit(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) {
+    handle->init      = &init;
-        initialized = 1;
+    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();
+    return globalInit(handle, false, false);
-    handle->init      = &init13i;
-    handle->filter    = &filter;
-    handle->localdata = pios_malloc(sizeof(struct data));
-    return STACK_REQUIRED;
 }
+
 int32_t filterEKF13Initialize(stateFilter *handle)
 {
-    globalInit();
+    return globalInit(handle, true, false);
-    handle->init      = &init13;
-    handle->filter    = &filter;
-    handle->localdata = pios_malloc(sizeof(struct data));
-    return STACK_REQUIRED;
 }
+
+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();
+    return filterEKFi13Initialize(handle);
-    handle->init      = &init13i;
-    handle->filter    = &filter;
-    handle->localdata = pios_malloc(sizeof(struct data));
-    return STACK_REQUIRED;
 }
 int32_t filterEKF16Initialize(stateFilter *handle)
 {
-    globalInit();
+    return filterEKF13Initialize(handle);
-    handle->init      = &init13;
-    handle->filter    = &filter;
-    handle->localdata = pios_malloc(sizeof(struct data));
-    return STACK_REQUIRED;
 }
+#endif
 
-
+static int32_t init(stateFilter *self)
-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)
 {
     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
+            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,6 +322,8 @@ 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
+    if(!this->navOnly){
+
         state->attitude[0] = Nav.q[0];
         state->attitude[1] = Nav.q[1];
         state->attitude[2] = Nav.q[2];
@@ -345,6 +331,12 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
         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];

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);
 

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

shared/uavobjectdefinition/attitudestate.xml

@@ -8,6 +8,7 @@
         <field name="Roll" units="degrees" type="float" elements="1"/>
         <field name="Pitch" units="degrees" type="float" elements="1"/>
         <field name="Yaw" units="degrees" type="float" elements="1"/>
+        <field name="NavYaw" units="degrees" type="float" elements="1"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
         <telemetryflight acked="false" updatemode="periodic" period="130"/>

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)" 
+        options="None,Basic (Complementary),Complementary+Mag,Complementary+Mag+GPSOutdoor,INS13Indoor,GPS Navigation (INS13),GPS Navigation (INS13+CF),Testing (INS Indoor+CF)" 
         defaultvalue="Basic (Complementary)"/>
 
         <!-- Low pass filter configuration to calculate offset of barometric altitude sensor.