diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 1b53488d7a..b2898be415 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -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(); diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 18a7ad3ea1..207af7ceb5 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -105,6 +105,14 @@ private: */ bool hasMissionLandStart() const; + + /** + * @brief Check whether there are more waypoints between current waypoint + * and the takeoff location than the end/land location. + * @return true if the reverse is more items away. + */ + bool reverseIsFurther() const; + /** * @brief function to call regularly to do background work */ diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 951993a724..af02033c3a 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -112,6 +112,7 @@ PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 10.0f); * @value 1 Return to closest safe point other than home (mission landing pattern or rally point), via direct path. If no mission landing or rally points are defined return home via direct path. Always chose closest safe landing point if vehicle is a VTOL in hover mode. * @value 2 Return to a planned mission landing, if available, using the mission path, else return to home via the reverse mission path. Do not consider rally points. * @value 3 Return via direct path to closest destination: home, start of mission landing pattern or safe point. If the destination is a mission landing pattern, follow the pattern to land. + * @value 4 Return to the planned mission landing, or to home via the reverse mission path, whichever is closer by counting waypoints. Do not consider rally points. * @group Return Mode */ PARAM_DEFINE_INT32(RTL_TYPE, 0);