fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 07:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW PositionController: circular landing: enable automatic landing aborts

Browse Source 
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 <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-02-20 17:04:39 +01:00
parent 37b6dccda9
commit 485785d81d
3 changed files with 16 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_path_navigation/FixedwingPositionControl.cpp
+11 -7
View File
@@ -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);
src/modules/fw_path_navigation/FixedwingPositionControl.hpp
+4 -1
View File
@@ -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
src/modules/fw_path_navigation/fw_path_navigation_params.c
+1 -1
View File
@@ -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
*/