AP_NavEKF3: Clarify distinct use cases for 'takeoff expected'

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -1440,7 +1440,8 @@ bool NavEKF3::getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, floa
 }
 
 // called by vehicle code to specify that a takeoff is happening
-// causes the EKF to compensate for expected barometer errors due to ground effect
+// causes the EKF to compensate for expected barometer errors due to rotor wash ground interaction
+// causes the EKF to start the EKF-GSF yaw estimator
 void NavEKF3::setTakeoffExpected(bool val)
 {
     if (core) {

libraries/AP_NavEKF3/AP_NavEKF3.h

@@ -307,7 +307,8 @@ public:
     void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms);
 
     // called by vehicle code to specify that a takeoff is happening
-    // causes the EKF to compensate for expected barometer errors due to ground effect
+    // causes the EKF to compensate for expected barometer errors due to rotor wash ground interaction
+    // causes the EKF to start the EKF-GSF yaw estimator
     void setTakeoffExpected(bool val);
 
     // called by vehicle code to specify that a touchdown is expected to happen

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -600,7 +600,7 @@ void  NavEKF3_core::updateFilterStatus(void)
     filterStatus.flags.pred_horiz_pos_rel = filterStatus.flags.horiz_pos_rel; // EKF3 enters the required mode before flight
     filterStatus.flags.pred_horiz_pos_abs = filterStatus.flags.horiz_pos_abs; // EKF3 enters the required mode before flight
     filterStatus.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected
-    filterStatus.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode
+    filterStatus.flags.takeoff = expectTakeoff; // The EKF has been told to expect takeoff is in a ground effect mitigation mode and has started the EKF-GSF yaw estimator
     filterStatus.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
     filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
     filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS != 3); // GPS glitching is affecting navigation accuracy

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -706,8 +706,8 @@ void NavEKF3_core::readBaroData()
         baroDataNew.hgt = baro.get_altitude();
 
         // 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
-        if (getTakeoffExpected() && !assume_zero_sideslip()) {
+        // This prevents negative baro disturbances due to rotor wash ground interaction corrupting the EKF altitude during initial ascent
+        if (expectGndEffectTakeoff) {
             baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff);
         }
 

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -839,7 +839,7 @@ void NavEKF3_core::FuseVelPosNED()
                     const float gndMaxBaroErr = 4.0f;
                     const float gndBaroInnovFloor = -0.5f;
 
-                    if(getTouchdownExpected() && activeHgtSource == HGT_SOURCE_BARO) {
+                    if(expectGndEffectTouchdown && activeHgtSource == HGT_SOURCE_BARO) {
                         // when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor
                         // constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr
                         // this function looks like this:
@@ -1071,7 +1071,7 @@ void NavEKF3_core::selectHeightForFusion()
         }
         // filtered baro data used to provide a reference for takeoff
         // it is is reset to last height measurement on disarming in performArmingChecks()
-        if (!getTakeoffExpected() || !assume_zero_sideslip()) {
+        if (!expectGndEffectTakeoff) {
             const float gndHgtFiltTC = 0.5f;
             const float dtBaro = frontend->hgtAvg_ms*1.0e-3f;
             float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
@@ -1131,13 +1131,13 @@ void NavEKF3_core::selectHeightForFusion()
         fuseHgtData = true;
         // set the observation noise
         posDownObsNoise = 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() && !assume_zero_sideslip()) || getTouchdownExpected()) {
+        // reduce weighting (increase observation noise) on baro if we are likely to be experiencing rotor wash ground interaction
+        if (expectGndEffectTakeoff || expectGndEffectTouchdown) {
             posDownObsNoise *= frontend->gndEffectBaroScaler;
         }
         // 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
-        if (motorsArmed && (getTakeoffExpected() && !assume_zero_sideslip())) {
+        // This prevents negative baro disturbances due to rotor wash ground interaction corrupting the EKF altitude during initial ascent
+        if (motorsArmed && expectGndEffectTakeoff) {
             hgtMea = MAX(hgtMea, meaHgtAtTakeOff);
         }
         velPosObs[5] = -hgtMea;

libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp

@@ -408,11 +408,16 @@ void NavEKF3_core::detectFlight()
         }
     }
 
+    // check if vehicle control code has told the EKF to prepare for takeoff or landing
+    // and if rotor-wash ground interaction is expected to cause Baro errors
+    expectGndEffectTakeoff = updateTakeoffExpected() && !assume_zero_sideslip();
+    updateTouchdownExpected();
+
     // handle reset of counters used to control how many times we will try to reset the yaw to the EKF-GSF value per flight
     if (!prevOnGround && onGround) {
         // landed so disable filter bank
         EKFGSF_run_filterbank = false;
-    } else if (!EKFGSF_run_filterbank && ((!prevInFlight && inFlight) || getTakeoffExpected())) {
+    } else if (!EKFGSF_run_filterbank && ((!prevInFlight && inFlight) || expectTakeoff)) {
         // started flying so reset counters and enable filter bank
         EKFGSF_yaw_reset_ms = 0;
         EKFGSF_yaw_reset_request_ms = 0;
@@ -429,28 +434,31 @@ void NavEKF3_core::detectFlight()
 
 }
 
-// determine if a takeoff is expected so that we can compensate for expected barometer errors due to rotor wash ground interaction
-// also used when in fixed wing mode so that GSG yaw estimator can be started before throw or takeoff roll
-bool NavEKF3_core::getTakeoffExpected()
+// update and return the status that indicates takeoff is expected so that we can compensate for expected
+// barometer errors due to rotor-wash ground interaction and start the EKF-GSF yaw estimator prior to
+// takeoff movement
+bool NavEKF3_core::updateTakeoffExpected()
 {
-    if (expectGndEffectTakeoff && imuSampleTime_ms - takeoffExpectedSet_ms > frontend->gndEffectTimeout_ms) {
-        expectGndEffectTakeoff = false;
+    if (expectTakeoff && imuSampleTime_ms - takeoffExpectedSet_ms > frontend->gndEffectTimeout_ms) {
+        expectTakeoff = false;
     }
 
-    return expectGndEffectTakeoff;
+    return expectTakeoff;
 }
 
 // called by vehicle code to specify that a takeoff is happening
-// causes the EKF to compensate for expected barometer errors due to ground effect
+// causes the EKF to compensate for expected barometer errors due to rotor wash ground interaction
+// causes the EKF to start the EKF-GSF yaw estimator
 void NavEKF3_core::setTakeoffExpected(bool val)
 {
     takeoffExpectedSet_ms = imuSampleTime_ms;
-    expectGndEffectTakeoff = val;
+    expectTakeoff = val;
 }
 
 
-// determine if a touchdown is expected so that we can compensate for expected barometer errors due to ground effect
-bool NavEKF3_core::getTouchdownExpected()
+// update and return the status that indicates touchdown is expected so that we can compensate for expected
+// barometer errors due to rotor-wash ground interaction
+bool NavEKF3_core::updateTouchdownExpected()
 {
     if (expectGndEffectTouchdown && imuSampleTime_ms - touchdownExpectedSet_ms > frontend->gndEffectTimeout_ms) {
         expectGndEffectTouchdown = false;

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -269,8 +269,9 @@ void NavEKF3_core::InitialiseVariables()
     gndOffsetValid =  false;
     validOrigin = false;
     takeoffExpectedSet_ms = 0;
-    expectGndEffectTakeoff = false;
+    expectTakeoff = false;
     touchdownExpectedSet_ms = 0;
+    expectGndEffectTakeoff = false;
     expectGndEffectTouchdown = false;
     gpsSpdAccuracy = 0.0f;
     gpsPosAccuracy = 0.0f;

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -316,7 +316,8 @@ public:
     void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms);
 
     // called by vehicle code to specify that a takeoff is happening
-    // causes the EKF to compensate for expected barometer errors due to ground effect
+    // causes the EKF to compensate for expected barometer errors due to rotor wash ground interaction
+    // causes the EKF to start the EKF-GSF yaw estimator
     void setTakeoffExpected(bool val);
 
     // called by vehicle code to specify that a touchdown is expected to happen
@@ -867,11 +868,14 @@ private:
     // Set the NED origin to be used until the next filter reset
     void setOrigin(const Location &loc);
 
-    // determine if a takeoff is expected so that we can compensate for expected barometer errors due to ground effect
-    bool getTakeoffExpected();
+    // update and return the status that indicates takeoff is expected so that we can compensate for expected
+    // barometer errors due to rotor-wash ground interaction and start the EKF-GSF yaw estimator prior to
+    // takeoff movement
+    bool updateTakeoffExpected();
 
-    // determine if a touchdown is expected so that we can compensate for expected barometer errors due to ground effect
-    bool getTouchdownExpected();
+    // update and return the status that indicates touchdown is expected so that we can compensate for expected
+    // barometer errors due to rotor-wash ground interaction
+    bool updateTouchdownExpected();
 
     // Assess GPS data quality and set gpsGoodToAlign
     void calcGpsGoodToAlign(void);
@@ -1327,12 +1331,15 @@ private:
     uint32_t timeAtArming_ms;       // time in msec that the vehicle armed
 
     // baro ground effect
-    bool expectGndEffectTakeoff;      // external state from ArduCopter - takeoff expected
-    uint32_t takeoffExpectedSet_ms;   // system time at which expectGndEffectTakeoff was set
-    bool expectGndEffectTouchdown;    // external state from ArduCopter - touchdown expected
+    bool expectGndEffectTakeoff;      // external state - takeoff expected in VTOL flight
+    bool expectGndEffectTouchdown;    // external state - touchdown expected in VTOL flight
     uint32_t touchdownExpectedSet_ms; // system time at which expectGndEffectTouchdown was set
     float meaHgtAtTakeOff;            // height measured at commencement of takeoff
 
+    // takeoff preparation used to start EKF-GSF yaw estimator and mitigate rotor-wash ground interaction Baro errors
+    uint32_t takeoffExpectedSet_ms;   // system time at which expectTakeoff was set
+    bool expectTakeoff;               // external state from vehicle conrol code - takeoff expected
+
     // control of post takeoff magentic field and heading resets
     bool finalInflightYawInit;      // true when the final post takeoff initialisation of yaw angle has been performed
     uint8_t magYawAnomallyCount;    // Number of times the yaw has been reset due to a magnetic anomaly during initial ascent