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

AP_NavEKF3: 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 e85539acf2
commit b633084df3
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_NavEKF3/AP_NavEKF3.cpp
View File

@ -480,7 +480,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 RNGFNDx_MAX_CM) and the primary height source is Baro or GPS (see EK3_SRCx_POSZ). 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 RNGFNDx_MAX) and the primary height source is Baro or GPS (see EK3_SRCx_POSZ). 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_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -25,7 +25,7 @@ void NavEKF3_core::readRangeFinder(void)
if (_rng == nullptr) {
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);
// limit update rate to maximum allowed by data buffers
if ((imuSampleTime_ms - lastRngMeasTime_ms) > frontend->sensorIntervalMin_ms) {
@ -47,7 +47,7 @@ void NavEKF3_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_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -74,7 +74,7 @@ bool NavEKF3_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_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -1208,7 +1208,7 @@ void NavEKF3_core::selectHeightForFusion()
activeHgtSource = AP_NavEKF_Source::SourceZ::RANGEFINDER;
} else if ((frontend->_useRngSwHgt > 0) && ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BARO) || (frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS)) && _rng && rangeFinderDataIsFresh) {
// determine if we are above or below the height switch region
ftype rangeMaxUse = 1e-4 * (ftype)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt;
const ftype rangeMaxUse = 1e-2 * (ftype)_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) && (imuSampleTime_ms - gndHgtValidTime_ms < 1000);