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

AP_NavEKF2: allow for more than 327m range rangefinders

Browse Source
This commit is contained in:
Peter Barker 2024-10-11 14:46:47 +11:00 committed by Andrew Tridgell
parent 680fb999c5
commit e85539acf2
4 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -483,7 +483,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). 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.
// @Description: Range finder can be used as the primary height source when below this percentage of its maximum range (see RNGFND*_MAX). 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

4
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -28,7 +28,7 @@ void NavEKF2_core::readRangeFinder(void)
return;
}
rngOnGnd = MAX(_rng->ground_clearance_cm_orient(ROTATION_PITCH_270) * 0.01f, 0.05f);
rngOnGnd = MAX(_rng->ground_clearance_orient(ROTATION_PITCH_270), 0.05f);
// read range finder at 20Hz
// TODO better way of knowing if it has new data
@ -51,7 +51,7 @@ void NavEKF2_core::readRangeFinder(void)
rngMeasIndex[sensorIndex] = 0;
}
storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25;
storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance_cm() * 0.01f;
storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance();
} else {
continue;
}

2
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -66,7 +66,7 @@ bool NavEKF2_core::getHeightControlLimit(float &height) const
// we really, really shouldn't be here.
return false;
}
height = MAX(float(_rng->max_distance_cm_orient(ROTATION_PITCH_270)) * 0.007f - 1.0f, 1.0f);
height = MAX(float(_rng->max_distance_orient(ROTATION_PITCH_270)) * 0.7f - 1.0f, 1.0f);
#else
return false;
#endif

2
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -979,7 +979,7 @@ void NavEKF2_core::selectHeightForFusion()
activeHgtSource = HGT_SOURCE_RNG;
} else if ((frontend->_useRngSwHgt > 0) && ((frontend->_altSource == 0) || (frontend->_altSource == 2)) && _rng && rangeFinderDataIsFresh) {
// determine if we are above or below the height switch region
ftype rangeMaxUse = 1e-4f * (float)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt;
const ftype rangeMaxUse = 1e-2f * _rng->max_distance_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt;
bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse;
bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse;