From 485785d81d87659402296f0ddb95864929a4924a Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 15 Feb 2023 11:20:12 +0100 Subject: [PATCH] FW PositionController: circular landing: enable automatic landing aborts Enable automatic landing abort on timed out distance sensor reading also for the circular landing. Do not enable the no-terrain timeout check, as, opposed to the straight landing, we here don't know when to expect the distance sensor to get valid. Signed-off-by: Silvan Fuhrer --- .../FixedwingPositionControl.cpp | 18 +++++++++++------- .../FixedwingPositionControl.hpp | 5 ++++- .../fw_path_navigation_params.c | 2 +- 3 files changed, 16 insertions(+), 9 deletions(-) diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp index b31ea75c1b..3031ec40f5 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp @@ -1549,7 +1549,12 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, const float glide_slope_rel_alt = math::min(along_track_dist_to_touchdown * glide_slope, _landing_approach_entrance_rel_alt); - const float terrain_alt = getLandingTerrainAltitudeEstimate(now, pos_sp_curr.alt); + const bool abort_on_terrain_measurement_timeout = checkLandingAbortBitMask(_param_fw_lnd_abort.get(), + position_controller_landing_status_s::TERRAIN_NOT_FOUND); + const bool abort_on_terrain_timeout = checkLandingAbortBitMask(_param_fw_lnd_abort.get(), + position_controller_landing_status_s::TERRAIN_TIMEOUT); + const float terrain_alt = getLandingTerrainAltitudeEstimate(now, pos_sp_curr.alt, abort_on_terrain_measurement_timeout, + abort_on_terrain_timeout); const float glide_slope_reference_alt = (_param_fw_lnd_useter.get() == TerrainEstimateUseOnLanding::kFollowTerrainRelativeLandingGlideSlope) ? terrain_alt : pos_sp_curr.alt; @@ -1756,7 +1761,9 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, _time_started_landing = now; } - const float terrain_alt = getLandingTerrainAltitudeEstimate(now, pos_sp_curr.alt); + const bool abort_on_terrain_timeout = checkLandingAbortBitMask(_param_fw_lnd_abort.get(), + position_controller_landing_status_s::TERRAIN_TIMEOUT); + const float terrain_alt = getLandingTerrainAltitudeEstimate(now, pos_sp_curr.alt, false, abort_on_terrain_timeout); // 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()); @@ -2678,7 +2685,8 @@ FixedwingPositionControl::calculateLandingApproachVector() const } float -FixedwingPositionControl::getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude) +FixedwingPositionControl::getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude, + const bool abort_on_terrain_measurement_timeout, const bool abort_on_terrain_timeout) { if (_param_fw_lnd_useter.get() > TerrainEstimateUseOnLanding::kDisableTerrainEstimation) { @@ -2694,8 +2702,6 @@ FixedwingPositionControl::getLandingTerrainAltitudeEstimate(const hrt_abstime &n if (_last_time_terrain_alt_was_valid == 0) { const bool terrain_first_measurement_timed_out = (now - _time_started_landing) > TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT; - const bool abort_on_terrain_measurement_timeout = checkLandingAbortBitMask(_param_fw_lnd_abort.get(), - position_controller_landing_status_s::TERRAIN_NOT_FOUND); if (terrain_first_measurement_timed_out && abort_on_terrain_measurement_timeout) { updateLandingAbortStatus(position_controller_landing_status_s::TERRAIN_NOT_FOUND); @@ -2707,8 +2713,6 @@ FixedwingPositionControl::getLandingTerrainAltitudeEstimate(const hrt_abstime &n if (!_local_pos.dist_bottom_valid) { const bool terrain_timed_out = (now - _last_time_terrain_alt_was_valid) > TERRAIN_ALT_TIMEOUT; - const bool abort_on_terrain_timeout = checkLandingAbortBitMask(_param_fw_lnd_abort.get(), - position_controller_landing_status_s::TERRAIN_TIMEOUT); if (terrain_timed_out && abort_on_terrain_timeout) { updateLandingAbortStatus(position_controller_landing_status_s::TERRAIN_TIMEOUT); diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp index 380ff88d71..7504bba3aa 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp @@ -756,9 +756,12 @@ private: * * @param now Current system time [us] * @param land_point_altitude Altitude (AMSL) of the land point [m] + * @param abort_on_terrain_measurement_timeout Abort if distance to ground estimation doesn't get valid when we expect it to + * @param abort_on_terrain_timeout Abort if distance to ground estimation is invalid after being valid before * @return Terrain altitude (AMSL) [m] */ - float getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude); + float getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude, + const bool abort_on_terrain_measurement_timeout, const bool abort_on_terrain_timeout); /** * @brief Initializes landing states diff --git a/src/modules/fw_path_navigation/fw_path_navigation_params.c b/src/modules/fw_path_navigation/fw_path_navigation_params.c index 6756fd2417..8dbb181ac0 100644 --- a/src/modules/fw_path_navigation/fw_path_navigation_params.c +++ b/src/modules/fw_path_navigation/fw_path_navigation_params.c @@ -987,7 +987,7 @@ PARAM_DEFINE_INT32(FW_LND_NUDGE, 2); * * @min 0 * @max 3 - * @bit 0 Abort if terrain is not found + * @bit 0 Abort if terrain is not found (only applies to mission landings) * @bit 1 Abort if terrain times out (after a first successful measurement) * @group FW Auto Landing */