Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

QuadPlane: fix loiter to RTL instant QRTL switch

This commit is contained in:
Iampete1 2021-03-02 23:28:36 +00:00 committed by Andrew Tridgell
parent 5a49d98555
commit 69275582fa
2 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/mode.h
View File

@ -288,7 +288,7 @@ protected:
private: private:
// Switch to QRTL if enabled and within radius // Switch to QRTL if enabled and within radius
bool switch_QRTL(); bool switch_QRTL(bool check_loiter_target = true);
}; };
class ModeStabilize : public Mode class ModeStabilize : public Mode

7
ArduPlane/mode_rtl.cpp
View File

@ -7,7 +7,8 @@ bool ModeRTL::_enter()
plane.do_RTL(plane.get_RTL_altitude()); plane.do_RTL(plane.get_RTL_altitude());
plane.rtl.done_climb = false; plane.rtl.done_climb = false;
switch_QRTL(); // 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);
return true; return true;
} }
@ -80,7 +81,7 @@ void ModeRTL::navigate()
// Switch to QRTL if enabled and within radius // Switch to QRTL if enabled and within radius
bool ModeRTL::switch_QRTL() 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 != 1)) {
return false; return false;
@ -91,7 +92,7 @@ bool ModeRTL::switch_QRTL()
qrtl_radius = abs(plane.aparm.loiter_radius); qrtl_radius = abs(plane.aparm.loiter_radius);
} }
if (plane.nav_controller->reached_loiter_target() || if ( (check_loiter_target && plane.nav_controller->reached_loiter_target()) ||
plane.current_loc.past_interval_finish_line(plane.prev_WP_loc, plane.next_WP_loc) || plane.current_loc.past_interval_finish_line(plane.prev_WP_loc, plane.next_WP_loc) ||
plane.auto_state.wp_distance < MAX(qrtl_radius, plane.quadplane.stopping_distance())) { plane.auto_state.wp_distance < MAX(qrtl_radius, plane.quadplane.stopping_distance())) {
/* /*