ardupilot/libraries/AP_Mission/AP_Mission.cpp

/// @file    AP_Mission.cpp
/// @brief   Handles the MAVLINK command mission stack.  Reads and writes mission to storage.

#include "AP_Mission_config.h"

#if AP_MISSION_ENABLED

#include <AP_AHRS/AP_AHRS.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_Camera/AP_Camera.h>
#include <AP_Gripper/AP_Gripper_config.h>
#include "AP_Mission.h"
#include <AP_Scripting/AP_Scripting.h>
#include <AP_ServoRelayEvents/AP_ServoRelayEvents_config.h>
#include <AP_Terrain/AP_Terrain.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <GCS_MAVLink/GCS.h>
#include <RC_Channel/RC_Channel_config.h>
#include <AC_Fence/AC_Fence.h>
#include <AP_Logger/AP_Logger.h>

const AP_Param::GroupInfo AP_Mission::var_info[] = {

    // @Param: TOTAL
    // @DisplayName: Total mission commands
    // @Description: The number of mission mission items that has been loaded by the ground station. Do not change this manually.
    // @Range: 0 32766
    // @Increment: 1
    // @User: Advanced
    // @ReadOnly: True
    AP_GROUPINFO_FLAGS("TOTAL",  0, AP_Mission, _cmd_total, 0, AP_PARAM_FLAG_INTERNAL_USE_ONLY),

    // @Param: RESTART
    // @DisplayName: Mission Restart when entering Auto mode
    // @Description: Controls mission starting point when entering Auto mode (either restart from beginning of mission or resume from last command run)
    // @Values: 0:Resume Mission, 1:Restart Mission
    // @User: Advanced
    AP_GROUPINFO("RESTART",  1, AP_Mission, _restart, AP_MISSION_RESTART_DEFAULT),

    // @Param: OPTIONS
    // @DisplayName: Mission options bitmask
    // @Description: Bitmask of what options to use in missions.
    // @Bitmask: 0:Clear Mission on reboot, 1:Use distance to land calc on battery failsafe,2:ContinueAfterLand
    // @Bitmask{Copter}: 0:Clear Mission on reboot, 2:ContinueAfterLand
    // @Bitmask{Rover, Sub}: 0:Clear Mission on reboot
    // @User: Advanced
    AP_GROUPINFO("OPTIONS",  2, AP_Mission, _options, AP_MISSION_OPTIONS_DEFAULT),

    AP_GROUPEND
};

extern const AP_HAL::HAL& hal;

// storage object
StorageAccess AP_Mission::_storage(StorageManager::StorageMission);

HAL_Semaphore AP_Mission::_rsem;

///
/// public mission methods
///

/// init - initialises this library including checks the version in eeprom matches this library
void AP_Mission::init()
{
#if AP_SDCARD_STORAGE_ENABLED
    // check for extra storage on microsd
    const auto *bc = AP::boardConfig();
    if (bc != nullptr) {
        const auto size_kb = bc->get_sdcard_mission_kb();
        if (size_kb > 0) {
            _failed_sdcard_storage = !_storage.attach_file(AP_MISSION_SDCARD_FILENAME, size_kb);
            if (_failed_sdcard_storage) {
                // wipe mission if storage not available, but don't save. This allows sdcard error to be fixed and reboot
                _cmd_total.set(0);
            }
        }
    }
#endif

    // work out maximum index for our storage size
    if (_storage.size() >= AP_MISSION_EEPROM_COMMAND_SIZE+4) {
        _commands_max = (_storage.size()-4U) / AP_MISSION_EEPROM_COMMAND_SIZE;
    }
    if (_cmd_total.get() > _commands_max) {
        // wipe mission if storage not available, but don't save. This allows sdcard error to be fixed and reboot
        _cmd_total.set(0);
    }


    // check_eeprom_version - checks version of missions stored in eeprom matches this library
    // command list will be cleared if they do not match
    check_eeprom_version();

    // initialize the jump tracking array
    init_jump_tracking();

    // If Mission Clear bit is set then it should clear the mission, otherwise retain the mission.
    if (option_is_set(Option::CLEAR_ON_BOOT)) {
        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Clearing Mission");
        clear();
    }

    _last_change_time_ms = AP_HAL::millis();
}

/// start - resets current commands to point to the beginning of the mission
///     To-Do: should we validate the mission first and return true/false?
void AP_Mission::start()
{
    _flags.state = MISSION_RUNNING;

    reset(); // reset mission to the first command, resets jump tracking

    // advance to the first command
    if (!advance_current_nav_cmd()) {
        // on failure set mission complete
        complete();
    }
}

/// stop - stops mission execution.  subsequent calls to update() will have no effect until the mission is started or resumed
void AP_Mission::stop()
{
    _flags.state = MISSION_STOPPED;
}

/// resume - continues the mission execution from where we last left off
///     previous running commands will be re-initialized
void AP_Mission::resume()
{
    // if mission had completed then start it from the first command
    if (_flags.state == MISSION_COMPLETE) {
        start();
        return;
    }

    // if mission had stopped then restart it
    if (_flags.state == MISSION_STOPPED) {
        _flags.state = MISSION_RUNNING;

        // if no valid nav command index restart from beginning
        if (_nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) {
            start();
            return;
        }
    }

    // ensure cache coherence
    if (!read_cmd_from_storage(_nav_cmd.index, _nav_cmd)) {
        // if we failed to read the command from storage, then the command may have
        // been from a previously loaded mission it is illogical to ever resume
        // flying to a command that has been excluded from the current mission
        start();
        return;
    }

    // rewind the mission wp if the repeat distance has been set via MAV_CMD_DO_SET_RESUME_REPEAT_DIST
    if (_repeat_dist > 0 && _wp_index_history[LAST_WP_PASSED] != AP_MISSION_CMD_INDEX_NONE) {
        // if not already in a resume state calculate the position to rewind to
        Mission_Command tmp_cmd;
        if (!_flags.resuming_mission && calc_rewind_pos(tmp_cmd)) {
            _resume_cmd = tmp_cmd;
        }

        // resume mission to rewound position
        if (_resume_cmd.index != AP_MISSION_CMD_INDEX_NONE && start_command(_resume_cmd)) {
            _nav_cmd = _resume_cmd;
            _flags.nav_cmd_loaded = true;
            // set flag to prevent history being re-written
            _flags.resuming_mission = true;
            return;
        }
    }

    // restart active navigation command. We run these on resume()
    // regardless of whether the mission was stopped, as we may be
    // re-entering AUTO mode and the nav_cmd callback needs to be run
    // to setup the current target waypoint

    if (_flags.do_cmd_loaded && _do_cmd.index != AP_MISSION_CMD_INDEX_NONE) {
        // restart the active do command, which will also load the nav command for us
        set_current_cmd(_do_cmd.index);
    } else if (_flags.nav_cmd_loaded) {
        // restart the active nav command
        set_current_cmd(_nav_cmd.index);
    }

    // Note: if there is no active command then the mission must have been stopped just after the previous nav command completed
    //      update will take care of finding and starting the nav command
}

/// check if the next nav command is a takeoff, skipping delays
bool AP_Mission::is_takeoff_next(uint16_t cmd_index)
{
    Mission_Command cmd = {};
    // check a maximum of 16 items, remembering that missions can have
    // loops in them
    for (uint8_t i=0; i<16; i++, cmd_index++) {
        if (!get_next_nav_cmd(cmd_index, cmd)) {
            return false;
        }
        switch (cmd.id) {
        // any of these are considered a takeoff command:
        case MAV_CMD_NAV_VTOL_TAKEOFF:
        case MAV_CMD_NAV_TAKEOFF:
        case MAV_CMD_NAV_TAKEOFF_LOCAL:
            return true;
        // any of these are considered "skippable" when determining if
        // we "start with a takeoff command"
        case MAV_CMD_DO_AUX_FUNCTION:
        case MAV_CMD_NAV_DELAY:
            continue;
        default:
            return false;
        }
    }
    return false;
}

/// check mission starts with a takeoff command
bool AP_Mission::starts_with_takeoff_cmd()
{
    uint16_t cmd_index = _restart ? AP_MISSION_CMD_INDEX_NONE : _nav_cmd.index;
    if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
        cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
    }
    return is_takeoff_next(cmd_index);
}

/*
    return true if MIS_OPTIONS is set to allow continue of mission
    logic after a land and the next waypoint is a takeoff. If this
    is false then after a landing is complete the vehicle should 
    disarm and mission logic should stop
*/
bool AP_Mission::continue_after_land_check_for_takeoff()
{
    if (!continue_after_land()) {
        return false;
    }
    if (_nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) {
        return false;
    }
    return is_takeoff_next(_nav_cmd.index+1);
}

/// start_or_resume - if MIS_AUTORESTART=0 this will call resume(), otherwise it will call start()
void AP_Mission::start_or_resume()
{
    if (_restart == 1 && !_force_resume) {
        start();
    } else {
        resume();
        _force_resume = false;
    }
}

/// reset - reset mission to the first command
void AP_Mission::reset()
{
    _flags.nav_cmd_loaded  = false;
    _flags.do_cmd_loaded   = false;
    _flags.do_cmd_all_done = false;
    _flags.in_landing_sequence = false;
    _flags.in_return_path = false;
    _nav_cmd.index         = AP_MISSION_CMD_INDEX_NONE;
    _do_cmd.index          = AP_MISSION_CMD_INDEX_NONE;
    _prev_nav_cmd_index    = AP_MISSION_CMD_INDEX_NONE;
    _prev_nav_cmd_wp_index = AP_MISSION_CMD_INDEX_NONE;
    _prev_nav_cmd_id       = AP_MISSION_CMD_ID_NONE;
    init_jump_tracking();
    reset_wp_history();
}

/// clear - clears out mission
///     returns false if mission was running so it could not be cleared
bool AP_Mission::clear()
{
    // do not allow clearing the mission while it is running unless disarmed
    if (hal.util->get_soft_armed() && _flags.state == MISSION_RUNNING) {
        return false;
    }

    // remove all commands
    truncate(0);

    // clear index to commands
    _nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
    _do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
    _flags.nav_cmd_loaded = false;
    _flags.do_cmd_loaded = false;
    _flags.state = MISSION_STOPPED;
    // return success
    return true;
}


/// trucate - truncate any mission items beyond index
void AP_Mission::truncate(uint16_t index)
{
    if ((unsigned)_cmd_total > index) {
        _cmd_total.set_and_save(index);
        _last_change_time_ms = AP_HAL::millis();
    }
}

/// update - ensures the command queues are loaded with the next command and calls main programs command_init and command_verify functions to progress the mission
///     should be called at 10hz or higher
void AP_Mission::update()
{
    // exit immediately if not running or no mission commands
    if (_flags.state != MISSION_RUNNING || _cmd_total == 0) {
        return;
    }

    update_exit_position();

    // mission_change events
    if (_last_change_time_prev_ms != _last_change_time_ms) {
        _last_change_time_prev_ms = _last_change_time_ms;
        on_mission_timestamp_change();
    }

    // save persistent waypoint_num for watchdog restore
    hal.util->persistent_data.waypoint_num = _nav_cmd.index;

    // check if we have an active nav command
    if (!_flags.nav_cmd_loaded || _nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) {
        // advance in mission if no active nav command
        if (!advance_current_nav_cmd()) {
            // failure to advance nav command means mission has completed
            complete();
            return;
        }
    } else {
        // run the active nav command
        if (verify_command(_nav_cmd)) {
            // market _nav_cmd as complete (it will be started on the next iteration)
            _flags.nav_cmd_loaded = false;
            // immediately advance to the next mission command
            if (!advance_current_nav_cmd()) {
                // failure to advance nav command means mission has completed
                complete();
                return;
            }
        }
    }

    // check if we have an active do command
    if (!_flags.do_cmd_loaded) {
        advance_current_do_cmd();
    } else {
        // check the active do command
        if (verify_command(_do_cmd)) {
            // mark _do_cmd as complete
            _flags.do_cmd_loaded = false;
        }
    }
}

// handle events for when the mission has been updated (but maybe not changed)
void AP_Mission::on_mission_timestamp_change()
{
    _jump_tag.age = 0;
}

bool AP_Mission::verify_command(const Mission_Command& cmd)
{
    switch (cmd.id) {
    // do-commands always return true for verify:
#if AP_GRIPPER_ENABLED
    case MAV_CMD_DO_GRIPPER:
#endif
    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_DIGICAM_CONFIGURE:
    case MAV_CMD_DO_DIGICAM_CONTROL:
    case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
    case MAV_CMD_DO_PARACHUTE:
    case MAV_CMD_DO_SEND_SCRIPT_MESSAGE:
    case MAV_CMD_DO_SPRAYER:
    case MAV_CMD_DO_AUX_FUNCTION:
    case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
    case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
    case MAV_CMD_JUMP_TAG:
    case MAV_CMD_IMAGE_START_CAPTURE:
    case MAV_CMD_IMAGE_STOP_CAPTURE:
    case MAV_CMD_SET_CAMERA_ZOOM:
    case MAV_CMD_SET_CAMERA_FOCUS:
    case MAV_CMD_SET_CAMERA_SOURCE:
    case MAV_CMD_VIDEO_START_CAPTURE:
    case MAV_CMD_VIDEO_STOP_CAPTURE:
        return true;
    default:
        return _cmd_verify_fn(cmd);
    }
}

bool AP_Mission::start_command(const Mission_Command& cmd)
{
#if HAL_LOGGING_ENABLED
    if (log_start_mission_item_bit != (uint32_t)-1) {
        auto &logger = AP::logger();
        if (logger.should_log(log_start_mission_item_bit)) {
            logger.Write_MISE(*this, cmd);
        }
    }
#endif

    // check for landing related commands and set flags
    if (is_landing_type_cmd(cmd.id) || cmd.id == MAV_CMD_DO_LAND_START) {
        _flags.in_landing_sequence = true;

    } else if (cmd.id == MAV_CMD_DO_RETURN_PATH_START) {
        _flags.in_return_path = true;

    } else if (is_takeoff_type_cmd(cmd.id)) {
        // Clear landing and return path flags on takeoff
        _flags.in_landing_sequence = false;
        _flags.in_return_path = false;

    }

    switch (cmd.id) {
    case MAV_CMD_DO_JUMP:
    case MAV_CMD_JUMP_TAG:
    case MAV_CMD_DO_JUMP_TAG:
        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: %u %s %u", cmd.index, cmd.type(), (unsigned)cmd.p1);
        break;

    default:
        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: %u %s", cmd.index, cmd.type());
        break;

    }

    switch (cmd.id) {
#if AP_RC_CHANNEL_ENABLED
    case MAV_CMD_DO_AUX_FUNCTION:
        return start_command_do_aux_function(cmd);
#endif
#if AP_GRIPPER_ENABLED
    case MAV_CMD_DO_GRIPPER:
        return start_command_do_gripper(cmd);
#endif
#if AP_SERVORELAYEVENTS_ENABLED
    case MAV_CMD_DO_SET_SERVO:
    case MAV_CMD_DO_SET_RELAY:
    case MAV_CMD_DO_REPEAT_SERVO:
    case MAV_CMD_DO_REPEAT_RELAY:
        return start_command_do_servorelayevents(cmd);
#endif
#if AP_CAMERA_ENABLED
    case MAV_CMD_DO_DIGICAM_CONFIGURE:
    case MAV_CMD_DO_DIGICAM_CONTROL:
    case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
    case MAV_CMD_IMAGE_START_CAPTURE:
    case MAV_CMD_IMAGE_STOP_CAPTURE:
    case MAV_CMD_SET_CAMERA_ZOOM:
    case MAV_CMD_SET_CAMERA_FOCUS:
    case MAV_CMD_SET_CAMERA_SOURCE:
    case MAV_CMD_VIDEO_START_CAPTURE:
    case MAV_CMD_VIDEO_STOP_CAPTURE:
        return start_command_camera(cmd);
#endif
#if AP_FENCE_ENABLED
    case MAV_CMD_DO_FENCE_ENABLE:
        return start_command_fence(cmd);
#endif
    case MAV_CMD_DO_PARACHUTE:
        return start_command_parachute(cmd);
    case MAV_CMD_DO_SEND_SCRIPT_MESSAGE:
        return start_command_do_scripting(cmd);
    case MAV_CMD_DO_SPRAYER:
        return start_command_do_sprayer(cmd);
    case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
        return command_do_set_repeat_dist(cmd);
    case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
        return start_command_do_gimbal_manager_pitchyaw(cmd);
    case MAV_CMD_JUMP_TAG:
        _jump_tag.tag = cmd.content.jump.target;
        _jump_tag.age = 1;
        FALLTHROUGH; // fall through in case the vehicle handles tag events
    default:
        return _cmd_start_fn(cmd);
    }
}

///
/// public command methods
///

/// add_cmd - adds a command to the end of the command list and writes to storage
///     returns true if successfully added, false on failure
///     cmd.index is updated with it's new position in the mission
bool AP_Mission::add_cmd(Mission_Command& cmd)
{
    // attempt to write the command to storage
    bool ret = write_cmd_to_storage(_cmd_total, cmd);

    if (ret) {
        // update command's index
        cmd.index = _cmd_total;
        // increment total number of commands
        _cmd_total.set_and_save(_cmd_total + 1);
    }

    return ret;
}

/// replace_cmd - replaces the command at position 'index' in the command list with the provided cmd
///     replacing the current active command will have no effect until the command is restarted
///     returns true if successfully replaced, false on failure
bool AP_Mission::replace_cmd(uint16_t index, const Mission_Command& cmd)
{
    // sanity check index
    if (index >= (unsigned)_cmd_total) {
        return false;
    }

    // attempt to write the command to storage
    return write_cmd_to_storage(index, cmd);
}

/// is_nav_cmd - returns true if the command's id is a "navigation" command, false if "do" or "conditional" command
bool AP_Mission::is_nav_cmd(const Mission_Command& cmd)
{
    // NAV commands all have ids below MAV_CMD_NAV_LAST, plus some exceptions
    return (cmd.id <= MAV_CMD_NAV_LAST ||
            cmd.id == MAV_CMD_NAV_SET_YAW_SPEED ||
            cmd.id == MAV_CMD_NAV_SCRIPT_TIME ||
            cmd.id == MAV_CMD_NAV_ATTITUDE_TIME);
}

/// get_next_nav_cmd - gets next "navigation" command found at or after start_index
///     returns true if found, false if not found (i.e. reached end of mission command list)
///     accounts for do_jump commands but never increments the jump's num_times_run (advance_current_nav_cmd is responsible for this)
bool AP_Mission::get_next_nav_cmd(uint16_t start_index, Mission_Command& cmd)
{
    // search until the end of the mission command list
    for (uint16_t cmd_index = start_index; cmd_index < (unsigned)_cmd_total; cmd_index++) {
        // get next command
        if (!get_next_cmd(cmd_index, cmd, false)) {
            // no more commands so return failure
            return false;
        }
        // if found a "navigation" command then return it
        if (is_nav_cmd(cmd)) {
            return true;
        }
    }

    // if we got this far we did not find a navigation command
    return false;
}

/// get the ground course of the next navigation leg in centidegrees
/// from 0 36000. Return default_angle if next navigation
/// leg cannot be determined
int32_t AP_Mission::get_next_ground_course_cd(int32_t default_angle)
{
    Mission_Command cmd;
    if (!get_next_nav_cmd(_nav_cmd.index+1, cmd)) {
        return default_angle;
    }
    // special handling for nav commands with no target location
    switch (cmd.id) {
    case MAV_CMD_NAV_GUIDED_ENABLE:
    case MAV_CMD_NAV_DELAY:
        return default_angle;

    case MAV_CMD_NAV_SET_YAW_SPEED:
        return (_nav_cmd.content.set_yaw_speed.angle_deg * 100);

    default:
        break;
    }

    return _nav_cmd.content.location.get_bearing_to(cmd.content.location);
}

// set_current_cmd - jumps to command specified by index
bool AP_Mission::set_current_cmd(uint16_t index)
{
    // Clear flags
    _flags.in_landing_sequence = false;
    _flags.in_return_path = false;

    // read command to check for DO_LAND_START and DO_RETURN_PATH_START
    Mission_Command cmd;
    if (read_cmd_from_storage(index, cmd)) {
        switch (cmd.id) {
        case MAV_CMD_DO_LAND_START:
            _flags.in_landing_sequence = true;
            break;

        case MAV_CMD_DO_RETURN_PATH_START:
            _flags.in_return_path = true;
            break;

        default:
            break;

        }
    }

    // mission command has been set, don't track history.
    reset_wp_history();

    // sanity check index and that we have a mission
    if (index >= (unsigned)_cmd_total || _cmd_total == 1) {
        return false;
    }

    // stop the current running do command
    _do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
    _flags.do_cmd_loaded = false;
    _flags.do_cmd_all_done = false;

    // stop current nav cmd
    _flags.nav_cmd_loaded = false;

    // if index is zero then the user wants to completely restart the mission
    if (index == 0 || _flags.state == MISSION_COMPLETE) {
        _prev_nav_cmd_id    = AP_MISSION_CMD_ID_NONE;
        _prev_nav_cmd_index = AP_MISSION_CMD_INDEX_NONE;
        _prev_nav_cmd_wp_index = AP_MISSION_CMD_INDEX_NONE;
        // reset the jump tracking to zero
        init_jump_tracking();
        if (index == 0) {
            index = 1;
        }
    }

    // if the mission is stopped or completed move the nav_cmd index to the specified point and set the state to stopped
    // so that if the user resumes the mission it will begin at the specified index
    if (_flags.state != MISSION_RUNNING) {
        // search until we find next nav command or reach end of command list
        while (!_flags.nav_cmd_loaded) {
            // get next command
            if (!get_next_cmd(index, cmd, true)) {
                _nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
                return false;
            }

            // check if navigation or "do" command
            if (is_nav_cmd(cmd)) {
                // set current navigation command
                _nav_cmd = cmd;
                _flags.nav_cmd_loaded = true;
            } else {
                // set current do command
                if (!_flags.do_cmd_loaded) {
                    _do_cmd = cmd;
                    _flags.do_cmd_loaded = true;
                }
            }
            // move onto next command
            index = cmd.index+1;
        }

        // if we have not found a do command then set flag to show there are no do-commands to be run before nav command completes
        if (!_flags.do_cmd_loaded) {
            _flags.do_cmd_all_done = true;
        }

        // if we got this far then the mission can safely be "resumed" from the specified index so we set the state to "stopped"
        _flags.state = MISSION_STOPPED;
        return true;
    }

    // the state must be MISSION_RUNNING, allow advance_current_nav_cmd() to manage starting the item
    if (!advance_current_nav_cmd(index)) {
        // on failure set mission complete
        complete();
        return false;
    }

    // if we got this far we must have successfully advanced the nav command
    return true;
}

// restart current navigation command.  Used to handle external changes to mission
// returns true on success, false if mission is not running or current nav command is invalid
bool AP_Mission::restart_current_nav_cmd()
{
    // return immediately if mission is not running
    if (_flags.state != MISSION_RUNNING) {
        return false;
    }

    // return immediately if nav command index is invalid
    const uint16_t nav_cmd_index = get_current_nav_index();
    if ((nav_cmd_index == 0) || (nav_cmd_index >= num_commands())) {
        return false;
    }

    return set_current_cmd(_nav_cmd.index);
}

// returns false on any issue at all.
bool AP_Mission::set_item(uint16_t index, mavlink_mission_item_int_t& src_packet)
{
    // this is the on-storage format
    AP_Mission::Mission_Command cmd {};

    // can't handle request for anything bigger than the mission size+1...
    if (index > num_commands()) {
        return false;
    }

    // convert from mavlink-ish format to storage format, if we can.
    if (mavlink_int_to_mission_cmd(src_packet, cmd) != MAV_MISSION_ACCEPTED) {
        return false;
    }

    // A request to set the 'next' item after the end is how we add an extra
    //  item to the list, thus allowing us to write entire missions if needed.
    if (index == num_commands()) {
        return add_cmd(cmd);
    }

    // replacing an existing mission item...
    return AP_Mission::replace_cmd(index, cmd);
}

bool AP_Mission::get_item(uint16_t index, mavlink_mission_item_int_t& ret_packet) const
{
    mavlink_mission_item_int_t tmp;

    // can't handle request for anything bigger than the mission size...
    if (index >= num_commands()) {
        return false;
    }

    // minimal placeholder values during read-from-storage
    tmp.target_system = 1;     // unused sysid
    tmp.target_component =  1; // unused compid

    // 0=home, higher number/s = mission item number.
    tmp.seq = index;

    // retrieve mission from eeprom
    AP_Mission::Mission_Command cmd {};
    if (!read_cmd_from_storage(tmp.seq, cmd)) {
        return false;
    }
    // convert into mavlink-ish format for lua and friends.
    if (!mission_cmd_to_mavlink_int(cmd, tmp)) {
        return false;
    }

    // set packet's current field to 1 if this is the command being executed
    if (cmd.id == (uint16_t)get_current_nav_cmd().index) {
        tmp.current = 1;
    } else {
        tmp.current = 0;
    }

    // set auto continue to 1, becasue that's what's done elsewhere.
    tmp.autocontinue = 1;     // 1 (true), 0 (false)
    tmp.command = cmd.id;

    ret_packet = tmp;

    return true;
}


struct PACKED Packed_Location_Option_Flags {
    uint8_t relative_alt : 1;           // 1 if altitude is relative to home
    uint8_t unused1      : 1;           // unused flag (defined so that loiter_ccw uses the correct bit)
    uint8_t loiter_ccw   : 1;           // 0 if clockwise, 1 if counter clockwise
    uint8_t terrain_alt  : 1;           // this altitude is above terrain
    uint8_t origin_alt   : 1;           // this altitude is above ekf origin
    uint8_t loiter_xtrack : 1;          // 0 to crosstrack from center of waypoint, 1 to crosstrack from tangent exit location
    uint8_t type_specific_bit_0 : 1;    // each mission item type can use this for storing 1 bit of extra data
    uint8_t type_specific_bit_1 : 1;    // each mission item type can use this for storing 1 bit of extra data
};

struct PACKED PackedLocation {
    union {
        Packed_Location_Option_Flags flags;                    ///< options bitmask (1<<0 = relative altitude)
        uint8_t options;                                /// allows writing all flags to eeprom as one byte
    };
    // by making alt 24 bit we can make p1 in a command 16 bit,
    // allowing an accurate angle in centi-degrees. This keeps the
    // storage cost per mission item at 15 bytes, and allows mission
    // altitudes of up to +/- 83km
    int32_t alt:24;                                     ///< param 2 - Altitude in centimeters (meters * 100) see LOCATION_ALT_MAX_M
    int32_t lat;                                        ///< param 3 - Latitude * 10**7
    int32_t lng;                                        ///< param 4 - Longitude * 10**7
};

union PackedContent {
    // location
    PackedLocation location;      // Waypoint location

    // raw bytes, for reading/writing to eeprom. Note that only 10
    // bytes are available if a 16 bit command ID is used
    uint8_t bytes[12];

};

/// load_cmd_from_storage - load command from storage
///     true is return if successful
bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) const
{
    ASSERT_STORAGE_SIZE(PackedContent, 12);

    WITH_SEMAPHORE(_rsem);

    // special handling for command #0 which is home
    if (index == 0) {
        cmd = {};
        cmd.id = MAV_CMD_NAV_WAYPOINT;
        cmd.content.location = AP::ahrs().get_home();
        return true;
    }
    if (index >= (unsigned)_cmd_total || index >= _commands_max) {
        return false;
    }

    // ensure all bytes of cmd are zeroed
    cmd = {};

    // Find out proper location in memory by using the start_byte position + the index
    // we can load a command, we don't process it yet
    // read WP position
    const uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE);

    PackedContent packed_content {};

    const uint8_t b1 = _storage.read_byte(pos_in_storage);
    if (b1 == 0 || b1 == 1) {
        cmd.id = _storage.read_uint16(pos_in_storage+1);
        cmd.p1 = _storage.read_uint16(pos_in_storage+3);
        _storage.read_block(packed_content.bytes, pos_in_storage+5, 10);
        format_conversion(b1, cmd, packed_content);
    } else {
        cmd.id = b1;
        cmd.p1 = _storage.read_uint16(pos_in_storage+1);
        _storage.read_block(packed_content.bytes, pos_in_storage+3, 12);
    }

    if (stored_in_location(cmd.id)) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        // NOTE!  no 16-bit command may be stored_in_location as only
        // 10 bytes are available for storage and lat/lon/alt required
        // 4*sizeof(float) == 12 bytes of storage.
        if (b1 == 0) {
            AP_HAL::panic("May not store location for 16-bit commands");
        }
#endif
        // Location is not PACKED; field-wise copy it:
        cmd.content.location.relative_alt = packed_content.location.flags.relative_alt;
        cmd.content.location.loiter_ccw = packed_content.location.flags.loiter_ccw;
        cmd.content.location.terrain_alt = packed_content.location.flags.terrain_alt;
        cmd.content.location.origin_alt = packed_content.location.flags.origin_alt;
        cmd.content.location.loiter_xtrack = packed_content.location.flags.loiter_xtrack;
        cmd.content.location.alt = packed_content.location.alt;
        cmd.content.location.lat = packed_content.location.lat;
        cmd.content.location.lng = packed_content.location.lng;

        if (packed_content.location.flags.type_specific_bit_0) {
            cmd.type_specific_bits |= 1U << 0;
        }
        if (packed_content.location.flags.type_specific_bit_1) {
            cmd.type_specific_bits |= 1U << 1;
        }
    } else {
        // all other options in Content are assumed to be packed:
        static_assert(sizeof(cmd.content) >= 12,
                      "content is big enough to take bytes");
        // (void *) cast to specify gcc that we know that we are copy byte into a non trivial type and leaving 4 bytes untouched
        memcpy((void *)&cmd.content, packed_content.bytes, 12);
    }

    // set command's index to it's position in eeprom
    cmd.index = index;

    // return success
    return true;
}

bool AP_Mission::stored_in_location(uint16_t id)
{
    switch (id) {
    case MAV_CMD_NAV_WAYPOINT:
    case MAV_CMD_NAV_LOITER_UNLIM:
    case MAV_CMD_NAV_LOITER_TURNS:
    case MAV_CMD_NAV_LOITER_TIME:
    case MAV_CMD_NAV_LAND:
    case MAV_CMD_NAV_TAKEOFF:
    case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
    case MAV_CMD_NAV_LOITER_TO_ALT:
    case MAV_CMD_NAV_SPLINE_WAYPOINT:
    case MAV_CMD_NAV_GUIDED_ENABLE:
    case MAV_CMD_DO_SET_HOME:
    case MAV_CMD_DO_RETURN_PATH_START:
    case MAV_CMD_DO_LAND_START:
    case MAV_CMD_DO_GO_AROUND:
    case MAV_CMD_DO_SET_ROI:
    case MAV_CMD_NAV_VTOL_TAKEOFF:
    case MAV_CMD_NAV_VTOL_LAND:
    case MAV_CMD_NAV_PAYLOAD_PLACE:
        return true;
    default:
        return false;
    }
}

/// write_cmd_to_storage - write a command to storage
///     index is used to calculate the storage location
///     true is returned if successful
bool AP_Mission::write_cmd_to_storage(uint16_t index, const Mission_Command& cmd)
{
    WITH_SEMAPHORE(_rsem);

    // range check cmd's index
    if (index >= num_commands_max()) {
        return false;
    }

    PackedContent packed {};
    if (stored_in_location(cmd.id)) {
        // Location is not PACKED; field-wise copy it:
        packed.location.flags.relative_alt = cmd.content.location.relative_alt;
        packed.location.flags.loiter_ccw = cmd.content.location.loiter_ccw;
        packed.location.flags.terrain_alt = cmd.content.location.terrain_alt;
        packed.location.flags.origin_alt = cmd.content.location.origin_alt;
        packed.location.flags.loiter_xtrack = cmd.content.location.loiter_xtrack;
        packed.location.flags.type_specific_bit_0 = (cmd.type_specific_bits & (1U<<0)) >> 0;
        packed.location.flags.type_specific_bit_1 = (cmd.type_specific_bits & (1U<<1)) >> 1;
        packed.location.alt = cmd.content.location.alt;
        packed.location.lat = cmd.content.location.lat;
        packed.location.lng = cmd.content.location.lng;
    } else {
        // all other options in Content are assumed to be packed:
        static_assert(sizeof(packed.bytes) >= 12,
                      "packed.bytes is big enough to take content");
        memcpy(packed.bytes, &cmd.content, 12);
    }

    // calculate where in storage the command should be placed
    uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE);

    if (cmd.id < 256) {
        // for commands below 256 we store up to 12 bytes
        _storage.write_byte(pos_in_storage, cmd.id);
        _storage.write_uint16(pos_in_storage+1, cmd.p1);
        _storage.write_block(pos_in_storage+3, packed.bytes, 12);
    } else {
        // if the command ID is above 256 we store a tag byte followed
        // by the 16 bit command ID. The tag byte is 1 for commands
        // where we have changed the storage format (see
        // format_conversion), 0 otherwise
        uint8_t tag_byte = 0;
        // currently the only converted structure is NAV_SCRIPT_TIME
        if (cmd.id == MAV_CMD_NAV_SCRIPT_TIME) {
            tag_byte = 1;
        }
        _storage.write_byte(pos_in_storage, tag_byte);
        _storage.write_uint16(pos_in_storage+1, cmd.id);
        _storage.write_uint16(pos_in_storage+3, cmd.p1);
        _storage.write_block(pos_in_storage+5, packed.bytes, 10);
    }

    // remember when the mission last changed
    _last_change_time_ms = AP_HAL::millis();

    // return success
    return true;
}

/// write_home_to_storage - writes the special purpose cmd 0 (home) to storage
///     home is taken directly from ahrs
void AP_Mission::write_home_to_storage()
{
    Mission_Command home_cmd = {};
    home_cmd.id = MAV_CMD_NAV_WAYPOINT;
    home_cmd.content.location = AP::ahrs().get_home();
    write_cmd_to_storage(0,home_cmd);
}

MAV_MISSION_RESULT AP_Mission::sanity_check_params(const mavlink_mission_item_int_t& packet)
{
    uint8_t nan_mask;
    switch (packet.command) {
    case MAV_CMD_NAV_WAYPOINT:
        nan_mask = ~(1 << 3); // param 4 can be nan
        break;
    case MAV_CMD_NAV_LOITER_UNLIM:
        nan_mask = ~(1 << 3); // param 4 can be nan
        break;
    case MAV_CMD_NAV_LAND:
        nan_mask = ~(1 << 3); // param 4 can be nan
        break;
    case MAV_CMD_NAV_TAKEOFF:
        nan_mask = ~(1 << 3); // param 4 can be nan
        break;
    case MAV_CMD_NAV_VTOL_TAKEOFF:
        nan_mask = ~(1 << 3); // param 4 can be nan
        break;
    case MAV_CMD_NAV_VTOL_LAND:
        nan_mask = ~((1 << 2) | (1 << 3)); // param 3 and 4 can be nan
        break;
    default:
        nan_mask = 0xff;
        break;
    }

    if (((nan_mask & (1 << 0)) && isnan(packet.param1)) ||
        isinf(packet.param1)) {
        return MAV_MISSION_INVALID_PARAM1;
    }
    if (((nan_mask & (1 << 1)) && isnan(packet.param2)) ||
        isinf(packet.param2)) {
        return MAV_MISSION_INVALID_PARAM2;
    }
    if (((nan_mask & (1 << 2)) && isnan(packet.param3)) ||
        isinf(packet.param3)) {
        return MAV_MISSION_INVALID_PARAM3;
    }
    if (((nan_mask & (1 << 3)) && isnan(packet.param4)) ||
        isinf(packet.param4)) {
        return MAV_MISSION_INVALID_PARAM4;
    }
    return MAV_MISSION_ACCEPTED;
}

// mavlink_int_to_mission_cmd - converts mavlink message to an AP_Mission::Mission_Command object which can be stored to eeprom
//  return MAV_MISSION_ACCEPTED on success, MAV_MISSION_RESULT error on failure
MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_item_int_t& packet, AP_Mission::Mission_Command& cmd)
{
    cmd = {};

    // command's position in mission list and mavlink id
    cmd.index = packet.seq;
    cmd.id = packet.command;
    cmd.content.location = {};

    MAV_MISSION_RESULT param_check = sanity_check_params(packet);
    if (param_check != MAV_MISSION_ACCEPTED) {
        return param_check;
    }

    // command specific conversions from mavlink packet to mission command
    switch (cmd.id) {

    case 0 ... 1:
        // these are reserved for storing 16 bit command IDs
        return MAV_MISSION_INVALID;

    case MAV_CMD_NAV_WAYPOINT: {                        // MAV ID: 16
        /*
          the 15 byte limit means we can't fit both delay and radius
          in the cmd structure. When we expand the mission structure
          we can do this properly
         */
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
        // acceptance radius in meters and pass by distance in meters
        uint16_t acp = packet.param2;           // param 2 is acceptance radius in meters is held in low p1
        uint16_t passby = packet.param3;        // param 3 is pass by distance in meters is held in high p1

        // limit to 255 so it does not wrap during the shift or mask operation
        passby = MIN(0xFF,passby);
        acp = MIN(0xFF,acp);

        cmd.p1 = (passby << 8) | (acp & 0x00FF);
#else
        // delay at waypoint in seconds (this is for copters???)
        cmd.p1 = packet.param1;
#endif
    }
    break;

    case MAV_CMD_NAV_LOITER_UNLIM:                      // MAV ID: 17
        cmd.p1 = fabsf(packet.param3);                  // store radius as 16bit since no other params are competing for space
        cmd.content.location.loiter_ccw = (packet.param3 < 0);    // -1 = counter clockwise, +1 = clockwise
        break;

    case MAV_CMD_NAV_LOITER_TURNS: {                    // MAV ID: 18
        // number of turns is stored in the lowest bits. Number of
        // turns 0 < N < 1 are stored multiplied by 256 and a bit set
        // in storage so that on retrieval they are divided by 256.
        // Radii below 255m are stored in the top 8 bits as an 8-bit integer.
        // Radii above 255m are stored divided by 10 and a bit set in
        // storage so that on retrieval they are multiplied by 10
        float param1_stored = packet.param1;
        if (param1_stored > 0 && param1_stored < 1) {
            param1_stored *= 256.0;
            cmd.type_specific_bits |= (1U << 1);
        }
        cmd.p1 = MIN(255, param1_stored); // store number of times to circle in low p1
        uint8_t radius_m;
        const float abs_radius = fabsf(packet.param3);
        if (abs_radius <= 255) {
            radius_m = abs_radius;
        } else {
            radius_m = MIN(255, abs_radius * 0.1);
            cmd.type_specific_bits |= (1U << 0);
        }
        cmd.p1 |= (radius_m<<8);   // store radius in high byte of p1
        cmd.content.location.loiter_ccw = (packet.param3 < 0);
        cmd.content.location.loiter_xtrack = (packet.param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
    }
    break;

    case MAV_CMD_NAV_LOITER_TIME:                       // MAV ID: 19
        cmd.p1 = packet.param1;                         // loiter time in seconds uses all 16 bits, 8bit seconds is too small. No room for radius.
        cmd.content.location.loiter_ccw = (packet.param3 < 0);
        cmd.content.location.loiter_xtrack = (packet.param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
        break;

    case MAV_CMD_NAV_RETURN_TO_LAUNCH:                  // MAV ID: 20
        break;

    case MAV_CMD_NAV_LAND:                              // MAV ID: 21
        cmd.p1 = packet.param1;                         // abort target altitude(m)  (plane only)
        if (!isnan(packet.param4)) {
            cmd.content.location.loiter_ccw = is_negative(packet.param4); // yaw direction, (plane deepstall only)
        }
        break;

    case MAV_CMD_NAV_TAKEOFF:                           // MAV ID: 22
        cmd.p1 = packet.param1;                         // minimum pitch (plane only)
        break;

    case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:           // MAV ID: 30
        cmd.p1 = packet.param1;                         // Climb/Descend
        // 0 = Neutral, cmd complete at +/- 5 of indicated alt.
        // 1 = Climb, cmd complete at or above indicated alt.
        // 2 = Descend, cmd complete at or below indicated alt.
        break;

    case MAV_CMD_NAV_LOITER_TO_ALT:                     // MAV ID: 31
        cmd.p1 = fabsf(packet.param2);                  // param2 is radius in meters
        cmd.content.location.loiter_ccw = (packet.param2 < 0);
        cmd.content.location.loiter_xtrack = (packet.param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
        break;

    case MAV_CMD_NAV_SPLINE_WAYPOINT:                   // MAV ID: 82
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
        return MAV_MISSION_UNSUPPORTED;
#else
        cmd.p1 = packet.param1;                         // delay at waypoint in seconds
        break;
#endif

    case MAV_CMD_NAV_GUIDED_ENABLE:                     // MAV ID: 92
        cmd.p1 = packet.param1;                         // on/off. >0.5 means "on", hand-over control to external controller
        break;

    case MAV_CMD_NAV_DELAY:                            // MAV ID: 93
        cmd.content.nav_delay.seconds = packet.param1; // delay in seconds
        cmd.content.nav_delay.hour_utc = packet.param2;// absolute time's hour (utc)
        cmd.content.nav_delay.min_utc = packet.param3;// absolute time's min (utc)
        cmd.content.nav_delay.sec_utc = packet.param4; // absolute time's second (utc)
        break;

    case MAV_CMD_CONDITION_DELAY:                       // MAV ID: 112
        cmd.content.delay.seconds = packet.param1;      // delay in seconds
        break;

    case MAV_CMD_CONDITION_DISTANCE:                    // MAV ID: 114
        cmd.content.distance.meters = packet.param1;    // distance in meters from next waypoint
        break;

    case MAV_CMD_CONDITION_YAW:                         // MAV ID: 115
        cmd.content.yaw.angle_deg = packet.param1;      // target angle in degrees
        cmd.content.yaw.turn_rate_dps = packet.param2;  // 0 = use default turn rate otherwise specific turn rate in deg/sec
        cmd.content.yaw.direction = packet.param3;      // -1 = ccw, +1 = cw
        cmd.content.yaw.relative_angle = packet.param4; // lng=0: absolute angle provided, lng=1: relative angle provided
        break;

    case MAV_CMD_DO_JUMP:                               // MAV ID: 177
    case MAV_CMD_DO_JUMP_TAG:                           // MAV ID: 601
        cmd.content.jump.target = packet.param1;        // jump-to command/tag number
        cmd.content.jump.num_times = packet.param2;     // repeat count
        break;

    case MAV_CMD_JUMP_TAG:                              // MAV ID: 600
        cmd.content.jump.target = packet.param1;        // jump-to tag number
        break;

    case MAV_CMD_DO_CHANGE_SPEED:                       // MAV ID: 178
        cmd.content.speed.speed_type = packet.param1;   // 0 = airspeed, 1 = ground speed
        cmd.content.speed.target_ms = packet.param2;    // target speed in m/s
        cmd.content.speed.throttle_pct = packet.param3; // throttle as a percentage from 1 ~ 100%
        break;

    case MAV_CMD_DO_SET_HOME:
        cmd.p1 = packet.param1;                         // p1=0 means use current location, p=1 means use provided location
        break;

    case MAV_CMD_DO_SET_RELAY:                          // MAV ID: 181
        cmd.content.relay.num = packet.param1;          // relay number
        cmd.content.relay.state = packet.param2;        // 0:off, 1:on
        break;

    case MAV_CMD_DO_REPEAT_RELAY:                       // MAV ID: 182
        cmd.content.repeat_relay.num = packet.param1;           // relay number
        cmd.content.repeat_relay.repeat_count = packet.param2;  // count
        cmd.content.repeat_relay.cycle_time = packet.param3;    // time converted from seconds to milliseconds
        break;

    case MAV_CMD_DO_SET_SERVO:                          // MAV ID: 183
        cmd.content.servo.channel = packet.param1;      // channel
        cmd.content.servo.pwm = packet.param2;          // PWM
        break;

    case MAV_CMD_DO_REPEAT_SERVO:                       // MAV ID: 184
        cmd.content.repeat_servo.channel = packet.param1;      // channel
        cmd.content.repeat_servo.pwm = packet.param2;          // PWM
        cmd.content.repeat_servo.repeat_count = packet.param3; // count
        cmd.content.repeat_servo.cycle_time = packet.param4;   // time in seconds
        break;

    case MAV_CMD_DO_RETURN_PATH_START:                  // MAV ID: 188
    case MAV_CMD_DO_LAND_START:                         // MAV ID: 189
        break;

    case MAV_CMD_DO_GO_AROUND:                          // MAV ID: 191
        break;

    case MAV_CMD_DO_SET_ROI:                            // MAV ID: 201
        cmd.p1 = packet.param1;                         // 0 = no roi, 1 = next waypoint, 2 = waypoint number, 3 = fixed location, 4 = given target (not supported)
        break;

    case MAV_CMD_DO_DIGICAM_CONFIGURE:                  // MAV ID: 202
        cmd.content.digicam_configure.shooting_mode = packet.param1;
        cmd.content.digicam_configure.shutter_speed = packet.param2;
        cmd.content.digicam_configure.aperture = packet.param3;
        cmd.content.digicam_configure.ISO = packet.param4;
        cmd.content.digicam_configure.exposure_type = packet.x;
        cmd.content.digicam_configure.cmd_id = packet.y;
        cmd.content.digicam_configure.engine_cutoff_time = packet.z;
        break;

    case MAV_CMD_DO_DIGICAM_CONTROL:                    // MAV ID: 203
        cmd.content.digicam_control.session = packet.param1;
        cmd.content.digicam_control.zoom_pos = packet.param2;
        cmd.content.digicam_control.zoom_step = packet.param3;
        cmd.content.digicam_control.focus_lock = packet.param4;
        cmd.content.digicam_control.shooting_cmd = packet.x;
        cmd.content.digicam_control.cmd_id = packet.y;
        break;

    case MAV_CMD_DO_MOUNT_CONTROL:                      // MAV ID: 205
        // TODO: this is only valid if packet.z == MAV_MOUNT_MODE_MAVLINK_TARGETING
        cmd.content.mount_control.pitch = packet.param1;
        cmd.content.mount_control.roll = packet.param2;
        cmd.content.mount_control.yaw = packet.param3;
        break;

    case MAV_CMD_DO_SET_CAM_TRIGG_DIST:                 // MAV ID: 206
        cmd.content.cam_trigg_dist.meters = packet.param1;  // distance between camera shots in meters
        cmd.content.cam_trigg_dist.trigger = packet.param3; // when enabled, camera triggers once immediately
        break;

    case MAV_CMD_DO_FENCE_ENABLE:                       // MAV ID: 207
        cmd.p1 = packet.param1;                         // action 0=disable, 1=enable, 2=disable floor
        break;

    case MAV_CMD_DO_AUX_FUNCTION:
        cmd.content.auxfunction.function = packet.param1;
        cmd.content.auxfunction.switchpos = packet.param2;
        break;

    case MAV_CMD_DO_PARACHUTE:                         // MAV ID: 208
        cmd.p1 = packet.param1;                        // action 0=disable, 1=enable, 2=release.  See PARACHUTE_ACTION enum
        break;

    case MAV_CMD_DO_INVERTED_FLIGHT:                    // MAV ID: 210
        cmd.p1 = packet.param1;                         // normal=0 inverted=1
        break;

#if AP_GRIPPER_ENABLED
    case MAV_CMD_DO_GRIPPER:                            // MAV ID: 211
        cmd.content.gripper.num = packet.param1;        // gripper number
        cmd.content.gripper.action = packet.param2;     // action 0=release, 1=grab.  See GRIPPER_ACTION enum
        break;
#endif

    case MAV_CMD_DO_GUIDED_LIMITS:                      // MAV ID: 222
        cmd.p1 = packet.param1;                         // max time in seconds the external controller will be allowed to control the vehicle
        cmd.content.guided_limits.alt_min = packet.param2;  // min alt below which the command will be aborted.  0 for no lower alt limit
        cmd.content.guided_limits.alt_max = packet.param3;  // max alt above which the command will be aborted.  0 for no upper alt limit
        cmd.content.guided_limits.horiz_max = packet.param4;// max horizontal distance the vehicle can move before the command will be aborted.  0 for no horizontal limit
        break;

    case MAV_CMD_DO_AUTOTUNE_ENABLE:                    // MAV ID: 211
        cmd.p1 = packet.param1;                         // disable=0 enable=1
        break;

    case MAV_CMD_NAV_ALTITUDE_WAIT:                     // MAV ID: 83
        cmd.content.altitude_wait.altitude = packet.param1;
        cmd.content.altitude_wait.descent_rate = packet.param2;
        cmd.content.altitude_wait.wiggle_time = packet.param3;
        break;

    case MAV_CMD_NAV_VTOL_TAKEOFF:
        break;

    case MAV_CMD_NAV_VTOL_LAND:
        cmd.p1 = (NAV_VTOL_LAND_OPTIONS)packet.param1;
        break;

    case MAV_CMD_DO_VTOL_TRANSITION:
        cmd.content.do_vtol_transition.target_state = packet.param1;
        break;

    case MAV_CMD_DO_SET_REVERSE:
        cmd.p1 = packet.param1; // 0 = forward, 1 = reverse
        break;

    case MAV_CMD_DO_ENGINE_CONTROL:
        cmd.content.do_engine_control.start_control = (packet.param1>0);
        cmd.content.do_engine_control.cold_start = (packet.param2>0);
        cmd.content.do_engine_control.height_delay_cm = packet.param3*100;
        cmd.content.do_engine_control.allow_disarmed_start = (((uint32_t)packet.param4) & ENGINE_CONTROL_OPTIONS_ALLOW_START_WHILE_DISARMED) != 0;
        break;

#if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED
    case MAV_CMD_NAV_PAYLOAD_PLACE:
        cmd.p1 = packet.param1*100; // copy max-descend parameter (m->cm)
        break;
#endif

    case MAV_CMD_NAV_SET_YAW_SPEED:
        cmd.content.set_yaw_speed.angle_deg = packet.param1;        // target angle in degrees
        cmd.content.set_yaw_speed.speed = packet.param2;            // speed in meters/second
        cmd.content.set_yaw_speed.relative_angle = packet.param3;   // 0 = absolute angle, 1 = relative angle
        break;

    case MAV_CMD_DO_WINCH:                              // MAV ID: 42600
        cmd.content.winch.num = packet.param1;          // winch number
        cmd.content.winch.action = packet.param2;       // action (0 = relax, 1 = length control, 2 = rate control).  See WINCH_ACTION enum
        cmd.content.winch.release_length = packet.param3;   // cable distance to unwind in meters, negative numbers to wind in cable
        cmd.content.winch.release_rate = packet.param4; // release rate in meters/second
        break;

    case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
        cmd.p1 = packet.param1; // Resume repeat distance (m)
        break;

    case MAV_CMD_DO_SPRAYER:
        cmd.p1 = packet.param1;                        // action 0=disable, 1=enable
        break;

    case MAV_CMD_DO_SEND_SCRIPT_MESSAGE:
        cmd.p1 = packet.param1;
        cmd.content.scripting.p1 = packet.param2;
        cmd.content.scripting.p2 = packet.param3;
        cmd.content.scripting.p3 = packet.param4;
        break;

#if AP_SCRIPTING_ENABLED
    case MAV_CMD_NAV_SCRIPT_TIME:
        cmd.content.nav_script_time.command = packet.param1;
        cmd.content.nav_script_time.timeout_s = packet.param2;
        cmd.content.nav_script_time.arg1.set(packet.param3);
        cmd.content.nav_script_time.arg2.set(packet.param4);
        cmd.content.nav_script_time.arg3 = int16_t(packet.x);
        cmd.content.nav_script_time.arg4 = int16_t(packet.y);
        break;
#endif

    case MAV_CMD_NAV_ATTITUDE_TIME:
        cmd.content.nav_attitude_time.time_sec = constrain_float(packet.param1, 0, UINT16_MAX);
        cmd.content.nav_attitude_time.roll_deg = (fabsf(packet.param2) <= 180) ? packet.param2 : 0;
        cmd.content.nav_attitude_time.pitch_deg = (fabsf(packet.param3) <= 90) ? packet.param3 : 0;
        cmd.content.nav_attitude_time.yaw_deg = ((packet.param4 >= -180) && (packet.param4 <= 360)) ? packet.param4 : 0;
        cmd.content.nav_attitude_time.climb_rate = packet.x;
        break;

    case MAV_CMD_DO_PAUSE_CONTINUE:
        cmd.p1 = packet.param1;
        break;

    case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
        cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg = packet.param1;
        cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg = packet.param2;
        cmd.content.gimbal_manager_pitchyaw.pitch_rate_degs = packet.param3;
        cmd.content.gimbal_manager_pitchyaw.yaw_rate_degs = packet.param4;
        cmd.content.gimbal_manager_pitchyaw.flags = packet.x;
        cmd.content.gimbal_manager_pitchyaw.gimbal_id = packet.z;
        break;

    case MAV_CMD_IMAGE_START_CAPTURE:
        cmd.content.image_start_capture.instance = packet.param1;
        cmd.content.image_start_capture.interval_s = packet.param2;
        cmd.content.image_start_capture.total_num_images = packet.param3;
        cmd.content.image_start_capture.start_seq_number = packet.param4;
        break;

    case MAV_CMD_IMAGE_STOP_CAPTURE:
        cmd.p1 = packet.param1;
        break;

    case MAV_CMD_SET_CAMERA_ZOOM:
        cmd.content.set_camera_zoom.zoom_type = packet.param1;
        cmd.content.set_camera_zoom.zoom_value = packet.param2;
        break;

    case MAV_CMD_SET_CAMERA_FOCUS:
        cmd.content.set_camera_focus.focus_type = packet.param1;
        cmd.content.set_camera_focus.focus_value = packet.param2;
        break;

    case MAV_CMD_SET_CAMERA_SOURCE:
        cmd.content.set_camera_source.instance = packet.param1;
        cmd.content.set_camera_source.primary_source = packet.param2;
        cmd.content.set_camera_source.secondary_source = packet.param3;
        break;

    case MAV_CMD_VIDEO_START_CAPTURE:
        cmd.content.video_start_capture.video_stream_id = packet.param1;
        break;

    case MAV_CMD_VIDEO_STOP_CAPTURE:
        cmd.content.video_stop_capture.video_stream_id = packet.param1;
        break;

    default:
        // unrecognised command
        return MAV_MISSION_UNSUPPORTED;
    }

    // copy location from mavlink to command
    if (stored_in_location(cmd.id)) {

        // sanity check location
        if (!check_lat(packet.x)) {
            return MAV_MISSION_INVALID_PARAM5_X;
        }
        if (!check_lng(packet.y)) {
            return MAV_MISSION_INVALID_PARAM6_Y;
        }
        if (isnan(packet.z) || fabsf(packet.z) >= LOCATION_ALT_MAX_M) {
            return MAV_MISSION_INVALID_PARAM7;
        }

        cmd.content.location.lat = packet.x;
        cmd.content.location.lng = packet.y;

        cmd.content.location.alt = packet.z * 100.0f;       // convert packet's alt (m) to cmd alt (cm)

        switch (packet.frame) {

        case MAV_FRAME_MISSION:
        case MAV_FRAME_GLOBAL:
        case MAV_FRAME_GLOBAL_INT:
            cmd.content.location.relative_alt = 0;
            break;

        case MAV_FRAME_GLOBAL_RELATIVE_ALT:
        case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
            cmd.content.location.relative_alt = 1;
            break;

#if AP_TERRAIN_AVAILABLE
        case MAV_FRAME_GLOBAL_TERRAIN_ALT:
        case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
            // we mark it as a relative altitude, as it doesn't have
            // home alt added
            cmd.content.location.relative_alt = 1;
            // mark altitude as above terrain, not above home
            cmd.content.location.terrain_alt = 1;
            break;
#endif

        default:
            return MAV_MISSION_UNSUPPORTED_FRAME;
        }
    }

    // if we got this far then it must have been successful
    return MAV_MISSION_ACCEPTED;
}

MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_to_MISSION_ITEM_INT(const mavlink_mission_item_t &packet,
        mavlink_mission_item_int_t &mav_cmd)
{
    // TODO: rename mav_cmd to mission_item_int
    // TODO: rename packet to mission_item
    mav_cmd.param1 = packet.param1;
    mav_cmd.param2 = packet.param2;
    mav_cmd.param3 = packet.param3;
    mav_cmd.param4 = packet.param4;
    mav_cmd.z = packet.z;
    mav_cmd.seq = packet.seq;
    mav_cmd.command = packet.command;
    mav_cmd.target_system = packet.target_system;
    mav_cmd.target_component = packet.target_component;
    mav_cmd.frame = packet.frame;
    mav_cmd.current = packet.current;
    mav_cmd.autocontinue = packet.autocontinue;
    mav_cmd.mission_type = packet.mission_type;

    /*
      the strategy for handling both MISSION_ITEM and MISSION_ITEM_INT
      is to pass the lat/lng in MISSION_ITEM_INT straight through, and
      for MISSION_ITEM multiply by 1e7 here. We need an exception for
      any commands which use the x and y fields not as
      latitude/longitude.
     */
    if (!cmd_has_location(packet.command)) {
        mav_cmd.x = packet.x;
        mav_cmd.y = packet.y;

    } else {
         //these commands use x and y as lat/lon. We need to
        // multiply by 1e7 to convert to int32_t
        if (!check_lat(packet.x)) {
            return MAV_MISSION_INVALID_PARAM5_X;
        }
        if (!check_lng(packet.y)) {
            return MAV_MISSION_INVALID_PARAM6_Y;
        }
        mav_cmd.x = packet.x * 1.0e7f;
        mav_cmd.y = packet.y * 1.0e7f;
    }

    return MAV_MISSION_ACCEPTED;
}

MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_INT_to_MISSION_ITEM(const mavlink_mission_item_int_t &item_int,
        mavlink_mission_item_t &item)
{
    item.param1 = item_int.param1;
    item.param2 = item_int.param2;
    item.param3 = item_int.param3;
    item.param4 = item_int.param4;
    item.z = item_int.z;
    item.seq = item_int.seq;
    item.command = item_int.command;
    item.target_system = item_int.target_system;
    item.target_component = item_int.target_component;
    item.frame = item_int.frame;
    item.current = item_int.current;
    item.autocontinue = item_int.autocontinue;
    item.mission_type = item_int.mission_type;

    if (!cmd_has_location(item_int.command)) {
        item.x = item_int.x;
        item.y = item_int.y;

    } else {
        // These commands use x and y as lat/lon. We need to
        // multiply by 1e-7 to convert to float
        item.x = item_int.x * 1.0e-7f;
        item.y = item_int.y * 1.0e-7f;
        if (!check_lat(item.x)) {
            return MAV_MISSION_INVALID_PARAM5_X;
        }
        if (!check_lng(item.y)) {
            return MAV_MISSION_INVALID_PARAM6_Y;
        }
    }

    return MAV_MISSION_ACCEPTED;
}

// mission_cmd_to_mavlink_int - converts an AP_Mission::Mission_Command object to a mavlink message which can be sent to the GCS
//  return true on success, false on failure
//  NOTE: callers to this method current fill parts of "packet" in before calling this method, so do NOT attempt to zero the entire packet in here
bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& cmd, mavlink_mission_item_int_t& packet)
{
    // command's position in mission list and mavlink id
    packet.seq = cmd.index;
    packet.command = cmd.id;

    // set defaults
    packet.current = 0;     // 1 if we are passing back the mission command that is currently being executed
    packet.param1 = 0;
    packet.param2 = 0;
    packet.param3 = 0;
    packet.param4 = 0;
    packet.frame = 0;
    packet.autocontinue = 1;

    // command specific conversions from mission command to mavlink packet
    switch (cmd.id) {
    case 0:
        // this is reserved for 16 bit command IDs
        return false;

    case MAV_CMD_NAV_WAYPOINT:                          // MAV ID: 16
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
        // acceptance radius in meters

        packet.param2 = LOWBYTE(cmd.p1);        // param 2 is acceptance radius in meters is held in low p1
        packet.param3 = HIGHBYTE(cmd.p1);       // param 3 is pass by distance in meters is held in high p1
#else
        // delay at waypoint in seconds
        packet.param1 = cmd.p1;
#endif
        break;

    case MAV_CMD_NAV_LOITER_UNLIM:                      // MAV ID: 17
        packet.param3 = (float)cmd.p1;
        if (cmd.content.location.loiter_ccw) {
            packet.param3 *= -1;
        }
        break;

    case MAV_CMD_NAV_LOITER_TURNS:                      // MAV ID: 18
        packet.param1 = LOWBYTE(cmd.p1);                // number of times to circle is held in low byte of p1
        packet.param3 = HIGHBYTE(cmd.p1);               // radius is held in high byte of p1
        if (cmd.content.location.loiter_ccw) {
            packet.param3 = -packet.param3;
        }
        if (cmd.type_specific_bits & (1U<<0)) {
            packet.param3 *= 10;
        }
        if (cmd.type_specific_bits & (1U<<1)) {
            packet.param1 /= 256.0;
        }
        packet.param4 = cmd.content.location.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
        break;

    case MAV_CMD_NAV_LOITER_TIME:                       // MAV ID: 19
        packet.param1 = cmd.p1;                         // loiter time in seconds
        if (cmd.content.location.loiter_ccw) {
            packet.param3 = -1;
        } else {
            packet.param3 = 1;
        }
        packet.param4 = cmd.content.location.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
        break;

    case MAV_CMD_NAV_RETURN_TO_LAUNCH:                  // MAV ID: 20
        break;

    case MAV_CMD_NAV_LAND:                              // MAV ID: 21
        packet.param1 = cmd.p1;                        // abort target altitude(m)  (plane only)
        packet.param4 = cmd.content.location.loiter_ccw ? -1 : 1; // yaw direction, (plane deepstall only)
        break;

    case MAV_CMD_NAV_TAKEOFF:                           // MAV ID: 22
        packet.param1 = cmd.p1;                         // minimum pitch (plane only)
        break;

    case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:           // MAV ID: 30
        packet.param1 = cmd.p1;                         // Climb/Descend
        // 0 = Neutral, cmd complete at +/- 5 of indicated alt.
        // 1 = Climb, cmd complete at or above indicated alt.
        // 2 = Descend, cmd complete at or below indicated alt.
        break;

    case MAV_CMD_NAV_LOITER_TO_ALT:                     // MAV ID: 31
        packet.param2 = cmd.p1;                        // loiter radius(m)
        if (cmd.content.location.loiter_ccw) {
            packet.param2 = -packet.param2;
        }
        packet.param4 = cmd.content.location.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
        break;

    case MAV_CMD_NAV_SPLINE_WAYPOINT:                   // MAV ID: 82
        packet.param1 = cmd.p1;                         // delay at waypoint in seconds
        break;

    case MAV_CMD_NAV_GUIDED_ENABLE:                     // MAV ID: 92
        packet.param1 = cmd.p1;                         // on/off. >0.5 means "on", hand-over control to external controller
        break;

    case MAV_CMD_NAV_DELAY:                            // MAV ID: 93
        packet.param1 = cmd.content.nav_delay.seconds; // delay in seconds
        packet.param2 = cmd.content.nav_delay.hour_utc; // absolute time's day of week (utc)
        packet.param3 = cmd.content.nav_delay.min_utc; // absolute time's hour (utc)
        packet.param4 = cmd.content.nav_delay.sec_utc; // absolute time's min (utc)
        break;

    case MAV_CMD_CONDITION_DELAY:                       // MAV ID: 112
        packet.param1 = cmd.content.delay.seconds;      // delay in seconds
        break;

    case MAV_CMD_CONDITION_DISTANCE:                    // MAV ID: 114
        packet.param1 = cmd.content.distance.meters;    // distance in meters from next waypoint
        break;

    case MAV_CMD_CONDITION_YAW:                         // MAV ID: 115
        packet.param1 = cmd.content.yaw.angle_deg;      // target angle in degrees
        packet.param2 = cmd.content.yaw.turn_rate_dps;  // 0 = use default turn rate otherwise specific turn rate in deg/sec
        packet.param3 = cmd.content.yaw.direction;      // -1 = ccw, +1 = cw
        packet.param4 = cmd.content.yaw.relative_angle; // 0 = absolute angle provided, 1 = relative angle provided
        break;

    case MAV_CMD_DO_JUMP:                               // MAV ID: 177
    case MAV_CMD_DO_JUMP_TAG:                           // MAV ID: 601
        packet.param1 = cmd.content.jump.target;        // jump-to command/tag number
        packet.param2 = cmd.content.jump.num_times;     // repeat count
        break;

    case MAV_CMD_JUMP_TAG:                              // MAV ID: 600
        packet.param1 = cmd.content.jump.target;        // jump-to tag number
        break;

    case MAV_CMD_DO_CHANGE_SPEED:                       // MAV ID: 178
        packet.param1 = cmd.content.speed.speed_type;   // 0 = airspeed, 1 = ground speed
        packet.param2 = cmd.content.speed.target_ms;    // speed in m/s
        packet.param3 = cmd.content.speed.throttle_pct; // throttle as a percentage from 1 ~ 100%
        break;

    case MAV_CMD_DO_SET_HOME:                           // MAV ID: 179
        packet.param1 = cmd.p1;                         // p1=0 means use current location, p=1 means use provided location
        break;

    case MAV_CMD_DO_SET_RELAY:                          // MAV ID: 181
        packet.param1 = cmd.content.relay.num;          // relay number
        packet.param2 = cmd.content.relay.state;        // 0:off, 1:on
        break;

    case MAV_CMD_DO_REPEAT_RELAY:                       // MAV ID: 182
        packet.param1 = cmd.content.repeat_relay.num;           // relay number
        packet.param2 = cmd.content.repeat_relay.repeat_count;  // count
        packet.param3 = cmd.content.repeat_relay.cycle_time;    // time in seconds
        break;

    case MAV_CMD_DO_SET_SERVO:                          // MAV ID: 183
        packet.param1 = cmd.content.servo.channel;      // channel
        packet.param2 = cmd.content.servo.pwm;          // PWM
        break;

    case MAV_CMD_DO_REPEAT_SERVO:                       // MAV ID: 184
        packet.param1 = cmd.content.repeat_servo.channel;       // channel
        packet.param2 = cmd.content.repeat_servo.pwm;           // PWM
        packet.param3 = cmd.content.repeat_servo.repeat_count;  // count
        packet.param4 = cmd.content.repeat_servo.cycle_time;    // time in milliseconds converted to seconds
        break;

    case MAV_CMD_DO_RETURN_PATH_START:                  // MAV ID: 188
    case MAV_CMD_DO_LAND_START:                         // MAV ID: 189
        break;

    case MAV_CMD_DO_GO_AROUND:                          // MAV ID: 191
        break;

    case MAV_CMD_DO_SET_ROI:                            // MAV ID: 201
        packet.param1 = cmd.p1;                         // 0 = no roi, 1 = next waypoint, 2 = waypoint number, 3 = fixed location, 4 = given target (not supported)
        break;

    case MAV_CMD_DO_DIGICAM_CONFIGURE:                  // MAV ID: 202
        packet.param1 = cmd.content.digicam_configure.shooting_mode;
        packet.param2 = cmd.content.digicam_configure.shutter_speed;
        packet.param3 = cmd.content.digicam_configure.aperture;
        packet.param4 = cmd.content.digicam_configure.ISO;
        packet.x = cmd.content.digicam_configure.exposure_type;
        packet.y = cmd.content.digicam_configure.cmd_id;
        packet.z = cmd.content.digicam_configure.engine_cutoff_time;
        break;

    case MAV_CMD_DO_DIGICAM_CONTROL:                    // MAV ID: 203
        packet.param1 = cmd.content.digicam_control.session;
        packet.param2 = cmd.content.digicam_control.zoom_pos;
        packet.param3 = cmd.content.digicam_control.zoom_step;
        packet.param4 = cmd.content.digicam_control.focus_lock;
        packet.x = cmd.content.digicam_control.shooting_cmd;
        packet.y = cmd.content.digicam_control.cmd_id;
        break;

    case MAV_CMD_DO_MOUNT_CONTROL:                      // MAV ID: 205
        packet.param1 = cmd.content.mount_control.pitch;
        packet.param2 = cmd.content.mount_control.roll;
        packet.param3 = cmd.content.mount_control.yaw;
        packet.z = MAV_MOUNT_MODE_MAVLINK_TARGETING;
        break;

    case MAV_CMD_DO_SET_CAM_TRIGG_DIST:                 // MAV ID: 206
        packet.param1 = cmd.content.cam_trigg_dist.meters;  // distance between camera shots in meters
        packet.param3 = cmd.content.cam_trigg_dist.trigger; // when enabled, camera triggers once immediately
        break;

    case MAV_CMD_DO_FENCE_ENABLE:                       // MAV ID: 207
        packet.param1 = cmd.p1;                         // action 0=disable, 1=enable, 2=disable floor, 3=enable except floor
        break;

    case MAV_CMD_DO_PARACHUTE:                          // MAV ID: 208
        packet.param1 = cmd.p1;                         // action 0=disable, 1=enable, 2=release.  See PARACHUTE_ACTION enum
        break;

    case MAV_CMD_DO_SPRAYER:
        packet.param1 = cmd.p1;                         // action 0=disable, 1=enable
        break;

    case MAV_CMD_DO_AUX_FUNCTION:
        packet.param1 = cmd.content.auxfunction.function;
        packet.param2 = cmd.content.auxfunction.switchpos;
        break;

    case MAV_CMD_DO_INVERTED_FLIGHT:                    // MAV ID: 210
        packet.param1 = cmd.p1;                         // normal=0 inverted=1
        break;

#if AP_GRIPPER_ENABLED
    case MAV_CMD_DO_GRIPPER:                            // MAV ID: 211
        packet.param1 = cmd.content.gripper.num;        // gripper number
        packet.param2 = cmd.content.gripper.action;     // action 0=release, 1=grab.  See GRIPPER_ACTION enum
        break;
#endif

    case MAV_CMD_DO_GUIDED_LIMITS:                      // MAV ID: 222
        packet.param1 = cmd.p1;                         // max time in seconds the external controller will be allowed to control the vehicle
        packet.param2 = cmd.content.guided_limits.alt_min;  // min alt below which the command will be aborted.  0 for no lower alt limit
        packet.param3 = cmd.content.guided_limits.alt_max;  // max alt above which the command will be aborted.  0 for no upper alt limit
        packet.param4 = cmd.content.guided_limits.horiz_max;// max horizontal distance the vehicle can move before the command will be aborted.  0 for no horizontal limit
        break;

    case MAV_CMD_DO_AUTOTUNE_ENABLE:
        packet.param1 = cmd.p1;                         // disable=0 enable=1
        break;

    case MAV_CMD_DO_SET_REVERSE:
        packet.param1 = cmd.p1;   // 0 = forward, 1 = reverse
        break;

    case MAV_CMD_NAV_ALTITUDE_WAIT:                     // MAV ID: 83
        packet.param1 = cmd.content.altitude_wait.altitude;
        packet.param2 = cmd.content.altitude_wait.descent_rate;
        packet.param3 = cmd.content.altitude_wait.wiggle_time;
        break;

    case MAV_CMD_NAV_VTOL_TAKEOFF:
        break;

    case MAV_CMD_NAV_VTOL_LAND:
        packet.param1 = cmd.p1;
        break;

    case MAV_CMD_DO_VTOL_TRANSITION:
        packet.param1 = cmd.content.do_vtol_transition.target_state;
        break;

    case MAV_CMD_DO_ENGINE_CONTROL:
        packet.param1 = cmd.content.do_engine_control.start_control?1:0;
        packet.param2 = cmd.content.do_engine_control.cold_start?1:0;
        packet.param3 = cmd.content.do_engine_control.height_delay_cm*0.01f;
        if (cmd.content.do_engine_control.allow_disarmed_start) {
            packet.param4 = ENGINE_CONTROL_OPTIONS_ALLOW_START_WHILE_DISARMED;
        }
        break;

#if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED
    case MAV_CMD_NAV_PAYLOAD_PLACE:
        packet.param1 = cmd.p1*0.01f; // copy max-descend parameter (cm->m)
        break;
#endif

    case MAV_CMD_NAV_SET_YAW_SPEED:
        packet.param1 = cmd.content.set_yaw_speed.angle_deg;        // target angle in degrees
        packet.param2 = cmd.content.set_yaw_speed.speed;            // speed in meters/second
        packet.param3 = cmd.content.set_yaw_speed.relative_angle;   // 0 = absolute angle, 1 = relative angle
        break;

    case MAV_CMD_DO_WINCH:
        packet.param1 = cmd.content.winch.num;              // winch number
        packet.param2 = cmd.content.winch.action;           // action (0 = relax, 1 = length control, 2 = rate control).  See WINCH_ACTION enum
        packet.param3 = cmd.content.winch.release_length;   // cable distance to unwind in meters, negative numbers to wind in cable
        packet.param4 = cmd.content.winch.release_rate;     // release rate in meters/second
        break;

    case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
        packet.param1 = cmd.p1; // Resume repeat distance (m)
        break;

    case MAV_CMD_DO_SEND_SCRIPT_MESSAGE:
        packet.param1 = cmd.p1;
        packet.param2 = cmd.content.scripting.p1;
        packet.param3 = cmd.content.scripting.p2;
        packet.param4 = cmd.content.scripting.p3;
        break;

#if AP_SCRIPTING_ENABLED
    case MAV_CMD_NAV_SCRIPT_TIME:
        packet.param1 = cmd.content.nav_script_time.command;
        packet.param2 = cmd.content.nav_script_time.timeout_s;
        packet.param3 = cmd.content.nav_script_time.arg1.get();
        packet.param4 = cmd.content.nav_script_time.arg2.get();
        packet.x = cmd.content.nav_script_time.arg3;
        packet.y = cmd.content.nav_script_time.arg4;
        break;
#endif

    case MAV_CMD_NAV_ATTITUDE_TIME:
        packet.param1 = cmd.content.nav_attitude_time.time_sec;
        packet.param2 = cmd.content.nav_attitude_time.roll_deg;
        packet.param3 = cmd.content.nav_attitude_time.pitch_deg;
        packet.param4 = cmd.content.nav_attitude_time.yaw_deg;
        packet.x = cmd.content.nav_attitude_time.climb_rate;
        break;

    case MAV_CMD_DO_PAUSE_CONTINUE:
        packet.param1 = cmd.p1;
        break;

    case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
        packet.param1 = cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg;
        packet.param2 = cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg;
        packet.param3 = cmd.content.gimbal_manager_pitchyaw.pitch_rate_degs;
        packet.param4 = cmd.content.gimbal_manager_pitchyaw.yaw_rate_degs;
        packet.x = cmd.content.gimbal_manager_pitchyaw.flags;
        packet.z = cmd.content.gimbal_manager_pitchyaw.gimbal_id;
        break;

    case MAV_CMD_IMAGE_START_CAPTURE:
        packet.param1 = cmd.content.image_start_capture.instance;
        packet.param2 = cmd.content.image_start_capture.interval_s;
        packet.param3 = cmd.content.image_start_capture.total_num_images;
        packet.param4 = cmd.content.image_start_capture.start_seq_number;
        break;

    case MAV_CMD_IMAGE_STOP_CAPTURE:
        packet.param1 = cmd.p1;
        break;

    case MAV_CMD_SET_CAMERA_ZOOM:
        packet.param1 = cmd.content.set_camera_zoom.zoom_type;
        packet.param2 = cmd.content.set_camera_zoom.zoom_value;
        break;

    case MAV_CMD_SET_CAMERA_FOCUS:
        packet.param1 = cmd.content.set_camera_focus.focus_type;
        packet.param2 = cmd.content.set_camera_focus.focus_value;
        break;

    case MAV_CMD_SET_CAMERA_SOURCE:
        packet.param1 = cmd.content.set_camera_source.instance;
        packet.param2 = cmd.content.set_camera_source.primary_source;
        packet.param3 = cmd.content.set_camera_source.secondary_source;
        break;

    case MAV_CMD_VIDEO_START_CAPTURE:
        packet.param1 = cmd.content.video_start_capture.video_stream_id;
        break;

    case MAV_CMD_VIDEO_STOP_CAPTURE:
        packet.param1 = cmd.content.video_stop_capture.video_stream_id;
        break;

    default:
        // unrecognised command
        return false;
    }

    // copy location from mavlink to command
    if (stored_in_location(cmd.id)) {
        packet.x = cmd.content.location.lat;
        packet.y = cmd.content.location.lng;

        packet.z = cmd.content.location.alt * 0.01f;   // cmd alt in cm to m
        if (cmd.content.location.relative_alt) {
            packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
        } else {
            packet.frame = MAV_FRAME_GLOBAL;
        }
#if AP_TERRAIN_AVAILABLE
        if (cmd.content.location.terrain_alt) {
            // this is a above-terrain altitude
            if (!cmd.content.location.relative_alt) {
                // refuse to return non-relative terrain mission
                // items. Internally we do have these, and they
                // have home.alt added, but we should never be
                // returning them to the GCS, as the GCS doesn't know
                // our home.alt, so it would have no way to properly
                // interpret it
                return false;
            }
            packet.z = cmd.content.location.alt * 0.01f;
            packet.frame = MAV_FRAME_GLOBAL_TERRAIN_ALT;
        }
#else
        // don't ever return terrain mission items if no terrain support
        if (cmd.content.location.terrain_alt) {
            return false;
        }
#endif
    }

    // if we got this far then it must have been successful
    return true;
}

///
/// private methods
///

/// complete - mission is marked complete and clean-up performed including calling the mission_complete_fn
void AP_Mission::complete()
{
    // flag mission as complete
    _flags.state = MISSION_COMPLETE;
    _flags.in_landing_sequence = false;
    _flags.in_return_path = false;

    // callback to main program's mission complete function
    _mission_complete_fn();
}

/// advance_current_nav_cmd - moves current nav command forward
///     do command will also be loaded
///     accounts for do-jump commands
//      returns true if command is advanced, false if failed (i.e. mission completed)
bool AP_Mission::advance_current_nav_cmd(uint16_t starting_index)
{
    // exit immediately if we're not running
    if (_flags.state != MISSION_RUNNING) {
        return false;
    }

    // exit immediately if current nav command has not completed
    if (_flags.nav_cmd_loaded) {
        return false;
    }

    // stop the current running do command
    _do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
    _flags.do_cmd_loaded = false;
    _flags.do_cmd_all_done = false;

    // get starting point for search
    uint16_t cmd_index = starting_index > 0 ? starting_index - 1 : _nav_cmd.index;
    if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
        // start from beginning of the mission command list
        cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
    } else {
        // start from one position past the current nav command
        cmd_index++;
    }

    // avoid endless loops
    uint8_t max_loops = 255;

    // search until we find next nav command or reach end of command list
    while (!_flags.nav_cmd_loaded && max_loops-- > 0) {
        // get next command
        Mission_Command cmd;
        if (!get_next_cmd(cmd_index, cmd, true)) {
            return false;
        }

        // check if navigation or "do" command
        if (is_nav_cmd(cmd)) {
            // save previous nav command index
            _prev_nav_cmd_id = _nav_cmd.id;
            _prev_nav_cmd_index = _nav_cmd.index;
            // save separate previous nav command index if it contains lat,long,alt
            if (!(cmd.content.location.lat == 0 && cmd.content.location.lng == 0)) {
                _prev_nav_cmd_wp_index = _nav_cmd.index;
            }
            // set current navigation command and start it
            _nav_cmd = cmd;
            if (start_command(_nav_cmd)) {
                _flags.nav_cmd_loaded = true;
                if (_jump_tag.age > 0 && _jump_tag.age < UINT16_MAX) {
                    // we're tracking a tag so increase it's age on every new NAV item
                    _jump_tag.age++;
                }
            }
            // save a loaded wp index in history array for when _repeat_dist is set via MAV_CMD_DO_SET_RESUME_REPEAT_DIST
            // and prevent history being re-written until vehicle returns to interrupted position
            if (_repeat_dist > 0 && !_flags.resuming_mission && _nav_cmd.index != AP_MISSION_CMD_INDEX_NONE && !(_nav_cmd.content.location.lat == 0 && _nav_cmd.content.location.lng == 0)) {
                // update mission history. last index position is always the most recent wp loaded.
                for (uint8_t i=0; i<AP_MISSION_MAX_WP_HISTORY-1; i++) {
                    _wp_index_history[i] = _wp_index_history[i+1];
                }
                _wp_index_history[AP_MISSION_MAX_WP_HISTORY-1] = _nav_cmd.index;
            }
            // check if the vehicle is resuming and has returned to where it was interrupted
            if (_flags.resuming_mission && _nav_cmd.index == _wp_index_history[AP_MISSION_MAX_WP_HISTORY-1]) {
                // vehicle has resumed previous position
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: Returned to interrupted WP");
                _flags.resuming_mission = false;
            }

        } else {
            // set current do command and start it (if not already set)
            if (!_flags.do_cmd_loaded) {
                _do_cmd = cmd;
                _flags.do_cmd_loaded = true;
                start_command(_do_cmd);
            }
        }
        // move onto next command
        cmd_index = cmd.index+1;
    }

    if (max_loops == 0) {
        // infinite loop.  This can happen if there's a loop involving
        // only nav commands (no DO commands) which won't start()
        return false;
    }

    // if we have not found a do command then set flag to show there are no do-commands to be run before nav command completes
    if (!_flags.do_cmd_loaded) {
        _flags.do_cmd_all_done = true;
    }

    // if we got this far we must have successfully advanced the nav command
    return true;
}

/// advance_current_do_cmd - moves current do command forward
///     accounts for do-jump commands
void AP_Mission::advance_current_do_cmd()
{
    // exit immediately if we're not running or we've completed all possible "do" commands
    if (_flags.state != MISSION_RUNNING || _flags.do_cmd_all_done) {
        return;
    }

    // get starting point for search
    uint16_t cmd_index = _do_cmd.index;
    if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
        cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
    } else {
        // start from one position past the current do command
        cmd_index = _do_cmd.index + 1;
    }

    // find next do command
    Mission_Command cmd;
    if (!get_next_do_cmd(cmd_index, cmd)) {
        // set flag to stop unnecessarily searching for do commands
        _flags.do_cmd_all_done = true;
        return;
    }

    // set current do command and start it
    _do_cmd = cmd;
    _flags.do_cmd_loaded = true;
    start_command(_do_cmd);
}

/// get_next_cmd - gets next command found at or after start_index
///     returns true if found, false if not found (i.e. mission complete)
///     accounts for do_jump commands
///     increment_jump_num_times_if_found should be set to true if advancing the active navigation command
bool AP_Mission::get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool increment_jump_num_times_if_found, bool send_gcs_msg)
{
    uint16_t cmd_index = start_index;
    Mission_Command temp_cmd;
    uint16_t jump_index = AP_MISSION_CMD_INDEX_NONE;

    // search until the end of the mission command list
    uint8_t max_loops = 64;
    while (cmd_index < (unsigned)_cmd_total) {
        // load the next command
        if (!read_cmd_from_storage(cmd_index, temp_cmd)) {
            // this should never happen because of check above but just in case
            return false;
        }

        // check for do-jump-tag command and convert target tag to do-jump target index and do-jump to it
        if (temp_cmd.id == MAV_CMD_DO_JUMP_TAG) {
            // convert tmp_cmd target from a target tag to a target index
            temp_cmd.content.jump.target = get_index_of_jump_tag(temp_cmd.content.jump.target);
            temp_cmd.id = MAV_CMD_DO_JUMP;
        }

        // check for do-jump command
        if (temp_cmd.id == MAV_CMD_DO_JUMP) {

            if (max_loops-- == 0) {
                return false;
            }

            // check for invalid target
            if ((temp_cmd.content.jump.target >= (unsigned)_cmd_total) || (temp_cmd.content.jump.target == 0)) {
                // To-Do: log an error?
                return false;
            }

            // check for endless loops
            if (!increment_jump_num_times_if_found && jump_index == cmd_index) {
                // we have somehow reached this jump command twice and there is no chance it will complete
                // To-Do: log an error?
                return false;
            }

            // record this command so we can check for endless loops
            if (jump_index == AP_MISSION_CMD_INDEX_NONE) {
                jump_index = cmd_index;
            }

            // get number of times jump command has already been run
            if (temp_cmd.content.jump.num_times == AP_MISSION_JUMP_REPEAT_FOREVER ||
                get_jump_times_run(temp_cmd) < temp_cmd.content.jump.num_times) {
                // update the record of the number of times run
                if (increment_jump_num_times_if_found && !_flags.resuming_mission) {
                    increment_jump_times_run(temp_cmd, send_gcs_msg);
                }
                // continue searching from jump target
                cmd_index = temp_cmd.content.jump.target;
            } else {
                // jump has been run specified number of times so move search to next command in mission
                cmd_index++;
            }
        } else {
            // this is a non-jump command so return it
            cmd = temp_cmd;
            return true;
        }
    }

    // if we got this far we did not find a navigation command
    return false;
}

/// get_next_do_cmd - gets next "do" or "conditional" command after start_index
///     returns true if found, false if not found
///     stops and returns false if it hits another navigation command before it finds the first do or conditional command
///     accounts for do_jump commands but never increments the jump's num_times_run (advance_current_nav_cmd is responsible for this)
bool AP_Mission::get_next_do_cmd(uint16_t start_index, Mission_Command& cmd)
{
    Mission_Command temp_cmd;

    // check we have not passed the end of the mission list
    if (start_index >= (unsigned)_cmd_total) {
        return false;
    }

    // get next command
    if (!get_next_cmd(start_index, temp_cmd, false)) {
        // no more commands so return failure
        return false;
    } else if (is_nav_cmd(temp_cmd)) {
        // if it's a "navigation" command then return false because we do not progress past nav commands
        return false;
    } else {
        // this must be a "do" or "conditional" and is not a do-jump command so return it
        cmd = temp_cmd;
        return true;
    }
}

///
/// jump handling methods
///

// Set the mission index to the first JUMP_TAG with this tag.
// Returns true on success, else false if no appropriate JUMP_TAG match can be found or if setting the index failed
bool AP_Mission::jump_to_tag(const uint16_t tag)
{
    const uint16_t index = get_index_of_jump_tag(tag);
    if (index == 0) {
        return false;
    }
    return set_current_cmd(index);
}

// find the first JUMP_TAG with this tag and return its index.
// Returns 0 if no appropriate JUMP_TAG match can be found.
uint16_t AP_Mission::get_index_of_jump_tag(const uint16_t tag) const
{
    const auto count = num_commands();
    for (uint16_t i = 1; i < count; i++) {
        if (get_command_id(i) != uint16_t(MAV_CMD_JUMP_TAG)) {
            continue;
        }
        Mission_Command tmp;
        if (!read_cmd_from_storage(i, tmp)) {
            continue;
        }
        if (tmp.id == MAV_CMD_JUMP_TAG && tmp.content.jump.target == tag) {
            return i;
        }
    }
    return 0;
}

#if AP_SCRIPTING_ENABLED
bool AP_Mission::get_last_jump_tag(uint16_t &tag, uint16_t &age) const
{
    if (_jump_tag.age == 0) {
        return false;
    }
    tag = _jump_tag.tag;
    age = _jump_tag.age;
    return true;
}
#endif

// init_jump_tracking - initialise jump_tracking variables
void AP_Mission::init_jump_tracking()
{
    for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
        _jump_tracking[i].index = AP_MISSION_CMD_INDEX_NONE;
        _jump_tracking[i].num_times_run = 0;
    }
}

/// get_jump_times_run - returns number of times the jump command has been run
int16_t AP_Mission::get_jump_times_run(const Mission_Command& cmd)
{
    // exit immediately if cmd is not a do-jump command or target is invalid
    if ((cmd.id != MAV_CMD_DO_JUMP) || (cmd.content.jump.target >= (unsigned)_cmd_total) || (cmd.content.jump.target == 0)) {
        // To-Do: log an error?
        return AP_MISSION_JUMP_TIMES_MAX;
    }

    // search through jump_tracking array for this cmd
    for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
        if (_jump_tracking[i].index == cmd.index) {
            return _jump_tracking[i].num_times_run;
        } else if (_jump_tracking[i].index == AP_MISSION_CMD_INDEX_NONE) {
            // we've searched through all known jump commands and haven't found it so allocate new space in _jump_tracking array
            _jump_tracking[i].index = cmd.index;
            _jump_tracking[i].num_times_run = 0;
            return 0;
        }
    }

    // if we've gotten this far then the _jump_tracking array must be full
    // To-Do: log an error?
    return AP_MISSION_JUMP_TIMES_MAX;
}

/// increment_jump_times_run - increments the recorded number of times the jump command has been run
void AP_Mission::increment_jump_times_run(Mission_Command& cmd, bool send_gcs_msg)
{
    // exit immediately if cmd is not a do-jump command
    if (cmd.id != MAV_CMD_DO_JUMP) {
        // To-Do: log an error?
        return;
    }

    // search through jump_tracking array for this cmd
    for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
        if (_jump_tracking[i].index == cmd.index) {
            _jump_tracking[i].num_times_run++;
            if (send_gcs_msg) {
                if (cmd.content.jump.num_times == AP_MISSION_JUMP_REPEAT_FOREVER) {
                    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: %u Jump %i/unlimited", _jump_tracking[i].index, _jump_tracking[i].num_times_run);
                } else {
                    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: %u Jump %i/%i", _jump_tracking[i].index, _jump_tracking[i].num_times_run, cmd.content.jump.num_times);
                }
            }
            return;
        } else if (_jump_tracking[i].index == AP_MISSION_CMD_INDEX_NONE) {
            // we've searched through all known jump commands and haven't found it so allocate new space in _jump_tracking array
            _jump_tracking[i].index = cmd.index;
            _jump_tracking[i].num_times_run = 1;
            return;
        }
    }

    // if we've gotten this far then the _jump_tracking array must be full
    // To-Do: log an error
    return;
}

// check_eeprom_version - checks version of missions stored in eeprom matches this library
// command list will be cleared if they do not match
void AP_Mission::check_eeprom_version()
{
    uint32_t eeprom_version = _storage.read_uint32(0);

    // if eeprom version does not match, clear the command list and update the eeprom version
    if (eeprom_version != AP_MISSION_EEPROM_VERSION) {
        if (clear()) {
            _storage.write_uint32(0, AP_MISSION_EEPROM_VERSION);
        }
    }
}

// find the nearest landing sequence starting point (DO_LAND_START) and
// return its index.  Returns 0 if no appropriate DO_LAND_START point can
// be found.
uint16_t AP_Mission::get_landing_sequence_start(const Location &current_loc)
{
    uint16_t landing_start_index = 0;
    float min_distance = -1;

    // Go through mission looking for nearest landing start command
    const auto count = num_commands();
    for (uint16_t i = 1; i < count; i++) {
        if (get_command_id(i) != uint16_t(MAV_CMD_DO_LAND_START)) {
            continue;
        }
        Mission_Command tmp;
        if (!read_cmd_from_storage(i, tmp)) {
            continue;
        }
        if (tmp.id == MAV_CMD_DO_LAND_START) {
            if (!tmp.content.location.initialised() && !get_next_nav_cmd(i, tmp)) {
                // command does not have a valid location and cannot get next valid
                continue;
            }

            const float tmp_distance = tmp.content.location.get_distance_NED_alt_frame(current_loc).length();
            if (min_distance < 0 || tmp_distance < min_distance) {
                min_distance = tmp_distance;
                landing_start_index = i;
            }
        }
    }

    return landing_start_index;
}

/*
   find the nearest landing sequence starting point (DO_LAND_START) and
   switch to that mission item.  Returns false if no DO_LAND_START
   available.
 */
bool AP_Mission::jump_to_landing_sequence(const Location &current_loc)
{
    uint16_t land_idx = get_landing_sequence_start(current_loc);
    if (land_idx != 0 && set_current_cmd(land_idx)) {

        //if the mission has ended it has to be restarted
        if (state() == AP_Mission::MISSION_STOPPED) {
            resume();
        }

        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Landing sequence start");
        return true;
    }

    GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unable to start landing sequence");
    return false;
}

/*
   find the closest point on the mission after a DO_RETURN_PATH_START and before DO_LAND_START or landing
 */
bool AP_Mission::jump_to_closest_mission_leg(const Location &current_loc)
{
    if (_flags.state == MISSION_RUNNING) {
        // if mission is already running don't switch away from a active landing or return path
        if (_flags.in_landing_sequence) {
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Landing sequence active");
            return true;

        } else if (_flags.in_return_path) {
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Return path active");
            return true;
        }
    }

    uint16_t landing_start_index = 0;
    float min_distance = -1;

    // This defines the maximum number of waypoints that will be searched, this limits the worst case runtime
    uint16_t search_remaining = 1000;

    // Go through mission and check each DO_RETURN_PATH_START
    for (uint16_t i = 1; i < num_commands(); i++) {
        if (get_command_id(i) == uint16_t(MAV_CMD_DO_RETURN_PATH_START)) {
            uint16_t tmp_index;
            float tmp_distance;
            if (distance_to_mission_leg(i, search_remaining, tmp_distance, tmp_index, current_loc) && (min_distance < 0 || tmp_distance <= min_distance)){
                min_distance = tmp_distance;
                landing_start_index = tmp_index;
            }
            if (search_remaining == 0) {
                // Run out of time to search, stop and return the best so far
                break;
            }
        }
    }

    if (landing_start_index != 0 && set_current_cmd(landing_start_index)) {

        // if the mission has ended it has to be restarted
        if (state() == AP_Mission::MISSION_STOPPED) {
            resume();
        }

        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Return path started");
        _flags.in_return_path = true;
        return true;
    }

    // Failed to find do land start
    return false;
}

// jumps the mission to the closest landing abort that is planned, returns false if unable to find a valid abort
bool AP_Mission::jump_to_abort_landing_sequence(const Location &current_loc)
{
    uint16_t abort_index = 0;
    float min_distance = FLT_MAX;

    const auto count = num_commands();
    for (uint16_t i = 1; i < count; i++) {
        if (get_command_id(i) != uint16_t(MAV_CMD_DO_GO_AROUND)) {
            continue;
        }
        Mission_Command tmp;
        if (!read_cmd_from_storage(i, tmp)) {
            continue;
        }
        if (tmp.id == MAV_CMD_DO_GO_AROUND) {
            float tmp_distance = tmp.content.location.get_distance(current_loc);
            if (tmp_distance < min_distance) {
                min_distance = tmp_distance;
                abort_index = i;
            }
        }
    }

    if (abort_index != 0 && set_current_cmd(abort_index)) {

        //if the mission has ended it has to be restarted
        if (state() == AP_Mission::MISSION_STOPPED) {
            resume();
        }

        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Landing abort sequence start");
        return true;
    }

    GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unable to start find a landing abort sequence");
    return false;
}

// check which is the shortest route to landing an RTL via a DO_LAND_START or continuing on the current mission plan
bool AP_Mission::is_best_land_sequence(const Location &current_loc)
{
    // check if there is even a running mission to interupt
    if (_flags.state != MISSION_RUNNING) {
        return false;
    }

    // check if aircraft has already jumped to a landing sequence
    if (_flags.in_landing_sequence) {
        return true;
    }

    // check if MIS_OPTIONS bit set to allow distance calculation to be done
    if (!option_is_set(Option::FAILSAFE_TO_BEST_LANDING)) {
        return false;
    }

    // The decision to allow a failsafe to interupt a potential landing approach
    // is a distance travelled minimization problem.  Look forward in
    // mission to evaluate the shortest remaining distance to land.

    // go through the mission for the nearest DO_LAND_START first as this is the most probable route
    // to a landing with the minimum number of WP.
    uint16_t do_land_start_index = get_landing_sequence_start(current_loc);
    if (do_land_start_index == 0) {
        // then no DO_LAND_START commands are in mission and normal failsafe behaviour should be maintained
        return false;
    }

    // get distance to landing if travelled to nearest DO_LAND_START via RTL
    float dist_via_do_land;
    if (!distance_to_landing(do_land_start_index, dist_via_do_land, current_loc)) {
        // cant get a valid distance to landing
        return false;
    }

    // get distance to landing if continue along current mission path
    float dist_continue_to_land;
    if (!distance_to_landing(_nav_cmd.index, dist_continue_to_land, current_loc)) {
        // cant get a valid distance to landing
        return false;
    }

    // compare distances
    if (dist_via_do_land >= dist_continue_to_land) {
        // then the mission should carry on uninterrupted as that is the shorter distance
        GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Rejecting RTL: closer land if mis continued");
        return true;
    } else {
        // allow failsafes to interrupt the current mission
        return false;
    }
}

// Approximate the distance travelled to get to a landing.  DO_JUMP commands are observed in look forward.
bool AP_Mission::distance_to_landing(uint16_t index, float &tot_distance, Location prev_loc)
{
    Mission_Command temp_cmd;
    tot_distance = 0.0f;
    bool ret = false;  // reached end of loop without getting to a landing

    // back up jump tracking to reset after distance calculation
    jump_tracking_struct _jump_tracking_backup[AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS];
    for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
        _jump_tracking_backup[i] = _jump_tracking[i];
    }

    // run through remainder of mission to approximate a distance to landing
    for (uint8_t i=0; i<UINT8_MAX; i++) {
        // search until the end of the mission command list
        for (uint16_t cmd_index = index; cmd_index < (unsigned)_cmd_total; cmd_index++) {
            // get next command
            if (!get_next_cmd(cmd_index, temp_cmd, true, false)) {
                // we got to the end of the mission
                goto reset_do_jump_tracking;
            }
            if (temp_cmd.id == MAV_CMD_NAV_WAYPOINT || temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT || is_landing_type_cmd(temp_cmd.id)) {
                break;
            } else if (is_nav_cmd(temp_cmd) || temp_cmd.id == MAV_CMD_CONDITION_DELAY) {
                // if we receive a nav command that we dont handle then give up as cant measure the distance e.g. MAV_CMD_NAV_LOITER_UNLIM
                goto reset_do_jump_tracking;
            }
        }
        index = temp_cmd.index+1;

        if (!(temp_cmd.content.location.lat == 0 && temp_cmd.content.location.lng == 0)) {
            // add distance to running total
            float disttemp = prev_loc.get_distance(temp_cmd.content.location);
            tot_distance = tot_distance + disttemp;

            // store wp location as previous
            prev_loc = temp_cmd.content.location;
        }

        if (is_landing_type_cmd(temp_cmd.id)) {
            // reached a landing!
            ret = true;
            goto reset_do_jump_tracking;
        }
    }

reset_do_jump_tracking:
    for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
        _jump_tracking[i] = _jump_tracking_backup[i];
    }

    return ret;
}

// Approximate the distance travelled to return to the mission path. DO_JUMP commands are observed in look forward.
// Stop searching once reaching a landing or do-land-start
bool AP_Mission::distance_to_mission_leg(uint16_t start_index, uint16_t &search_remaining, float &rejoin_distance, uint16_t &rejoin_index, const Location& current_loc)
{
    Location prev_loc;
    Mission_Command temp_cmd;
    rejoin_distance = -1;
    rejoin_index = -1;
    bool ret = false;

    // back up jump tracking to reset after distance calculation
    jump_tracking_struct _jump_tracking_backup[AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS];
    for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
        _jump_tracking_backup[i] = _jump_tracking[i];
    }

    // run through remainder of mission to approximate a distance to landing
    uint16_t index = start_index;
    for (; search_remaining > 0; search_remaining--) {
        // search until the end of the mission command list
        for (uint16_t cmd_index = index; cmd_index <= (unsigned)_cmd_total; cmd_index++) {
            if (get_next_cmd(cmd_index, temp_cmd, true, false)) {
                break;
            } else {
                // got to the end of the mission
                goto reset_do_jump_tracking;
            }
        }
        index = temp_cmd.index + 1;

        if (stored_in_location(temp_cmd.id) && temp_cmd.content.location.initialised()) {
            if (prev_loc.lat == 0 && prev_loc.lng == 0) {
                // Need a valid previous location to do distance to leg calculation
                prev_loc = temp_cmd.content.location;

                // single point dist calc
                rejoin_distance = prev_loc.get_distance_NED_alt_frame(current_loc).length();
                rejoin_index = temp_cmd.index;
                ret = true;

            } else {
                // Calculate the distance to rejoin
                const Vector3f mission_vector = prev_loc.get_distance_NED_alt_frame(temp_cmd.content.location);
                if (!mission_vector.is_zero()) {
                    Vector3f pos = prev_loc.get_distance_NED_alt_frame(current_loc);

                    // project pos vector on to mission vector
                    Vector3f p = pos.projected(mission_vector);

                    // constrain to mission line
                    p.x = constrain_float(p.x, MIN(0,mission_vector.x), MAX(0,mission_vector.x));
                    p.y = constrain_float(p.y, MIN(0,mission_vector.y), MAX(0,mission_vector.y));
                    p.z = constrain_float(p.z, MIN(0,mission_vector.z), MAX(0,mission_vector.z));

                    const float disttemp = (p - pos).length();

                    // store wp location as previous
                    prev_loc = temp_cmd.content.location;

                    if (disttemp < rejoin_distance || is_negative(rejoin_distance)) {
                        rejoin_distance = disttemp;
                        rejoin_index = temp_cmd.index;
                    }
                    ret = true;
                }
            }
        }

        if (is_landing_type_cmd(temp_cmd.id) || (temp_cmd.id == MAV_CMD_DO_LAND_START)) {
            // reached a landing!
            goto reset_do_jump_tracking;
        }
    }
    // reached end of loop without getting to a landing or DO_LAND_START
    ret = false;

reset_do_jump_tracking:
    for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
        _jump_tracking[i] = _jump_tracking_backup[i];
    }

    return ret;
}

// check if command is a landing type command.
bool AP_Mission::is_landing_type_cmd(uint16_t id) const
{
    switch (id) {
    case MAV_CMD_NAV_LAND:
    case MAV_CMD_NAV_VTOL_LAND:
    case MAV_CMD_DO_PARACHUTE:
        return true;
    default:
        return false;
    }
}

// check if command is a takeoff type command.
bool AP_Mission::is_takeoff_type_cmd(uint16_t id) const
{
    switch (id) {
    case MAV_CMD_NAV_TAKEOFF:
    case MAV_CMD_NAV_VTOL_TAKEOFF:
        return true;
    default:
        return false;
    }
}

const char *AP_Mission::Mission_Command::type() const
{
    switch (id) {
    case MAV_CMD_NAV_WAYPOINT:
        return "WP";
    case MAV_CMD_NAV_SPLINE_WAYPOINT:
        return "SplineWP";
    case MAV_CMD_NAV_RETURN_TO_LAUNCH:
        return "RTL";
    case MAV_CMD_NAV_LOITER_UNLIM:
        return "LoitUnlim";
    case MAV_CMD_NAV_LOITER_TIME:
        return "LoitTime";
    case MAV_CMD_NAV_GUIDED_ENABLE:
        return "GuidedEnable";
    case MAV_CMD_NAV_LOITER_TURNS:
        return "LoitTurns";
    case MAV_CMD_NAV_LOITER_TO_ALT:
        return "LoitAltitude";
    case MAV_CMD_NAV_SET_YAW_SPEED:
        return "SetYawSpd";
    case MAV_CMD_CONDITION_DELAY:
        return "CondDelay";
    case MAV_CMD_CONDITION_DISTANCE:
        return "CondDist";
    case MAV_CMD_DO_CHANGE_SPEED:
        return "ChangeSpeed";
    case MAV_CMD_DO_SET_HOME:
        return "SetHome";
    case MAV_CMD_DO_SET_SERVO:
        return "SetServo";
    case MAV_CMD_DO_SET_RELAY:
        return "SetRelay";
    case MAV_CMD_DO_REPEAT_SERVO:
        return "RepeatServo";
    case MAV_CMD_DO_REPEAT_RELAY:
        return "RepeatRelay";
    case MAV_CMD_DO_DIGICAM_CONFIGURE:
        return "DigiCamCfg";
    case MAV_CMD_DO_DIGICAM_CONTROL:
        return "DigiCamCtrl";
    case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
        return "SetCamTrigDst";
    case MAV_CMD_DO_SET_ROI:
        return "SetROI";
    case MAV_CMD_DO_SET_REVERSE:
        return "SetReverse";
    case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
        return "SetRepeatDist";
    case MAV_CMD_DO_GUIDED_LIMITS:
        return "GuidedLimits";
    case MAV_CMD_NAV_TAKEOFF:
        return "Takeoff";
    case MAV_CMD_NAV_LAND:
        return "Land";
    case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
        return "ContinueAndChangeAlt";
    case MAV_CMD_NAV_ALTITUDE_WAIT:
        return "AltitudeWait";
    case MAV_CMD_NAV_VTOL_TAKEOFF:
        return "VTOLTakeoff";
    case MAV_CMD_NAV_VTOL_LAND:
        return "VTOLLand";
    case MAV_CMD_DO_INVERTED_FLIGHT:
        return "InvertedFlight";
    case MAV_CMD_DO_FENCE_ENABLE:
        return "FenceEnable";
    case MAV_CMD_DO_AUTOTUNE_ENABLE:
        return "AutoTuneEnable";
    case MAV_CMD_DO_VTOL_TRANSITION:
        return "VTOLTransition";
    case MAV_CMD_DO_ENGINE_CONTROL:
        return "EngineControl";
    case MAV_CMD_CONDITION_YAW:
        return "CondYaw";
    case MAV_CMD_DO_RETURN_PATH_START:
        return "ReturnPathStart";
    case MAV_CMD_DO_LAND_START:
        return "LandStart";
    case MAV_CMD_NAV_DELAY:
        return "Delay";
#if AP_GRIPPER_ENABLED
    case MAV_CMD_DO_GRIPPER:
        return "Gripper";
#endif
#if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED
    case MAV_CMD_NAV_PAYLOAD_PLACE:
        return "PayloadPlace";
#endif
    case MAV_CMD_DO_PARACHUTE:
        return "Parachute";
    case MAV_CMD_DO_SPRAYER:
        return "Sprayer";
    case MAV_CMD_DO_AUX_FUNCTION:
        return "AuxFunction";
    case MAV_CMD_DO_MOUNT_CONTROL:
        return "MountControl";
    case MAV_CMD_DO_WINCH:
        return "Winch";
    case MAV_CMD_DO_SEND_SCRIPT_MESSAGE:
        return "Scripting";
    case MAV_CMD_DO_JUMP:
        return "Jump";
    case MAV_CMD_DO_JUMP_TAG:
        return "JumpToTag";
    case MAV_CMD_JUMP_TAG:
        return "Tag";
    case MAV_CMD_DO_GO_AROUND:
        return "Go Around";
#if AP_SCRIPTING_ENABLED
    case MAV_CMD_NAV_SCRIPT_TIME:
        return "NavScriptTime";
#endif
    case MAV_CMD_NAV_ATTITUDE_TIME:
        return "NavAttitudeTime";
    case MAV_CMD_DO_PAUSE_CONTINUE:
        return "PauseContinue";
    case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
        return "GimbalPitchYaw";
    case MAV_CMD_IMAGE_START_CAPTURE:
        return "ImageStartCapture";
    case MAV_CMD_IMAGE_STOP_CAPTURE:
        return "ImageStopCapture";
    case MAV_CMD_SET_CAMERA_ZOOM:
        return "SetCameraZoom";
    case MAV_CMD_SET_CAMERA_FOCUS:
        return "SetCameraFocus";
    case MAV_CMD_SET_CAMERA_SOURCE:
        return "SetCameraSource";
    case MAV_CMD_VIDEO_START_CAPTURE:
        return "VideoStartCapture";
    case MAV_CMD_VIDEO_STOP_CAPTURE:
        return "VideoStopCapture";
    default:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        AP_HAL::panic("Mission command with ID %u has no string", id);
#endif
        return "?";
    }
}

/*
  get the command ID of a mission index. Caller should have checked the index is in range
 */
uint16_t AP_Mission::get_command_id(uint16_t index) const
{
    const uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
    uint8_t b[3] {};
    if (!_storage.read_block(b, pos_in_storage, sizeof(b))) {
        return 0U;
    }
    uint16_t id = 0;
    if (b[0] == 0 || b[0] == 1) {
        memcpy((void*)&id, (void*)&b[1], 2);
    } else {
        id = b[0];
    }
    return id;
}

/*
  see if the mission contains a particular item
 */
bool AP_Mission::contains_item(MAV_CMD command) const
{
    const auto count = num_commands();
    for (uint16_t i = 1; i < count; i++) {
        if (get_command_id(i) != uint16_t(command)) {
            continue;
        }
        // confirm with full read
        Mission_Command tmp;
        if (!read_cmd_from_storage(i, tmp)) {
            continue;
        }
        if (tmp.id == command) {
            return true;
        }
    }
    return false;
}

/*
  return true if the mission item has a location
*/

bool AP_Mission::cmd_has_location(const uint16_t command)
{
    return stored_in_location(command);
}

/*
  return true if the mission has a terrain relative item.  ~2200us for 530 items on H7
 */
bool AP_Mission::contains_terrain_alt_items(void)
{
    if (_last_contains_relative_calculated_ms != _last_change_time_ms) {
        _contains_terrain_alt_items = calculate_contains_terrain_alt_items();
        _last_contains_relative_calculated_ms = _last_change_time_ms;
    }
    return _contains_terrain_alt_items;
}

bool AP_Mission::calculate_contains_terrain_alt_items(void) const
{
    const auto count = num_commands();
    for (uint16_t i = 1; i < count; i++) {
        if (!stored_in_location(get_command_id(i))) {
            continue;
        }
        Mission_Command tmp;
        if (!read_cmd_from_storage(i, tmp)) {
            continue;
        }
        if (stored_in_location(tmp.id) && tmp.content.location.terrain_alt) {
            return true;
        }
    }
    return false;
}

// reset the mission history to prevent recalling previous mission histories after a mission restart.
void AP_Mission::reset_wp_history(void)
{
    for (uint8_t i = 0; i<AP_MISSION_MAX_WP_HISTORY; i++) {
        _wp_index_history[i] = AP_MISSION_CMD_INDEX_NONE;
    }
    _resume_cmd.index = AP_MISSION_CMD_INDEX_NONE;
    _flags.resuming_mission = false;
    _repeat_dist = 0;
}

// store the latest reported position incase of mission exit and rewind resume
void AP_Mission::update_exit_position(void)
{
    if (!AP::ahrs().get_location(_exit_position)) {
        _exit_position.lat = 0;
        _exit_position.lng = 0;
    }
}

// calculate the location of the _resume_cmd wp and set as current
bool AP_Mission::calc_rewind_pos(Mission_Command& rewind_cmd)
{
    // check for a recent history
    if (_wp_index_history[LAST_WP_PASSED] == AP_MISSION_CMD_INDEX_NONE) {
        // no saved history so can't rewind
        return false;
    }

    // check that we have a valid exit position
    if (_exit_position.lat == 0 && _exit_position.lng == 0) {
        return false;
    }

    Mission_Command temp_cmd;
    float rewind_distance = _repeat_dist; //(m)
    uint16_t resume_index;
    Location prev_loc = _exit_position;

    for (int8_t i = (LAST_WP_PASSED); i>=0; i--) {

        // to get this far there has to be at least one 'passed wp' stored in history.  This is to check incase
        // of history array no being completely filled with valid waypoints upon resume.
        if (_wp_index_history[i] == AP_MISSION_CMD_INDEX_NONE) {
            // no more stored history
            resume_index = i+1;
            break;
        }

        if (!read_cmd_from_storage(_wp_index_history[i], temp_cmd)) {
            // if read from storage failed then don't use rewind
            return false;
        }

        // calculate distance
        float disttemp = prev_loc.get_distance(temp_cmd.content.location); //(m)
        rewind_distance -= disttemp;
        resume_index = i;

        if (rewind_distance <= 0.0f) {
            // history rewound enough distance to meet _repeat_dist requirement
            rewind_cmd = temp_cmd;
            break;
        }

        // store wp location as previous
        prev_loc = temp_cmd.content.location;
    }

    if (rewind_distance > 0.0f) {
        // then the history array was rewound all of the way without finding a wp distance > _repeat_dist distance.
        // the last read temp_cmd will be the furthest cmd back in the history array so resume to that.
        rewind_cmd = temp_cmd;
        return true;
    }

    // if we have got this far the desired rewind distance lies between two waypoints stored in history array.
    // calculate the location for the mission to resume

    // the last wp read from storage is the wp that is before the resume wp in the mission order
    Location passed_wp_loc = temp_cmd.content.location;

    // fetch next destination wp
    if (!read_cmd_from_storage(_wp_index_history[resume_index+1], temp_cmd)) {
        // if read from storage failed then don't use rewind
        return false;
    }

    // determine the length of the mission leg that the resume wp lies in
    float leg_length = passed_wp_loc.get_distance(temp_cmd.content.location); //(m)

    // calculate the percentage along the leg that resume wp will be positioned
    float leg_percent = fabsf(rewind_distance)/leg_length;

    // calculate difference vector of mission leg
    Vector3f dist_vec = passed_wp_loc.get_distance_NED(temp_cmd.content.location);

    // calculate the resume wp position
    rewind_cmd.content.location.offset(dist_vec.x * leg_percent, dist_vec.y * leg_percent);
    rewind_cmd.content.location.alt -= dist_vec.z * leg_percent * 100; //(cm)

    // The rewind_cmd.index has the index of the 'last passed wp' from the history array.  This ensures that the mission order
    // continues as planned without further intervention.  The resume wp is not written to memory so will not perminantely change the mission.

    // if we got this far then mission rewind position was successfully calculated
    return true;
}

/*
  handle format conversion of storage format to allow us to update
  format to take advantage of new packing. This is particularly useful
  for conversion to Float16 to get extra parameter space
*/
void AP_Mission::format_conversion(uint8_t tag_byte, const Mission_Command &cmd, PackedContent &packed_content) const
{
    // currently only one conversion needed, more can be added
#if AP_SCRIPTING_ENABLED
    if (tag_byte == 0 && cmd.id == MAV_CMD_NAV_SCRIPT_TIME) {
        // PARAMETER_CONVERSION: conversion code added Oct 2022
        struct nav_script_time_Command_tag0 old_fmt;
        struct nav_script_time_Command new_fmt;
        memcpy((void*)&old_fmt, packed_content.bytes, sizeof(old_fmt));
        new_fmt.command = old_fmt.command;
        new_fmt.timeout_s = old_fmt.timeout_s;
        new_fmt.arg1.set(old_fmt.arg1);
        new_fmt.arg2.set(old_fmt.arg2);
        new_fmt.arg3 = 0;
        new_fmt.arg4 = 0;
        memcpy(packed_content.bytes, (void*)&new_fmt, sizeof(new_fmt));
    }
#endif
}

// Helpers to fill in location for scripting
#if AP_SCRIPTING_ENABLED
bool AP_Mission::jump_to_landing_sequence(void)
{
    Location loc;
    if (AP::ahrs().get_location(loc)) {
        return jump_to_landing_sequence(loc);
    }
    GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unable to start landing sequence");
    return false;
}

bool AP_Mission::jump_to_abort_landing_sequence(void)
{
    Location loc;
    if (AP::ahrs().get_location(loc)) {
        return jump_to_abort_landing_sequence(loc);
    }
    GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unable to start find a landing abort sequence");
    return false;
}
#endif // AP_SCRIPTING_ENABLED


// singleton instance
AP_Mission *AP_Mission::_singleton;

namespace AP
{

AP_Mission *mission()
{
    return AP_Mission::get_singleton();
}

}

#endif  // AP_MISSION_ENABLED