From 8ff67803236c24eab3af6f60559017f0c9a700ad Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 14 Jun 2020 09:39:55 +1000 Subject: [PATCH] AP_NavEKF3: Clarify distinct use cases for 'takeoff expected' --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 3 +- libraries/AP_NavEKF3/AP_NavEKF3.h | 3 +- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 2 +- .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 4 +-- .../AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 12 ++++---- .../AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp | 30 ++++++++++++------- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 3 +- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 23 +++++++++----- 8 files changed, 49 insertions(+), 31 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 92ccc0ab3c..98659da982 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/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) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 0aaf1b5872..084199397d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 22b50955c5..7b962280e9 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index cc464f1046..202d2b443e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/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); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 585b776980..9b81e02fbb 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/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; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index e245c9127f..0c9ff082c8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/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; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index dacb866561..329060c932 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/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; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 99bafb6a93..65d3eb6ee0 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/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