Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-19 10:57:46 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

Do not stop at the first rally point close enough if invalid

Browse Source
This commit is contained in:
jonas
2026-04-10 13:32:19 +02:00
parent 65dc94f763
commit fd45f4e459
2 changed files with 66 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+32 -28
View File
@@ -441,20 +441,21 @@ PositionYawSetpoint RTL::findClosestSafePoint(float min_dist, uint8_t &safe_poin
continue;
}
PositionYawSetpoint safepoint_position{};
if (!extractValidSafePointPosition(mission_safe_point, _home_pos_sub.get().alt, safepoint_position)) {
continue;
}
// Ignore safepoints which are too close to the homepoint (only if home is an option to return to)
const bool far_from_home = get_distance_to_next_waypoint(_home_pos_sub.get().lat, _home_pos_sub.get().lon,
mission_safe_point.lat, mission_safe_point.lon) > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES;
safepoint_position.lat, safepoint_position.lon) > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES;
if (far_from_home || (_param_rtl_type.get() == 5)) {
const float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)};
const float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
safepoint_position.lat, safepoint_position.lon)};
PositionYawSetpoint safepoint_position;
if (!extractValidSafePointPosition(mission_safe_point, _home_pos_sub.get().alt, safepoint_position)) {
continue;
}
const bool current_safe_point_has_approaches{hasVtolLandApproach(current_seq, _home_pos_sub.get().alt)};
const bool current_safe_point_has_approaches{hasVtolLandApproachesAtSafePointIndex(current_seq, _home_pos_sub.get().alt)};
_one_rally_point_has_land_approach |= current_safe_point_has_approaches;
@@ -482,7 +483,7 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
float min_dist = FLT_MAX;
if (_param_rtl_type.get() != 5) {
_home_has_land_approach = hasVtolLandApproach(destination, _home_pos_sub.get().alt);
_home_has_land_approach = hasVtolLandApproachesNearLocation(destination, _home_pos_sub.get().alt);
const bool prioritize_safe_points_over_home = ((_param_rtl_type.get() == 1) && !vtol_in_rw_mode);
const bool required_approach_missing_for_home = (vtol_in_fw_mode && (_param_rtl_appr_force.get() == 1) && !_home_has_land_approach);
@@ -703,14 +704,14 @@ bool RTL::reverseIsFurther() const
}
bool RTL::findAssociatedSafePointIndex(const PositionYawSetpoint &rtl_position, int &safe_point_index,
mission_item_s &safe_point_item) const
bool RTL::findAssociatedSafePointIndex(const PositionYawSetpoint &rtl_position, float home_altitude_amsl,
int &safe_point_index, mission_item_s &safe_point_item) const
{
if (!_safe_points_updated) {
return false;
}
// Find the first rally point within the home-association distance
// The first nearby valid rally point defines the block that belongs to this destination.
for (int current_seq = 0; current_seq < _stats.num_items; ++current_seq) {
mission_item_s mission_item{};
@@ -726,7 +727,14 @@ bool RTL::findAssociatedSafePointIndex(const PositionYawSetpoint &rtl_position,
continue;
}
const float dist_to_safepoint = get_distance_to_next_waypoint(mission_item.lat, mission_item.lon,
PositionYawSetpoint safe_point_position{};
// Skip invalid rally points so later valid rally points are considered
if (!extractValidSafePointPosition(mission_item, home_altitude_amsl, safe_point_position)) {
continue;;
}
const float dist_to_safepoint = get_distance_to_next_waypoint(safe_point_position.lat, safe_point_position.lon,
rtl_position.lat, rtl_position.lon);
if (dist_to_safepoint < MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) {
@@ -756,6 +764,7 @@ bool RTL::scanVtolLandApproachBlock(int safe_point_index, float home_altitude_am
}
if (mission_item.nav_cmd == NAV_CMD_RALLY_POINT) {
// A rally point starts the next block, break
break;
}
@@ -790,37 +799,32 @@ loiter_point_s RTL::makeVtolLandApproachPoint(const mission_item_s &mission_item
return approach;
}
land_approaches_s RTL::readVtolLandApproaches(const PositionYawSetpoint &rtl_position, float home_altitude_amsl) const
land_approaches_s RTL::getVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position,
float home_altitude_amsl) const
{
int safe_point_index = -1;
mission_item_s safe_point_item{};
if (!findAssociatedSafePointIndex(rtl_position, safe_point_index, safe_point_item)) {
return {};
}
PositionYawSetpoint safe_point_position{};
if (!extractValidSafePointPosition(safe_point_item, home_altitude_amsl, safe_point_position)) {
if (!findAssociatedSafePointIndex(rtl_position, home_altitude_amsl, safe_point_index, safe_point_item)) {
return {};
}
land_approaches_s vtol_land_approaches{};
vtol_land_approaches.land_location_lat_lon = matrix::Vector2d(safe_point_position.lat, safe_point_position.lon);
vtol_land_approaches.land_location_lat_lon = matrix::Vector2d(safe_point_item.lat, safe_point_item.lon);
scanVtolLandApproachBlock(safe_point_index, home_altitude_amsl, &vtol_land_approaches);
return vtol_land_approaches;
}
bool RTL::hasVtolLandApproach(const PositionYawSetpoint &rtl_position, float home_altitude_amsl) const
bool RTL::hasVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position, float home_altitude_amsl) const
{
int safe_point_index = -1;
mission_item_s safe_point_item{};
return findAssociatedSafePointIndex(rtl_position, safe_point_index, safe_point_item)
&& hasVtolLandApproach(safe_point_index, home_altitude_amsl);
return findAssociatedSafePointIndex(rtl_position, home_altitude_amsl, safe_point_index, safe_point_item)
&& hasVtolLandApproachesAtSafePointIndex(safe_point_index, home_altitude_amsl);
}
bool RTL::hasVtolLandApproach(int safe_point_index, float home_altitude_amsl) const
bool RTL::hasVtolLandApproachesAtSafePointIndex(int safe_point_index, float home_altitude_amsl) const
{
return scanVtolLandApproachBlock(safe_point_index, home_altitude_amsl, nullptr);
}
@@ -835,7 +839,7 @@ loiter_point_s RTL::selectLandingApproach(const PositionYawSetpoint &destination
return landing_approach;
}
const land_approaches_s vtol_land_approaches = readVtolLandApproaches(destination, _home_pos_sub.get().alt);
const land_approaches_s vtol_land_approaches = getVtolLandApproachesNearLocation(destination, _home_pos_sub.get().alt);
if (vtol_land_approaches.isAnyApproachValid()) {
landing_approach = chooseBestLandingApproach(vtol_land_approaches);
src/modules/navigator/rtl.h
+34 -24
View File
@@ -177,44 +177,51 @@ private:
void parameters_update();
/**
* @brief Read all VTOL landing approaches associated with rtl_position.
*/
land_approaches_s readVtolLandApproaches(const PositionYawSetpoint &rtl_position, float home_altitude_amsl) const;
/**
* @brief Return whether rtl_position has at least one valid VTOL landing approach.
*/
bool hasVtolLandApproach(const PositionYawSetpoint &rtl_position, float home_altitude_amsl) const;
/**
* @brief Return whether the indexed safe point has at least one valid VTOL landing approach.
*/
bool hasVtolLandApproach(int safe_point_index, float home_altitude_amsl) const;
/**
* @brief Choose best landing approach
* @brief Read the landing-approach block associated with the first valid rally point near rtl_position.
*
* Choose the wind-aligned landing approach for the associated land location.
* A block starts at the associated rally point and contains the consecutive NAV_CMD_LOITER_TO_ALT
* items that follow it. The next rally point starts a new block.
* Invalid rally points are skipped so a later nearby valid rally point can still be considered.
*/
land_approaches_s getVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position,
float home_altitude_amsl) const;
/**
* @brief Return whether a valid associated block near rtl_position has at least one valid approach.
*/
bool hasVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position, float home_altitude_amsl) const;
/**
* @brief Return whether the block after safe_point_index contains at least one valid approach.
*/
bool hasVtolLandApproachesAtSafePointIndex(int safe_point_index, float home_altitude_amsl) const;
/**
* @brief Choose the most wind-aligned approach in a landing-approach block.
*
* @return loiter_point_s best landing approach
* Bearings are evaluated from the block's land location.
*/
loiter_point_s chooseBestLandingApproach(const land_approaches_s &vtol_land_approaches) const;
/**
* @brief Return the wind-selected VTOL landing approach for a destination, or an invalid loiter if none exists.
* @brief Return the wind-selected VTOL approach for destination, or an invalid loiter if none exists.
*/
loiter_point_s selectLandingApproach(const PositionYawSetpoint &destination) const;
/**
* @brief Find the dataman index of the first rally point within association distance of rtl_position.
* @brief Find the first rally point whose block should be associated with rtl_position.
*
* On success, safe_point_index and safe_point_item are populated with the found item.
* Invalid rally points are skipped so nearby valid fallbacks can still be associated.
* On success, safe_point_index and safe_point_item are populated with the rally point that starts the block.
*/
bool findAssociatedSafePointIndex(const PositionYawSetpoint &rtl_position, int &safe_point_index,
mission_item_s &safe_point_item) const;
bool findAssociatedSafePointIndex(const PositionYawSetpoint &rtl_position, float home_altitude_amsl,
int &safe_point_index, mission_item_s &safe_point_item) const;
/**
* @brief Scan the loiter-to-alt block following a safe point.
* @brief Scan one landing-approach block after a rally point.
*
* A block is the consecutive NAV_CMD_LOITER_TO_ALT items after safe_point_index.
* Scanning stops at the next rally point because it starts a different safe-point block.
*
* If result is non-null, all valid approaches are collected into it.
* If result is null, returns true on the first valid approach (early exit).
@@ -224,6 +231,9 @@ private:
bool scanVtolLandApproachBlock(int safe_point_index, float home_altitude_amsl,
land_approaches_s *result) const;
/**
* @brief Convert one loiter mission item into a landing-approach entry.
*/
loiter_point_s makeVtolLandApproachPoint(const mission_item_s &mission_item, float home_altitude_amsl) const;
enum class DatamanState {