// verify_command - this will be called repeatedly by ap_mission lib to ensure the active commands are progressing
// should return true once the active navigation command completes successfully
// called at 10hz or higher
bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
{
switch(cmd.id) {
//
// navigation commands
//
case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff();
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp(cmd);
case MAV_CMD_NAV_LAND:
return verify_land();
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlimited();
case MAV_CMD_NAV_LOITER_TURNS:
return verify_circle(cmd);
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time();
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
case MAV_CMD_NAV_SPLINE_WAYPOINT:
return verify_spline_wp(cmd);
#if NAV_GUIDED == ENABLED
case MAV_CMD_NAV_GUIDED_ENABLE:
return verify_nav_guided_enable(cmd);
#endif
///
/// conditional commands
///
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
case MAV_CMD_CONDITION_CHANGE_ALT:
return verify_change_alt();
case MAV_CMD_CONDITION_YAW:
return verify_yaw();
case MAV_CMD_DO_PARACHUTE:
// assume parachute was released successfully
return true;
default:
// return true if we do not recognize the command so that we move on to the next command
return true;
}
}
bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
{
// exit immediately if not in AUTO mode
// we return true or we will continue to be called by ap-mission
if (control_mode != AUTO) {
return true;
}
switch(cmd.id) {
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp(cmd);
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
break;
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
break;
default:
if (cmd.id > MAV_CMD_CONDITION_LAST) {
// this is a command that doesn't require verify
return true;
}
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Unsupported command"));
return true;
break;
}
return false;
}
bool Rover::verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
// Continually set loiter start time to now so it never finishes
loiter_start_time += millis();
verify_nav_wp(cmd);
return false;
}
// verify_loiter_time - check if we have loitered long enough
bool Rover::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
{
const bool result = verify_nav_wp(cmd);
if (result) {
gcs().send_text(MAV_SEVERITY_WARNING, "Finished active loiter\n");
}
return result;
}
// verify_loiter_time - check if we have loitered long enough
bool Rover::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
{
bool result = verify_nav_wp(cmd);
if (result) {
gcs_send_text(MAV_SEVERITY_WARNING, "Finished active loiter\n");
// if we have finished active loitering - turn it off
active_loiter = false;
}
return result;
}
bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
{
switch (cmd.id) {
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp(cmd);
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlimited(cmd);
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time(cmd);
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
case MAV_CMD_NAV_SET_YAW_SPEED:
return verify_nav_set_yaw_speed();
// do commands (always return true)
case MAV_CMD_DO_CHANGE_SPEED:
case MAV_CMD_DO_SET_HOME:
case MAV_CMD_DO_SET_SERVO:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_REPEAT_SERVO:
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_CONTROL_VIDEO:
case MAV_CMD_DO_DIGICAM_CONFIGURE:
case MAV_CMD_DO_DIGICAM_CONTROL:
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
case MAV_CMD_DO_SET_ROI:
case MAV_CMD_DO_SET_REVERSE:
case MAV_CMD_DO_FENCE_ENABLE:
return true;
default:
// error message
gcs().send_text(MAV_SEVERITY_WARNING, "Skipping invalid cmd #%i", cmd.id);
// return true if we do not recognize the command so that we move on to the next command
return true;
}
}
bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Returns true if command complete
{
switch(cmd.id) {
case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff();
case MAV_CMD_NAV_LAND:
return verify_land();
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp(cmd);
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlim();
case MAV_CMD_NAV_LOITER_TURNS:
return verify_loiter_turns();
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time();
case MAV_CMD_NAV_LOITER_TO_ALT:
return verify_loiter_to_alt();
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:
return verify_wait_delay();
break;
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
return verify_change_alt();
break;
// do commands (always return true)
case MAV_CMD_DO_CHANGE_SPEED:
case MAV_CMD_DO_SET_HOME:
case MAV_CMD_DO_SET_SERVO:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_REPEAT_SERVO:
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_CONTROL_VIDEO:
case MAV_CMD_DO_DIGICAM_CONFIGURE:
case MAV_CMD_DO_DIGICAM_CONTROL:
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
case MAV_CMD_NAV_ROI:
case MAV_CMD_DO_MOUNT_CONFIGURE:
case MAV_CMD_DO_MOUNT_CONTROL:
case MAV_CMD_DO_INVERTED_FLIGHT:
case MAV_CMD_DO_LAND_START:
case MAV_CMD_DO_FENCE_ENABLE:
return true;
default:
// error message
if (AP_Mission::is_nav_cmd(cmd)) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd"));
}else{
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd"));
}
// return true so that we do not get stuck at this command
return true;
}
}
bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
{
switch(cmd.id) {
//
// navigation commands
//
case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff();
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp(cmd);
case MAV_CMD_NAV_LAND:
return verify_land();
case MAV_CMD_NAV_PAYLOAD_PLACE:
return verify_payload_place();
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlimited();
case MAV_CMD_NAV_LOITER_TURNS:
return verify_circle(cmd);
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time();
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
case MAV_CMD_NAV_SPLINE_WAYPOINT:
return verify_spline_wp(cmd);
#if NAV_GUIDED == ENABLED
case MAV_CMD_NAV_GUIDED_ENABLE:
return verify_nav_guided_enable(cmd);
#endif
case MAV_CMD_NAV_DELAY:
return verify_nav_delay(cmd);
///
/// conditional commands
///
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
case MAV_CMD_CONDITION_YAW:
return verify_yaw();
// do commands (always return true)
case MAV_CMD_DO_CHANGE_SPEED:
case MAV_CMD_DO_SET_HOME:
case MAV_CMD_DO_SET_SERVO:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_REPEAT_SERVO:
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_SET_ROI:
case MAV_CMD_DO_MOUNT_CONTROL:
case MAV_CMD_DO_CONTROL_VIDEO:
case MAV_CMD_DO_DIGICAM_CONFIGURE:
case MAV_CMD_DO_DIGICAM_CONTROL:
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
case MAV_CMD_DO_PARACHUTE: // assume parachute was released successfully
case MAV_CMD_DO_GRIPPER:
case MAV_CMD_DO_GUIDED_LIMITS:
return true;
default:
// error message
gcs_send_text_fmt(MAV_SEVERITY_WARNING,"Skipping invalid cmd #%i",cmd.id);
// return true if we do not recognize the command so that we move on to the next command
return true;
}
}
bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Returns true if command complete
{
switch(cmd.id) {
case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff();
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp(cmd);
case MAV_CMD_NAV_LAND:
if (flight_stage == AP_Vehicle::FixedWing::FlightStage::FLIGHT_ABORT_LAND) {
return landing.verify_abort_landing(prev_WP_loc, next_WP_loc, current_loc, auto_state.takeoff_altitude_rel_cm, throttle_suppressed);
} else {
// use rangefinder to correct if possible
const float height = height_above_target() - rangefinder_correction();
return landing.verify_land(prev_WP_loc, next_WP_loc, current_loc,
height, auto_state.sink_rate, auto_state.wp_proportion, auto_state.last_flying_ms, arming.is_armed(), is_flying(), rangefinder_state.in_range);
}
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlim();
case MAV_CMD_NAV_LOITER_TURNS:
return verify_loiter_turns();
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time();
case MAV_CMD_NAV_LOITER_TO_ALT:
return verify_loiter_to_alt();
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
return verify_continue_and_change_alt();
case MAV_CMD_NAV_ALTITUDE_WAIT:
return verify_altitude_wait(cmd);
case MAV_CMD_NAV_VTOL_TAKEOFF:
return quadplane.verify_vtol_takeoff(cmd);
case MAV_CMD_NAV_VTOL_LAND:
return quadplane.verify_vtol_land();
// Conditional commands
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
#if PARACHUTE == ENABLED
case MAV_CMD_DO_PARACHUTE:
// assume parachute was released successfully
return true;
#endif
// do commands (always return true)
case MAV_CMD_DO_CHANGE_SPEED:
case MAV_CMD_DO_SET_HOME:
case MAV_CMD_DO_SET_SERVO:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_REPEAT_SERVO:
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_INVERTED_FLIGHT:
case MAV_CMD_DO_LAND_START:
case MAV_CMD_DO_FENCE_ENABLE:
case MAV_CMD_DO_AUTOTUNE_ENABLE:
case MAV_CMD_DO_CONTROL_VIDEO:
case MAV_CMD_DO_DIGICAM_CONFIGURE:
case MAV_CMD_DO_DIGICAM_CONTROL:
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
case MAV_CMD_DO_SET_ROI:
case MAV_CMD_DO_MOUNT_CONTROL:
case MAV_CMD_DO_VTOL_TRANSITION:
case MAV_CMD_DO_ENGINE_CONTROL:
return true;
default:
// error message
gcs_send_text_fmt(MAV_SEVERITY_WARNING,"Skipping invalid cmd #%i",cmd.id);
// return true if we do not recognize the command so that we move on to the next command
return true;
}
}
bool Rover::verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
verify_nav_wp(cmd);
return false;
}
bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Returns true if command complete
{
switch(cmd.id) {
case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff();
case MAV_CMD_NAV_LAND:
return verify_land();
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp(cmd);
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlim();
case MAV_CMD_NAV_LOITER_TURNS:
return verify_loiter_turns();
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time();
case MAV_CMD_NAV_LOITER_TO_ALT:
return verify_loiter_to_alt();
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
return verify_continue_and_change_alt();
case MAV_CMD_NAV_ALTITUDE_WAIT:
return verify_altitude_wait(cmd);
// Conditional commands
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
case MAV_CMD_CONDITION_CHANGE_ALT:
return verify_change_alt();
#if PARACHUTE == ENABLED
case MAV_CMD_DO_PARACHUTE:
// assume parachute was released successfully
return true;
break;
#endif
// do commands (always return true)
case MAV_CMD_DO_CHANGE_SPEED:
case MAV_CMD_DO_SET_HOME:
case MAV_CMD_DO_SET_SERVO:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_REPEAT_SERVO:
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_CONTROL_VIDEO:
case MAV_CMD_DO_DIGICAM_CONFIGURE:
case MAV_CMD_DO_DIGICAM_CONTROL:
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
case MAV_CMD_NAV_ROI:
case MAV_CMD_DO_MOUNT_CONFIGURE:
case MAV_CMD_DO_INVERTED_FLIGHT:
case MAV_CMD_DO_LAND_START:
case MAV_CMD_DO_FENCE_ENABLE:
case MAV_CMD_DO_AUTOTUNE_ENABLE:
return true;
default:
// error message
if (AP_Mission::is_nav_cmd(cmd)) {
gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav. Invalid or no current nav cmd");
}else{
gcs_send_text(MAV_SEVERITY_WARNING,"Verify conditon. Invalid or no current condition cmd");
}
// return true so that we do not get stuck at this command
return true;
}
}
// check if current mission command has completed
bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
{
switch (cmd.id) {
//
// navigation commands
//
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp(cmd);
case MAV_CMD_NAV_LAND:
return verify_surface(cmd);
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlimited();
case MAV_CMD_NAV_LOITER_TURNS:
return verify_circle(cmd);
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time();
case MAV_CMD_NAV_SPLINE_WAYPOINT:
return verify_spline_wp(cmd);
#if NAV_GUIDED == ENABLED
case MAV_CMD_NAV_GUIDED_ENABLE:
return verify_nav_guided_enable(cmd);
#endif
case MAV_CMD_NAV_DELAY:
return verify_nav_delay(cmd);
///
/// conditional commands
///
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
case MAV_CMD_CONDITION_YAW:
return verify_yaw();
// do commands (always return true)
case MAV_CMD_DO_CHANGE_SPEED:
case MAV_CMD_DO_SET_HOME:
case MAV_CMD_DO_SET_ROI:
case MAV_CMD_DO_MOUNT_CONTROL:
case MAV_CMD_DO_CONTROL_VIDEO:
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
case MAV_CMD_DO_GUIDED_LIMITS:
return true;
default:
// error message
gcs().send_text(MAV_SEVERITY_WARNING,"Skipping invalid cmd #%i",cmd.id);
// return true if we do not recognize the command so that we move on to the next command
return true;
}
}
