aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: use RTL_RADIUS for QRTL threshold

Browse Source 
when Q_RTL_MODE=1 in a quadplane use RTL_RADIUS as the distance to
switch to QRTL mode. We navigate to the destination using direct
waypoint navigation instead of using a loiter circle.
This commit is contained in:
Andrew Tridgell 2017-10-30 16:24:32 +11:00
parent 552faa8f67
commit aa430a6e2c
2 changed files with 16 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/ArduPlane.cpp
View File

@ -807,7 +807,9 @@ void Plane::update_navigation()
case RTL:
if (quadplane.available() && quadplane.rtl_mode == 1 &&
nav_controller->reached_loiter_target() &&
(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) &&
AP_HAL::millis() - last_mode_change_ms > 1000) {
set_mode(QRTL, MODE_REASON_UNKNOWN);
break;

19
ArduPlane/navigation.cpp
View File

@ -185,12 +185,19 @@ void Plane::update_loiter(uint16_t radius)
loiter.start_time_ms = 0;
quadplane.guided_start();
}
} else if (loiter.start_time_ms == 0 &&
control_mode == AUTO &&
!auto_state.no_crosstrack &&
get_distance(current_loc, next_WP_loc) > radius*3) {
// 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
} else if ((loiter.start_time_ms == 0 &&
control_mode == AUTO &&
!auto_state.no_crosstrack &&
get_distance(current_loc, next_WP_loc) > radius*3) ||
(quadplane.available() && quadplane.rtl_mode == 1)) {
/*
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
we also use direct waypoint navigation if we are a quadplane
that is going to be switching to QRTL when it gets within
RTL_RADIUS
*/
nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
} else {
nav_controller->update_loiter(next_WP_loc, radius, loiter.direction);