diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index e3eb839bcd..e4ca59b300 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -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 // 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 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) { diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index cf4c9c9e95..26190ba151 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -350,6 +350,12 @@ private: // [us] time at which we had last valid terrain alt hrt_abstime _last_time_terrain_alt_was_valid{0}; + enum TerrainEstimateUseOnLanding { + kDisableTerrainEstimation = 0, + kTriggerFlareWithTerrainEstimate, + kFollowTerrainRelativeLandingGlideSlope + }; + // AIRSPEED float _airspeed{0.0f}; @@ -794,7 +800,7 @@ private: (ParamFloat) _param_fw_lnd_flalt, (ParamFloat) _param_fw_thrtc_sc, (ParamBool) _param_fw_lnd_earlycfg, - (ParamBool) _param_fw_lnd_useter, + (ParamInt) _param_fw_lnd_useter, (ParamFloat) _param_fw_p_lim_max, (ParamFloat) _param_fw_p_lim_min, @@ -856,8 +862,7 @@ private: (ParamFloat) _param_fw_lnd_fl_sink, (ParamFloat) _param_fw_lnd_td_off, (ParamInt) _param_fw_lnd_nudge, - (ParamInt) _param_fw_lnd_abort, - (ParamBool) _param_fw_lnd_ter_rel + (ParamInt) _param_fw_lnd_abort ) }; 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 5853222e30..25f39040a3 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 @@ -411,14 +411,20 @@ PARAM_DEFINE_FLOAT(FW_TKO_PITCH_MIN, 10.0f); PARAM_DEFINE_FLOAT(FW_LND_FLALT, 0.5f); /** - * Use terrain estimate during landing. This is critical for detecting when to flare, and should be enabled if possible. + * Use terrain estimation during landing. This is critical for detecting when to flare, and should be enabled if possible. * - * If enabled and no measurement is found within a given timeout, the landing waypoint will be used or the - * landing will be aborted, depending on the criteria set in FW_LND_ABORT. + * NOTE: terrain estimate is currently solely derived from a distance sensor. + * + * If enabled and no measurement is found within a given timeout, the landing waypoint altitude will be used OR the landing + * will be aborted, depending on the criteria set in FW_LND_ABORT. * * If disabled, FW_LND_ABORT terrain based criteria are ignored. * - * @boolean + * @min 0 + * @max 2 + * @value 0 Disable the terrain estimate + * @value 1 Use the terrain estimate to trigger the flare (only) + * @value 2 Calculate landing glide slope relative to the terrain estimate * @group FW L1 Control */ PARAM_DEFINE_INT32(FW_LND_USETER, 1); @@ -1070,17 +1076,3 @@ PARAM_DEFINE_INT32(FW_LND_NUDGE, 2); * @group FW L1 Control */ PARAM_DEFINE_INT32(FW_LND_ABORT, 3); - -/** - * 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);