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()
{
gcs_send_text_fmt(PSTR("Returning to Home"));
next_nav_command = home;
next_nav_command.alt = read_alt_to_hold();
next_nav_command = rally_find_best_location(current_loc, home);
next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM;
nav_command_ID = MAV_CMD_NAV_LOITER_UNLIM;
non_nav_command_ID = WAIT_COMMAND;
handle_process_nav_cmd();
}
/*******************************************************************************
@ -233,18 +231,7 @@ static void do_RTL(void)
{
control_mode = RTL;
prev_WP = current_loc;
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();
}
next_WP = rally_find_best_location(current_loc, home);
if (g.loiter_radius < 0) {
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 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;
}