|
|
|
@ -1686,13 +1686,10 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
@@ -1686,13 +1686,10 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
|
|
|
|
|
_landing_approach_entrance_rel_alt); |
|
|
|
|
|
|
|
|
|
const float terrain_alt = getLandingTerrainAltitudeEstimate(now, pos_sp_curr.alt); |
|
|
|
|
float altitude_setpoint; |
|
|
|
|
const float glide_slope_reference_alt = (_param_fw_lnd_useter.get() == |
|
|
|
|
TerrainEstimateUseOnLanding::kFollowTerrainRelativeLandingGlideSlope) ? terrain_alt : pos_sp_curr.alt; |
|
|
|
|
|
|
|
|
|
// by default the landing waypoint altitude is used for the glide slope reference. this ensures a constant slope
|
|
|
|
|
// during the landing approach, despite any terrain bumps (think tall trees below the landing approach) disrupting
|
|
|
|
|
// the glide behavior. in this case, when FW_LND_USETER==1, the terrain estimate is only used to trigger the flare.
|
|
|
|
|
// however - the option still exists to make the glide slope terrain relative via FW_LND_TER_REL.
|
|
|
|
|
const float glide_slope_reference_alt = (_param_fw_lnd_ter_rel.get()) ? terrain_alt : pos_sp_curr.alt; |
|
|
|
|
float altitude_setpoint; |
|
|
|
|
|
|
|
|
|
if (_current_altitude > glide_slope_reference_alt + glide_slope_rel_alt) { |
|
|
|
|
// descend to the glide slope
|
|
|
|
@ -1706,6 +1703,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
@@ -1706,6 +1703,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
|
|
|
|
|
// flare at the maximum of the altitude determined by the time before touchdown and a minimum flare altitude
|
|
|
|
|
const float flare_rel_alt = math::max(_param_fw_lnd_fl_time.get() * _local_pos.vz, _param_fw_lnd_flalt.get()); |
|
|
|
|
|
|
|
|
|
// the terrain estimate (if enabled) is always used to determine the flaring altitude
|
|
|
|
|
if ((_current_altitude < terrain_alt + flare_rel_alt) || _flaring) { |
|
|
|
|
// flare and land with minimal speed
|
|
|
|
|
|
|
|
|
@ -2655,7 +2653,7 @@ FixedwingPositionControl::calculateLandingApproachVector() const
@@ -2655,7 +2653,7 @@ FixedwingPositionControl::calculateLandingApproachVector() const
|
|
|
|
|
float |
|
|
|
|
FixedwingPositionControl::getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude) |
|
|
|
|
{ |
|
|
|
|
if (_param_fw_lnd_useter.get()) { |
|
|
|
|
if (_param_fw_lnd_useter.get() > TerrainEstimateUseOnLanding::kDisableTerrainEstimation) { |
|
|
|
|
|
|
|
|
|
if (_local_pos.dist_bottom_valid) { |
|
|
|
|
|
|
|
|
|