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

Plane: Implementation of MAV_CMD_CONTINUE_AND_CHANGE_ALT.

Browse Source
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_alt(const AP_Mission::Mission_Command& cmd);
static void do_change_speed(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_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); 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); set_mode(RTL);
break; break;
case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
do_continue_and_change_alt(cmd);
// Conditional commands // Conditional commands
case MAV_CMD_CONDITION_DELAY: 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: case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL(); return verify_RTL();
case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
return verify_continue_and_change_alt();
// Conditional commands // Conditional commands
case MAV_CMD_CONDITION_DELAY: 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); 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 // 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 // Condition (May) commands
/********************************************************************************/ /********************************************************************************/