Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: process cmds from mission lib

Browse Source
This commit is contained in:
Randy Mackay 2014-02-28 11:19:05 +09:00
parent 6e0e672fb2
commit 359f1a27ec
3 changed files with 151 additions and 154 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

291
ArduCopter/commands_logic.pde
View File

@ -1,112 +1,113 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/********************************************************************************/ // forward declarations to make compiler happy
// Command Event Handlers static void do_takeoff(const AP_Mission::Mission_Command& cmd);
/********************************************************************************/ static void do_nav_wp(const AP_Mission::Mission_Command& cmd);
// process_nav_command - main switch statement to initiate the next nav command in the command_nav_queue static void do_land(const AP_Mission::Mission_Command& cmd);
static void process_nav_command() static void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
{ static void do_circle(const AP_Mission::Mission_Command& cmd);
switch(command_nav_queue.id) { static void do_loiter_time(const AP_Mission::Mission_Command& cmd);
static void do_wait_delay(const AP_Mission::Mission_Command& cmd);
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_yaw(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_roi(const AP_Mission::Mission_Command& cmd);
static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
// start_command - this function will be called when the ap_mission lib wishes to start a new command
static bool start_command(const AP_Mission::Mission_Command& cmd)
{
// To-Do: logging when new commands start/end
if (g.log_bitmask & MASK_LOG_CMD) {
Log_Write_Cmd(cmd);
}
switch(cmd.id) {
///
/// navigation commands
///
case MAV_CMD_NAV_TAKEOFF: // 22 case MAV_CMD_NAV_TAKEOFF: // 22
do_takeoff(); do_takeoff(cmd);
break; break;
case MAV_CMD_NAV_WAYPOINT: // 16 Navigate to Waypoint case MAV_CMD_NAV_WAYPOINT: // 16 Navigate to Waypoint
do_nav_wp(); do_nav_wp(cmd);
break; break;
case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint
do_land(&command_nav_queue); do_land(cmd);
break; break;
case MAV_CMD_NAV_LOITER_UNLIM: // 17 Loiter indefinitely case MAV_CMD_NAV_LOITER_UNLIM: // 17 Loiter indefinitely
do_loiter_unlimited(); do_loiter_unlimited(cmd);
break; break;
case MAV_CMD_NAV_LOITER_TURNS: //18 Loiter N Times case MAV_CMD_NAV_LOITER_TURNS: //18 Loiter N Times
do_circle(); do_circle(cmd);
break; break;
case MAV_CMD_NAV_LOITER_TIME: // 19 case MAV_CMD_NAV_LOITER_TIME: // 19
do_loiter_time(); do_loiter_time(cmd);
break; break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH: //20 case MAV_CMD_NAV_RETURN_TO_LAUNCH: //20
do_RTL(); do_RTL();
break; break;
default: //
break; // conditional commands
} //
}
// process_cond_command - main switch statement to initiate the next conditional command in the command_cond_queue
static void process_cond_command()
{
switch(command_cond_queue.id) {
case MAV_CMD_CONDITION_DELAY: // 112 case MAV_CMD_CONDITION_DELAY: // 112
do_wait_delay(); do_wait_delay(cmd);
break; break;
case MAV_CMD_CONDITION_DISTANCE: // 114 case MAV_CMD_CONDITION_DISTANCE: // 114
do_within_distance(); do_within_distance(cmd);
break; break;
case MAV_CMD_CONDITION_CHANGE_ALT: // 113 case MAV_CMD_CONDITION_CHANGE_ALT: // 113
do_change_alt(); do_change_alt(cmd);
break; break;
case MAV_CMD_CONDITION_YAW: // 115 case MAV_CMD_CONDITION_YAW: // 115
do_yaw(); do_yaw(cmd);
break;
default:
break;
}
}
// process_now_command - main switch statement to initiate the next now command in the command_cond_queue
// now commands are conditional commands that are executed immediately so they do not require a corresponding verify to be run later
static void process_now_command()
{
switch(command_cond_queue.id) {
case MAV_CMD_DO_JUMP: // 177
do_jump();
break; break;
///
/// do commands
///
case MAV_CMD_DO_CHANGE_SPEED: // 178 case MAV_CMD_DO_CHANGE_SPEED: // 178
do_change_speed(); do_change_speed(cmd);
break; break;
case MAV_CMD_DO_SET_HOME: // 179 case MAV_CMD_DO_SET_HOME: // 179
do_set_home(); do_set_home(cmd);
break; break;
case MAV_CMD_DO_SET_SERVO: case MAV_CMD_DO_SET_SERVO:
ServoRelayEvents.do_set_servo(command_cond_queue.p1, command_cond_queue.alt); ServoRelayEvents.do_set_servo(cmd.content.location.p1, cmd.content.location.alt);
break; break;
case MAV_CMD_DO_SET_RELAY: case MAV_CMD_DO_SET_RELAY:
ServoRelayEvents.do_set_relay(command_cond_queue.p1, command_cond_queue.alt); ServoRelayEvents.do_set_relay(cmd.content.location.p1, cmd.content.location.alt);
break; break;
case MAV_CMD_DO_REPEAT_SERVO: case MAV_CMD_DO_REPEAT_SERVO:
ServoRelayEvents.do_repeat_servo(command_cond_queue.p1, command_cond_queue.alt, ServoRelayEvents.do_repeat_servo(cmd.content.location.p1, cmd.content.location.alt,
command_cond_queue.lat, command_cond_queue.lng); cmd.content.location.lat, cmd.content.location.lng);
break; break;
case MAV_CMD_DO_REPEAT_RELAY: case MAV_CMD_DO_REPEAT_RELAY:
ServoRelayEvents.do_repeat_relay(command_cond_queue.p1, command_cond_queue.alt, ServoRelayEvents.do_repeat_relay(cmd.content.location.p1, cmd.content.location.alt,
command_cond_queue.lat); cmd.content.location.lat);
break; break;
case MAV_CMD_DO_SET_ROI: // 201 case MAV_CMD_DO_SET_ROI: // 201
// point the copter and camera at a region of interest (ROI) // point the copter and camera at a region of interest (ROI)
do_roi(); do_roi(cmd);
break; break;
#if CAMERA == ENABLED #if CAMERA == ENABLED
@ -121,7 +122,7 @@ static void process_now_command()
break; break;
case MAV_CMD_DO_SET_CAM_TRIGG_DIST: case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
camera.set_trigger_distance(command_cond_queue.alt); camera.set_trigger_distance(cmd.content.location.alt);
break; break;
#endif #endif
@ -139,24 +140,31 @@ static void process_now_command()
// do nothing with unrecognized MAVLink messages // do nothing with unrecognized MAVLink messages
break; break;
} }
// always return success
return true;
} }
/********************************************************************************/ /********************************************************************************/
// Verify command Handlers // Verify command Handlers
/********************************************************************************/ /********************************************************************************/
// verify_nav_command - switch statement to ensure the active navigation command is progressing // verify_command - this will be called repeatedly by ap_mission lib to ensure the active commands are progressing
// returns true once the active navigation command completes successfully // should return true once the active navigation command completes successfully
static bool verify_nav_command() // called at 10hz or higher
static bool verify_command(const AP_Mission::Mission_Command& cmd)
{ {
switch(command_nav_queue.id) { switch(cmd.id) {
//
// navigation commands
//
case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff(); return verify_takeoff();
break; break;
case MAV_CMD_NAV_WAYPOINT: case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp(); return verify_nav_wp(cmd);
break; break;
case MAV_CMD_NAV_LAND: case MAV_CMD_NAV_LAND:
@ -179,18 +187,9 @@ static bool verify_nav_command()
return verify_RTL(); return verify_RTL();
break; break;
default: ///
return false; /// conditional commands
break; ///
}
}
// verify_cond_command - switch statement to ensure the active conditional command is progressing
// returns true once the active conditional command completes successfully
static bool verify_cond_command()
{
switch(command_cond_queue.id) {
case MAV_CMD_CONDITION_DELAY: case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay(); return verify_wait_delay();
break; break;
@ -208,11 +207,34 @@ static bool verify_cond_command()
break; break;
default: default:
return false; // return true if we do not recognise the command so that we move on to the next command
return true;
break; break;
} }
} }
// exit_mission - function that is called once the mission completes
static void exit_mission()
{
// if we are not on the ground switch to loiter or land
if(!ap.land_complete) {
// try to enter loiter but if that fails land
if (!set_mode(LOITER)) {
set_mode(LAND);
}
}else{
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
// disarm when the landing detector says we've landed and throttle is at minimum
if (g.rc_3.control_in == 0 || failsafe.radio) {
init_disarm_motors();
}
#else
// if we've landed it's safe to disarm
init_disarm_motors();
#endif
}
}
/********************************************************************************/ /********************************************************************************/
// //
/********************************************************************************/ /********************************************************************************/
@ -229,20 +251,21 @@ static void do_RTL(void)
/********************************************************************************/ /********************************************************************************/
// do_takeoff - initiate takeoff navigation command // do_takeoff - initiate takeoff navigation command
static void do_takeoff() static void do_takeoff(const AP_Mission::Mission_Command& cmd)
{ {
// Set wp navigation target to safe altitude above current position // Set wp navigation target to safe altitude above current position
float takeoff_alt = command_nav_queue.alt; float takeoff_alt = cmd.content.location.alt;
takeoff_alt = max(takeoff_alt,current_loc.alt); takeoff_alt = max(takeoff_alt,current_loc.alt);
takeoff_alt = max(takeoff_alt,100.0f); takeoff_alt = max(takeoff_alt,100.0f);
auto_takeoff_start(takeoff_alt); auto_takeoff_start(takeoff_alt);
} }
// do_nav_wp - initiate move to next waypoint // do_nav_wp - initiate move to next waypoint
static void do_nav_wp() static void do_nav_wp(const AP_Mission::Mission_Command& cmd)
{ {
Vector3f local_pos = pv_location_to_vector(cmd.content.location);
// Set wp navigation target // Set wp navigation target
auto_wp_start(pv_location_to_vector(command_nav_queue)); auto_wp_start(local_pos);
// initialise original_wp_bearing which is used to check if we have missed the waypoint // initialise original_wp_bearing which is used to check if we have missed the waypoint
wp_bearing = wp_nav.get_wp_bearing_to_destination(); wp_bearing = wp_nav.get_wp_bearing_to_destination();
@ -251,7 +274,8 @@ static void do_nav_wp()
// this will be used to remember the time in millis after we reach or pass the WP. // this will be used to remember the time in millis after we reach or pass the WP.
loiter_time = 0; loiter_time = 0;
// this is the delay, stored in seconds and expanded to millis // this is the delay, stored in seconds and expanded to millis
loiter_time_max = command_nav_queue.p1; // To-Do: move waypoint delay out of location structure and into cmd
loiter_time_max = cmd.content.location.p1;
// if no delay set the waypoint as "fast" // if no delay set the waypoint as "fast"
if (loiter_time_max == 0 ) { if (loiter_time_max == 0 ) {
wp_nav.set_fast_waypoint(true); wp_nav.set_fast_waypoint(true);
@ -259,17 +283,17 @@ static void do_nav_wp()
} }
// do_land - initiate landing procedure // do_land - initiate landing procedure
static void do_land(const struct Location *cmd) static void do_land(const AP_Mission::Mission_Command& cmd)
{ {
// To-Do: check if we have already landed // To-Do: check if we have already landed
// if location provided we fly to that location at current altitude // if location provided we fly to that location at current altitude
if (cmd != NULL && (cmd->lat != 0 || cmd->lng != 0)) { if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
// set state to fly to location // set state to fly to location
land_state = LAND_STATE_FLY_TO_LOCATION; land_state = LAND_STATE_FLY_TO_LOCATION;
// calculate and set desired location above landing target // calculate and set desired location above landing target
Vector3f pos = pv_location_to_vector(*cmd); Vector3f pos = pv_location_to_vector(cmd.content.location);
pos.z = min(current_loc.alt, RTL_ALT_MAX); pos.z = min(current_loc.alt, RTL_ALT_MAX);
auto_wp_start(pos); auto_wp_start(pos);
@ -287,7 +311,7 @@ static void do_land(const struct Location *cmd)
// do_loiter_unlimited - start loitering with no end conditions // do_loiter_unlimited - start loitering with no end conditions
// note: caller should set yaw_mode // note: caller should set yaw_mode
static void do_loiter_unlimited() static void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{ {
Vector3f target_pos; Vector3f target_pos;
@ -295,16 +319,16 @@ static void do_loiter_unlimited()
Vector3f curr_pos = inertial_nav.get_position(); Vector3f curr_pos = inertial_nav.get_position();
// default to use position provided // default to use position provided
target_pos = pv_location_to_vector(command_nav_queue); target_pos = pv_location_to_vector(cmd.content.location);
// use current location if not provided // use current location if not provided
if(command_nav_queue.lat == 0 && command_nav_queue.lng == 0) { if(cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
wp_nav.get_wp_stopping_point_xy(target_pos); wp_nav.get_wp_stopping_point_xy(target_pos);
} }
// use current altitude if not provided // use current altitude if not provided
// To-Do: use z-axis stopping point instead of current alt // To-Do: use z-axis stopping point instead of current alt
if( command_nav_queue.alt == 0 ) { if( cmd.content.location.alt == 0 ) {
target_pos.z = curr_pos.z; target_pos.z = curr_pos.z;
} }
@ -313,10 +337,10 @@ static void do_loiter_unlimited()
} }
// do_circle - initiate moving in a circle // do_circle - initiate moving in a circle
static void do_circle() static void do_circle(const AP_Mission::Mission_Command& cmd)
{ {
Vector3f curr_pos = inertial_nav.get_position(); Vector3f curr_pos = inertial_nav.get_position();
Vector3f circle_center = pv_location_to_vector(command_nav_queue); Vector3f circle_center = pv_location_to_vector(cmd.content.location);
// set target altitude if not provided // set target altitude if not provided
if (circle_center.z == 0) { if (circle_center.z == 0) {
@ -325,7 +349,7 @@ static void do_circle()
// set lat/lon position if not provided // set lat/lon position if not provided
// To-Do: use stopping point instead of current location // To-Do: use stopping point instead of current location
if (command_nav_queue.lat == 0 && command_nav_queue.lng == 0) { if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
circle_center.x = curr_pos.x; circle_center.x = curr_pos.x;
circle_center.y = curr_pos.y; circle_center.y = curr_pos.y;
} }
@ -334,12 +358,13 @@ static void do_circle()
auto_circle_start(circle_center); auto_circle_start(circle_center);
// record number of desired rotations from mission command // record number of desired rotations from mission command
circle_desired_rotations = command_nav_queue.p1; // To-Do: move p1 from location structure to cmd structure
circle_desired_rotations = cmd.content.location.p1;
} }
// do_loiter_time - initiate loitering at a point for a given time period // do_loiter_time - initiate loitering at a point for a given time period
// note: caller should set yaw_mode // note: caller should set yaw_mode
static void do_loiter_time() static void do_loiter_time(const AP_Mission::Mission_Command& cmd)
{ {
Vector3f target_pos; Vector3f target_pos;
@ -347,15 +372,15 @@ static void do_loiter_time()
Vector3f curr_pos = inertial_nav.get_position(); Vector3f curr_pos = inertial_nav.get_position();
// default to use position provided // default to use position provided
target_pos = pv_location_to_vector(command_nav_queue); target_pos = pv_location_to_vector(cmd.content.location);
// use current location if not provided // use current location if not provided
if(command_nav_queue.lat == 0 && command_nav_queue.lng == 0) { if(cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
wp_nav.get_wp_stopping_point_xy(target_pos); wp_nav.get_wp_stopping_point_xy(target_pos);
} }
// use current altitude if not provided // use current altitude if not provided
if( command_nav_queue.alt == 0 ) { if( cmd.content.location.alt == 0 ) {
target_pos.z = curr_pos.z; target_pos.z = curr_pos.z;
} }
@ -364,7 +389,7 @@ static void do_loiter_time()
// setup loiter timer // setup loiter timer
loiter_time = 0; loiter_time = 0;
loiter_time_max = command_nav_queue.p1; // units are (seconds) loiter_time_max = cmd.content.location.p1; // units are (seconds)
} }
/********************************************************************************/ /********************************************************************************/
@ -415,7 +440,7 @@ static bool verify_land()
} }
// verify_nav_wp - check if we have reached the next way point // verify_nav_wp - check if we have reached the next way point
static bool verify_nav_wp() static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd)
{ {
// check if we have reached the waypoint // check if we have reached the waypoint
if( !wp_nav.reached_wp_destination() ) { if( !wp_nav.reached_wp_destination() ) {
@ -429,7 +454,7 @@ static bool verify_nav_wp()
// check if timer has run out // check if timer has run out
if (((millis() - loiter_time) / 1000) >= loiter_time_max) { if (((millis() - loiter_time) / 1000) >= loiter_time_max) {
gcs_send_text_fmt(PSTR("Reached Command #%i"),command_nav_index); gcs_send_text_fmt(PSTR("Reached Command #%i"),cmd.index);
return true; return true;
}else{ }else{
return false; return false;
@ -480,15 +505,15 @@ static bool verify_RTL()
// Condition (May) commands // Condition (May) commands
/********************************************************************************/ /********************************************************************************/
static void do_wait_delay() static void do_wait_delay(const AP_Mission::Mission_Command& cmd)
{ {
//cliSerial->print("dwd "); //cliSerial->print("dwd ");
condition_start = millis(); condition_start = millis();
condition_value = command_cond_queue.lat * 1000; // convert to milliseconds condition_value = cmd.content.location.lat * 1000; // convert to milliseconds
//cliSerial->println(condition_value,DEC); //cliSerial->println(condition_value,DEC);
} }
static void do_change_alt() static void do_change_alt(const AP_Mission::Mission_Command& cmd)
{ {
// adjust target appropriately for each nav mode // adjust target appropriately for each nav mode
if (control_mode == AUTO) { if (control_mode == AUTO) {
@ -511,31 +536,32 @@ static void do_change_alt()
// To-Do: store desired altitude in a variable so that it can be verified later // To-Do: store desired altitude in a variable so that it can be verified later
} }
static void do_within_distance() static void do_within_distance(const AP_Mission::Mission_Command& cmd)
{ {
condition_value = command_cond_queue.lat * 100; // To-Do: define another union of Mission_Command instead of using location's lat
condition_value = cmd.content.location.lat * 100;
} }
static void do_yaw() static void do_yaw(const AP_Mission::Mission_Command& cmd)
{ {
// get current yaw target // get current yaw target
int32_t curr_yaw_target = attitude_control.angle_ef_targets().z; int32_t curr_yaw_target = attitude_control.angle_ef_targets().z;
// get final angle, 1 = Relative, 0 = Absolute // get final angle, 1 = Relative, 0 = Absolute
if( command_cond_queue.lng == 0 ) { if( cmd.content.location.lng == 0 ) {
// absolute angle // absolute angle
yaw_look_at_heading = wrap_360_cd(command_cond_queue.alt * 100); yaw_look_at_heading = wrap_360_cd(cmd.content.location.alt * 100);
}else{ }else{
// relative angle // relative angle
yaw_look_at_heading = wrap_360_cd(curr_yaw_target + command_cond_queue.alt * 100); yaw_look_at_heading = wrap_360_cd(curr_yaw_target + cmd.content.location.alt * 100);
} }
// get turn speed // get turn speed
if( command_cond_queue.lat == 0 ) { if (cmd.content.location.lat == 0 ) {
// default to regular auto slew rate // default to regular auto slew rate
yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE; yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE;
}else{ }else{
int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - curr_yaw_target) / 100) / command_cond_queue.lat; int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - curr_yaw_target) / 100) / cmd.content.location.lat;
yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec
} }
@ -593,60 +619,33 @@ static bool verify_yaw()
/********************************************************************************/ /********************************************************************************/
// do_guided - start guided mode // do_guided - start guided mode
// this is not actually a mission command but rather a static bool do_guided(const AP_Mission::Mission_Command& cmd)
static void do_guided(const struct Location *cmd)
{ {
// switch to guided mode if we're not already in guided mode // switch to guided mode if we're not already in guided mode
if (control_mode != GUIDED) { if (control_mode != GUIDED) {
if (!set_mode(GUIDED)) { if (!set_mode(GUIDED)) {
// if we failed to enter guided mode return immediately // if we failed to enter guided mode return immediately
return; return false;
} }
} }
// set wp_nav's destination // set wp_nav's destination
Vector3f pos = pv_location_to_vector(*cmd); Vector3f pos = pv_location_to_vector(cmd.content.location);
guided_set_destination(pos); guided_set_destination(pos);
return true;
} }
static void do_change_speed() static void do_change_speed(const AP_Mission::Mission_Command& cmd)
{ {
wp_nav.set_horizontal_velocity(command_cond_queue.p1 * 100); wp_nav.set_horizontal_velocity(cmd.content.location.p1 * 100);
} }
static void do_jump() static void do_set_home(const AP_Mission::Mission_Command& cmd)
{ {
// Used to track the state of the jump command in Mission scripting if(cmd.content.location.p1 == 1) {
// -10 is a value that means the register is unused
// when in use, it contains the current remaining jumps
static int8_t jump = -10; // used to track loops in jump command
if(jump == -10) {
// we use a locally stored index for jump
jump = command_cond_queue.lat;
}
if(jump > 0) {
jump--;
change_command(command_cond_queue.p1);
} else if (jump == 0) {
// we're done, move along
jump = -11;
} else if (jump == -1) {
// repeat forever
change_command(command_cond_queue.p1);
}
}
static void do_set_home()
{
if(command_cond_queue.p1 == 1) {
init_home(); init_home();
} else { } else {
ahrs.set_home(command_cond_queue.lat, command_cond_queue.lng, 0); ahrs.set_home(cmd.content.location.lat, cmd.content.location.lng, 0);
//home_is_set = true;
set_home_is_set(true); set_home_is_set(true);
} }
} }
@ -656,10 +655,10 @@ static void do_set_home()
// and possibly rotating the copter to point at the ROI if our mount type does not support a yaw feature // and possibly rotating the copter to point at the ROI if our mount type does not support a yaw feature
// Note: the ROI should already be in the command_nav_queue global variable // Note: the ROI should already be in the command_nav_queue global variable
// TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint // TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint
static void do_roi() static void do_roi(const AP_Mission::Mission_Command& cmd)
{ {
// if location is zero lat, lon and altitude turn off ROI // if location is zero lat, lon and altitude turn off ROI
if (auto_yaw_mode == AUTO_YAW_ROI && (command_cond_queue.alt == 0 && command_cond_queue.lat == 0 && command_cond_queue.lng == 0)) { if (auto_yaw_mode == AUTO_YAW_ROI && (cmd.content.location.alt == 0 && cmd.content.location.lat == 0 && cmd.content.location.lng == 0)) {
// set auto yaw mode back to default assuming the active command is a waypoint command. A more sophisticated method is required to ensure we return to the proper yaw control for the active command // set auto yaw mode back to default assuming the active command is a waypoint command. A more sophisticated method is required to ensure we return to the proper yaw control for the active command
set_auto_yaw_mode(get_default_auto_yaw_mode(false)); set_auto_yaw_mode(get_default_auto_yaw_mode(false));
#if MOUNT == ENABLED #if MOUNT == ENABLED
@ -672,11 +671,11 @@ static void do_roi()
#if MOUNT == ENABLED #if MOUNT == ENABLED
// check if mount type requires us to rotate the quad // check if mount type requires us to rotate the quad
if(camera_mount.get_mount_type() != AP_Mount::k_pan_tilt && camera_mount.get_mount_type() != AP_Mount::k_pan_tilt_roll) { if(camera_mount.get_mount_type() != AP_Mount::k_pan_tilt && camera_mount.get_mount_type() != AP_Mount::k_pan_tilt_roll) {
roi_WP = pv_location_to_vector(command_cond_queue); roi_WP = pv_location_to_vector(cmd.content.location);
set_auto_yaw_mode(AUTO_YAW_ROI); set_auto_yaw_mode(AUTO_YAW_ROI);
} }
// send the command to the camera mount // send the command to the camera mount
camera_mount.set_roi_cmd(&command_cond_queue); camera_mount.set_roi_cmd(&cmd.content.location);
// TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink. Currently we only handle mode 4 (see below) // TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink. Currently we only handle mode 4 (see below)
// 0: do nothing // 0: do nothing
@ -686,7 +685,7 @@ static void do_roi()
// 4: point at a target given a target id (can't be implemented) // 4: point at a target given a target id (can't be implemented)
#else #else
// if we have no camera mount aim the quad at the location // if we have no camera mount aim the quad at the location
roi_WP = pv_location_to_vector(command_cond_queue); roi_WP = pv_location_to_vector(cmd.content.location);
set_auto_yaw_mode(AUTO_YAW_ROI); set_auto_yaw_mode(AUTO_YAW_ROI);
#endif // MOUNT == ENABLED #endif // MOUNT == ENABLED
} }

6
ArduCopter/control_auto.pde
View File

@ -20,14 +20,14 @@
// auto_init - initialise auto controller // auto_init - initialise auto controller
static bool auto_init(bool ignore_checks) static bool auto_init(bool ignore_checks)
{ {
if ((GPS_ok() && inertial_nav.position_ok() && g.command_total > 1) || ignore_checks) { if ((GPS_ok() && inertial_nav.position_ok() && mission.num_commands() > 0) || ignore_checks) {
// stop ROI from carrying over from previous runs of the mission // stop ROI from carrying over from previous runs of the mission
// To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
if (auto_yaw_mode == AUTO_YAW_ROI) { if (auto_yaw_mode == AUTO_YAW_ROI) {
set_auto_yaw_mode(AUTO_YAW_HOLD); set_auto_yaw_mode(AUTO_YAW_HOLD);
} }
// clear the command queues. will be reloaded when "run_autopilot" calls "update_commands" function // start the mission
init_commands(); mission.start();
return true; return true;
}else{ }else{
return false; return false;

8
ArduCopter/navigation.pde
View File

@ -55,11 +55,9 @@ static void calc_distance_and_bearing()
static void run_autopilot() static void run_autopilot()
{ {
if (control_mode == AUTO) { if (control_mode == AUTO) {
// load the next command if the command queues are empty // update state of mission
update_commands(); // may call commands_process.pde's start_command and verify_command functions
mission.update();
// process the active navigation and conditional commands
verify_commands();
} }
} }