AP_NavEKF: Latch use of position hold mode for duration of flight

Required to prevent acquisition of GPS mid flight causing unwanted change in position and velocity
A distinction has been mad between the arm and disarm transition and the decision to use position hold mode (formerly static mode)

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -374,7 +374,6 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
     covDelAngMax(0.05f),        // maximum delta angle between covariance prediction updates
     TASmsecMax(200),            // maximum allowed interval between airspeed measurement updates
     posHoldMode(true),          // forces position fusion with zero values
-    prevPosHoldMode(true),      // posHoldMode from previous filter update
     yawAligned(false),          // set true when heading or yaw angle has been aligned
     inhibitWindStates(true),    // inhibit wind state updates on startup
     inhibitMagStates(true)      // inhibit magnetometer state updates on startup
@@ -685,22 +684,18 @@ void NavEKF::UpdateFilter()
     // check if on ground
     SetFlightAndFusionModes();
 
-    // define rules used to set position hold mode
+    // determine vehicle arm status
-    // position hold enables attitude only estimates without GPS by fusing zeros for position
+    prevVehicleArmed = vehicleArmed;
-    if (vehicleNotArmed() || (_fusionModeGPS == 3 && !optFlowDataPresent())) {
+    vehicleArmed = getVehicleArmStatus();
-        posHoldMode = true;
-    } else {
-        posHoldMode = false;
-    }
 
-    // check to see if position hold mode has changed and reset states if it has
+    // check to see if arm status has changed and reset states if it has
-    if (prevPosHoldMode != posHoldMode) {
+    if (vehicleArmed != prevVehicleArmed) {
         ResetVelocity();
         ResetPosition();
         ResetHeight();
         StoreStatesReset();
         // only reset the magnetic field and heading on the first arm. This prevents in-flight learning being forgotten for vehicles that do multiple short flights and disarm in-between.
-        if (!posHoldMode && !firstArmComplete) {
+        if (vehicleArmed && !firstArmComplete) {
             firstArmComplete = true;
             state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
             firstArmPosD = state.position.z;
@@ -708,29 +703,41 @@ void NavEKF::UpdateFilter()
         // zero stored velocities used to do dead-reckoning
         heldVelNE.zero();
         // set various  useage modes based on the condition at arming. These are then held until the vehicle is disarmed.
-        if (!prevPosHoldMode) {
+        if (!vehicleArmed) {
-            gpsInhibitMode = 0; // When entering potion hold mode (dis-arming), clear any GPS mode inhibits
+            gpsInhibitMode = 0; // When dis-arming, clear any GPS mode inhibits
-
+        } else if (_fusionModeGPS == 3) { // arming when GPS useage has been prohibited
-        } else if (_fusionModeGPS == 3) { // GPS useage has been explicitly prohibited
             if (optFlowDataPresent()) {
                 gpsInhibitMode = 2; // we have optical flow data and can estimate all vehicle states
+                posTimeout = true;
+                velTimeout = true;
             } else {
                 gpsInhibitMode = 1; // we don't have optical flow data and will only be able to estimate orientation and height
+                posTimeout = true;
+                velTimeout = true;
             }
-        } else { //GPS useage is allowed
+        } else { // arming when GPS useage is allowed
-            if ((imuSampleTime_ms - lastFixTime_ms) < 500) {
+            if (gpsNotAvailable) {
-                gpsInhibitMode = 0; // we have GPS data and can estimate all vehicle states
-            } else {
                 gpsInhibitMode = 1; // we don't have have GPS data and will only be able to estimate orientation and height
+                posTimeout = true;
+                velTimeout = true;
+            } else {
+                gpsInhibitMode = 0; // we have GPS data and can estimate all vehicle states
             }
         }
-        prevPosHoldMode = posHoldMode;
+    } else if (vehicleArmed && !finalMagYawInit && firstArmPosD - state.position.z > 1.5f && !assume_zero_sideslip()) {
-    } else if (!posHoldMode && !finalMagYawInit && firstArmPosD - state.position.z > 1.5f && !assume_zero_sideslip()) {
         // Do a final yaw and earth mag field initialisation when the vehicle has gained 1.5m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff)
         state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
         finalMagYawInit = true;
     }
 
+    // define rules used to set position hold mode
+    // position hold enables attitude only estimates without GPS by fusing zeros for position
+    if ((vehicleArmed && gpsInhibitMode == 1) || !vehicleArmed) {
+        posHoldMode = true;
+    } else {
+        posHoldMode = false;
+    }
+
     // run the strapdown INS equations every IMU update
     UpdateStrapdownEquationsNED();
 
@@ -4017,6 +4024,12 @@ void NavEKF::readGpsData()
         // calculate a position offset which is applied to NE position and velocity wherever it is used throughout code to allow GPS position jumps to be accommodated gradually
         decayGpsOffset();
     }
+    // If too long since last fix time, we declare no GPS present
+    if (imuSampleTime_ms - lastFixTime_ms < 1000) {
+        gpsNotAvailable = false;
+    } else {
+        gpsNotAvailable = true;
+    }
 }
 
 // check for new altitude measurement data and update stored measurement if available
@@ -4397,6 +4410,8 @@ void NavEKF::ZeroVariables()
     heldVelNE.zero();
     gpsInhibitMode = 0;
     gpsVelGlitchOffset.zero();
+    vehicleArmed = false;
+    prevVehicleArmed = false;
 }
 
 // return true if we should use the airspeed sensor
@@ -4425,10 +4440,10 @@ bool NavEKF::optFlowDataPresent(void) const
     }
 }
 
-// return true if the vehicle is not armed or requesting a static vehicle assumption for correction of attitude errors
+// return true if the vehicle is requesting the filter to be ready for flight
-bool NavEKF::vehicleNotArmed(void) const
+bool NavEKF::getVehicleArmStatus(void) const
 {
-    return !_ahrs->get_armed() || !_ahrs->get_correct_centrifugal();
+    return _ahrs->get_armed() || _ahrs->get_correct_centrifugal();
 }
 
 // return true if we should use the compass

libraries/AP_NavEKF/AP_NavEKF.h

@@ -157,9 +157,6 @@ public:
     // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
     void  getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
 
-    // return StaticMode state
-    bool getStaticMode(void) const { return posHoldMode; }
-
     // should we use the compass? This is public so it can be used for
     // reporting via ahrs.use_compass()
     bool use_compass(void) const;
@@ -354,9 +351,8 @@ private:
     // return true if we should use the airspeed sensor
     bool useAirspeed(void) const;
 
-    // return true if the vehicle code has requested operation in a pre-armed state where GPS data isnt used to correct attitude
+    // return true if the vehicle code has requested the filter to be ready for flight
-    // this causes operation in a mode producing attitude and height only
+    bool getVehicleArmStatus(void) const;
-    bool vehicleNotArmed(void) const;
 
     // decay GPS horizontal position offset to close to zero at a rate of 1 m/s
     // this allows large GPS position jumps to be accomodated gradually
@@ -522,7 +518,6 @@ private:
     uint32_t HGTmsecPrev;           // time stamp of last height measurement fusion step
     bool inhibitLoadLeveling;       // boolean that turns off delay of fusion to level processor loading
     bool posHoldMode;               // boolean to force position measurements to zero for operation without GPS
-    bool prevPosHoldMode;           // value of static mode from last update
     uint32_t lastMagUpdate;         // last time compass was updated
     Vector3f velDotNED;             // rate of change of velocity in NED frame
     Vector3f velDotNEDfilt;         // low pass filtered velDotNED
@@ -571,6 +566,9 @@ private:
     bool finalMagYawInit;           // true when the final post takeoff initialisation of earth field and yaw angle has been performed
     bool flowTimeout;               // true when optical flow measurements have time out
     Vector2f gpsVelGlitchOffset;    // Offset applied to the GPS velocity when the gltch radius is being  decayed back to zero
+    bool gpsNotAvailable;           // bool true when valid GPS data is not available
+    bool vehicleArmed;              // true when the vehicle is disarmed
+    bool prevVehicleArmed;          // vehicleArmed from previous frame
 
     // Used by smoothing of state corrections
     float gpsIncrStateDelta[10];    // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement