diff --git a/Tools/AntennaTracker/AntennaTracker.pde b/Tools/AntennaTracker/AntennaTracker.pde
index 17f9adc71b..3533453131 100644
--- a/Tools/AntennaTracker/AntennaTracker.pde
+++ b/Tools/AntennaTracker/AntennaTracker.pde
@@ -53,6 +53,7 @@
 #include <AP_NavEKF.h>
 
 #include <AP_Vehicle.h>
+#include <AP_Mission.h>
 #include <AP_Notify.h>      // Notify library
 #include <AP_BattMonitor.h> // Battery monitor library
 #include <AP_Airspeed.h>
diff --git a/Tools/AntennaTracker/GCS_Mavlink.pde b/Tools/AntennaTracker/GCS_Mavlink.pde
index f5d77b6ca0..27a1c83f2b 100644
--- a/Tools/AntennaTracker/GCS_Mavlink.pde
+++ b/Tools/AntennaTracker/GCS_Mavlink.pde
@@ -658,203 +658,34 @@ GCS_MAVLINK::send_message(enum ap_message id)
     mavlink_send_message(chan,id, packet_drops);
 }
 
-void
-GCS_MAVLINK::send_text_P(gcs_severity severity, const prog_char_t *str)
-{
-    mavlink_statustext_t m;
-    uint8_t i;
-    for (i=0; i<sizeof(m.text); i++) {
-        m.text[i] = pgm_read_byte((const prog_char *)(str++));
-        if (m.text[i] == '\0') {
-            break;
-        }
-    }
-    if (i < sizeof(m.text)) m.text[i] = 0;
-    mavlink_send_text(chan, severity, (const char *)m.text);
-}
-
 void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 {
     switch (msg->msgid) {
 
     case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
     {
-        // decode
-        mavlink_request_data_stream_t packet;
-        mavlink_msg_request_data_stream_decode(msg, &packet);
-
-        if (mavlink_check_target(packet.target_system, packet.target_component))
-            break;
-
-        int16_t freq = 0;     // packet frequency
-
-        if (packet.start_stop == 0)
-            freq = 0;                     // stop sending
-        else if (packet.start_stop == 1)
-            freq = packet.req_message_rate;                     // start sending
-        else
-            break;
-
-        switch (packet.req_stream_id) {
-        case MAV_DATA_STREAM_ALL:
-            // note that we don't set STREAM_PARAMS - that is internal only
-            for (uint8_t i=0; i<STREAM_PARAMS; i++) {
-                streamRates[i].set_and_save_ifchanged(freq);
-            }
-            break;
-        case MAV_DATA_STREAM_RAW_SENSORS:
-            streamRates[STREAM_RAW_SENSORS].set_and_save_ifchanged(freq);
-            break;
-        case MAV_DATA_STREAM_EXTENDED_STATUS:
-            streamRates[STREAM_EXTENDED_STATUS].set_and_save_ifchanged(freq);
-            break;
-        case MAV_DATA_STREAM_RC_CHANNELS:
-            streamRates[STREAM_RC_CHANNELS].set_and_save_ifchanged(freq);
-            break;
-        case MAV_DATA_STREAM_RAW_CONTROLLER:
-            streamRates[STREAM_RAW_CONTROLLER].set_and_save_ifchanged(freq);
-            break;
-        case MAV_DATA_STREAM_POSITION:
-            streamRates[STREAM_POSITION].set_and_save_ifchanged(freq);
-            break;
-        case MAV_DATA_STREAM_EXTRA1:
-            streamRates[STREAM_EXTRA1].set_and_save_ifchanged(freq);
-            break;
-        case MAV_DATA_STREAM_EXTRA2:
-            streamRates[STREAM_EXTRA2].set_and_save_ifchanged(freq);
-            break;
-        case MAV_DATA_STREAM_EXTRA3:
-            streamRates[STREAM_EXTRA3].set_and_save_ifchanged(freq);
-            break;
-        }
+        handle_request_data_stream(msg, true);
         break;
     }
 
 
     case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
     {
-        // decode
-        mavlink_param_request_list_t packet;
-        mavlink_msg_param_request_list_decode(msg, &packet);
-        if (mavlink_check_target(packet.target_system,packet.target_component)) break;
-
-        // Start sending parameters - next call to ::update will kick the first one out
-
-        _queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type);
-        _queued_parameter_index = 0;
-        _queued_parameter_count = _count_parameters();
+        handle_param_request_list(msg);
         break;
     }
 
     case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
     {
-        // decode
-        mavlink_param_request_read_t packet;
-        mavlink_msg_param_request_read_decode(msg, &packet);
-        if (mavlink_check_target(packet.target_system,packet.target_component)) break;
-        enum ap_var_type p_type;
-        AP_Param *vp;
-        char param_name[AP_MAX_NAME_SIZE+1];
-        if (packet.param_index != -1) {
-            AP_Param::ParamToken token;
-            vp = AP_Param::find_by_index(packet.param_index, &p_type, &token);
-            if (vp == NULL) {
-                gcs_send_text_fmt(PSTR("Unknown parameter index %d"), packet.param_index);
-                break;
-            }
-            vp->copy_name_token(token, param_name, AP_MAX_NAME_SIZE, true);
-            param_name[AP_MAX_NAME_SIZE] = 0;
-        } else {
-            strncpy(param_name, packet.param_id, AP_MAX_NAME_SIZE);
-            param_name[AP_MAX_NAME_SIZE] = 0;
-            vp = AP_Param::find(param_name, &p_type);
-            if (vp == NULL) {
-                gcs_send_text_fmt(PSTR("Unknown parameter %.16s"), packet.param_id);
-                break;
-            }
-        }
-
-        float value = vp->cast_to_float(p_type);
-        mavlink_msg_param_value_send(
-            chan,
-            param_name,
-            value,
-            mav_var_type(p_type),
-            _count_parameters(),
-            packet.param_index);
+        handle_param_request_read(msg);
         break;
     }
 
     case MAVLINK_MSG_ID_PARAM_SET:
     {
-        AP_Param                  *vp;
-        enum ap_var_type var_type;
-
-        // decode
-        mavlink_param_set_t packet;
-        mavlink_msg_param_set_decode(msg, &packet);
-
-        if (mavlink_check_target(packet.target_system, packet.target_component))
-            break;
-
-        // set parameter
-
-        char key[AP_MAX_NAME_SIZE+1];
-        strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);
-        key[AP_MAX_NAME_SIZE] = 0;
-
-        // find the requested parameter
-        vp = AP_Param::find(key, &var_type);
-        if ((NULL != vp) &&                                 // exists
-            !isnan(packet.param_value) &&                       // not nan
-            !isinf(packet.param_value)) {                       // not inf
-
-            // add a small amount before casting parameter values
-            // from float to integer to avoid truncating to the
-            // next lower integer value.
-            float rounding_addition = 0.01;
-
-            // handle variables with standard type IDs
-            if (var_type == AP_PARAM_FLOAT) {
-                ((AP_Float *)vp)->set_and_save(packet.param_value);
-            } else if (var_type == AP_PARAM_INT32) {
-                if (packet.param_value < 0) rounding_addition = -rounding_addition;
-                float v = packet.param_value+rounding_addition;
-                v = constrain_float(v, -2147483648.0, 2147483647.0);
-                ((AP_Int32 *)vp)->set_and_save(v);
-            } else if (var_type == AP_PARAM_INT16) {
-                if (packet.param_value < 0) rounding_addition = -rounding_addition;
-                float v = packet.param_value+rounding_addition;
-                v = constrain_float(v, -32768, 32767);
-                ((AP_Int16 *)vp)->set_and_save(v);
-            } else if (var_type == AP_PARAM_INT8) {
-                if (packet.param_value < 0) rounding_addition = -rounding_addition;
-                float v = packet.param_value+rounding_addition;
-                v = constrain_float(v, -128, 127);
-                ((AP_Int8 *)vp)->set_and_save(v);
-            } else {
-                // we don't support mavlink set on this parameter
-                break;
-            }
-
-            // Report back the new value if we accepted the change
-            // we send the value we actually set, which could be
-            // different from the value sent, in case someone sent
-            // a fractional value to an integer type
-            mavlink_msg_param_value_send(
-                chan,
-                key,
-                vp->cast_to_float(var_type),
-                mav_var_type(var_type),
-                _count_parameters(),
-                -1);     // XXX we don't actually know what its index is...
-#if LOGGING_ENABLED == ENABLED
-            DataFlash.Log_Write_Parameter(key, vp->cast_to_float(var_type));
-#endif
-        }
-
+        handle_param_set(msg, NULL);
         break;
-    }     // end case
+    }
 
     case MAVLINK_MSG_ID_HEARTBEAT:
     {
@@ -995,9 +826,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
         struct Location tell_command = {};
 
-        // defaults
-        tell_command.id = packet.command;
-
         switch (packet.frame)
         {
         case MAV_FRAME_MISSION:
diff --git a/Tools/AntennaTracker/system.pde b/Tools/AntennaTracker/system.pde
index 1e0d9286c6..01f0307d62 100644
--- a/Tools/AntennaTracker/system.pde
+++ b/Tools/AntennaTracker/system.pde
@@ -136,25 +136,18 @@ static struct Location get_home_eeprom()
     // --------------------------------------------------------------------------------
     if (g.command_total.get() == 0) {
         memset(&temp, 0, sizeof(temp));
-        temp.id = CMD_BLANK;
     }else{
         // read WP position
         mem = WP_START_BYTE;
-        temp.id = hal.storage->read_byte(mem);
-
-        mem++;
         temp.options = hal.storage->read_byte(mem);
-
         mem++;
-        temp.p1 = hal.storage->read_byte(mem);
 
-        mem++;
         temp.alt = hal.storage->read_dword(mem);
-
         mem += 4;
-        temp.lat = hal.storage->read_dword(mem);
 
+        temp.lat = hal.storage->read_dword(mem);
         mem += 4;
+
         temp.lng = hal.storage->read_dword(mem);
     }
 
@@ -165,21 +158,15 @@ static void set_home_eeprom(struct Location temp)
 {
     uint16_t mem = WP_START_BYTE;
 
-    hal.storage->write_byte(mem, temp.id);
-
-    mem++;
     hal.storage->write_byte(mem, temp.options);
-
     mem++;
-    hal.storage->write_byte(mem, temp.p1);
 
-    mem++;
     hal.storage->write_dword(mem, temp.alt);
-
     mem += 4;
-    hal.storage->write_dword(mem, temp.lat);
 
+    hal.storage->write_dword(mem, temp.lat);
     mem += 4;
+
     hal.storage->write_dword(mem, temp.lng);
 
     // Now have a home location in EEPROM