Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-02 19:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: use DAL APIs for takeoff/touchdown expected

Browse Source
This commit is contained in:
Peter Barker 2021-05-28 14:01:20 +10:00 committed by Randy Mackay
parent f4f713a04f
commit c00f110f3d
8 changed files with 11 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -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; i<num_cores; i++) {
core[i].setTakeoffExpected(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 NavEKF3::setTouchdownExpected(bool val)
{
if (val) {
AP::dal().log_event3(AP_DAL::Event::setTouchdownExpected);
} else {
AP::dal().log_event3(AP_DAL::Event::unsetTouchdownExpected);
}
if (core) {
for (uint8_t i=0; i<num_cores; i++) {
core[i].setTouchdownExpected(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

10
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -270,15 +270,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
@ -478,7 +469,6 @@ private:
const float fScaleFactorPnoise = 1e-10f; // Process noise added to focal length scale factor state variance at each time step
const uint8_t flowTimeDeltaAvg_ms = 100; // average interval between optical flow measurements (msec)
const uint32_t flowIntervalMax_ms = 100; // maximum allowable time between flow fusion events
const uint16_t gndEffectTimeout_ms = 1000; // time in msec that ground effect mode is active after being activated
const float gndEffectBaroScaler = 4.0f; // scaler applied to the barometer observation variance when ground effect mode is active
const uint8_t gndGradientSigma = 50; // RMS terrain gradient percentage assumed by the terrain height estimation
const uint16_t fusionTimeStep_ms = 10; // The minimum time interval between covariance predictions and measurement fusions in msec

4
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -678,8 +678,8 @@ 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 = 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.takeoff = dal.get_takeoff_expected(); // 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 = dal.get_touchdown_expected(); // 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->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS); // GPS glitching is affecting navigation accuracy
filterStatus.flags.gps_quality_good = gpsGoodToAlign;

2
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

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

8
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

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

50
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

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

9
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

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

25
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

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