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

AP_NavEKF2: Fix range aid switching logic 

Clarifies use of EK2_RANGE_USE_HGT and EK2_ALT_SOURCE parameters to control range finder use.
Setting EK2_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 EK2_ALT_SOURCE parameter.
zr-v5.1
Paul Riseborough 5 years ago committed by Randy Mackay
parent
906731222d
commit
d118c8fc38
2 changed files with 37 additions and 39 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_NavEKF2/AP_NavEKF2.cpp
  2. 72
      libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

4
libraries/AP_NavEKF2/AP_NavEKF2.cpp
Unescape View File

@ -199,7 +199,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { @@ -199,7 +199,7 @@ const AP_Param::GroupInfo NavEKF2::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 (EK2_GPS_TYPE = 3), Do not use "1" for terrain following. NOTE: the EK2_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground.
// @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 EK2_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground in conjunction with EK2_ALT_SOURCE = 0 or 2 (Baro or GPS).
// @Values: 0:Use Baro, 1:Use Range Finder, 2:Use GPS, 3:Use Range Beacon
// @User: Advanced
// @RebootRequired: True
@ -482,7 +482,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { @@ -482,7 +482,7 @@ const AP_Param::GroupInfo NavEKF2::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 EK2_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 EK2_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

72
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
Unescape View File

@ -987,50 +987,48 @@ void NavEKF2_core::selectHeightForFusion() @@ -987,50 +987,48 @@ void NavEKF2_core::selectHeightForFusion()
baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);
// select height source
if (extNavUsedForPos) {
// always use external vision as the height source if using for position.
if (extNavUsedForPos) {
// 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 {
// 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
float horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y);
bool dontTrustTerrain = ((horizSpeed > frontend->_useRngSwSpd) && filterStatus.flags.horiz_vel) || !terrainHgtStable;
float trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f));
bool trustTerrain = (horizSpeed < trust_spd_trigger) && 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;
} 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
float horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y);
bool dontTrustTerrain = ((horizSpeed > frontend->_useRngSwSpd) && filterStatus.flags.horiz_vel) || !terrainHgtStable;
float trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f));
bool trustTerrain = (horizSpeed < trust_spd_trigger) && 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 && (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