zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF3: Fix range aid switching logic 

Clarifies use of EK3_RANGE_USE_HGT and EK3_ALT_SOURCE parameters to control range finder use.
Setting EK3_RNG_USE_HGT to a positive number to enable automatic switching between the primary height source (baro or GPS) and range finder should be disabled if any primary height source other than Baro or GPS is selected via the EK3_ALT_SOURCE parameter.
zr-v5.1
Paul Riseborough 5 years ago committed by Randy Mackay
parent
3737c13784
commit
906731222d
2 changed files with 41 additions and 43 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      libraries/AP_NavEKF3/AP_NavEKF3.cpp
  2. 80
      libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

4
libraries/AP_NavEKF3/AP_NavEKF3.cpp
Unescape View File

@ -197,7 +197,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { @@ -197,7 +197,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Param: ALT_SOURCE
// @DisplayName: Primary altitude sensor source
// @Description: Primary height sensor used by the EKF. If the selected option cannot be used, baro is used. 1 uses the range finder and only with optical flow navigation (EK3_GPS_TYPE = 3), Do not use "1" for terrain following. NOTE: the EK3_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground. Setting 4 uses external nav system data, but only if data is also being used for horizontal position
// @Description: Primary height sensor used by the EKF. If a sensor other than Baro is selected and becomes unavailable, then the Baro sensor will be used as a fallback. NOTE: the EK3_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground in conjunction with EK3_ALT_SOURCE = 0 or 2 (Baro or GPS). NOTE: Setting EK3_ALT_SOURCE = 4 uses external nav system data only if data is also being used for horizontal position
// @Values: 0:Use Baro, 1:Use Range Finder, 2:Use GPS, 3:Use Range Beacon, 4:Use External Nav
// @User: Advanced
// @RebootRequired: True
@ -477,7 +477,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { @@ -477,7 +477,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Param: RNG_USE_HGT
// @DisplayName: Range finder switch height percentage
// @Description: Range finder can be used as the primary height source when below this percentage of its maximum range (see RNGFND_MAX_CM). Set to -1 when EK3_ALT_SOURCE is not set to range finder. This is not for terrain following.
// @Description: Range finder can be used as the primary height source when below this percentage of its maximum range (see RNGFND_MAX_CM). This will not work unless Baro or GPS height is selected as the primary height source vis EK3_ALT_SOURCE = 0 or 2 respectively. This feature should not be used for terrain following as it is designed for vertical takeoff and landing with climb above the range finder use height before commencing the mission, and with horizontal position changes below that height being limited to a flat region around the takeoff and landing point.
// @Range: -1 70
// @Increment: 1
// @User: Advanced

80
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
Unescape View File

@ -981,55 +981,53 @@ void NavEKF3_core::selectHeightForFusion() @@ -981,55 +981,53 @@ void NavEKF3_core::selectHeightForFusion()
if (extNavUsedForPos && (frontend->_altSource == 4)) {
// always use external navigation as the height source if using for position.
activeHgtSource = HGT_SOURCE_EXTNAV;
} else if (_rng && ((frontend->_useRngSwHgt > 0) && (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) {
if (frontend->_altSource == 1) {
// always use range finder
activeHgtSource = HGT_SOURCE_RNG;
} else if ((frontend->_altSource == 1) && _rng && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) {
// user has specified the range finder as a primary height source
activeHgtSource = HGT_SOURCE_RNG;
} else if ((frontend->_useRngSwHgt > 0) && ((frontend->_altSource == 0) || (frontend->_altSource == 2)) && _rng && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) {
// determine if we are above or below the height switch region
float rangeMaxUse = 1e-4f * (float)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (float)frontend->_useRngSwHgt;
bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse;
bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse;
// If the terrain height is consistent and we are moving slowly, then it can be
// used as a height reference in combination with a range finder
// apply a hysteresis to the speed check to prevent rapid switching
bool dontTrustTerrain, trustTerrain;
if (filterStatus.flags.horiz_vel) {
// We can use the velocity estimate
float horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y);
dontTrustTerrain = (horizSpeed > frontend->_useRngSwSpd) || !terrainHgtStable;
float trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f));
trustTerrain = (horizSpeed < trust_spd_trigger) && terrainHgtStable;
} else {
// determine if we are above or below the height switch region
float rangeMaxUse = 1e-4f * (float)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (float)frontend->_useRngSwHgt;
bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse;
bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse;
// If the terrain height is consistent and we are moving slowly, then it can be
// used as a height reference in combination with a range finder
// apply a hysteresis to the speed check to prevent rapid switching
bool dontTrustTerrain, trustTerrain;
if (filterStatus.flags.horiz_vel) {
// We can use the velocity estimate
float horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y);
dontTrustTerrain = (horizSpeed > frontend->_useRngSwSpd) || !terrainHgtStable;
float trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f));
trustTerrain = (horizSpeed < trust_spd_trigger) && terrainHgtStable;
} else {
// We can't use the velocity estimate
dontTrustTerrain = !terrainHgtStable;
trustTerrain = terrainHgtStable;
}
// We can't use the velocity estimate
dontTrustTerrain = !terrainHgtStable;
trustTerrain = terrainHgtStable;
}
/*
* Switch between range finder and primary height source using height above ground and speed thresholds with
* hysteresis to avoid rapid switching. Using range finder for height requires a consistent terrain height
* which cannot be assumed if the vehicle is moving horizontally.
*/
if ((aboveUpperSwHgt || dontTrustTerrain) && (activeHgtSource == HGT_SOURCE_RNG)) {
// cannot trust terrain or range finder so stop using range finder height
if (frontend->_altSource == 0) {
activeHgtSource = HGT_SOURCE_BARO;
} else if (frontend->_altSource == 2) {
activeHgtSource = HGT_SOURCE_GPS;
}
} else if (belowLowerSwHgt && trustTerrain && (activeHgtSource != HGT_SOURCE_RNG)) {
// reliable terrain and range finder so start using range finder height
activeHgtSource = HGT_SOURCE_RNG;
/*
* Switch between range finder and primary height source using height above ground and speed thresholds with
* hysteresis to avoid rapid switching. Using range finder for height requires a consistent terrain height
* which cannot be assumed if the vehicle is moving horizontally.
*/
if ((aboveUpperSwHgt || dontTrustTerrain) && (activeHgtSource == HGT_SOURCE_RNG)) {
// cannot trust terrain or range finder so stop using range finder height
if (frontend->_altSource == 0) {
activeHgtSource = HGT_SOURCE_BARO;
} else if (frontend->_altSource == 2) {
activeHgtSource = HGT_SOURCE_GPS;
}
} else if (belowLowerSwHgt && trustTerrain && (prevTnb.c.z >= 0.7f)) {
// reliable terrain and range finder so start using range finder height
activeHgtSource = HGT_SOURCE_RNG;
}
} else if (frontend->_altSource == 0) {
activeHgtSource = HGT_SOURCE_BARO;
} else if ((frontend->_altSource == 2) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood) {
activeHgtSource = HGT_SOURCE_GPS;
} else if ((frontend->_altSource == 3) && validOrigin && rngBcnGoodToAlign) {
activeHgtSource = HGT_SOURCE_BCN;
} else {
activeHgtSource = HGT_SOURCE_BARO;
}
// Use Baro alt as a fallback if we lose range finder, GPS or external nav

Write Preview
Loading…
Cancel
Save