AP_NavEKF3: Prevent possible race condition re-zeroing state variances

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -59,8 +59,10 @@ void NavEKF3_core::setWindMagStateLearningMode()
     bool setWindInhibit = (!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE);
     if (!inhibitWindStates && setWindInhibit) {
         inhibitWindStates = true;
+        updateStateIndexLim();
     } else if (inhibitWindStates && !setWindInhibit) {
         inhibitWindStates = false;
+        updateStateIndexLim();
         // set states and variances
         if (yawAlignComplete && useAirspeed()) {
             // if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading
@@ -106,8 +108,10 @@ void NavEKF3_core::setWindMagStateLearningMode()
     bool setMagInhibit = !magCalRequested || magCalDenied;
     if (!inhibitMagStates && setMagInhibit) {
         inhibitMagStates = true;
+        updateStateIndexLim();
     } else if (inhibitMagStates && !setMagInhibit) {
         inhibitMagStates = false;
+        updateStateIndexLim();
         if (magFieldLearned) {
             // if we have already learned the field states, then retain the learned variances
             P[16][16] = earthMagFieldVar.x;
@@ -137,6 +141,8 @@ void NavEKF3_core::setWindMagStateLearningMode()
     if (tiltAlignComplete && inhibitDelVelBiasStates) {
         // activate the states
         inhibitDelVelBiasStates = false;
+        updateStateIndexLim();
+
         // set the initial covariance values
         P[13][13] = sq(ACCEL_BIAS_LIM_SCALER * frontend->_accBiasLim * dtEkfAvg);
         P[14][14] = P[13][13];
@@ -146,6 +152,8 @@ void NavEKF3_core::setWindMagStateLearningMode()
     if (tiltAlignComplete && inhibitDelAngBiasStates) {
         // activate the states
         inhibitDelAngBiasStates = false;
+        updateStateIndexLim();
+
         // set the initial covariance values
         P[10][10] = sq(radians(InitialGyroBiasUncertainty() * dtEkfAvg));
         P[11][11] = P[10][10];
@@ -159,8 +167,13 @@ void NavEKF3_core::setWindMagStateLearningMode()
         finalInflightMagInit = false;
     }
 
+    updateStateIndexLim();
+}
+
 // Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
 // if we are not using those states
+void NavEKF3_core::updateStateIndexLim()
+{
     if (inhibitWindStates) {
         if (inhibitMagStates) {
             if (inhibitDelVelBiasStates) {

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -767,6 +767,9 @@ private:
     // update timing statistics structure
     void updateTimingStatistics(void);
 
+    // Update the state index limit based on which states are active
+    void updateStateIndexLim(void);
+    
     // Variables
     bool statesInitialised;         // boolean true when filter states have been initialised
     bool velHealth;                 // boolean true if velocity measurements have passed innovation consistency check
@@ -853,8 +856,8 @@ private:
     float tasTestRatio;             // sum of squares of true airspeed innovation divided by fail threshold
     bool inhibitWindStates;         // true when wind states and covariances are to remain constant
     bool inhibitMagStates;          // true when magnetic field states are inactive
-    bool inhibitDelVelBiasStates;   // true when delta velocity bias states are inactive
+    bool inhibitDelVelBiasStates;   // true when IMU delta velocity bias states are inactive
-    bool inhibitDelAngBiasStates;
+    bool inhibitDelAngBiasStates;   // true when IMU delta angle bias states are inactive
     bool gpsNotAvailable;           // bool true when valid GPS data is not available
     struct Location EKF_origin;     // LLH origin of the NED axis system
     bool validOrigin;               // true when the EKF origin is valid