diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 3386cbc903..05fda4a900 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1681,9 +1681,15 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo const float terrain_alt = getLandingTerrainAltitudeEstimate(now, pos_sp_curr.alt); float altitude_setpoint; - if (_current_altitude > terrain_alt + glide_slope_rel_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; + + if (_current_altitude > glide_slope_reference_alt + glide_slope_rel_alt) { // descend to the glide slope - altitude_setpoint = terrain_alt + glide_slope_rel_alt; + altitude_setpoint = glide_slope_reference_alt + glide_slope_rel_alt; } else { // continue horizontally diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 9d1b6f817d..a37bb1ef62 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -849,7 +849,8 @@ private: (ParamFloat) _param_fw_lnd_fl_sink, (ParamFloat) _param_fw_lnd_td_off, (ParamInt) _param_fw_lnd_nudge, - (ParamInt) _param_fw_lnd_abort + (ParamInt) _param_fw_lnd_abort, + (ParamBool) _param_fw_lnd_ter_rel ) }; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 448a4195a1..e7aca00b88 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -1068,3 +1068,17 @@ PARAM_DEFINE_INT32(FW_LND_NUDGE, 0); * @group FW L1 Control */ PARAM_DEFINE_INT32(FW_LND_ABORT, 0); + +/** + * Calculate the landing glide slope relative to the terrain estimate. + * + * If enabled, the terrain estimate (e.g. via distance sensor) will be used as the glide slope reference altitude, following + * all bumps in the terrain below the landing approach. + * + * If disabled, the land waypoint altitude will be a fixed glide slope reference, and the distance sensor (if enabled via + * FW_LND_USETER) will only be used to trigger the flare. + * + * @boolean + * @group FW L1 Control + */ +PARAM_DEFINE_INT32(FW_LND_TER_REL, 0);