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

Plane: Implementation of MAV_CMD_CONTINUE_AND_CHANGE_ALT.

This commit is contained in:
Michael Day 2014-10-21 13:46:01 -07:00 committed by Andrew Tridgell
parent b36c1b2c3d
commit 442c188ae0
1 changed files with 24 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
ArduPlane/commands_logic.pde
View File

@ -12,6 +12,7 @@ static void do_within_distance(const AP_Mission::Mission_Command& cmd);
static void do_change_alt(const AP_Mission::Mission_Command& cmd);
static void do_change_speed(const AP_Mission::Mission_Command& cmd);
static void do_set_home(const AP_Mission::Mission_Command& cmd);
static void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd);
static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
@ -78,6 +79,9 @@ start_command(const AP_Mission::Mission_Command& cmd)
set_mode(RTL);
break;
case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
do_continue_and_change_alt(cmd);
// Conditional commands
case MAV_CMD_CONDITION_DELAY:
@ -212,6 +216,9 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd) // Ret
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
return verify_continue_and_change_alt();
// Conditional commands
case MAV_CMD_CONDITION_DELAY:
@ -342,6 +349,11 @@ static void do_loiter_time(const AP_Mission::Mission_Command& cmd)
loiter_set_direction_wp(cmd);
}
static void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd)
{
next_WP_loc.alt = cmd.content.location.alt + home.alt;
}
/********************************************************************************/
// Verify Nav (Must) commands
/********************************************************************************/
@ -496,6 +508,18 @@ static bool verify_RTL()
}
}
static bool verify_continue_and_change_alt()
{
if (abs(adjusted_altitude_cm() - next_WP_loc.alt) <= 500) {
return true;
}
//keep flying the same course
nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
return false;
}
/********************************************************************************/
// Condition (May) commands
/********************************************************************************/