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

Add RTL_TYPE to continue or reverse (#24581)

Browse Source 
This adds RTL_TYPE 4 which means continue the mission or reverse back to
the takeoff location, whichever is closer in terms of mission items
in-between.

This would be nicer to have on a distance rather than mission item count
basis but that would require access to the dataman and make it more
complex.
This commit is contained in:
Julian Oes
2025-03-28 06:29:42 +13:00
committed by GitHub
parent 3870992bac
commit 8acf273917
3 changed files with 24 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+15 -1
View File
@@ -318,7 +318,7 @@ void RTL::setRtlTypeAndDestination()
uint8_t safe_point_index{0U};
if (_param_rtl_type.get() != 2) {
if (_param_rtl_type.get() != 2 && _param_rtl_type.get() != 4) {
// check the closest allowed destination.
DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME};
PositionYawSetpoint rtl_position;
@@ -566,6 +566,14 @@ void RTL::init_rtl_mission_type()
} else {
new_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE;
}
} else if (_param_rtl_type.get() == 4) {
if (hasMissionLandStart() && reverseIsFurther()) {
new_rtl_mission_type = RtlType::RTL_MISSION_FAST;
} else {
new_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE;
}
}
if (_set_rtl_mission_type == new_rtl_mission_type) {
@@ -630,6 +638,12 @@ bool RTL::hasMissionLandStart() const
&& _navigator->get_mission_result()->valid;
}
bool RTL::reverseIsFurther() const
{
return (_mission_sub.get().land_start_index - _mission_sub.get().current_seq) < _mission_sub.get().current_seq;
}
bool RTL::hasVtolLandApproach(const PositionYawSetpoint &rtl_position) const
{
return readVtolLandApproaches(rtl_position).isAnyApproachValid();