aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: Clarify distinct use cases for 'takeoff expected'

Browse Source
This commit is contained in:
Paul Riseborough 2020-06-14 09:39:55 +10:00 committed by Peter Barker
parent e5bd99a66f
commit 8ff6780323
8 changed files with 49 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -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) {

3
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -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

2
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -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

4
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -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);
}

12
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -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;

30
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -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;

3
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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;

23
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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