Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: use DAL APIs for takeoff/touchdown expected

Browse Source
This commit is contained in:
Andrew Tridgell 2021-05-29 09:54:14 +10:00 committed by Randy Mackay
parent c00f110f3d
commit 32b079911f
8 changed files with 8 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -1233,40 +1233,6 @@ void NavEKF2::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &raw
}
}
// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect
void NavEKF2::setTakeoffExpected(bool val)
{
if (val) {
AP::dal().log_event2(AP_DAL::Event::setTakeoffExpected);
} else {
AP::dal().log_event2(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 NavEKF2::setTouchdownExpected(bool val)
{
if (val) {
AP::dal().log_event2(AP_DAL::Event::setTouchdownExpected);
} else {
AP::dal().log_event2(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

9
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -192,14 +192,6 @@ public:
// posOffset is the XYZ flow sensor position in the body frame in m
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset);
// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect
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
@ -407,7 +399,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 uint8_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 fusionTimeStep_ms = 10; // The minimum time interval between covariance predictions and measurement fusions in msec
const float maxYawEstVelInnov = 2.0f; // Maximum acceptable length of the velocity innovation returned by the EKF-GSF yaw estimator (m/s)

4
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -512,8 +512,8 @@ void NavEKF2_core::updateFilterStatus(void)
filterStatus.flags.pred_horiz_pos_rel = ((optFlowNavPossible || gpsNavPossible) && filterHealthy) || filterStatus.flags.horiz_pos_rel; // we should be able to estimate a relative position when we enter flight mode
filterStatus.flags.pred_horiz_pos_abs = (gpsNavPossible && filterHealthy) || filterStatus.flags.horiz_pos_abs; // we should be able to estimate an absolute position when we enter flight mode
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.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 and is in a ground effect mitigation mode
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) && !extNavUsedForPos; // GPS glitching is affecting navigation accuracy
filterStatus.flags.gps_quality_good = gpsGoodToAlign;

2
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -672,7 +672,7 @@ void NavEKF2_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 copter downwash corrupting the EKF altitude during initial ascent
if (getTakeoffExpected()) {
if (dal.get_takeoff_expected()) {
baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff);
}

8
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -815,7 +815,7 @@ void NavEKF2_core::FuseVelPosNED()
const float gndMaxBaroErr = 4.0f;
const float gndBaroInnovFloor = -0.5f;
if(getTouchdownExpected() && activeHgtSource == HGT_SOURCE_BARO) {
if(dal.get_touchdown_expected() && 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:
@ -1029,7 +1029,7 @@ void NavEKF2_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()) {
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);
@ -1091,12 +1091,12 @@ void NavEKF2_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 in ground effect
if (getTakeoffExpected() || getTouchdownExpected()) {
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 copter downwash corrupting the EKF altitude during initial ascent
if (motorsArmed && getTakeoffExpected()) {
if (motorsArmed && dal.get_takeoff_expected()) {
hgtMea = MAX(hgtMea, meaHgtAtTakeOff);
}
} else {

37
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
View File

@ -417,43 +417,6 @@ void NavEKF2_core::detectFlight()
}
// determine if a takeoff is expected so that we can compensate for expected barometer errors due to ground effect
bool NavEKF2_core::getTakeoffExpected()
{
if (expectGndEffectTakeoff && imuSampleTime_ms - takeoffExpectedSet_ms > frontend->gndEffectTimeout_ms) {
expectGndEffectTakeoff = false;
}
return expectGndEffectTakeoff;
}
// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect
void NavEKF2_core::setTakeoffExpected(bool val)
{
takeoffExpectedSet_ms = imuSampleTime_ms;
expectGndEffectTakeoff = val;
}
// determine if a touchdown is expected so that we can compensate for expected barometer errors due to ground effect
bool NavEKF2_core::getTouchdownExpected()
{
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 NavEKF2_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

6
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -190,10 +190,6 @@ void NavEKF2_core::InitialiseVariables()
inhibitWindStates = true;
gndOffsetValid = false;
validOrigin = false;
takeoffExpectedSet_ms = 0;
expectGndEffectTakeoff = false;
touchdownExpectedSet_ms = 0;
expectGndEffectTouchdown = false;
gpsSpdAccuracy = 0.0f;
gpsPosAccuracy = 0.0f;
gpsHgtAccuracy = 0.0f;
@ -935,7 +931,7 @@ void NavEKF2_core::CovariancePrediction()
for (uint8_t i= 0; i<=8; i++) processNoise[i] = 0.0f;
for (uint8_t i=9; i<=11; i++) processNoise[i] = dAngBiasSigma;
for (uint8_t i=12; i<=14; i++) processNoise[i] = dAngScaleSigma;
if (expectGndEffectTakeoff) {
if (dal.get_takeoff_expected()) {
processNoise[15] = 0.0f;
} else {
processNoise[15] = dVelBiasSigma;

18
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -207,14 +207,6 @@ public:
*/
bool getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow);
// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect
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
@ -680,12 +672,6 @@ 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();
// determine if a touchdown is expected so that we can compensate for expected barometer errors due to ground effect
bool getTouchdownExpected();
// Assess GPS data quality and set gpsGoodToAlign if good enough to align the EKF
void calcGpsGoodToAlign(void);
@ -1068,10 +1054,6 @@ 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
uint32_t touchdownExpectedSet_ms; // system time at which expectGndEffectTouchdown was set
float meaHgtAtTakeOff; // height measured at commencement of takeoff
// control of post takeoff magnetic field and heading resets