GCS_MAVLink: factor mission handling, use for Rally

GCS_MAVLink: allow all mission types to be cleared

GCS_MAVLink: remove unused parameters from handle-mission-count

libraries/GCS_MAVLink/GCS.cpp

@@ -14,6 +14,14 @@ void GCS::get_sensor_status_flags(uint32_t &present,
     health = control_sensors_health;
 }
 
+MissionItemProtocol_Waypoints *GCS::_missionitemprotocol_waypoints;
+MissionItemProtocol_Rally *GCS::_missionitemprotocol_rally;
+
+const MAV_MISSION_TYPE GCS_MAVLINK::supported_mission_types[] = {
+    MAV_MISSION_TYPE_MISSION,
+    MAV_MISSION_TYPE_RALLY,
+};
+
 /*
   send a text message to all GCS
  */

libraries/GCS_MAVLink/GCS.h

@@ -18,6 +18,7 @@
 #include <AP_Common/Bitmask.h>
 #include <AP_Devo_Telem/AP_Devo_Telem.h>
 #include <RC_Channel/RC_Channel.h>
+#include <AP_Rally/AP_Rally.h>
 
 #define GCS_DEBUG_SEND_MESSAGE_TIMINGS 0
 
@@ -58,7 +59,8 @@ enum ap_message : uint8_t {
     MSG_GPS2_RTK,
     MSG_SYSTEM_TIME,
     MSG_SERVO_OUT,
-    MSG_NEXT_MISSION_REQUEST,
+    MSG_NEXT_MISSION_REQUEST_WAYPOINTS,
+    MSG_NEXT_MISSION_REQUEST_RALLY,
     MSG_NEXT_PARAM,
     MSG_FENCE_STATUS,
     MSG_AHRS,
@@ -107,6 +109,183 @@ enum ap_message : uint8_t {
     }
 #define MAV_STREAM_TERMINATOR { (streams)0, nullptr, 0 }
 
+// MissionItemProtocol objects are used for transfering missions from
+// a GCS to ArduPilot and vice-versa.
+//
+// There exists one MissionItemProtocol instance for each of the types
+// of item that might be transfered - e.g. MissionItemProtocol_Rally
+// for rally point uploads.  These objects are static in GCS_MAVLINK
+// and used by all of the backends.
+//
+// While prompting the GCS for items required to complete the mission,
+// a link is stored to the link the MissionItemProtocol should send
+// requests out on, and the "receiving" boolean is true.  In this
+// state downloading of items (and the item count!) is blocked.
+// Starting of uploads (for the same protocol) is also blocked -
+// essentially the GCS uploading a set of items (e.g. a mission) has a
+// mutex over the mission.
+class MissionItemProtocol
+{
+public:
+
+    // note that all of these methods are named after the packet they
+    // are handling; the "mission" part just comes as part of that.
+    void handle_mission_request_list(const GCS_MAVLINK &link,
+                                     const mavlink_mission_request_list_t &packet,
+                                     const mavlink_message_t &msg);
+    void handle_mission_request_int(const GCS_MAVLINK &link,
+                                    const mavlink_mission_request_int_t &packet,
+                                    const mavlink_message_t &msg);
+    void handle_mission_request(const GCS_MAVLINK &link,
+                                const mavlink_mission_request_t &packet,
+                                const mavlink_message_t &msg);
+
+    void handle_mission_count(class GCS_MAVLINK &link,
+                              const mavlink_mission_count_t &packet,
+                              const mavlink_message_t &msg);
+    void handle_mission_write_partial_list(GCS_MAVLINK &link,
+                                           const mavlink_message_t &msg,
+                                           const mavlink_mission_write_partial_list_t &packet);
+
+    // called on receipt of a MISSION_ITEM or MISSION_ITEM_INT packet;
+    // the former is converted to the latter.
+    void handle_mission_item(const mavlink_message_t &msg,
+                             const mavlink_mission_item_int_t &cmd);
+
+    void handle_mission_clear_all(const GCS_MAVLINK &link,
+                                  const mavlink_message_t &msg);
+
+    void queued_request_send();
+    void update();
+
+    bool active_link_is(const GCS_MAVLINK *_link) const { return _link == link; };
+
+    virtual MAV_MISSION_TYPE mission_type() const = 0;
+
+    bool receiving; // currently sending requests and expecting items
+
+protected:
+
+    GCS_MAVLINK *link; // link currently receiving waypoints on
+
+    // return the ap_message which can be queued to be sent to send a
+    // item request to the GCS:
+    virtual ap_message next_item_ap_message_id() const = 0;
+
+    virtual bool clear_all_items() = 0;
+
+    uint16_t        request_last; // last request index
+
+private:
+
+    virtual void truncate(const mavlink_mission_count_t &packet) = 0;
+
+    uint16_t        request_i; // request index
+
+    // waypoints
+    uint8_t         dest_sysid;  // where to send requests
+    uint8_t         dest_compid; // "
+    uint32_t        timelast_receive_ms;
+    uint32_t        timelast_request_ms;
+    const uint16_t  upload_timeout_ms = 8000;
+
+    // support for GCS getting waypoints etc from us:
+    virtual MAV_MISSION_RESULT get_item(const GCS_MAVLINK &_link,
+                                        const mavlink_message_t &msg,
+                                        const mavlink_mission_request_int_t &packet,
+                                        mavlink_mission_item_int_t &ret_packet) = 0;
+
+    void init_send_requests(GCS_MAVLINK &_link,
+                            const mavlink_message_t &msg,
+                            const int16_t _request_first,
+                            const int16_t _request_last);
+
+    void send_mission_ack(const mavlink_message_t &msg, MAV_MISSION_RESULT result) const;
+    void send_mission_ack(const GCS_MAVLINK &link, const mavlink_message_t &msg, MAV_MISSION_RESULT result) const;
+
+    virtual uint16_t item_count() const = 0;
+    virtual uint16_t max_items() const = 0;
+
+    virtual MAV_MISSION_RESULT replace_item(const mavlink_mission_item_int_t &mission_item_int) = 0;
+    virtual MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t &mission_item_int) = 0;
+
+    virtual void complete(const GCS_MAVLINK &_link) {};
+    virtual void timeout() {};
+
+    void convert_MISSION_REQUEST_to_MISSION_REQUEST_INT(const mavlink_mission_request_t &request,
+                                                        mavlink_mission_request_int_t &request_int);
+};
+
+class MissionItemProtocol_Waypoints : public MissionItemProtocol {
+public:
+    MissionItemProtocol_Waypoints(AP_Mission &_mission) :
+        mission(_mission) {}
+    void truncate(const mavlink_mission_count_t &packet) override;
+    MAV_MISSION_TYPE mission_type() const override { return MAV_MISSION_TYPE_MISSION; }
+
+    void complete(const GCS_MAVLINK &_link) override;
+    void timeout() override;
+
+protected:
+
+    bool clear_all_items() override WARN_IF_UNUSED;
+
+    ap_message next_item_ap_message_id() const override {
+        return MSG_NEXT_MISSION_REQUEST_WAYPOINTS;
+    }
+
+private:
+    AP_Mission &mission;
+
+    uint16_t item_count() const override { return mission.num_commands(); }
+    uint16_t max_items() const override { return mission.num_commands_max(); }
+
+    MAV_MISSION_RESULT replace_item(const mavlink_mission_item_int_t &) override WARN_IF_UNUSED;
+    MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t &) override WARN_IF_UNUSED;
+
+    MAV_MISSION_RESULT get_item(const GCS_MAVLINK &_link,
+                                const mavlink_message_t &msg,
+                                const mavlink_mission_request_int_t &packet,
+                                mavlink_mission_item_int_t &ret_packet) override WARN_IF_UNUSED;
+};
+
+class MissionItemProtocol_Rally : public MissionItemProtocol {
+public:
+    MissionItemProtocol_Rally(AP_Rally &_rally) :
+        rally(_rally) {}
+    void truncate(const mavlink_mission_count_t &packet) override;
+    MAV_MISSION_TYPE mission_type() const override { return MAV_MISSION_TYPE_RALLY; }
+
+    void complete(const GCS_MAVLINK &_link) override;
+    void timeout() override;
+
+protected:
+
+    ap_message next_item_ap_message_id() const override {
+        return MSG_NEXT_MISSION_REQUEST_RALLY;
+    }
+    bool clear_all_items() override WARN_IF_UNUSED;
+
+private:
+    AP_Rally &rally;
+
+    uint16_t item_count() const override {
+        return rally.get_rally_total();
+    }
+    uint16_t max_items() const override { return rally.get_rally_max(); }
+
+    MAV_MISSION_RESULT replace_item(const mavlink_mission_item_int_t&) override WARN_IF_UNUSED;
+    MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t&) override WARN_IF_UNUSED;
+
+    MAV_MISSION_RESULT get_item(const GCS_MAVLINK &_link,
+                                const mavlink_message_t &msg,
+                                const mavlink_mission_request_int_t &packet,
+                                mavlink_mission_item_int_t &ret_packet) override WARN_IF_UNUSED;
+
+    static MAV_MISSION_RESULT convert_MISSION_ITEM_INT_to_RallyLocation(const mavlink_mission_item_int_t &cmd, RallyLocation &ret) WARN_IF_UNUSED;
+
+};
+
 ///
 /// @class	GCS_MAVLINK
 /// @brief	MAVLink transport control class
@@ -125,6 +304,22 @@ public:
     void        send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list) const;
     void        queued_param_send();
     void        queued_mission_request_send();
+
+    // returns true if we are requesting any items from the GCS:
+    bool requesting_mission_items() const;
+
+    void send_mission_ack(const mavlink_message_t &msg,
+                          MAV_MISSION_TYPE mission_type,
+                          MAV_MISSION_RESULT result) const {
+        mavlink_msg_mission_ack_send(chan,
+                                     msg.sysid,
+                                     msg.compid,
+                                     result,
+                                     mission_type);
+    }
+
+    static const MAV_MISSION_TYPE supported_mission_types[2];
+
     // packetReceived is called on any successful decode of a mavlink message
     virtual void packetReceived(const mavlink_status_t &status,
                                 mavlink_message_t &msg);
@@ -286,6 +481,7 @@ public:
     static const struct stream_entries all_stream_entries[];
 
     virtual uint64_t capabilities() const;
+    uint8_t get_stream_slowdown_ms() const { return stream_slowdown_ms; }
 
 protected:
 
@@ -307,11 +503,6 @@ protected:
     virtual MAV_VTOL_STATE vtol_state() const { return MAV_VTOL_STATE_UNDEFINED; }
     virtual MAV_LANDED_STATE landed_state() const { return MAV_LANDED_STATE_UNDEFINED; }
 
-    bool            waypoint_receiving; // currently receiving
-    // the following two variables are only here because of Tracker
-    uint16_t        waypoint_request_i; // request index
-    uint16_t        waypoint_request_last; // last request index
-
     AP_Param *                  _queued_parameter;      ///< next parameter to
                                                         // be sent in queue
     mavlink_channel_t           chan;
@@ -335,13 +526,14 @@ protected:
 
     MAV_RESULT handle_command_do_set_home(const mavlink_command_long_t &packet);
 
-    void handle_mission_request_list(AP_Mission &mission, mavlink_message_t *msg);
+    void handle_mission_request_list(const mavlink_message_t *msg);
-    void handle_mission_request(AP_Mission &mission, mavlink_message_t *msg);
+    void handle_mission_request(mavlink_message_t *msg);
-    void handle_mission_clear_all(AP_Mission &mission, mavlink_message_t *msg);
+    void handle_mission_request_int(mavlink_message_t *msg);
+    void handle_mission_clear_all(const mavlink_message_t *msg);
     virtual void handle_mission_set_current(AP_Mission &mission, mavlink_message_t *msg);
-    void handle_mission_count(AP_Mission &mission, mavlink_message_t *msg);
+    void handle_mission_count(const mavlink_message_t *msg);
-    void handle_mission_write_partial_list(AP_Mission &mission, mavlink_message_t *msg);
+    void handle_mission_write_partial_list(const mavlink_message_t *msg);
-    bool handle_mission_item(mavlink_message_t *msg, AP_Mission &mission);
+    void handle_mission_item(const mavlink_message_t *msg);
 
     void handle_common_param_message(mavlink_message_t *msg);
     void handle_param_set(mavlink_message_t *msg);
@@ -493,13 +685,6 @@ private:
     ///
     uint16_t                    packet_drops;
 
-    // waypoints
-    uint16_t        waypoint_dest_sysid; // where to send requests
-    uint16_t        waypoint_dest_compid; // "
-    uint32_t        waypoint_timelast_receive; // milliseconds
-    uint32_t        waypoint_timelast_request; // milliseconds
-    const uint16_t  waypoint_receive_timeout = 8000; // milliseconds
-
     // number of extra ms to add to slow things down for the radio
     uint16_t         stream_slowdown_ms;
 
@@ -754,6 +939,11 @@ public:
                               ap_var_type param_type,
                               float param_value);
 
+    static MissionItemProtocol_Waypoints *_missionitemprotocol_waypoints;
+    static MissionItemProtocol_Rally *_missionitemprotocol_rally;
+    MissionItemProtocol *get_prot_for_mission_type(const MAV_MISSION_TYPE mission_type) const;
+    void try_send_queued_message_for_type(MAV_MISSION_TYPE type);
+
     void update_send();
     void update_receive();
     virtual void setup_uarts(AP_SerialManager &serial_manager);
@@ -822,6 +1012,9 @@ private:
     // true if we are running short on time in our main loop
     bool _out_of_time;
 
+    // true if we have already allocated protocol objects:
+    bool initialised_missionitemprotocol_objects;
+
     // handle passthru between two UARTs
     struct {
         bool enabled;

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -179,19 +179,71 @@ GCS_MAVLINK::setup_uart(const AP_SerialManager& serial_manager, AP_SerialManager
  * @brief Send the next pending waypoint, called from deferred message
  * handling code
  */
-void
+void MissionItemProtocol::queued_request_send()
-GCS_MAVLINK::queued_mission_request_send()
 {
-    if (initialised &&
+    if (!receiving) {
-        waypoint_receiving &&
+        return;
-        waypoint_request_i <= waypoint_request_last) {
+    }
-        mavlink_msg_mission_request_send(
+    if (request_i > request_last) {
-            chan,
+        return;
-            waypoint_dest_sysid,
+    }
-            waypoint_dest_compid,
+    if (link == nullptr) {
-            waypoint_request_i,
+        AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
-            MAV_MISSION_TYPE_MISSION);
+        return;
     }
+    mavlink_msg_mission_request_send(
+        link->get_chan(),
+        dest_sysid,
+        dest_compid,
+        request_i,
+        mission_type());
+    timelast_request_ms = AP_HAL::millis();
+}
+
+void MissionItemProtocol::update()
+{
+    if (!receiving) {
+        // we don't need to do anything unless we're sending requests
+        return;
+    }
+    if (link == nullptr) {
+        AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
+        return;
+    }
+    // stop waypoint receiving if timeout
+    const uint32_t tnow = AP_HAL::millis();
+    if (tnow - timelast_receive_ms > upload_timeout_ms) {
+        receiving = false;
+        timeout();
+        link = nullptr;
+        return;
+    }
+    // resend request if we haven't gotten one:
+    const uint32_t wp_recv_timeout_ms = 1000U + (link->get_stream_slowdown_ms()*20);
+    if (tnow - timelast_request_ms > wp_recv_timeout_ms) {
+        timelast_request_ms = tnow;
+        link->send_message(next_item_ap_message_id());
+    }
+}
+
+void MissionItemProtocol::send_mission_ack(const mavlink_message_t &msg,
+                                           MAV_MISSION_RESULT result) const
+{
+    if (link == nullptr) {
+        AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
+        return;
+    }
+    send_mission_ack(*link, msg, result);
+}
+void MissionItemProtocol::send_mission_ack(const GCS_MAVLINK &_link,
+                                           const mavlink_message_t &msg,
+                                           MAV_MISSION_RESULT result) const
+{
+    mavlink_msg_mission_ack_send(_link.get_chan(),
+                                 msg.sysid,
+                                 msg.compid,
+                                 result,
+                                 mission_type());
 }
 
 void GCS_MAVLINK::send_meminfo(void)
@@ -439,44 +491,135 @@ void GCS_MAVLINK::send_ahrs3()
 #endif
 }
 
-/*
+MissionItemProtocol *GCS::get_prot_for_mission_type(const MAV_MISSION_TYPE mission_type) const
-  handle a MISSION_REQUEST_LIST mavlink packet
+{
- */
+    switch (mission_type) {
-void GCS_MAVLINK::handle_mission_request_list(AP_Mission &mission, mavlink_message_t *msg)
+    case MAV_MISSION_TYPE_MISSION:
+        return _missionitemprotocol_waypoints;
+    case MAV_MISSION_TYPE_RALLY:
+        return _missionitemprotocol_rally;
+    default:
+        return nullptr;
+    }
+}
+
+// handle a request for the number of items we have stored for a mission type:
+void GCS_MAVLINK::handle_mission_request_list(const mavlink_message_t *msg)
 {
     // decode
     mavlink_mission_request_list_t packet;
     mavlink_msg_mission_request_list_decode(msg, &packet);
 
-    // reply with number of commands in the mission.  The GCS will then request each command separately
+    MissionItemProtocol *prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type);
-    mavlink_msg_mission_count_send(chan,msg->sysid, msg->compid, mission.num_commands(),
+    if (prot == nullptr) {
-                                   MAV_MISSION_TYPE_MISSION);
+        mavlink_msg_mission_ack_send(chan,
+                                     msg->sysid,
+                                     msg->compid,
+                                     MAV_MISSION_UNSUPPORTED,
+                                     packet.mission_type);
+        return;
+    }
+
+    prot->handle_mission_request_list(*this, packet, *msg);
+}
+
+void MissionItemProtocol::handle_mission_request_list(
+    const GCS_MAVLINK &_link,
+    const mavlink_mission_request_list_t &packet,
+    const mavlink_message_t &msg)
+{
+    if (receiving) {
+        // someone is uploading a mission; reject fetching of points
+        // until done or timeout
+        send_mission_ack(_link, msg, MAV_MISSION_DENIED);
+        return;
+    }
+
+    // reply with number of commands in the mission.  The GCS will
+    // then request each command separately
+    mavlink_msg_mission_count_send(_link.get_chan(),
+                                   msg.sysid,
+                                   msg.compid,
+                                   item_count(),
+                                   mission_type());
+}
+
+void MissionItemProtocol::handle_mission_request_int(const GCS_MAVLINK &_link,
+                                                     const mavlink_mission_request_int_t &packet,
+                                                     const mavlink_message_t &msg)
+{
+    if (receiving) {
+        // someone is uploading a mission; reject fetching of points
+        // until done or timeout
+        send_mission_ack(_link, msg, MAV_MISSION_DENIED);
+        return;
+    }
+
+    mavlink_mission_item_int_t ret_packet{};
+
+    ret_packet.target_system = msg.sysid;
+    ret_packet.target_component = msg.compid;
+    ret_packet.seq = packet.seq;
+    ret_packet.mission_type = packet.mission_type;
+
+    const MAV_MISSION_RESULT result_code = get_item(_link, msg, packet, ret_packet);
+
+    if (result_code != MAV_MISSION_ACCEPTED) {
+        // send failure message
+        send_mission_ack(_link, msg, result_code);
+        return;
+    }
 
-    // set variables to help handle the expected sending of commands to the GCS
+    // we already have a filled structure, use it in place of _send:
-    waypoint_receiving = false;             // record that we are sending commands (i.e. not receiving)
+    _mav_finalize_message_chan_send(_link.get_chan(),
+                                    MAVLINK_MSG_ID_MISSION_ITEM_INT,
+                                    (const char *)&ret_packet,
+                                    MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN,
+                                    MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN,
+                                    MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC);
 }
 
 /*
   handle a MISSION_REQUEST mavlink packet
  */
-void GCS_MAVLINK::handle_mission_request(AP_Mission &mission, mavlink_message_t *msg)
+void GCS_MAVLINK::handle_mission_request_int(mavlink_message_t *msg)
 {
-    AP_Mission::Mission_Command cmd;
-
-    if (msg->msgid == MAVLINK_MSG_ID_MISSION_REQUEST_INT) {  
         // decode
         mavlink_mission_request_int_t packet;
         mavlink_msg_mission_request_int_decode(msg, &packet);
 
+        MissionItemProtocol *prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type);
+        if (prot == nullptr) {
+            return;
+        }
+        prot->handle_mission_request_int(*this, packet, *msg);
+}
+
+MAV_MISSION_RESULT MissionItemProtocol_Waypoints::get_item(const GCS_MAVLINK &_link,
+                                                           const mavlink_message_t &msg,
+                                                           const mavlink_mission_request_int_t &packet,
+                                                           mavlink_mission_item_int_t &ret_packet)
+{
+    if (packet.seq != 0 && // always allow HOME to be read
+        packet.seq >= mission.num_commands()) {
+        // try to educate the GCS on the actual size of the mission:
+        mavlink_msg_mission_count_send(_link.get_chan(),
+                                       msg.sysid,
+                                       msg.compid,
+                                       mission.num_commands(),
+                                       MAV_MISSION_TYPE_MISSION);
+        return MAV_MISSION_ERROR;
+    }
+
+    AP_Mission::Mission_Command cmd;
+
         // retrieve mission from eeprom
         if (!mission.read_cmd_from_storage(packet.seq, cmd)) {
-            goto mission_item_send_failed;
+            return MAV_MISSION_ERROR;
         }
 
-        mavlink_mission_item_int_t ret_packet;
-        memset(&ret_packet, 0, sizeof(ret_packet));
         if (!AP_Mission::mission_cmd_to_mavlink_int(cmd, ret_packet)) {
-            goto mission_item_send_failed;
+            return MAV_MISSION_ERROR;
         }
 
         // set packet's current field to 1 if this is the command being executed
@@ -489,80 +632,59 @@ void GCS_MAVLINK::handle_mission_request(AP_Mission &mission, mavlink_message_t
         // set auto continue to 1
         ret_packet.autocontinue = 1;     // 1 (true), 0 (false)
 
-        /*
-          avoid the _send() function to save memory, as it avoids
-          the stack usage of the _send() function by using the already
-          declared ret_packet above
-         */
-        ret_packet.target_system = msg->sysid;
-        ret_packet.target_component = msg->compid;
-        ret_packet.seq = packet.seq;
         ret_packet.command = cmd.id;
 
-        _mav_finalize_message_chan_send(chan, 
+        return MAV_MISSION_ACCEPTED;
-                                        MAVLINK_MSG_ID_MISSION_ITEM_INT,
+}
-                                        (const char *)&ret_packet,
+
-                                        MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN,
+void GCS_MAVLINK::handle_mission_request(mavlink_message_t *msg)
-                                        MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN,
+{
-                                        MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC);
-    } else {
         // decode
         mavlink_mission_request_t packet;
         mavlink_msg_mission_request_decode(msg, &packet);
 
-        if (packet.seq != 0 && // always allow HOME to be read
+        MissionItemProtocol *prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type);
-            packet.seq >= mission.num_commands()) {
+        if (prot == nullptr) {
-            // try to educate the GCS on the actual size of the mission:
+            return;
-            mavlink_msg_mission_count_send(chan,msg->sysid, msg->compid, mission.num_commands(),
-                                           MAV_MISSION_TYPE_MISSION);
-            goto mission_item_send_failed;
-        }
-
-        // retrieve mission from eeprom
-        if (!mission.read_cmd_from_storage(packet.seq, cmd)) {
-            goto mission_item_send_failed;
-        }
-
-        mavlink_mission_item_t ret_packet;
-        memset(&ret_packet, 0, sizeof(ret_packet));
-        if (!AP_Mission::mission_cmd_to_mavlink(cmd, ret_packet)) {
-            goto mission_item_send_failed;
-        }
-            
-        // set packet's current field to 1 if this is the command being executed
-        if (cmd.id == (uint16_t)mission.get_current_nav_cmd().index) {
-            ret_packet.current = 1;
-        } else {
-            ret_packet.current = 0;
         }
+        prot->handle_mission_request(*this, packet, *msg);
+}
 
-        // set auto continue to 1
+void MissionItemProtocol::convert_MISSION_REQUEST_to_MISSION_REQUEST_INT(const mavlink_mission_request_t &request, mavlink_mission_request_int_t &request_int)
-        ret_packet.autocontinue = 1;     // 1 (true), 0 (false)
+{
+    request_int.target_system = request.target_system;
+    request_int.target_component = request.target_component;
+    request_int.seq = request.seq;
+    request_int.mission_type = request.mission_type;
+}
 
-        /*
+void MissionItemProtocol::handle_mission_request(const GCS_MAVLINK &_link,
-          avoid the _send() function to save memory, as it avoids
+                                                 const mavlink_mission_request_t &packet,
-          the stack usage of the _send() function by using the already
+                                                 const mavlink_message_t &msg
-          declared ret_packet above
+)
-         */
+{
-        ret_packet.target_system = msg->sysid;
+    mavlink_mission_request_int_t request_int;
-        ret_packet.target_component = msg->compid;
+    convert_MISSION_REQUEST_to_MISSION_REQUEST_INT(packet, request_int);
-        ret_packet.seq = packet.seq;
-        ret_packet.command = cmd.id;
 
-        _mav_finalize_message_chan_send(chan, 
+    mavlink_mission_item_int_t item_int{};
-                                        MAVLINK_MSG_ID_MISSION_ITEM,
+    MAV_MISSION_RESULT ret = get_item(_link, msg, request_int, item_int);
-                                        (const char *)&ret_packet,
+    if (ret != MAV_MISSION_ACCEPTED) {
-                                        MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN,
+        return;
-                                        MAVLINK_MSG_ID_MISSION_ITEM_LEN,
-                                        MAVLINK_MSG_ID_MISSION_ITEM_CRC);
     }
 
-    return;
+    mavlink_mission_item_t ret_packet{};
+    ret = AP_Mission::convert_MISSION_ITEM_INT_to_MISSION_ITEM(item_int, ret_packet);
+    if (ret != MAV_MISSION_ACCEPTED) {
+        return;
+    }
 
-mission_item_send_failed:
+    // we already have a filled structure, use it in place of _send:
-    // send failure message
+    _mav_finalize_message_chan_send(_link.get_chan(),
-    mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ERROR,
+                                    MAVLINK_MSG_ID_MISSION_ITEM,
-                                 MAV_MISSION_TYPE_MISSION);
+                                    (const char *)&ret_packet,
+                                    MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN,
+                                    MAVLINK_MSG_ID_MISSION_ITEM_LEN,
+                                    MAVLINK_MSG_ID_MISSION_ITEM_CRC);
 }
 
 /*
@@ -583,81 +705,164 @@ void GCS_MAVLINK::handle_mission_set_current(AP_Mission &mission, mavlink_messag
 /*
   handle a MISSION_COUNT mavlink packet
  */
-void GCS_MAVLINK::handle_mission_count(AP_Mission &mission, mavlink_message_t *msg)
+void GCS_MAVLINK::handle_mission_count(const mavlink_message_t *msg)
 {
     // decode
     mavlink_mission_count_t packet;
     mavlink_msg_mission_count_decode(msg, &packet);
 
-    // start waypoint receiving
+    MissionItemProtocol *prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type);
-    if (packet.count > mission.num_commands_max()) {
+    if (prot == nullptr) {
-        // send NAK
+        mavlink_msg_mission_ack_send(chan,
-        mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_NO_SPACE,
+                                     msg->sysid,
-                                     MAV_MISSION_TYPE_MISSION);
+                                     msg->compid,
+                                     MAV_MISSION_UNSUPPORTED,
+                                     packet.mission_type);
         return;
     }
 
-    // new mission arriving, truncate mission to be the same length
+    prot->handle_mission_count(*this, packet, *msg);
-    mission.truncate(packet.count);
+}
 
+void MissionItemProtocol::init_send_requests(GCS_MAVLINK &_link,
+                                             const mavlink_message_t &msg,
+                                             const int16_t _request_first,
+                                             const int16_t _request_last)
+{
     // set variables to help handle the expected receiving of commands from the GCS
-    waypoint_timelast_receive = AP_HAL::millis();    // set time we last received commands to now
+    timelast_receive_ms = AP_HAL::millis();    // set time we last received commands to now
-    waypoint_receiving = true;              // record that we expect to receive commands
+    receiving = true;              // record that we expect to receive commands
-    waypoint_request_i = 0;                 // reset the next expected command number to zero
+    request_i = _request_first;                 // reset the next expected command number to zero
-    waypoint_request_last = packet.count;   // record how many commands we expect to receive
+    request_last = _request_last;         // record how many commands we expect to receive
-    waypoint_timelast_request = 0;          // set time we last requested commands to zero
+
+    dest_sysid = msg.sysid;       // record system id of GCS who wants to upload the mission
+    dest_compid = msg.compid;     // record component id of GCS who wants to upload the mission
+
+    link = &_link;
+
+    timelast_request_ms = AP_HAL::millis();
+    link->send_message(next_item_ap_message_id());
+}
+
+void MissionItemProtocol::handle_mission_count(
+    GCS_MAVLINK &_link,
+    const mavlink_mission_count_t &packet,
+    const mavlink_message_t &msg)
+{
+    if (receiving) {
+        // someone is already uploading a mission.  If we are
+        // receiving from someone then we will allow them to restart -
+        // otherwise we deny.
+        if (msg.sysid != dest_sysid || msg.compid != dest_compid) {
+            // reject another upload until
+            send_mission_ack(_link, msg, MAV_MISSION_DENIED);
+            return;
+        }
+    }
+
+    if (packet.count > max_items()) {
+        send_mission_ack(_link, msg, MAV_MISSION_NO_SPACE);
+        return;
+    }
+
+    truncate(packet);
+
+    if (packet.count == 0) {
+        // no requests to send...
+        send_mission_ack(_link, msg, MAV_MISSION_ACCEPTED);
+        complete(_link);
+        return;
+    }
 
-    waypoint_dest_sysid = msg->sysid;       // record system id of GCS who wants to upload the mission
+    // start waypoint receiving
-    waypoint_dest_compid = msg->compid;     // record component id of GCS who wants to upload the mission
+    init_send_requests(_link, msg, 0, packet.count-1);
+}
+
+void MissionItemProtocol_Waypoints::truncate(const mavlink_mission_count_t &packet)
+{
+    // new mission arriving, truncate mission to be the same length
+    mission.truncate(packet.count);
 }
 
 /*
   handle a MISSION_CLEAR_ALL mavlink packet
  */
-void GCS_MAVLINK::handle_mission_clear_all(AP_Mission &mission, mavlink_message_t *msg)
+void GCS_MAVLINK::handle_mission_clear_all(const mavlink_message_t *msg)
 {
     // decode
     mavlink_mission_clear_all_t packet;
     mavlink_msg_mission_clear_all_decode(msg, &packet);
 
-    // clear all waypoints
+    const MAV_MISSION_TYPE mission_type = (MAV_MISSION_TYPE)packet.mission_type;
-    if (mission.clear()) {
+    MissionItemProtocol *prot = gcs().get_prot_for_mission_type(mission_type);
-        // send ack
+    if (prot == nullptr) {
-        mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ACCEPTED,
+        send_mission_ack(*msg, mission_type, MAV_MISSION_UNSUPPORTED);
-                                     MAV_MISSION_TYPE_MISSION);
+        return;
-    }else{
-        // send nack
-        mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ERROR,
-                                     MAV_MISSION_TYPE_MISSION);
     }
+
+    prot->handle_mission_clear_all(*this, *msg);
 }
 
-/*
+bool MissionItemProtocol_Waypoints::clear_all_items()
-  handle a MISSION_WRITE_PARTIAL_LIST mavlink packet
+{
- */
+    return mission.clear();
-void GCS_MAVLINK::handle_mission_write_partial_list(AP_Mission &mission, mavlink_message_t *msg)
+}
+
+bool MissionItemProtocol_Rally::clear_all_items()
+{
+    rally.truncate(0);
+    return true;
+}
+
+void MissionItemProtocol::handle_mission_clear_all(const GCS_MAVLINK &_link,
+                                                   const mavlink_message_t &msg)
+{
+    bool success = true;
+    success = success && !receiving;
+    success = success && clear_all_items();
+    send_mission_ack(_link, msg, success ? MAV_MISSION_ACCEPTED : MAV_MISSION_ERROR);
+}
+
+bool GCS_MAVLINK::requesting_mission_items() const
+{
+    for (uint8_t i=0; i<ARRAY_SIZE(supported_mission_types); i++) {
+        MissionItemProtocol *prot = gcs().get_prot_for_mission_type(supported_mission_types[i]);
+        if (prot && prot->receiving && prot->active_link_is(this)) {
+            return true;
+        }
+    }
+    return false;
+}
+
+void GCS_MAVLINK::handle_mission_write_partial_list(const mavlink_message_t *msg)
 {
     // decode
     mavlink_mission_write_partial_list_t packet;
     mavlink_msg_mission_write_partial_list_decode(msg, &packet);
 
+    MissionItemProtocol *use_prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type);
+    if (use_prot == nullptr) {
+        send_mission_ack(*msg, (MAV_MISSION_TYPE)packet.mission_type, MAV_MISSION_UNSUPPORTED);
+        return;
+    }
+    use_prot->handle_mission_write_partial_list(*this, *msg, packet);
+}
+
+void MissionItemProtocol::handle_mission_write_partial_list(GCS_MAVLINK &_link,
+                                                            const mavlink_message_t &msg,
+                                                            const mavlink_mission_write_partial_list_t &packet)
+{
+
     // start waypoint receiving
-    if ((unsigned)packet.start_index > mission.num_commands() ||
+    if ((unsigned)packet.start_index > item_count() ||
-        (unsigned)packet.end_index > mission.num_commands() ||
+        (unsigned)packet.end_index > item_count() ||
         packet.end_index < packet.start_index) {
-        send_text(MAV_SEVERITY_WARNING,"Flight plan update rejected"); // FIXME: Remove this anytime after 2020-01-22
+        gcs().send_text(MAV_SEVERITY_WARNING,"Flight plan update rejected"); // FIXME: Remove this anytime after 2020-01-22
-        mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION);
+        send_mission_ack(_link, msg, MAV_MISSION_ERROR);
         return;
     }
 
-    waypoint_timelast_receive = AP_HAL::millis();
+    init_send_requests(_link, msg, packet.start_index, packet.end_index);
-    waypoint_timelast_request = 0;
-    waypoint_receiving   = true;
-    waypoint_request_i   = packet.start_index;
-    waypoint_request_last= packet.end_index;
-
-    waypoint_dest_sysid = msg->sysid;       // record system id of GCS who wants to partially update the mission
-    waypoint_dest_compid = msg->compid;     // record component id of GCS who wants to partially update the mission
 }
 
 
@@ -749,140 +954,176 @@ uint16_t GCS::sys_status_errors1()
   handle an incoming mission item
   return true if this is the last mission item, otherwise false
  */
-bool GCS_MAVLINK::handle_mission_item(mavlink_message_t *msg, AP_Mission &mission)
+void GCS_MAVLINK::handle_mission_item(const mavlink_message_t *msg)
 {
-    MAV_MISSION_RESULT result = MAV_MISSION_ACCEPTED;
+    // TODO: rename packet to mission_item_int
-    struct AP_Mission::Mission_Command cmd = {};
+    mavlink_mission_item_int_t packet;
-    bool mission_is_complete = false;
-    uint16_t seq=0;
-    uint16_t current = 0;
-    
     if (msg->msgid == MAVLINK_MSG_ID_MISSION_ITEM) {      
-        mavlink_mission_item_t packet;    
+        mavlink_mission_item_t mission_item;
-        mavlink_msg_mission_item_decode(msg, &packet);
+        mavlink_msg_mission_item_decode(msg, &mission_item);
-        
+        MAV_MISSION_RESULT ret = AP_Mission::convert_MISSION_ITEM_to_MISSION_ITEM_INT(mission_item, packet);
-        // convert mavlink packet to mission command
+        if (ret != MAV_MISSION_ACCEPTED) {
-        result = AP_Mission::mavlink_to_mission_cmd(packet, cmd);
+            const MAV_MISSION_TYPE type = (MAV_MISSION_TYPE)packet.mission_type;
-        if (result != MAV_MISSION_ACCEPTED) {
+            send_mission_ack(*msg, type, ret);
-            goto mission_ack;
+            return;
         }
-        
-        seq = packet.seq;
-        current = packet.current;
     } else {
-        mavlink_mission_item_int_t packet;
         mavlink_msg_mission_item_int_decode(msg, &packet);
-        
+    }
-        // convert mavlink packet to mission command
+    const uint8_t current = packet.current;
-        result = AP_Mission::mavlink_int_to_mission_cmd(packet, cmd);
+    const MAV_MISSION_TYPE type = (MAV_MISSION_TYPE)packet.mission_type;
+
+    if (type == MAV_MISSION_TYPE_MISSION && (current == 2 || current == 3)) {
+        struct AP_Mission::Mission_Command cmd = {};
+        MAV_MISSION_RESULT result = AP_Mission::mavlink_int_to_mission_cmd(packet, cmd);
         if (result != MAV_MISSION_ACCEPTED) {
-            goto mission_ack;
+            //decode failed
+            send_mission_ack(*msg, MAV_MISSION_TYPE_MISSION, result);
+            return;
         }
-        
+        // guided or change-alt
-        seq = packet.seq;
+        if (current == 2) {
-        current = packet.current;
+            // current = 2 is a flag to tell us this is a "guided mode"
+            // waypoint and not for the mission
+            result = (handle_guided_request(cmd) ? MAV_MISSION_ACCEPTED
+                      : MAV_MISSION_ERROR) ;
+        } else if (current == 3) {
+            //current = 3 is a flag to tell us this is a alt change only
+            // add home alt if needed
+            handle_change_alt_request(cmd);
+
+            // verify we recevied the command
+            result = MAV_MISSION_ACCEPTED;
+        }
+        send_mission_ack(*msg, MAV_MISSION_TYPE_MISSION, result);
+        return;
     }
 
-    if (current == 2) {                                               
+    // not a guided-mode reqest
-        // current = 2 is a flag to tell us this is a "guided mode"
+    MissionItemProtocol *prot = gcs().get_prot_for_mission_type(type);
-        // waypoint and not for the mission
+    if (prot == nullptr) {
-        result = (handle_guided_request(cmd) ? MAV_MISSION_ACCEPTED
+        send_mission_ack(*msg, type, MAV_MISSION_UNSUPPORTED);
-                                             : MAV_MISSION_ERROR) ;
+        return;
+    }
 
-        // verify we received the command
+    if (!prot->receiving) {
-        goto mission_ack;
+        send_mission_ack(*msg, type, MAV_MISSION_ERROR);
+        return;
     }
 
-    if (current == 3) {
+    prot->handle_mission_item(*msg, packet);
-        //current = 3 is a flag to tell us this is a alt change only
+}
-        // add home alt if needed
-        handle_change_alt_request(cmd);
 
-        // verify we recevied the command
+MAV_MISSION_RESULT MissionItemProtocol_Waypoints::replace_item(const mavlink_mission_item_int_t &mission_item_int)
-        result = MAV_MISSION_ACCEPTED;
+{
-        goto mission_ack;
+    AP_Mission::Mission_Command cmd;
-    }
 
-    // Check if receiving waypoints (mission upload expected)
+    const MAV_MISSION_RESULT res = AP_Mission::mavlink_int_to_mission_cmd(mission_item_int, cmd);
-    if (!waypoint_receiving) {
+    if (res != MAV_MISSION_ACCEPTED) {
-        result = MAV_MISSION_ERROR;
+        return res;
-        goto mission_ack;
     }
 
-    // check if this is the requested waypoint
+    // sanity check for DO_JUMP command
-    if (seq != waypoint_request_i) {
+    if (cmd.id == MAV_CMD_DO_JUMP) {
-        result = MAV_MISSION_INVALID_SEQUENCE;
+        if ((cmd.content.jump.target >= item_count() && cmd.content.jump.target > request_last) || cmd.content.jump.target == 0) {
-        goto mission_ack;
+            return MAV_MISSION_ERROR;
+        }
+    }
+    if (!mission.replace_cmd(cmd.index, cmd)) {
+        return MAV_MISSION_ERROR;
     }
+    return MAV_MISSION_ACCEPTED;
+}
 
+MAV_MISSION_RESULT MissionItemProtocol_Waypoints::append_item(const mavlink_mission_item_int_t &mission_item_int)
+{
     // sanity check for DO_JUMP command
+    AP_Mission::Mission_Command cmd;
+
+    const MAV_MISSION_RESULT res = AP_Mission::mavlink_int_to_mission_cmd(mission_item_int, cmd);
+    if (res != MAV_MISSION_ACCEPTED) {
+        return res;
+    }
+
     if (cmd.id == MAV_CMD_DO_JUMP) {
-        if ((cmd.content.jump.target >= mission.num_commands() && cmd.content.jump.target >= waypoint_request_last) || cmd.content.jump.target == 0) {
+        if ((cmd.content.jump.target >= item_count() && cmd.content.jump.target > request_last) || cmd.content.jump.target == 0) {
-            result = MAV_MISSION_ERROR;
+            return MAV_MISSION_ERROR;
-            goto mission_ack;
         }
     }
-    
+
-    // if command index is within the existing list, replace the command
+    if (!mission.add_cmd(cmd)) {
-    if (seq < mission.num_commands()) {
+        return MAV_MISSION_ERROR;
-        if (mission.replace_cmd(seq,cmd)) {
+    }
-            result = MAV_MISSION_ACCEPTED;
+    return MAV_MISSION_ACCEPTED;
-        }else{
+}
-            result = MAV_MISSION_ERROR;
+
-            goto mission_ack;
+void MissionItemProtocol_Waypoints::complete(const GCS_MAVLINK &_link)
-        }
+{
-        // if command is at the end of command list, add the command
+    _link.send_text(MAV_SEVERITY_INFO, "Flight plan received");
-    } else if (seq == mission.num_commands()) {
+    AP::logger().Write_EntireMission();
-        if (mission.add_cmd(cmd)) {
+}
-            result = MAV_MISSION_ACCEPTED;
+void MissionItemProtocol_Waypoints::timeout()
-        }else{
+{
-            result = MAV_MISSION_ERROR;
+    link->send_text(MAV_SEVERITY_WARNING, "Mission upload timeout");
-            goto mission_ack;
+}
-        }
+
-        // if beyond the end of the command list, return an error
+void MissionItemProtocol::handle_mission_item(const mavlink_message_t &msg, const mavlink_mission_item_int_t &cmd)
+{
+    if (link == nullptr) {
+        AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
+        return;
+    }
+
+    // check if this is the requested waypoint
+    if (cmd.seq != request_i) {
+        send_mission_ack(msg, MAV_MISSION_INVALID_SEQUENCE);
+        return;
+    }
+    // make sure the item is coming from the system that initiated the upload
+    if (msg.sysid != dest_sysid) {
+        send_mission_ack(msg, MAV_MISSION_DENIED);
+        return;
+    }
+    if (msg.compid != dest_compid) {
+        send_mission_ack(msg, MAV_MISSION_DENIED);
+        return;
+    }
+
+    const uint16_t _item_count = item_count();
+
+    MAV_MISSION_RESULT result;
+    if (cmd.seq < _item_count) {
+        // command index is within the existing list, replace the command
+        result = replace_item(cmd);
+    } else if (cmd.seq == _item_count) {
+        // command is at the end of command list, add the command
+        result = append_item(cmd);
     } else {
+        // beyond the end of the command list, return an error
         result = MAV_MISSION_ERROR;
-        goto mission_ack;
     }
-    
+    if (result != MAV_MISSION_ACCEPTED) {
+        send_mission_ack(msg, result);
+        return;
+    }
+
     // update waypoint receiving state machine
-    waypoint_timelast_receive = AP_HAL::millis();
+    timelast_receive_ms = AP_HAL::millis();
-    waypoint_request_i++;
+    request_i++;
-    
+
-    if (waypoint_request_i >= waypoint_request_last) {
+    if (request_i > request_last) {
-        mavlink_msg_mission_ack_send(
+        send_mission_ack(msg, MAV_MISSION_ACCEPTED);
-                chan,
+        complete(*link);
-                msg->sysid,
+        receiving = false;
-                msg->compid,
+        link = nullptr;
-                MAV_MISSION_ACCEPTED,
+        return;
-                MAV_MISSION_TYPE_MISSION);
+    }
-        
+    // if we have enough space, then send the next WP request immediately
-        send_text(MAV_SEVERITY_INFO,"Flight plan received");
+    if (HAVE_PAYLOAD_SPACE(link->get_chan(), MISSION_REQUEST)) {
-        waypoint_receiving = false;
+        queued_request_send();
-        mission_is_complete = true;
-        // XXX ignores waypoint radius for individual waypoints, can
-        // only set WP_RADIUS parameter
     } else {
-        waypoint_timelast_request = AP_HAL::millis();
+        link->send_message(next_item_ap_message_id());
-        // if we have enough space, then send the next WP request immediately
-        if (HAVE_PAYLOAD_SPACE(chan, MISSION_REQUEST)) {
-            queued_mission_request_send();
-        } else {
-            send_message(MSG_NEXT_MISSION_REQUEST);
-        }
     }
-    return mission_is_complete;
-
-mission_ack:
-    // we are rejecting the mission/waypoint
-    mavlink_msg_mission_ack_send(
-            chan,
-            msg->sysid,
-            msg->compid,
-            result,
-            MAV_MISSION_TYPE_MISSION);
-
-    return mission_is_complete;
 }
 
 ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) const
@@ -1013,7 +1254,7 @@ uint16_t GCS_MAVLINK::get_reschedule_interval_ms(const deferred_message_bucket_t
         // we are sending parameters, penalize streams:
         interval_ms *= 4;
     }
-    if (waypoint_receiving) {
+    if (requesting_mission_items()) {
         // we are sending requests for waypoints, penalize streams:
         interval_ms *= 4;
     }
@@ -1611,19 +1852,6 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us)
     }
 #endif
 
-    if (waypoint_receiving) {
-        const uint32_t wp_recv_time = 1000U + stream_slowdown_ms;
-
-        // stop waypoint receiving if timeout
-        if (tnow - waypoint_timelast_receive > wp_recv_time+waypoint_receive_timeout) {
-            waypoint_receiving = false;
-            gcs().send_text(MAV_SEVERITY_WARNING, "Mission upload timeout");
-        } else if (tnow - waypoint_timelast_request > wp_recv_time) {
-            waypoint_timelast_request = tnow;
-            send_message(MSG_NEXT_MISSION_REQUEST);
-        }
-    }
-
     hal.util->perf_end(_perf_update);    
 }
 
@@ -1993,6 +2221,24 @@ void GCS::send_message(enum ap_message id)
 
 void GCS::update_send()
 {
+    if (!initialised_missionitemprotocol_objects) {
+        initialised_missionitemprotocol_objects = true;
+        // once-only initialisation of MissionItemProtocol objects:
+        AP_Mission *mission = AP::mission();
+        if (mission != nullptr) {
+            _missionitemprotocol_waypoints = new MissionItemProtocol_Waypoints(*mission);
+        }
+        AP_Rally *rally = AP::rally();
+        if (rally != nullptr) {
+            _missionitemprotocol_rally = new MissionItemProtocol_Rally(*rally);
+        }
+    }
+    if (_missionitemprotocol_waypoints != nullptr) {
+        _missionitemprotocol_waypoints->update();
+    }
+    if (_missionitemprotocol_rally != nullptr) {
+        _missionitemprotocol_rally->update();
+    }
     for (uint8_t i=0; i<num_gcs(); i++) {
         if (chan(i).initialised) {
             chan(i).update_send();
@@ -3229,27 +3475,23 @@ void GCS_MAVLINK::handle_common_mission_message(mavlink_message_t *msg)
     switch (msg->msgid) {
     case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: // MAV ID: 38
     {
-        handle_mission_write_partial_list(*_mission, msg);
+        handle_mission_write_partial_list(msg);
         break;
     }
 
     // GCS has sent us a mission item, store to EEPROM
     case MAVLINK_MSG_ID_MISSION_ITEM:           // MAV ID: 39
     case MAVLINK_MSG_ID_MISSION_ITEM_INT:
-    {
+        handle_mission_item(msg);
-        if (handle_mission_item(msg, *_mission)) {
-            AP::logger().Write_EntireMission();
-        }
         break;
-    }
 
     // read an individual command from EEPROM and send it to the GCS
     case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
-    case MAVLINK_MSG_ID_MISSION_REQUEST:     // MAV ID: 40, 51
+        handle_mission_request_int(msg);
-    {
+        break;
-        handle_mission_request(*_mission, msg);
+    case MAVLINK_MSG_ID_MISSION_REQUEST:
+        handle_mission_request(msg);
         break;
-    }
 
     case MAVLINK_MSG_ID_MISSION_SET_CURRENT:    // MAV ID: 41
     {
@@ -3260,7 +3502,7 @@ void GCS_MAVLINK::handle_common_mission_message(mavlink_message_t *msg)
     // GCS request the full list of commands, we return just the number and leave the GCS to then request each command individually
     case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:       // MAV ID: 43
     {
-        handle_mission_request_list(*_mission, msg);
+        handle_mission_request_list(msg);
         break;
     }
 
@@ -3268,13 +3510,13 @@ void GCS_MAVLINK::handle_common_mission_message(mavlink_message_t *msg)
     //  individual commands will then be sent from the GCS using the MAVLINK_MSG_ID_MISSION_ITEM message
     case MAVLINK_MSG_ID_MISSION_COUNT:          // MAV ID: 44
     {
-        handle_mission_count(*_mission, msg);
+        handle_mission_count(msg);
         break;
     }
 
     case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:      // MAV ID: 45
     {
-        handle_mission_clear_all(*_mission, msg);
+        handle_mission_clear_all(msg);
         break;
     }
 
@@ -3890,6 +4132,14 @@ bool GCS_MAVLINK::try_send_compass_message(const enum ap_message id)
     return ret;
 }
 
+void GCS::try_send_queued_message_for_type(MAV_MISSION_TYPE type) {
+    MissionItemProtocol *prot = get_prot_for_mission_type(type);
+    if (prot == nullptr) {
+        return;
+    }
+    prot->queued_request_send();
+}
+
 bool GCS_MAVLINK::try_send_mission_message(const enum ap_message id)
 {
     AP_Mission *mission = AP::mission();
@@ -3909,9 +4159,14 @@ bool GCS_MAVLINK::try_send_mission_message(const enum ap_message id)
         mavlink_msg_mission_item_reached_send(chan, mission_item_reached_index);
         ret = true;
         break;
-    case MSG_NEXT_MISSION_REQUEST:
+    case MSG_NEXT_MISSION_REQUEST_WAYPOINTS:
         CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
-        queued_mission_request_send();
+        gcs().try_send_queued_message_for_type(MAV_MISSION_TYPE_MISSION);
+        ret = true;
+        break;
+    case MSG_NEXT_MISSION_REQUEST_RALLY:
+        CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
+        gcs().try_send_queued_message_for_type(MAV_MISSION_TYPE_RALLY);
         ret = true;
         break;
     default:
@@ -4140,7 +4395,8 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
 
     case MSG_CURRENT_WAYPOINT:
     case MSG_MISSION_ITEM_REACHED:
-    case MSG_NEXT_MISSION_REQUEST:
+    case MSG_NEXT_MISSION_REQUEST_WAYPOINTS:
+    case MSG_NEXT_MISSION_REQUEST_RALLY:
         ret = try_send_mission_message(id);
         break;
 
@@ -4645,6 +4901,9 @@ uint64_t GCS_MAVLINK::capabilities() const
         ret |= MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION;
     }
 
+    if (AP::rally()) {
+        ret |= MAV_PROTOCOL_CAPABILITY_MISSION_RALLY;
+    }
     return ret;
 }
 

libraries/GCS_MAVLink/GCS_Rally.cpp

@@ -17,6 +17,7 @@
 
 #include "GCS.h"
 #include <AP_Rally/AP_Rally.h>
+#include <AP_Logger/AP_Logger.h>
 
 void GCS_MAVLINK::handle_rally_point(mavlink_message_t *msg)
 {
@@ -101,3 +102,84 @@ void GCS_MAVLINK::handle_common_rally_message(mavlink_message_t *msg)
     }
 }
 
+MAV_MISSION_RESULT MissionItemProtocol_Rally::get_item(const GCS_MAVLINK &_link,
+                                                       const mavlink_message_t &msg,
+                                                       const mavlink_mission_request_int_t &packet,
+                                                       mavlink_mission_item_int_t &ret_packet)
+{
+    RallyLocation rallypoint;
+
+    if (!rally.get_rally_point_with_index(packet.seq, rallypoint)) {
+        return MAV_MISSION_INVALID_SEQUENCE;
+    }
+
+    ret_packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT;
+    ret_packet.command = MAV_CMD_NAV_RALLY_POINT;
+    ret_packet.x = rallypoint.lat;
+    ret_packet.y = rallypoint.lng;
+    ret_packet.z = rallypoint.alt;
+
+    return MAV_MISSION_ACCEPTED;
+}
+
+void MissionItemProtocol_Rally::truncate(const mavlink_mission_count_t &packet)
+{
+    rally.truncate(packet.count);
+}
+void MissionItemProtocol_Rally::timeout()
+{
+    link->send_text(MAV_SEVERITY_WARNING, "Rally upload timeout");
+}
+
+MAV_MISSION_RESULT MissionItemProtocol_Rally::convert_MISSION_ITEM_INT_to_RallyLocation(const mavlink_mission_item_int_t &cmd, RallyLocation &ret)
+{
+    if (cmd.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT &&
+        cmd.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT) {
+        return MAV_MISSION_UNSUPPORTED_FRAME;
+    }
+    if (!check_lat(cmd.x)) {
+        return MAV_MISSION_INVALID_PARAM5_X;
+    }
+    if (!check_lng(cmd.y)) {
+        return MAV_MISSION_INVALID_PARAM6_Y;
+    }
+    if (cmd.z < INT16_MIN || cmd.z > INT16_MAX) {
+        return MAV_MISSION_INVALID_PARAM7;
+    }
+    ret.lat = cmd.x;
+    ret.lng = cmd.y;
+    ret.alt = cmd.z;
+    return MAV_MISSION_ACCEPTED;
+}
+
+MAV_MISSION_RESULT MissionItemProtocol_Rally::replace_item(const mavlink_mission_item_int_t &cmd)
+{
+    RallyLocation rallyloc;
+    const MAV_MISSION_RESULT ret = convert_MISSION_ITEM_INT_to_RallyLocation(cmd, rallyloc);
+    if (ret != MAV_MISSION_ACCEPTED) {
+        return ret;
+    }
+    if (!rally.set_rally_point_with_index(cmd.seq, rallyloc)) {
+        return MAV_MISSION_ERROR;
+    }
+    return MAV_MISSION_ACCEPTED;
+}
+
+MAV_MISSION_RESULT MissionItemProtocol_Rally::append_item(const mavlink_mission_item_int_t &cmd)
+{
+    RallyLocation rallyloc;
+    const MAV_MISSION_RESULT ret = convert_MISSION_ITEM_INT_to_RallyLocation(cmd, rallyloc);
+    if (ret != MAV_MISSION_ACCEPTED) {
+        return ret;
+    }
+    if (!rally.append(rallyloc)) {
+        return MAV_MISSION_ERROR;
+    }
+    return MAV_MISSION_ACCEPTED;
+}
+
+void MissionItemProtocol_Rally::complete(const GCS_MAVLINK &_link)
+{
+    _link.send_text(MAV_SEVERITY_INFO, "Rally points received");
+    AP::logger().Write_Rally();
+}