Plane: use WP_LOITER_RAD if RTL_RADIUS is 0 for QRTL

This commit is contained in:
Andrew Tridgell 2017-10-30 16:51:28 +11:00
parent aa430a6e2c
commit 1085069851
2 changed files with 6 additions and 2 deletions

View File

@ -797,6 +797,10 @@ void Plane::update_navigation()
// ------------------------------------------------------------------------
uint16_t radius = 0;
uint16_t qrtl_radius = abs(g.rtl_radius);
if (qrtl_radius == 0) {
qrtl_radius = abs(aparm.loiter_radius);
}
switch(control_mode) {
case AUTO:
@ -809,7 +813,7 @@ void Plane::update_navigation()
if (quadplane.available() && quadplane.rtl_mode == 1 &&
(nav_controller->reached_loiter_target() ||
location_passed_point(current_loc, prev_WP_loc, next_WP_loc) ||
auto_state.wp_distance < (uint16_t)g.rtl_radius) &&
auto_state.wp_distance < qrtl_radius) &&
AP_HAL::millis() - last_mode_change_ms > 1000) {
set_mode(QRTL, MODE_REASON_UNKNOWN);
break;

View File

@ -259,7 +259,7 @@ 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 the loiter target is reached
// @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
// @User: Standard
AP_GROUPINFO("RTL_MODE", 36, QuadPlane, rtl_mode, 0),