From c00f110f3d5140e85bf5e779d51c65f3a92131bd Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 28 May 2021 14:01:20 +1000 Subject: [PATCH] AP_NavEKF3: use DAL APIs for takeoff/touchdown expected --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 35 ------------- libraries/AP_NavEKF3/AP_NavEKF3.h | 10 ---- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 4 +- .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 2 +- .../AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 8 +-- .../AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp | 50 +------------------ libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 9 +--- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 25 ---------- 8 files changed, 11 insertions(+), 132 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 959b4e3535..80fce507ad 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1696,41 +1696,6 @@ void NavEKF3::convert_parameters() } } -// called by vehicle code to specify that a takeoff is happening -// 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 (val) { - AP::dal().log_event3(AP_DAL::Event::setTakeoffExpected); - } else { - AP::dal().log_event3(AP_DAL::Event::unsetTakeoffExpected); - } - - if (core) { - for (uint8_t i=0; isources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS); // GPS glitching is affecting navigation accuracy filterStatus.flags.gps_quality_good = gpsGoodToAlign; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 44ae5739d0..9c93a285bb 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -737,7 +737,7 @@ void NavEKF3_core::readBaroData() // 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 rotor wash ground interaction corrupting the EKF altitude during initial ascent - if (expectGndEffectTakeoff) { + if (dal.get_takeoff_expected()) { 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 94b4dff55e..21dd4d68d1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -851,7 +851,7 @@ void NavEKF3_core::FuseVelPosNED() const float gndMaxBaroErr = 4.0f; const float gndBaroInnovFloor = -0.5f; - if (expectGndEffectTouchdown && activeHgtSource == AP_NavEKF_Source::SourceZ::BARO) { + if (dal.get_touchdown_expected() && activeHgtSource == AP_NavEKF_Source::SourceZ::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: @@ -1111,7 +1111,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 (!expectGndEffectTakeoff) { + if (!dal.get_takeoff_expected()) { 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); @@ -1177,12 +1177,12 @@ void NavEKF3_core::selectHeightForFusion() // 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 experiencing rotor wash ground interaction - if (expectGndEffectTakeoff || expectGndEffectTouchdown) { + if (dal.get_takeoff_expected() || dal.get_touchdown_expected()) { 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 rotor wash ground interaction corrupting the EKF altitude during initial ascent - if (motorsArmed && expectGndEffectTakeoff) { + if (motorsArmed && dal.get_takeoff_expected()) { 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 f6dac0b8ff..5d962e15f6 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -331,7 +331,7 @@ void NavEKF3_core::detectFlight() if (motorsArmed) { onGround = false; - if (highGndSpd && (expectTakeoff || highAirSpd || largeHgtChange)) { + if (highGndSpd && (dal.get_takeoff_expected() || highAirSpd || largeHgtChange)) { // to a high certainty we are flying inFlight = true; } @@ -387,16 +387,11 @@ 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) || !gpsSpdAccPass) { // disable filter bank EKFGSF_run_filterbank = false; - } else if (yawEstimator != nullptr && !EKFGSF_run_filterbank && (inFlight || expectTakeoff) && gpsSpdAccPass) { + } else if (yawEstimator != nullptr && !EKFGSF_run_filterbank && (inFlight || dal.get_takeoff_expected()) && gpsSpdAccPass) { // flying or about to fly so reset counters and enable filter bank when GPS is good EKFGSF_yaw_reset_ms = 0; EKFGSF_yaw_reset_request_ms = 0; @@ -414,47 +409,6 @@ void NavEKF3_core::detectFlight() } -// 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 (expectTakeoff && imuSampleTime_ms - takeoffExpectedSet_ms > frontend->gndEffectTimeout_ms) { - expectTakeoff = false; - } - - return expectTakeoff; -} - -// called by vehicle code to specify that a takeoff is happening -// 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; - expectTakeoff = val; -} - - -// 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; - } - - return expectGndEffectTouchdown; -} - -// called by vehicle code to specify that a touchdown is expected to happen -// causes the EKF to compensate for expected barometer errors due to ground effect -void NavEKF3_core::setTouchdownExpected(bool val) -{ - touchdownExpectedSet_ms = imuSampleTime_ms; - expectGndEffectTouchdown = val; -} - // Set to true if the terrain underneath is stable enough to be used as a height reference // in combination with a range finder. Set to false if the terrain underneath the vehicle // cannot be used as a height reference. Use to prevent range finder operation otherwise diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 464d6896f0..d94edcbb6a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -268,11 +268,6 @@ void NavEKF3_core::InitialiseVariables() inhibitDelAngBiasStates = true; gndOffsetValid = false; validOrigin = false; - takeoffExpectedSet_ms = 0; - expectTakeoff = false; - touchdownExpectedSet_ms = 0; - expectGndEffectTakeoff = false; - expectGndEffectTouchdown = false; gpsSpdAccuracy = 0.0f; gpsPosAccuracy = 0.0f; gpsHgtAccuracy = 0.0f; @@ -901,10 +896,10 @@ void NavEKF3_core::calcOutputStates() // Detect fixed wing launch acceleration using latest data from IMU to enable early startup of filter functions // that use launch acceleration to detect start of flight - if (!inFlight && !expectTakeoff && assume_zero_sideslip()) { + if (!inFlight && !dal.get_takeoff_expected() && assume_zero_sideslip()) { const float launchDelVel = imuDataNew.delVel.x + GRAVITY_MSS * imuDataNew.delVelDT * Tbn_temp.c.x; if (launchDelVel > GRAVITY_MSS * imuDataNew.delVelDT) { - setTakeoffExpected(true); + dal.set_takeoff_expected(); } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index cbdcfae2c4..779e961d6f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -291,15 +291,6 @@ 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 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 - // causes the EKF to compensate for expected barometer errors due to ground effect - void setTouchdownExpected(bool val); - // Set to true if the terrain underneath is stable enough to be used as a height reference // in combination with a range finder. Set to false if the terrain underneath the vehicle // cannot be used as a height reference. Use to prevent range finder operation otherwise @@ -823,15 +814,6 @@ private: // Set the NED origin to be used until the next filter reset void setOrigin(const Location &loc); - // 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(); - - // 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); @@ -1295,15 +1277,8 @@ private: uint32_t timeAtArming_ms; // time in msec that the vehicle armed // baro ground effect - 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 magnetic 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