AP_NavEKF2: Clean up mode change logic

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -385,7 +385,6 @@ NavEKF2::NavEKF2(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) :
     magFailTimeLimit_ms(10000),     // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
     magVarRateScale(0.05f),         // scale factor applied to magnetometer variance due to angular rate
     gyroBiasNoiseScaler(2.0f),      // scale factor applied to imu gyro bias learning before the vehicle is armed
-    accelBiasNoiseScaler(1.0f),     // scale factor applied to imu accel bias learning before the vehicle is armed
     hgtAvg_ms(100),                 // average number of msec between height measurements
     betaAvg_ms(100),                // average number of msec between synthetic sideslip measurements
     covTimeStepMax(0.07f),          // maximum time (sec) between covariance prediction updates

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -275,7 +275,6 @@ private:
     const uint32_t magFailTimeLimit_ms; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
     const float magVarRateScale;        // scale factor applied to magnetometer variance due to angular rate
     const float gyroBiasNoiseScaler;    // scale factor applied to gyro bias state process noise when on ground
-    const float accelBiasNoiseScaler;   // scale factor applied to accel bias state process noise when on ground
     const uint16_t hgtAvg_ms;           // average number of msec between height measurements
     const uint16_t betaAvg_ms;          // average number of msec between synthetic sideslip measurements
     const float covTimeStepMax;         // maximum time (sec) between covariance prediction updates

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -69,7 +69,7 @@ bool NavEKF2_core::healthy(void) const
     }
     // barometer and position innovations must be within limits when on-ground
     float horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
-    if (!filterArmed && (fabsf(innovVelPos[5]) > 1.0f || horizErrSq > 1.0f)) {
+    if (!motorsArmed && (fabsf(innovVelPos[5]) > 1.0f || horizErrSq > 1.0f)) {
         return false;
     }
 
@@ -229,15 +229,12 @@ void NavEKF2_core::UpdateFilter()
     //get starting time for update step
     imuSampleTime_ms = hal.scheduler->millis();
 
+    // Check arm status and perform required checks and mode changes
+    controlFilterModes();
+
     // read IMU data and convert to delta angles and velocities
     readIMUData();
 
-    // check if on ground
-    SetFlightAndFusionModes();
-
-    // Check arm status and perform required checks and mode changes
-    performArmingChecks();
-
     // run the strapdown INS equations every IMU update
     UpdateStrapdownEquationsNED();
 
@@ -263,25 +260,6 @@ void NavEKF2_core::UpdateFilter()
     // Update states using optical flow data
     SelectFlowFusion();
 
-    // Check for tilt convergence
-    float alpha = 1.0f*dtIMUavg;
-    float temp=tiltErrVec.length();
-    tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt;
-    if (tiltErrFilt < 0.005f && !tiltAlignComplete) {
-        tiltAlignComplete = true;
-        hal.console->printf("EKF tilt alignment complete\n");
-    }
-
-    // once tilt has converged, align yaw using magnetic field measurements
-    if (tiltAlignComplete && !yawAlignComplete) {
-        Vector3f eulerAngles;
-        getEulerAngles(eulerAngles);
-        stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
-        StoreQuatReset();
-        yawAlignComplete = true;
-        hal.console->printf("EKF yaw alignment complete\n");
-    }
-
     // Update states using  magnetometer data
     SelectMagFusion();
 
@@ -446,11 +424,12 @@ void NavEKF2_core::SelectFlowFusion()
     // Perform tilt check
     bool tiltOK = (Tnb_flow.c.z > frontend.DCM33FlowMin);
     // Constrain measurements to zero if we are using optical flow and are on the ground
-    if (frontend._fusionModeGPS == 3 && !takeOffDetected && filterArmed) {
+    if (frontend._fusionModeGPS == 3 && !takeOffDetected && isAiding) {
         ofDataDelayed.flowRadXYcomp.zero();
         ofDataDelayed.flowRadXY.zero();
         flowDataValid = true;
     }
+
     // If the flow measurements have been rejected for too long and we are relying on them, then revert to constant position mode
     if ((flowSensorTimeout || flowFusionTimeout) && PV_AidingMode == AID_RELATIVE) {
         PV_AidingMode = AID_NONE;
@@ -462,6 +441,7 @@ void NavEKF2_core::SelectFlowFusion()
         // reset the position
         ResetPosition();
     }
+
     // if we do have valid flow measurements, fuse data into a 1-state EKF to estimate terrain height
     // we don't do terrain height estimation in optical flow only mode as the ground becomes our zero height reference
     if ((newDataFlow || newDataRng) && tiltOK) {
@@ -767,8 +747,6 @@ void NavEKF2_core::CovariancePrediction()
     processNoise[15] = dVelBiasSigma;
     if (expectGndEffectTakeoff) {
         processNoise[15] = 0.0f;
-    } else if (!filterArmed) {
-        processNoise[15] = dVelBiasSigma * frontend.accelBiasNoiseScaler;
     } else {
         processNoise[15] = dVelBiasSigma;
     }
@@ -1307,7 +1285,7 @@ void NavEKF2_core::FuseVelPosNED()
         R_OBS[5] = sq(constrain_float(frontend._baroAltNoise, 0.1f, 10.0f));
 
         // reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
-        if ((getTakeoffExpected() || getTouchdownExpected()) && filterArmed) {
+        if ((getTakeoffExpected() || getTouchdownExpected()) && motorsArmed) {
             R_OBS[5] *= frontend.gndEffectBaroScaler;
         }
 
@@ -1876,7 +1854,7 @@ void NavEKF2_core::FuseMagnetometer()
             }
             // If in the air and there is no other form of heading reference or we are yawing rapidly which creates larger inertial yaw errors,
             // we strengthen the magnetometer attitude correction
-            if (filterArmed && ((PV_AidingMode == AID_NONE) || highYawRate) && j <= 3) {
+            if (motorsArmed && ((PV_AidingMode == AID_NONE) || highYawRate) && j <= 3) {
                 Kfusion[j] *= 4.0f;
             }
             statesArray[j] = statesArray[j] - Kfusion[j] * innovMag[obsIndex];
@@ -2721,7 +2699,7 @@ void NavEKF2_core::EstimateTerrainOffset()
     float losRateSq = velHorizSq / sq(heightAboveGndEst);
 
     // don't update terrain offset state if there is no range finder and not generating enough LOS rate, or without GPS, as it is poorly observable
-    if (!fuseRngData && (gpsNotAvailable || PV_AidingMode == AID_RELATIVE || velHorizSq < 25.0f || losRateSq < 0.01f || onGround)) {
+    if (!fuseRngData && (gpsNotAvailable || PV_AidingMode == AID_RELATIVE || velHorizSq < 25.0f || losRateSq < 0.01f)) {
         inhibitGndState = true;
     } else {
         inhibitGndState = false;
@@ -3302,7 +3280,7 @@ bool NavEKF2_core::resetHeightDatum(void)
 // Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
 uint8_t NavEKF2_core::setInhibitGPS(void)
 {
-    if(!filterArmed) {
+    if(!isAiding) {
         return 0;
     }
     if (optFlowDataPresent()) {
@@ -3442,79 +3420,6 @@ void NavEKF2_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInn
     gndOffsetErr = sqrtf(Popt); // note Popt is constrained to be non-negative in EstimateTerrainOffset()
 }
 
-// calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed
-void NavEKF2_core::SetFlightAndFusionModes()
-{
-    // determine if the vehicle is manoevring
-    if (accNavMagHoriz > 0.5f) {
-        manoeuvring = true;
-    } else {
-        manoeuvring = false;
-    }
-    // if we are a fly forward type vehicle, then in-air mode can be determined through a combination of speed and height criteria
-    if (assume_zero_sideslip()) {
-        // Evaluate a numerical score that defines the likelihood we are in the air
-        float gndSpdSq = sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y);
-        bool highGndSpd = false;
-        bool highAirSpd = false;
-        bool largeHgtChange = false;
-
-        // trigger at 8 m/s airspeed
-        if (_ahrs->airspeed_sensor_enabled()) {
-            const AP_Airspeed *airspeed = _ahrs->get_airspeed();
-            if (airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 10.0f) {
-                highAirSpd = true;
-            }
-        }
-
-        // trigger at 10 m/s GPS velocity, but not if GPS is reporting bad velocity errors
-        if (gndSpdSq > 100.0f && gpsSpdAccuracy < 1.0f) {
-            highGndSpd = true;
-        }
-
-        // trigger if more than 10m away from initial height
-        if (fabsf(baroDataDelayed.hgt) > 10.0f) {
-            largeHgtChange = true;
-        }
-
-        // to go to in-air mode we also need enough GPS velocity to be able to calculate a reliable ground track heading and either a lerge height or airspeed change
-        if (onGround && highGndSpd && (highAirSpd || largeHgtChange)) {
-            onGround = false;
-        }
-        // if is possible we are in flight, set the time this condition was last detected
-        if (highGndSpd || highAirSpd || largeHgtChange) {
-            airborneDetectTime_ms = imuSampleTime_ms;
-        }
-        // after 5 seconds of not detecting a possible flight condition, we transition to on-ground mode
-        if(!onGround && ((imuSampleTime_ms - airborneDetectTime_ms) > 5000)) {
-            onGround = true;
-        }
-        // perform a yaw alignment check against GPS if exiting on-ground mode, bu tonly if we have enough ground speed
-        // this is done to protect against unrecoverable heading alignment errors due to compass faults
-        if (!onGround && prevOnGround) {
-            alignYawGPS();
-        }
-    }
-    // store current on-ground status for next time
-    prevOnGround = onGround;
-    // If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
-    inhibitWindStates = ((!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE));
-    // request mag calibration for both in-air and manoeuvre threshold options
-    bool magCalRequested = ((frontend._magCal == 0) && !onGround) || ((frontend._magCal == 1) && manoeuvring)  || (frontend._magCal == 3);
-    // deny mag calibration request if we aren't using the compass, are in the pre-arm constant position mode or it has been inhibited by the user
-    bool magCalDenied = !use_compass() || (PV_AidingMode == AID_NONE) || (frontend._magCal == 2);
-    // inhibit the magnetic field calibration if not requested or denied
-    inhibitMagStates = (!magCalRequested || magCalDenied);
-
-    if (inhibitMagStates && inhibitWindStates) {
-        stateIndexLim = 15;
-    } else if (inhibitWindStates) {
-        stateIndexLim = 21;
-    } else {
-        stateIndexLim = 23;
-    }
-}
-
 // initialise the covariance matrix
 void NavEKF2_core::CovarianceInit()
 {
@@ -3750,7 +3655,7 @@ void NavEKF2_core::readGpsData()
             // We are by definition at the origin at the instant of alignment so set NE position to zero
             gpsDataNew.pos.zero();
             // If GPS useage isn't explicitly prohibited, we switch to absolute position mode
-            if (filterArmed && frontend._fusionModeGPS != 3) {
+            if (isAiding && frontend._fusionModeGPS != 3) {
                 PV_AidingMode = AID_ABSOLUTE;
                 // Initialise EKF position and velocity states
                 ResetPosition();
@@ -3849,7 +3754,7 @@ void NavEKF2_core::readHgtData()
                 // calculate offset to baro data that enables baro to be used as a backup
                 // filter offset to reduce effect of baro noise and other transient errors on estimate
                 baroHgtOffset = 0.1f * (_baro.get_altitude() + stateStruct.position.z) + 0.9f * baroHgtOffset;
-            } else if (filterArmed && takeOffDetected) {
+            } else if (isAiding && takeOffDetected) {
                 // use baro measurement and correct for baro offset - failsafe use only as baro will drift
                 baroDataNew.hgt = max(_baro.get_altitude() - baroHgtOffset, rngOnGnd);
             } else {
@@ -3871,7 +3776,7 @@ void NavEKF2_core::readHgtData()
             const float dtBaro = frontend.hgtAvg_ms*1.0e-3f;
             float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
             meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha;
-        } else if (filterArmed && getTakeoffExpected()) {
+        } else if (isAiding && getTakeoffExpected()) {
             // If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
             // This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
             baroDataNew.hgt = max(baroDataNew.hgt, meaHgtAtTakeOff);
@@ -4201,7 +4106,6 @@ void NavEKF2_core::InitialiseVariables()
     tasTimeout = true;
     badMag = false;
     badIMUdata = false;
-    firstArmComplete = false;
     firstMagYawInit = false;
     secondMagYawInit = false;
     dtIMUavg = 0.0025f;
@@ -4231,8 +4135,8 @@ void NavEKF2_core::InitialiseVariables()
     posTimeout = true;
     velTimeout = true;
     gpsVelGlitchOffset.zero();
-    filterArmed = false;
-    prevFilterArmed = false;
+    isAiding = false;
+    prevIsAiding = false;
     memset(&faultStatus, 0, sizeof(faultStatus));
     hgtRate = 0.0f;
     mag_state.q0 = 1;
@@ -4269,7 +4173,7 @@ void NavEKF2_core::InitialiseVariables()
     velCorrection.zero();
     gpsQualGood = false;
     gpsNotAvailable = true;
-
+    motorsArmed = false;
 }
 
 // return true if we should use the airspeed sensor
@@ -4294,7 +4198,7 @@ bool NavEKF2_core::optFlowDataPresent(void) const
 // return true if the filter to be ready to use gps
 bool NavEKF2_core::readyToUseGPS(void) const
 {
-    return validOrigin && tiltAlignComplete && yawAlignComplete;
+    return validOrigin && tiltAlignComplete && yawAlignComplete && gpsQualGood;
 }
 
 // return true if we should use the compass
@@ -4487,73 +4391,191 @@ void NavEKF2_core::send_status_report(mavlink_channel_t chan)
 
 }
 
-// Check arm status and perform required checks and mode changes
-void NavEKF2_core::performArmingChecks()
+// Control filter mode transitions
+void NavEKF2_core::controlFilterModes()
 {
-    // don't allow filter to arm until it has been running for long enough to stabilise
-    prevFilterArmed = filterArmed;
-    filterArmed = ((readyToUseGPS() || frontend._fusionModeGPS == 3) && (imuSampleTime_ms - ekfStartTime_ms) > 1000);
-
-    // check to see if arm status has changed and reset states if it has
-    if (filterArmed != prevFilterArmed) {
-        // 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 (filterArmed && !firstArmComplete) {
-            firstArmComplete = true;
+    // determine if the vehicle is manoevring
+    if (accNavMagHoriz > 0.5f) {
+        manoeuvring = true;
+    } else {
+        manoeuvring = false;
+    }
+
+    // Detect if we are in flight
+    detectFlight();
+
+    // If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
+    inhibitWindStates = ((!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE));
+    // request mag calibration for both in-air and manoeuvre threshold options
+    bool magCalRequested = ((frontend._magCal == 0) && !onGround) || ((frontend._magCal == 1) && manoeuvring)  || (frontend._magCal == 3);
+    // deny mag calibration request if we aren't using the compass, are in the pre-arm constant position mode or it has been inhibited by the user
+    bool magCalDenied = !use_compass() || (PV_AidingMode == AID_NONE) || (frontend._magCal == 2);
+    // inhibit the magnetic field calibration if not requested or denied
+    inhibitMagStates = (!magCalRequested || magCalDenied);
+
+    if (inhibitMagStates && inhibitWindStates) {
+        stateIndexLim = 15;
+    } else if (inhibitWindStates) {
+        stateIndexLim = 21;
+    } else {
+        stateIndexLim = 23;
+    }
+
+    // Check for tilt convergence
+    float alpha = 1.0f*dtIMUavg;
+    float temp=tiltErrVec.length();
+    tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt;
+    if (tiltErrFilt < 0.005f && !tiltAlignComplete) {
+        tiltAlignComplete = true;
+        hal.console->printf("EKF tilt alignment complete\n");
+    }
+
+    // once tilt has converged, align yaw using magnetic field measurements
+    if (tiltAlignComplete && !yawAlignComplete) {
+        Vector3f eulerAngles;
+        getEulerAngles(eulerAngles);
+        stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
+        StoreQuatReset();
+        yawAlignComplete = true;
+        hal.console->printf("EKF yaw alignment complete\n");
+    }
+
+    // Detect if we are about to fly so that reference readings can be taken
+    // Use change in motor arm status as a surrogate
+    if (hal.util->get_soft_armed() && !motorsArmed) {
+        // store vertical position at start of flight to use as a reference for ground relative checks
+        posDownAtTakeoff = stateStruct.position.z;
+        // store the range finder measurement which will be used as a reference to detect when we have taken off
+        rngAtStartOfFlight = rngMea;
+        // set the time at which we arm to assist with takeoff detection
+        timeAtArming_ms =  imuSampleTime_ms;
+    }
+    motorsArmed = hal.util->get_soft_armed();
+
+    // Monitor the gain in height and reetthe magnetic field states and heading when initial altitude has been gained
+    // This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
+    if (motorsArmed && !firstMagYawInit && (stateStruct.position.z  - posDownAtTakeoff) < -1.5f) {
+           // Do the first in-air yaw and earth mag field initialisation when the vehicle has gained 1.5m of altitude after commencement of flight
+           Vector3f eulerAngles;
+           getEulerAngles(eulerAngles);
+           stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
+           StoreQuatReset();
+           firstMagYawInit = true;
+       } else if (motorsArmed && !secondMagYawInit && (stateStruct.position.z - posDownAtTakeoff) < -5.0f) {
+           // Do the second and final yaw and earth mag field initialisation when the vehicle has gained 5.0m of altitude after commencement of flight
+           // This second and final correction is needed for flight from large metal structures where the magnetic field distortion can extend up to 5m
            Vector3f eulerAngles;
            getEulerAngles(eulerAngles);
            stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
            StoreQuatReset();
+           secondMagYawInit = true;
+       }
+
+
+    // Set the type of inertial navigation aiding
+    setAidingMode();
+
+}
+
+// Detect if we are in flight
+void NavEKF2_core::detectFlight()
+{
+    // if we are a fly forward type vehicle (eg plane), then in-air mode can be determined through a combination of speed and height criteria
+    if (assume_zero_sideslip()) {
+        // Evaluate a numerical score that defines the likelihood we are in the air
+        float gndSpdSq = sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y);
+        bool highGndSpd = false;
+        bool highAirSpd = false;
+        bool largeHgtChange = false;
+
+        // trigger at 8 m/s airspeed
+        if (_ahrs->airspeed_sensor_enabled()) {
+            const AP_Airspeed *airspeed = _ahrs->get_airspeed();
+            if (airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 10.0f) {
+                highAirSpd = true;
+            }
+        }
+
+        // trigger at 10 m/s GPS velocity, but not if GPS is reporting bad velocity errors
+        if (gndSpdSq > 100.0f && gpsSpdAccuracy < 1.0f) {
+            highGndSpd = true;
         }
-        // store vertical position at arming to use as a reference for ground relative cehcks
-        if (filterArmed) {
-            posDownAtArming = stateStruct.position.z;
+
+        // trigger if more than 10m away from initial height
+        if (fabsf(baroDataDelayed.hgt) > 10.0f) {
+            largeHgtChange = true;
         }
+
+        // to go to in-air mode we also need enough GPS velocity to be able to calculate a reliable ground track heading and either a lerge height or airspeed change
+        if (onGround && highGndSpd && (highAirSpd || largeHgtChange)) {
+            onGround = false;
+        }
+        // if is possible we are in flight, set the time this condition was last detected
+        if (highGndSpd || highAirSpd || largeHgtChange) {
+            airborneDetectTime_ms = imuSampleTime_ms;
+        }
+        // after 5 seconds of not detecting a possible flight condition, we transition to on-ground mode
+        if(!onGround && ((imuSampleTime_ms - airborneDetectTime_ms) > 5000)) {
+            onGround = true;
+        }
+        // perform a yaw alignment check against GPS if exiting on-ground mode, bu tonly if we have enough ground speed
+        // this is done to protect against unrecoverable heading alignment errors due to compass faults
+        if (!onGround && prevOnGround) {
+            alignYawGPS();
+        }
+    }
+    // store current on-ground status for next time
+    prevOnGround = onGround;
+}
+
+// Set inertial navigation aiding mode
+void NavEKF2_core::setAidingMode()
+{
+    // Determine when to commence aiding for inertial navigation
+    // Save the previous status so we can detect when it has changed
+    prevIsAiding = isAiding;
+    // Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
+    bool filterIsStable = tiltAlignComplete && yawAlignComplete;
+    // If GPS useage has been prohiited then we use flow aiding provided optical flow data is present
+    bool useFlowAiding = (frontend._fusionModeGPS == 3) && optFlowDataPresent();
+    // Start aiding if we have a source of aiding data and the filter attitude algnment is complete
+    // Latch to on. Aiding can be turned off by setting both
+    isAiding = ((readyToUseGPS() || useFlowAiding) && filterIsStable) || isAiding;
+
+    // check to see if we are starting or stopping aiding and set states and modes as required
+    if (isAiding != prevIsAiding) {
+        // We have transitioned either into or out of aiding
         // zero stored velocities used to do dead-reckoning
         heldVelNE.zero();
         // reset the flag that indicates takeoff for use by optical flow navigation
         takeOffDetected = false;
-        // set various  useage modes based on the condition at arming. These are then held until the filter is disarmed.
-        if (!filterArmed) {
-            PV_AidingMode = AID_NONE; // When dis-armed, we only estimate orientation & height using the constant position mode
+        // set various  useage modes based on the condition when we start aiding. These are then held until aiding is stopped.
+        if (!isAiding) {
+            // We have ceased aiding
+            // When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
+            PV_AidingMode = AID_NONE;
             posTimeout = true;
             velTimeout = true;
-             // store the current position to be used to keep reporting the last known position when disarmed
+             // store the current position to be used to keep reporting the last known position
             lastKnownPositionNE.x = stateStruct.position.x;
             lastKnownPositionNE.y = stateStruct.position.y;
             // initialise filtered altitude used to provide a takeoff reference to current baro on disarm
-            // this reduces the time required for the filter to settle before the estimate can be used
+            // this reduces the time required for the baro noise filter to settle before the filtered baro data can be used
             meaHgtAtTakeOff = baroDataDelayed.hgt;
             // reset the vertical position state to faster recover from baro errors experienced during touchdown
             stateStruct.position.z = -meaHgtAtTakeOff;
-        } else if (frontend._fusionModeGPS == 3) { // arming when GPS useage has been prohibited
-            if (optFlowDataPresent()) {
+        } else if (frontend._fusionModeGPS == 3) {
+            // We have commenced aiding, but GPS useage has been prohibited so use optical flow only
             hal.console->printf("EKF is using optical flow\n");
             PV_AidingMode = AID_RELATIVE; // we have optical flow data and can estimate all vehicle states
             posTimeout = true;
             velTimeout = true;
-            } else {
-                hal.console->printf("EKF cannot use aiding\n");
-                PV_AidingMode = AID_NONE; // we don't have optical flow data and will only be able to estimate orientation and height
-                posTimeout = true;
-                velTimeout = true;
-            }
             // Reset the last valid flow measurement time
             flowValidMeaTime_ms = imuSampleTime_ms;
             // Reset the last valid flow fusion time
             prevFlowFuseTime_ms = imuSampleTime_ms;
-            // this avoids issues casued by the time delay associated with arming that can trigger short timeouts
-            rngValidMeaTime_ms = imuSampleTime_ms;
-            // store the range finder measurement which will be used as a reference to detect when we have taken off
-            rangeAtArming = rngMea;
-            // set the time at which we arm to assist with takeoff detection
-            timeAtArming_ms =  imuSampleTime_ms;
-        } else { // arming when GPS useage is allowed
-            if (!gpsQualGood) {
-                hal.console->printf("EKF cannot use aiding\n");
-                PV_AidingMode = AID_NONE; // we don't have have GPS data and will only be able to estimate orientation and height
-                posTimeout = true;
-                velTimeout = true;
         } else {
+            // We have commenced aiding and GPS useage is allowed
             hal.console->printf("EKF is using GPS\n");
             PV_AidingMode = AID_ABSOLUTE; // we have GPS data and can estimate all vehicle states
             posTimeout = false;
@@ -4565,37 +4587,19 @@ void NavEKF2_core::performArmingChecks()
             // reset the last valid position fix time to prevent unwanted activation of GPS glitch logic
             lastPosPassTime_ms = imuSampleTime_ms;
         }
-        }
         // Reset all position, velocity and covariance
         ResetVelocity();
         ResetPosition();
         CovarianceInit();
 
-    } else if (filterArmed && !firstMagYawInit && (stateStruct.position.z  - posDownAtArming) < -1.5f && !assume_zero_sideslip()) {
-        // Do the first in-air 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)
-        // This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
-        Vector3f eulerAngles;
-        getEulerAngles(eulerAngles);
-        stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
-        StoreQuatReset();
-        firstMagYawInit = true;
-    } else if (filterArmed && !secondMagYawInit && (stateStruct.position.z - posDownAtArming) < -5.0f && !assume_zero_sideslip()) {
-        // Do the second and final yaw and earth mag field initialisation when the vehicle has gained 5.0m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff)
-        // This second and final correction is needed for flight from large metal structures where the magnetic field distortion can extend up to 5m
-        Vector3f eulerAngles;
-        getEulerAngles(eulerAngles);
-        stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
-        StoreQuatReset();
-        secondMagYawInit = true;
     }
 
     // Always turn aiding off when the vehicle is disarmed
-    if (!filterArmed) {
+    if (!isAiding) {
         PV_AidingMode = AID_NONE;
         posTimeout = true;
         velTimeout = true;
     }
-
 }
 
 // Set the NED origin to be used until the next filter reset
@@ -4621,7 +4625,7 @@ bool NavEKF2_core::getOriginLLH(struct Location &loc) const
 // set the LLH location of the filters NED origin
 bool NavEKF2_core::setOriginLLH(struct Location &loc)
 {
-    if (filterArmed) {
+    if (isAiding) {
         return false;
     }
     EKF_origin = loc;
@@ -4811,7 +4815,7 @@ void NavEKF2_core::readRangeFinder(void)
             rngMea = max(storedRngMeas[midIndex],rngOnGnd);
             newDataRng = true;
             rngValidMeaTime_ms = imuSampleTime_ms;
-        } else if (!filterArmed) {
+        } else if (!motorsArmed) {
             // if not armed and no return, we assume on ground range
             rngMea = rngOnGnd;
             newDataRng = true;
@@ -4826,7 +4830,7 @@ void NavEKF2_core::readRangeFinder(void)
 // Detect takeoff for optical flow navigation
 void NavEKF2_core::detectOptFlowTakeoff(void)
 {
-    if (filterArmed && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) {
+    if (motorsArmed && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) {
         const AP_InertialSensor &ins = _ahrs->get_ins();
         Vector3f angRateVec;
         Vector3f gyroBias;
@@ -4838,7 +4842,7 @@ void NavEKF2_core::detectOptFlowTakeoff(void)
             angRateVec = ins.get_gyro() - gyroBias;
         }
 
-        takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rngMea > (rangeAtArming + 0.1f)));
+        takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rngMea > (rngAtStartOfFlight + 0.1f)));
     }
 }
 

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -426,9 +426,6 @@ private:
     // calculate the NED earth spin vector in rad/sec
     void calcEarthRateNED(Vector3f &omega, int32_t latitude) const;
 
-    // calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed
-    void SetFlightAndFusionModes();
-
     // initialise the covariance matrix
     void CovarianceInit();
 
@@ -513,8 +510,14 @@ private:
     // fuse optical flow measurements into the main filter
     void FuseOptFlow();
 
-    // Check arm status and perform required checks and mode changes
-    void performArmingChecks();
+    // Control filter mode changes
+    void controlFilterModes();
+
+    // Determine if we are flying or on the ground
+    void detectFlight();
+
+    // Set inertial navigaton aiding mode
+    void setAidingMode();
 
     // Set the NED origin to be used until the next filter reset
     void setOrigin();
@@ -646,20 +649,19 @@ 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 and covariances are to remain constant
-    bool firstArmComplete;          // true when first transition out of static mode has been performed after start up
     bool firstMagYawInit;           // true when the first post takeoff initialisation of earth field and yaw angle has been performed
     bool secondMagYawInit;          // true when the second post takeoff initialisation of earth field and yaw angle has been performed
     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 filterArmed;               // true when the vehicle is disarmed
-    bool prevFilterArmed;           // vehicleArmed from previous frame
+    bool isAiding;                  // true when the filter is fusing position, velocity or flow measurements
+    bool prevIsAiding;              // isAiding from previous frame
     struct Location EKF_origin;     // LLH origin of the NED axis system - do not change unless filter is reset
     bool validOrigin;               // true when the EKF origin is valid
     float gpsSpdAccuracy;           // estimated speed accuracy in m/s returned by the UBlox GPS receiver
     uint32_t lastGpsVelFail_ms;     // time of last GPS vertical velocity consistency check fail
     Vector3f lastMagOffsets;        // magnetometer offsets returned by compass object from previous update
     uint32_t lastGpsAidBadTime_ms;  // time in msec gps aiding was last detected to be bad
-    float posDownAtArming;          // flight vehicle vertical position at arming used as a reference point
+    float posDownAtTakeoff;         // flight vehicle vertical position at arming used as a reference point
     bool highYawRate;               // true when the vehicle is doing rapid yaw rotation where gyro scale factor errors could cause loss of heading reference
     float yawRateFilt;              // filtered yaw rate used to determine when the vehicle is doing rapid yaw rotation where gyro scel factor errors could cause loss of heading reference
     bool useGpsVertVel;             // true if GPS vertical velocity should be used
@@ -698,6 +700,7 @@ private:
     bool gpsQualGood;               // true when the GPS quality can be used to initialise the navigation system
     uint32_t magYawResetTimer_ms;   // timer in msec used to track how long good magnetometer data is failing innovation consistency checks
     bool consistentMagData;         // true when the magnetometers are passing consistency checks
+    bool motorsArmed;               // true when the motors have been armed
 
     // variables added for optical flow fusion
     of_elements storedOF[OBS_BUFFER_LENGTH];    // OF data buffer
@@ -756,7 +759,7 @@ private:
 
     // Movement detector
     bool takeOffDetected;           // true when takeoff for optical flow navigation has been detected
-    float rangeAtArming;            // range finder measurement when armed
+    float rngAtStartOfFlight;       // range finder measurement at start of flight
     uint32_t timeAtArming_ms;       // time in msec that the vehicle armed
 
     // baro ground effect