diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 5ef9e57fbc..aca3657358 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -348,6 +348,7 @@ private: } failsafe; enum Landing_ApproachStage { + RTL, LOITER_TO_ALT, ENSURE_RADIUS, WAIT_FOR_BREAKOUT, diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 24e8bc3b07..86c3de2913 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -953,6 +953,18 @@ void Plane::exit_mission_callback() bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd) { switch (vtol_approach_s.approach_stage) { + case RTL: + { + // fly home and loiter at RTL alt + update_loiter(fabsf(quadplane.fw_land_approach_radius)); + if (plane.reached_loiter_target()) { + // decend to Q RTL alt + plane.do_RTL(plane.home.alt + plane.quadplane.qrtl_alt*100UL); + plane.loiter_angle_reset(); + vtol_approach_s.approach_stage = LOITER_TO_ALT; + } + break; + } case LOITER_TO_ALT: { update_loiter(fabsf(quadplane.fw_land_approach_radius)); diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index ab3acc667e..28e94aa718 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/ArduPlane/mode_rtl.cpp @@ -6,6 +6,7 @@ bool ModeRTL::_enter() plane.prev_WP_loc = plane.current_loc; plane.do_RTL(plane.get_RTL_altitude()); plane.rtl.done_climb = false; + plane.vtol_approach_s.approach_stage = Plane::Landing_ApproachStage::RTL; // do not check if we have reached the loiter target if switching from loiter this will trigger as the nav controller has not yet proceeded the new destination switch_QRTL(false); @@ -49,9 +50,23 @@ void ModeRTL::update() void ModeRTL::navigate() { + if (plane.control_mode->mode_number() != QRTL) { + // QRTL shares this navigate function with RTL - if ((AP_HAL::millis() - plane.last_mode_change_ms > 1000) && switch_QRTL()) { - return; + if (plane.quadplane.available() && (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::VTOL_APPROACH_QRTL)) { + // VTOL approach landing + AP_Mission::Mission_Command cmd; + cmd.content.location = plane.next_WP_loc; + plane.verify_landing_vtol_approach(cmd); + if (plane.vtol_approach_s.approach_stage == Plane::Landing_ApproachStage::VTOL_LANDING) { + plane.set_mode(plane.mode_qrtl, ModeReason::RTL_COMPLETE_SWITCHING_TO_VTOL_LAND_RTL); + } + return; + } + + if ((AP_HAL::millis() - plane.last_mode_change_ms > 1000) && switch_QRTL()) { + return; + } } if (plane.g.rtl_autoland == 1 && @@ -94,7 +109,7 @@ void ModeRTL::navigate() // Switch to QRTL if enabled and within radius bool ModeRTL::switch_QRTL(bool check_loiter_target) { - if (!plane.quadplane.available() || (plane.quadplane.rtl_mode != 1)) { + if (!plane.quadplane.available() || (plane.quadplane.rtl_mode != QuadPlane::RTL_MODE::SWITCH_QRTL)) { return false; } diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index b2d23dc892..5ee5f1deae 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -286,7 +286,7 @@ void Plane::update_loiter(uint16_t radius) (control_mode == &mode_auto || control_mode == &mode_guided) && auto_state.crosstrack && current_loc.get_distance(next_WP_loc) > radius*3) || - (control_mode == &mode_rtl && quadplane.available() && quadplane.rtl_mode == 1)) { + (control_mode == &mode_rtl && quadplane.available() && quadplane.rtl_mode == QuadPlane::RTL_MODE::SWITCH_QRTL)) { /* if never reached loiter point and using crosstrack and somewhat far away from loiter point navigate to it like in auto-mode for normal crosstrack behavior diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 49aaf8ee68..195199b438 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -194,8 +194,8 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Param: RTL_MODE // @DisplayName: VTOL RTL mode - // @Description: If this is set to 1 then an RTL will change to QRTL when within RTL_RADIUS meters of the RTL destination - // @Values: 0:Disabled,1:Enabled + // @Description: If this is set to 1 then an RTL will change to QRTL when within RTL_RADIUS meters of the RTL destination, VTOL approach: vehicle will RTL at RTL alt and circle with a radius of Q_FW_LND_APR_RAD down to Q_RLT_ALT and then transission into the wind and QRTL, see 'AUTO VTOL Landing' + // @Values: 0:Disabled,1:Enabled,2:VTOL approach // @User: Standard AP_GROUPINFO("RTL_MODE", 36, QuadPlane, rtl_mode, 0), diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index d7f4b02fe2..665fc6f005 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -350,6 +350,11 @@ private: // control if a VTOL RTL will be used AP_Int8 rtl_mode; + enum RTL_MODE{ + NONE, + SWITCH_QRTL, + VTOL_APPROACH_QRTL, + }; // control if a VTOL GUIDED will be used AP_Int8 guided_mode;