AP_Mission: initial draft library

libraries/AP_Mission/AP_Mission.cpp

@@ -0,0 +1,410 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/// @file    AP_Mission.cpp
+/// @brief   Handles the MAVLINK command mission stack.  Reads and writes mission to storage.
+
+#include "AP_Mission.h"
+
+const AP_Param::GroupInfo AP_Mission::var_info[] PROGMEM = {
+
+    // @Param: TOTAL
+    // @DisplayName: Total number of commands in the mission
+    // @Description: The number of mission mission items that has been loaded by the ground station. Do not change this manually.
+    // @Range: 0 255
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("TOTAL",  0, AP_Mission, _cmd_total, 0),
+
+    AP_GROUPEND
+};
+
+extern const AP_HAL::HAL& hal;
+
+/// public methods
+
+/// public methods
+/// 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;
+    }
+
+    // check if nav, do, cond commands are loaded
+        // if not load next nav-cmd
+            // this should also prompt loading of new do or conditional commands
+        // call command_init for each command loaded
+
+    // if we're running nav_command, verify it
+        // if command completed also push it to the prev_cmd
+        // if command completed, mark nav command as unloaded (so it'll be loaded next time through)
+
+    // if we're running do_command or conditional command, verify it
+        // if it completes, unload the do-command queue (so a new one will be loaded next time through)
+}
+
+/// 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;
+    _flags.nav_cmd_loaded = false;
+    _flags.do_cmd_loaded = false;
+}
+
+/// 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-initialised
+void AP_Mission::resume()
+{
+    _flags.state = MISSION_RUNNING;
+}
+
+/// get_next_nav_cmd - returns the next navigation command
+///     offset parameter controls how many commands forward we should look.  Defaults to 1 meaning the very next nav command
+//const AP_Mission::Mission_Command& AP_Mission::get_next_nav_cmd(uint8_t offset) const
+//{
+//    get_next_cmd_index
+//}
+
+/// advance_current_nav_cmd - moves current nav command forward
+///     do command will also be loaded
+///     accounts for do-jump commands
+bool AP_Mission::advance_current_nav_cmd(uint8_t start_index)
+{
+}
+
+/// clear - clears out mission
+///     returns true if mission was running so it could not be cleared
+bool AP_Mission::clear()
+{
+    // do not allow clearing the mission while it is running
+    if (_flags.state == MISSION_RUNNING) {
+        return false;
+    }
+
+    // remove all commands
+    _cmd_total.set_and_save(0);
+
+    // clear index to commands
+    _nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
+    _do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
+}
+
+/// valid - validate the mission has no errors
+///     currently only checks that the number of do-commands does not exceed the AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS
+bool AP_Mission::valid()
+{
+    return true;
+}
+
+/// 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;
+}
+
+/// load_cmd_from_storage - load command from storage
+///     true is return if successful
+bool AP_Mission::read_cmd_from_storage(uint8_t index, Mission_Command& cmd) const
+{
+    uint16_t pos_in_storage;    // position in storage from where we will read the next byte
+
+    // exit immediately if index is beyond last command
+    if (index > _cmd_total) {
+        // return a command with a blank id
+        cmd.id = AP_MISSION_CMD_ID_NONE;
+        return false;
+    }
+
+    // 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
+    pos_in_storage = (AP_MISSION_EEPROM_START_BYTE) + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
+
+    // set command's index to it's position in eeprom
+    cmd.index = index;
+
+    // read command from eeprom
+    hal.storage->read_block(cmd.content.bytes, pos_in_storage, AP_MISSION_EEPROM_COMMAND_SIZE);
+
+    // set command from location's command
+    // To-Do: remove id (and p1?) from Location structure
+    cmd.id = cmd.content.location.id;
+
+    // return success
+    return true;
+}
+
+/// 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(uint8_t index, Mission_Command& cmd)
+{
+    // range check cmd's index
+    if (index >= AP_MISSION_MAX_COMMANDS) {
+        // debug
+        hal.console->printf_P(PSTR("fail %d > %d\n"),(int)index,(int)AP_MISSION_MAX_COMMANDS);
+        return false;
+    }
+
+    // calculate where in storage the command should be placed
+    uint16_t pos_in_storage = AP_MISSION_EEPROM_START_BYTE + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
+
+    // force home wp to absolute height
+    //if (i == 0) {
+    //    temp.options &= ~(AP_MISSION_MASK_OPTIONS_RELATIVE_ALT);
+    //}
+
+    // set location id to cmd.id
+    // To-Do: remove id (and p1?) from Location structure
+    cmd.content.location.id = cmd.id;
+
+    // write block to eeprom
+    hal.storage->write_block(pos_in_storage, cmd.content.bytes, AP_MISSION_EEPROM_COMMAND_SIZE);
+
+    // return success
+    return true;
+}
+
+/// get_next_cmd_index - gets next command after curr_cmd_index which is of type cmd_type
+///     returns MISSION_CMD_NONE if no command is found
+///     accounts for do_jump commands
+///     if requesting MISSION_CMD_DO or MISSION_CMD_COND it will stop and return MISSION_CMD_NONE if it hits a MISSION_CMD_NAV first
+uint8_t AP_Mission::get_next_cmd_index(uint8_t curr_cmd_index, mission_cmd_type cmd_type)
+{
+    // To-Do: actually implement this function!
+    return curr_cmd_index+1;
+}
+
+
+/////////////// OLD STUFF ///////////////////////
+/*
+void AP_Mission::init_commands()
+{
+    uint8_t tmp_index=_find_nav_index(1);
+    change_waypoint_index(tmp_index);
+    _prev_index_overriden= false;
+    _mission_status = true;
+}
+
+bool AP_Mission::increment_waypoint_index()
+{
+    //Check if the current and after waypoint is home, if so the mission is complete.
+    if (_index[1] == 0 && _index[2] == 0) {
+        _mission_status = false;
+        return false;
+    }
+
+    _index[0]=_index[1];
+    _prev_index_overriden= false;
+
+    if (_sync_waypoint_index(_index[2])) {
+        _mission_status = true;
+        return true;
+    } else {
+        _mission_status = false;
+        return false;
+    }
+}
+
+bool AP_Mission::change_waypoint_index(uint8_t new_index)
+{
+    //Current index is requested, no change.
+    if(new_index == _index[1]) {
+        return false;
+    }
+
+    //Home is requested.
+    if(new_index == 0) {
+        goto_home();
+        return true;
+    }
+
+    Location tmp=get_cmd_with_index(new_index);
+    if(_check_nav_valid(tmp)) {
+        if(_sync_waypoint_index(new_index)) {
+            _nav_waypoints[0]=_current_loc;
+            _prev_index_overriden = true;
+            _mission_status = true;
+            return true;
+        }
+    }
+    return false;
+}
+
+bool AP_Mission::get_new_cmd(struct Location &new_CMD)
+{
+    struct Location temp;
+    temp = get_cmd_with_index(_cmd_index);
+
+    if(temp.id <= MAV_CMD_NAV_LAST || _prev_index_overriden) {
+        return false;                       //no more commands for this leg
+    } else {
+
+        //  This code is required to properly handle when there is a
+        //   conditional command prior to a DO_JUMP
+        if(temp.id == MAV_CMD_DO_JUMP && (temp.lat > 0 || temp.lat == -1)) {
+            uint8_t old_cmd_index = _cmd_index;
+
+            if(change_waypoint_index(temp.p1)) {
+                if( temp.lat > 0) {
+                    temp.lat--;
+                    temp.lat=constrain_int16(temp.lat, 0, 100);
+                    set_cmd_with_index(temp, old_cmd_index);
+                }
+            } else { //Waypoint is already current, or do_jump was invalid.
+                _cmd_index++;
+                return false;
+            }
+        }
+
+        new_CMD=temp;
+        _cmd_index++;
+        return true;
+    }
+}
+
+//---------------------Utility Methods------------------------------
+
+void AP_Mission::set_command_total(uint8_t max_index)
+{
+    _cmd_max=max_index;
+    _cmd_max.set_and_save(max_index);
+}
+
+void AP_Mission::set_home(const struct Location &home)
+{
+    _home=home;
+    set_cmd_with_index(_home,0);
+}
+
+bool AP_Mission::_sync_waypoint_index(const uint8_t &new_index)
+{
+    Location tmp=get_cmd_with_index(new_index);
+    if (new_index <= _cmd_max) {
+
+        if (_check_nav_valid(tmp)) {
+
+            _index[0]=_index[1];
+
+            if (new_index == _cmd_max) { //The last waypoint in msn was requested.
+                _index[1]=_cmd_max;
+                _index[2]=0;
+            } else if (new_index == 0) { //Home was requested.
+                _index[1]=0;
+                _index[2]=0;
+            } else {
+                _index[1]= new_index;
+                _index[2]=_find_nav_index(_index[1]+1);
+            }
+
+            _cmd_index=_index[0]+1; //Reset command index to read commands associated with current mission leg.
+
+            _sync_nav_waypoints();
+            return true;
+        }
+    }
+    return false;
+}
+
+void AP_Mission::_sync_nav_waypoints(){
+    //TODO: this could be optimimzed by making use of the fact some waypoints are already loaded.
+    for(int i=0; i<3; i++) {
+        _nav_waypoints[i]=get_cmd_with_index(_index[i]);
+
+        //Special handling for home, to ensure waypoint handed to vehicle is not 0 ft AGL.
+        if(_index[i] == 0 && _nav_waypoints[i].id != MAV_CMD_NAV_LAND) {
+            _safe_home(_nav_waypoints[i]);
+        }
+    }
+}
+
+void AP_Mission::_safe_home(struct Location &safe_home){ 
+    
+    safe_home     = _home;
+    safe_home.id  = MAV_CMD_NAV_LOITER_UNLIM;
+    safe_home.alt = get_home_hold_alt();
+}
+
+//
+//  Search for the next navigation index in the stack.  Designed to only return a valid index.
+//
+uint8_t AP_Mission::_find_nav_index(uint8_t search_index)
+{
+    Location tmp;
+    bool condition_cmd=false;
+
+    while(search_index <= _cmd_max && search_index >= 0) {
+
+        tmp = get_cmd_with_index(search_index);
+
+        //check to see if there is a condition command.
+        //don't want to do_jump before the condition command is executed.
+        if (tmp.id > MAV_CMD_NAV_LAST && tmp.id < MAV_CMD_CONDITION_LAST) {
+            condition_cmd=true;
+        }
+
+        //if there is a do_jump without a condition command preceding it, jump now.
+        if (tmp.id == MAV_CMD_DO_JUMP && !condition_cmd) {
+            if(tmp.p1 <= command_total() && (tmp.lat > 0 || tmp.lat == -1) ) {
+                Location tmp_jump_to;
+                tmp_jump_to=get_cmd_with_index(tmp.p1);
+
+                if (_check_nav_valid(tmp_jump_to)) {
+                    if (tmp.lat > 0) {
+                        tmp.lat--;
+                        set_cmd_with_index(tmp, search_index);
+                    }
+                    return tmp.p1;
+                }
+            }
+        }
+
+        //otherwise, if we come across a nav command, just pass that index along.
+        if (_check_nav_valid(tmp)) {
+            return search_index;
+        }
+        search_index++;
+    }
+    return 0;
+}
+
+
+bool AP_Mission::_check_nav_valid(const struct Location &temp)
+{
+    // Other checks could be added such as:
+    //   -Distance and altitude diff, if the new waypoint is hundreds of miles away from others, reject.
+    //   -others?
+    //
+    if (temp.id > MAV_CMD_NAV_LAST) {
+        return false;
+    }
+
+    if ((temp.lat < -900000000 || temp.lat > 900000000) ||
+        (temp.lng < -1800000000 || temp.lng > 1800000000)) {
+        return false;
+    }
+
+    return true;
+}
+*/

libraries/AP_Mission/AP_Mission.h

@@ -0,0 +1,206 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/// @file    AP_Mission.h
+/// @brief   Handles the MAVLINK command mission stack.  Reads and writes mission to storage.
+
+/*
+ *   The AP_Mission library:
+ *   - responsible for managing a list of commands made up of "nav", "do" and "conditional" commands
+ *   - reads and writes the mission commands to storage.
+ *   - provides easy acces to current, previous and upcoming waypoints
+ *   - calls main program's command execution and verify functions.
+ *   - accounts for the DO_JUMP command
+ *
+ */
+#ifndef AP_Mission_h
+#define AP_Mission_h
+
+#include <GCS_MAVLink.h>
+#include <AP_Math.h>
+#include <AP_Common.h>
+#include <AP_Param.h>
+#include <AP_AHRS.h>
+#include <AP_HAL.h>
+
+// definitions
+#define AP_MISSION_EEPROM_MAX_ADDR          4096    // parameters get the first 1536 bytes of EEPROM, remainder is for waypoints
+#define AP_MISSION_EEPROM_START_BYTE        0x600   // where in memory home WP is stored, all mission commands appear afterthis
+#define AP_MISSION_EEPROM_COMMAND_SIZE      15      // size in bytes of all mission commands
+#define AP_MISSION_FENCEPOINTS_MAX          6       // we reserve space for 6 fence points at the end of EEPROM although this is not currently implemented
+#define AP_MISSION_FENCEPOINTS_SIZE         sizeof(Vector2l)    // each fence points size in eeprom
+#define AP_MISSION_FENCE_START_BYTE         (AP_MISSION_EEPROM_MAX_ADDR-(AP_MISSION_FENCEPOINTS_MAX*AP_MISSION_FENCEPOINTS_SIZE))   // space reserved for fence points
+#define AP_MISSION_MAX_COMMANDS             ((AP_MISSION_FENCE_START_BYTE - AP_MISSION_EEPROM_START_BYTE) / AP_MISSION_EEPROM_COMMAND_SIZE) - 1 // -1 to be safe
+
+#define AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS 3       // only allow up to 3 do-jump commands (due to RAM limitations on the APM2)
+
+#define AP_MISSION_CMD_ID_NONE              0       // mavlink cmd id of zero means invalid or missing command
+#define AP_MISSION_CMD_INDEX_NONE           255     // command index of 255 means invalid or missing command
+
+/// @class    AP_Mission
+/// @brief    Object managing Mission
+class AP_Mission {
+
+public:
+
+    // jump command structure
+    struct Jump_Command {
+        uint8_t id;         // mavlink command id
+        uint8_t target;     // DO_JUMP target command id
+        uint8_t num_times;  // DO_JUMP num times to repeat
+    };
+
+    union Content {
+        // jump structure
+        Jump_Command jump;
+
+        // location
+        Location location;      // Waypoint location
+
+        uint8_t bytes[AP_MISSION_EEPROM_COMMAND_SIZE];
+    };
+
+    // command structure
+    struct Mission_Command {
+        uint8_t index;              // this commands position in the command list
+        uint8_t id;                 // mavlink command id
+
+        Content content;
+    };
+
+    // main program function pointers
+    typedef bool (*mission_cmd_fn_t)(const Mission_Command& cmd);
+
+    // mission state enumeration
+    enum mission_state {
+        MISSION_STOPPED=0,
+        MISSION_RUNNING=1,
+        MISSION_COMPLETE=2
+    };
+
+    // mission command type enumeration
+    enum mission_cmd_type {
+        MISSION_CMD_NONE = 0,
+        MISSION_CMD_NAV = 1,
+        MISSION_CMD_DO = 2,
+        MISSION_CMD_COND = 3
+    };
+
+    /// constructor
+    AP_Mission(mission_cmd_fn_t cmd_start_fn, mission_cmd_fn_t cmd_verify_fn) :
+        _cmd_start_fn(cmd_start_fn),
+        _cmd_verify_fn(cmd_verify_fn)
+    {
+        // load parameter defaults
+        AP_Param::setup_object_defaults(this, var_info);
+
+        // clear commands
+        _nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
+        _do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
+
+        // initialise other internal variables
+        _flags.state = MISSION_STOPPED;
+        _flags.nav_cmd_loaded = false;
+        _flags.do_cmd_loaded = false;
+    }
+
+    ///
+    /// mission methods
+    ///
+
+    /// status - returns the status of the mission (i.e. Mission_Started, Mission_Complete, Mission_Stopped
+    mission_state state() const { return _flags.state; }
+
+    /// num_commands - returns total number of commands in the mission
+    uint8_t num_commands() const { return _cmd_total; }
+
+    /// 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 update();
+
+    /// 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 start();
+
+    /// stop - stops mission execution.  subsequent calls to update() will have no effect until the mission is started or resumed
+    void stop();
+
+    /// resume - continues the mission execution from where we last left off
+    ///     previous running commands will be re-initialised
+    void resume();
+
+    /// clear - clears out mission
+    ///     returns true if mission was running so it could not be cleared
+    bool clear();
+
+    /// valid - validate the mission has no errors
+    ///     currently only checks that the number of do-commands does not exceed the AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS
+    bool valid();
+
+    ///
+    /// command methods
+    ///
+
+    /// get_active_nav_cmd - returns the current "navigation" command
+    const Mission_Command& get_current_nav_cmd() const { return _nav_cmd; }
+
+    /// set_current_nav_cmd - sets the current "navigation" command to the command number
+    ///     returns true if successful, false on failure (i.e. if the index does not refer to a navigation command)
+    ///     current do and conditional commands will also be modified
+    bool set_current_nav_cmd(uint8_t index);
+
+    /// advance_current_nav_cmd - moves current nav command forward
+    ///     do command will also be loaded
+    ///     accounts for do-jump commands
+    bool advance_current_nav_cmd(uint8_t start_index);
+
+    /// get_next_nav_cmd - returns the next navigation command
+    ///     offset parameter controls how many commands forward we should look.  Defaults to 1 meaning the very next nav command
+    //bool load_next_nav_cmd(Mission_Command& cmd, uint8_t offset=1) const;
+
+    /// get_active_do_cmd - returns active "do" command
+    const Mission_Command& get_current_do_cmd() const { return _do_cmd; }
+
+    /// 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 add_cmd(Mission_Command& cmd);
+
+    /// load_cmd_from_storage - load command from storage
+    ///     true is return if successful
+    bool read_cmd_from_storage(uint8_t index, Mission_Command& cmd) const;
+
+    /// write_cmd_to_storage - write a command to storage
+    ///     cmd.index is used to calculate the storage location
+    ///     true is returned if successful
+    bool write_cmd_to_storage(uint8_t index, Mission_Command& cmd);
+
+    // user settable parameters
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+
+    struct Mission_Flags {
+        mission_state state;
+        uint8_t nav_cmd_loaded  : 1; // true if a "navigation" command has been loaded into _nav_cmd
+        uint8_t do_cmd_loaded   : 1; // true if a "do"/"conditional" command has been loaded into _do_cmd
+    } _flags;
+
+    /// get_next_cmd_index - gets next command after curr_cmd_index which is of type cmd_type
+    ///     returns MISSION_CMD_NONE if no command is found
+    ///     accounts for do_jump commands
+    ///     if requesting MISSION_CMD_DO or MISSION_CMD_COND it will stop and return MISSION_CMD_NONE if it hits a MISSION_CMD_NAV first
+    uint8_t get_next_cmd_index(uint8_t curr_cmd_index, mission_cmd_type cmd_type);
+
+    // parameters
+    AP_Int16                _cmd_total; // total number of commands in the mission
+
+    // pointer to main program functions
+    mission_cmd_fn_t        _cmd_start_fn;  // pointer to function which will be called when a new command is started
+    mission_cmd_fn_t        _cmd_verify_fn; // pointer to function which will be called repeatedly to ensure a command is progressing
+
+    // internal variables
+    struct Mission_Command  _nav_cmd;   // current "navigation" command.  It's position in the command list is held in _nav_cmd.index
+    struct Mission_Command  _do_cmd;    // current "do" command.  It's position in the command list is held in _do_cmd.index
+};
+
+#endif

libraries/AP_Mission/examples/AP_Mission_test/AP_Mission_test.pde

@@ -0,0 +1,177 @@
+/*
+ *  Example of AP_Mission Library.
+ *  DIYDrones.com
+ */
+
+#include <AP_Common.h>
+#include <AP_Progmem.h>
+#include <AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
+#include <AP_Curve.h>           // Curve used to linearlise throttle pwm to thrust
+#include <AP_Param.h>
+#include <AP_HAL.h>
+#include <AP_HAL_AVR.h>
+#include <GCS_MAVLink.h>
+#include <AP_Notify.h>
+#include <AP_Vehicle.h>
+#include <DataFlash.h>
+#include <AP_Mission.h>
+#include <AP_GPS.h>             // ArduPilot GPS library
+#include <AP_GPS_Glitch.h>      // GPS glitch protection library
+#include <AP_ADC.h>             // ArduPilot Mega Analog to Digital Converter Library
+#include <AP_ADC_AnalogSource.h>
+#include <AP_Baro.h>            // ArduPilot Mega Barometer Library
+#include <Filter.h>
+#include <AP_Compass.h>         // ArduPilot Mega Magnetometer Library
+#include <AP_Declination.h>
+#include <AP_InertialSensor.h>  // ArduPilot Mega Inertial Sensor (accel & gyro) Library
+#include <AP_AHRS.h>
+#include <AP_Airspeed.h>
+#include <AP_Buffer.h>          // ArduPilot general purpose FIFO buffer
+#include <GCS_MAVLink.h>
+
+const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
+
+// start_cmd - function that is called when new command is started
+//      should return true if command is successfully started
+bool start_cmd(const AP_Mission::Mission_Command& cmd)
+{
+    hal.console->printf_P(PSTR("started cmd #%d id:%d\n"),(int)cmd.index, (int)cmd.id);
+    return true;
+}
+
+// verify_mcd - function that is called repeatedly to ensure a command is progressing
+//      should return true once command is completed
+bool verify_cmd(const AP_Mission::Mission_Command& cmd)
+{
+    return true;
+}
+
+// declaration
+AP_Mission mission(&start_cmd, &verify_cmd);
+
+// setup
+void setup(void)
+{
+    hal.console->println("AP_Mission library test\n");
+
+    // display basic info about command sizes
+    hal.console->printf_P(PSTR("Max Num Commands: %d\n"),(int)AP_MISSION_MAX_COMMANDS);
+    hal.console->printf_P(PSTR("Command size: %d bytes\n"),(int)AP_MISSION_EEPROM_COMMAND_SIZE);
+    hal.console->printf_P(PSTR("Command start in Eeprom: %x\n"),(int)AP_MISSION_EEPROM_START_BYTE);
+}
+
+// loop
+void loop(void)
+{
+    // initialise mission
+    init_mission();
+
+    // print current mission
+    print_mission();
+
+    // start mission
+    hal.console->printf_P(PSTR("\nRunning missions\n"));
+    mission.start();
+
+    // update mission until it completes
+    while(!mission.state() != AP_Mission::MISSION_COMPLETE) {
+        mission.update();
+    }
+    hal.console->printf_P(PSTR("Mission Complete!\n"));
+
+    // wait forever
+    while(true);
+}
+
+// init_mission - initialise the mission to hold something
+void init_mission()
+{
+    AP_Mission::Mission_Command cmd;
+
+    // clear mission
+    mission.clear();
+
+    // Command #0 : take-off to 10m
+    cmd.id = MAV_CMD_NAV_TAKEOFF;
+    cmd.content.location.options = 0;
+    cmd.content.location.p1 = 0;
+    cmd.content.location.alt = 10;
+    cmd.content.location.lat = 0;
+    cmd.content.location.lng = 0;
+    if (!mission.add_cmd(cmd)) {
+        hal.console->printf_P(PSTR("failed to add command\n"));
+    }
+
+    // Command #1 : first waypoint
+    cmd.id = MAV_CMD_NAV_WAYPOINT;
+    cmd.content.location.options = 0;
+    cmd.content.location.p1 = 0;
+    cmd.content.location.alt = 11;
+    cmd.content.location.lat = 12345678;
+    cmd.content.location.lng = 23456789;
+    if (!mission.add_cmd(cmd)) {
+        hal.console->printf_P(PSTR("failed to add command\n"));
+    }
+
+    // Command #2 : second waypoint
+    cmd.id = MAV_CMD_NAV_WAYPOINT;
+    cmd.content.location.p1 = 0;
+    cmd.content.location.lat = 1234567890;
+    cmd.content.location.lng = -1234567890;
+    cmd.content.location.alt = 22;
+    if (!mission.add_cmd(cmd)) {
+        hal.console->printf_P(PSTR("failed to add command\n"));
+    }
+
+    // Command #3 : do-jump to first waypoint 3 times
+    cmd.id = MAV_CMD_DO_JUMP;
+    cmd.content.jump.target = 1;
+    cmd.content.jump.num_times = 3;
+    if (!mission.add_cmd(cmd)) {
+        hal.console->printf_P(PSTR("failed to add command\n"));
+    }
+    cmd.index = 3;
+
+    // add RTL
+    cmd.id = MAV_CMD_NAV_RETURN_TO_LAUNCH;
+    cmd.content.location.p1 = 0;
+    cmd.content.location.lat = 0;
+    cmd.content.location.lng = 0;
+    cmd.content.location.alt = 0;
+    if (!mission.add_cmd(cmd)) {
+        hal.console->printf_P(PSTR("failed to add command\n"));
+    }
+}
+
+// print_mission - print out the entire mission to the console
+void print_mission()
+{
+    AP_Mission::Mission_Command cmd;
+
+    // check for empty mission
+    if (mission.num_commands() == 0) {
+        hal.console->printf_P(PSTR("No Mission!\n"));
+        return;
+    }
+
+    hal.console->printf_P(PSTR("Mission: %d commands\n"),(int)mission.num_commands());
+
+    // print each command
+    for(uint16_t i=0; i<mission.num_commands(); i++) {
+        // get next command from eeprom
+        mission.read_cmd_from_storage(i,cmd);
+
+        // print command position in list and mavlink id
+        hal.console->printf_P(PSTR("Cmd#%d mav-id:%d "), (int)cmd.index, (int)cmd.id);
+
+        // print command contents
+        if (cmd.id == MAV_CMD_DO_JUMP) {
+            hal.console->printf_P(PSTR("jump-to:%d num_times:%d\n"), (int)cmd.content.jump.target, (int)cmd.content.jump.num_times);
+        }else{
+            hal.console->printf_P(PSTR("p1:%d lat:%ld lng:%ld alt:%ld\n"),(int)cmd.content.location.p1, (long)cmd.content.location.lat, (long)cmd.content.location.lng, (long)cmd.content.location.alt);
+        }
+    }
+    hal.console->printf_P(PSTR("--------\n"));
+}
+
+AP_HAL_MAIN();