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

RTL: publish a status message on currently chosen RTL point

Browse Source
This commit is contained in:
Konrad
2024-02-14 10:40:15 +01:00
committed by Silvan Fuhrer
parent 74303a79e1
commit 68100650da
6 changed files with 62 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+35 -4
View File
@@ -294,12 +294,14 @@ void RTL::setRtlTypeAndDestination()
init_rtl_mission_type();
uint8_t safe_point_index{0U};
if (_param_rtl_type.get() != 2) {
// check the closest allowed destination.
DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME};
PositionYawSetpoint rtl_position;
float rtl_alt;
findRtlDestination(destination_type, rtl_position, rtl_alt);
findRtlDestination(destination_type, rtl_position, rtl_alt, safe_point_index);
switch (destination_type) {
case DestinationType::DESTINATION_TYPE_MISSION_LAND:
@@ -331,9 +333,29 @@ void RTL::setRtlTypeAndDestination()
break;
}
}
// Publish rtl status
_rtl_status_pub.get().timestamp = hrt_absolute_time();
_rtl_status_pub.get().safe_points_id = _safe_points_id;
_rtl_status_pub.get().is_evaluation_pending = _dataman_state != DatamanState::UpdateRequestWait;
_rtl_status_pub.get().has_vtol_approach = false;
if ((_param_rtl_type.get() == 0) || (_param_rtl_type.get() == 3)) {
_rtl_status_pub.get().has_vtol_approach = _home_has_land_approach || _one_rally_point_has_land_approach;
} else if (_param_rtl_type.get() == 1) {
_rtl_status_pub.get().has_vtol_approach = _one_rally_point_has_land_approach;
}
_rtl_status_pub.get().rtl_type = static_cast<uint8_t>(_rtl_type);
_rtl_status_pub.get().safe_point_index = safe_point_index;
_rtl_status_pub.update();
}
void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt)
void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt,
uint8_t &safe_point_index)
{
// set destination to home per default, then check if other valid landing spot is closer
rtl_position.alt = _home_pos_sub.get().alt;
@@ -352,8 +374,10 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
float home_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, rtl_position.lat, rtl_position.lon)};
float min_dist;
_home_has_land_approach = hasVtolLandApproach(rtl_position);
if (((_param_rtl_type.get() == 1) && !vtol_in_rw_mode) || (vtol_in_fw_mode && (_param_rtl_approach_force.get() == 1)
&& !hasVtolLandApproach(rtl_position))) {
&& !_home_has_land_approach)) {
// Set minimum distance to maximum value when RTL_TYPE is set to 1 and we are not in RW mode or we forces approach landing for vtol in fw and it is not defined for home.
min_dist = FLT_MAX;
@@ -394,6 +418,8 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
if (_safe_points_updated) {
_one_rally_point_has_land_approach = false;
for (int current_seq = 0; current_seq < _dataman_cache_safepoint.size(); ++current_seq) {
mission_item_s mission_safe_point;
@@ -416,11 +442,16 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
PositionYawSetpoint safepoint_position;
setSafepointAsDestination(safepoint_position, mission_safe_point);
bool current_safe_point_has_approaches{hasVtolLandApproach(safepoint_position)};
_one_rally_point_has_land_approach |= current_safe_point_has_approaches;
if (((dist + MIN_DIST_THRESHOLD) < min_dist) && (!vtol_in_fw_mode || (_param_rtl_approach_force.get() == 0)
|| hasVtolLandApproach(safepoint_position))) {
|| current_safe_point_has_approaches)) {
min_dist = dist;
rtl_position = safepoint_position;
destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT;
safe_point_index = current_seq;
}
}
}