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

Plane: use a rally point when out of mission items

This commit is contained in:
Andrew Tridgell 2013-10-05 23:07:32 +10:00
parent 1518217952
commit 365e82aecb
2 changed files with 19 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
ArduPlane/commands_logic.pde
View File

@ -147,13 +147,11 @@ static void handle_process_do_command()
static void handle_no_commands() static void handle_no_commands()
{ {
gcs_send_text_fmt(PSTR("Returning to Home")); gcs_send_text_fmt(PSTR("Returning to Home"));
next_nav_command = home; next_nav_command = rally_find_best_location(current_loc, home);
next_nav_command.alt = read_alt_to_hold();
next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM; next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM;
nav_command_ID = MAV_CMD_NAV_LOITER_UNLIM; nav_command_ID = MAV_CMD_NAV_LOITER_UNLIM;
non_nav_command_ID = WAIT_COMMAND; non_nav_command_ID = WAIT_COMMAND;
handle_process_nav_cmd(); handle_process_nav_cmd();
} }
/******************************************************************************* /*******************************************************************************
@ -233,18 +231,7 @@ static void do_RTL(void)
{ {
control_mode = RTL; control_mode = RTL;
prev_WP = current_loc; prev_WP = current_loc;
next_WP = rally_find_best_location(current_loc, home);
RallyLocation ral_loc;
if (find_best_rally_point(current_loc, home, ral_loc)) {
//we have setup Rally points: use them instead of Home for RTL
next_WP = rally_location_to_location(ral_loc, home);
} else {
next_WP = home;
// Altitude to hold over home
// Set by configuration tool
// -------------------------
next_WP.alt = read_alt_to_hold();
}
if (g.loiter_radius < 0) { if (g.loiter_radius < 0) {
loiter.direction = -1; loiter.direction = -1;

17
ArduPlane/rally.pde
View File

@ -74,3 +74,20 @@ static Location rally_location_to_location(const RallyLocation &r_loc, const Loc
return ret; return ret;
} }
// return best RTL location from current position
static Location rally_find_best_location(const Location &myloc, const Location &homeloc)
{
RallyLocation ral_loc;
Location ret;
if (find_best_rally_point(myloc, home, ral_loc)) {
//we have setup Rally points: use them instead of Home for RTL
ret = rally_location_to_location(ral_loc, home);
} else {
ret = homeloc;
// Altitude to hold over home
ret.alt = read_alt_to_hold();
}
return ret;
}