diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 8ecae11fa7..f0efeb54a5 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -713,7 +713,8 @@ bool RTL::reverseIsFurther() const } -bool RTL::findAssociatedSafePointIndex(const PositionYawSetpoint &rtl_position, int &safe_point_index) const +bool RTL::findAssociatedSafePointIndex(const PositionYawSetpoint &rtl_position, int &safe_point_index, + mission_item_s &safe_point_item) const { if (!_safe_points_updated) { return false; @@ -740,6 +741,7 @@ bool RTL::findAssociatedSafePointIndex(const PositionYawSetpoint &rtl_position, if (dist_to_safepoint < MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) { safe_point_index = current_seq; + safe_point_item = mission_item; return true; } } @@ -747,10 +749,10 @@ bool RTL::findAssociatedSafePointIndex(const PositionYawSetpoint &rtl_position, return false; } -void RTL::collectVtolLandApproachBlock(int safe_point_index, float home_altitude_amsl, - land_approaches_s &vtol_land_approaches) const +bool RTL::scanVtolLandApproachBlock(int safe_point_index, float home_altitude_amsl, + land_approaches_s *result) const { - uint8_t approach_counter = 0; + uint8_t valid_approach_counter = 0; for (int current_seq = safe_point_index + 1; current_seq < _stats.num_items; ++current_seq) { mission_item_s mission_item{}; @@ -768,111 +770,59 @@ void RTL::collectVtolLandApproachBlock(int safe_point_index, float home_altitude } if (mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT - || approach_counter >= land_approaches_s::num_approaches_max) { - continue; - } - - // land_approaches_s is fixed-size. Ignore additional approaches. - vtol_land_approaches.approaches[approach_counter] = makeVtolLandApproachPoint(mission_item, home_altitude_amsl); - approach_counter++; - } -} - -bool RTL::hasValidVtolLandApproachInBlock(int safe_point_index, float home_altitude_amsl) const -{ - uint8_t approach_counter = 0; - - for (int current_seq = safe_point_index + 1; current_seq < _stats.num_items; ++current_seq) { - mission_item_s mission_item{}; - - const bool success = _dataman_cache_safepoint.loadWait(static_cast(_stats.dataman_id), current_seq, - reinterpret_cast(&mission_item), sizeof(mission_item_s)); - - if (!success) { - PX4_ERR("dm_read failed"); - break; - } - - if (mission_item.nav_cmd == NAV_CMD_RALLY_POINT) { - break; - } - - if (mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT - || approach_counter >= land_approaches_s::num_approaches_max) { + || valid_approach_counter >= land_approaches_s::num_approaches_max) { continue; } const loiter_point_s approach = makeVtolLandApproachPoint(mission_item, home_altitude_amsl); - approach_counter++; if (approach.isValid()) { - return true; + if (result) { + result->approaches[valid_approach_counter] = approach; + valid_approach_counter++; + + } else { + return true; // Early exit for the check-only + } } } - return false; -} - -land_approaches_s RTL::readVtolLandApproach(int safe_point_index, float home_altitude_amsl) const -{ - land_approaches_s vtol_land_approaches{}; - mission_item_s safe_point_item{}; - PositionYawSetpoint safe_point_position{}; - - const bool success = _dataman_cache_safepoint.loadWait(static_cast(_stats.dataman_id), safe_point_index, - reinterpret_cast(&safe_point_item), sizeof(mission_item_s)); - - if (!success) { - PX4_ERR("dm_read failed"); - return vtol_land_approaches; - } - - if (!extractValidSafePointPosition(safe_point_item, home_altitude_amsl, safe_point_position)) { - return vtol_land_approaches; - } - - vtol_land_approaches.land_location_lat_lon = matrix::Vector2d(safe_point_position.lat, safe_point_position.lon); - collectVtolLandApproachBlock(safe_point_index, home_altitude_amsl, vtol_land_approaches); - return vtol_land_approaches; + return result ? (valid_approach_counter > 0) : false; } land_approaches_s RTL::readVtolLandApproaches(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)) { - return readVtolLandApproach(safe_point_index, home_altitude_amsl); + if (!findAssociatedSafePointIndex(rtl_position, safe_point_index, safe_point_item)) { + return {}; } - return {}; + PositionYawSetpoint safe_point_position{}; + + if (!extractValidSafePointPosition(safe_point_item, home_altitude_amsl, safe_point_position)) { + return {}; + } + + land_approaches_s vtol_land_approaches{}; + vtol_land_approaches.land_location_lat_lon = matrix::Vector2d(safe_point_position.lat, safe_point_position.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 { int safe_point_index = -1; + mission_item_s safe_point_item{}; - return findAssociatedSafePointIndex(rtl_position, safe_point_index) + return findAssociatedSafePointIndex(rtl_position, safe_point_index, safe_point_item) && hasVtolLandApproach(safe_point_index, home_altitude_amsl); } bool RTL::hasVtolLandApproach(int safe_point_index, float home_altitude_amsl) const { - mission_item_s safe_point_item{}; - PositionYawSetpoint ignored_safe_point_position{}; - - const bool success = _dataman_cache_safepoint.loadWait(static_cast(_stats.dataman_id), safe_point_index, - reinterpret_cast(&safe_point_item), sizeof(mission_item_s)); - - if (!success) { - PX4_ERR("dm_read failed"); - return false; - } - - if (!extractValidSafePointPosition(safe_point_item, home_altitude_amsl, ignored_safe_point_position)) { - return false; - } - - return hasValidVtolLandApproachInBlock(safe_point_index, home_altitude_amsl); + return scanVtolLandApproachBlock(safe_point_index, home_altitude_amsl, nullptr); } loiter_point_s RTL::selectLandingApproach(const PositionYawSetpoint &destination) const diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 8f16d3d9b3..c293751b07 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -176,11 +176,6 @@ private: */ void parameters_update(); - /** - * @brief Read all VTOL landing approaches associated with one safe point. - */ - land_approaches_s readVtolLandApproach(int safe_point_index, float home_altitude_amsl) const; - /** * @brief Read all VTOL landing approaches associated with rtl_position. */ @@ -210,10 +205,24 @@ private: */ loiter_point_s selectLandingApproach(const PositionYawSetpoint &destination) const; - bool findAssociatedSafePointIndex(const PositionYawSetpoint &rtl_position, int &safe_point_index) const; - bool hasValidVtolLandApproachInBlock(int safe_point_index, float home_altitude_amsl) const; - void collectVtolLandApproachBlock(int safe_point_index, float home_altitude_amsl, - land_approaches_s &vtol_land_approaches) const; + /** + * @brief Find the dataman index of the first rally point within association distance of rtl_position. + * + * On success, safe_point_index and safe_point_item are populated with the found item. + */ + bool findAssociatedSafePointIndex(const PositionYawSetpoint &rtl_position, int &safe_point_index, + mission_item_s &safe_point_item) const; + + /** + * @brief Scan the loiter-to-alt block following a safe point. + * + * 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). + * + * @return true if at least one valid approach was found. + */ + bool scanVtolLandApproachBlock(int safe_point_index, float home_altitude_amsl, + land_approaches_s *result) const; enum class DatamanState { UpdateRequestWait,