diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index 29f586d111..101f5fd72e 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -1111,6 +1111,29 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             }
             break;
 
+        case MAV_CMD_DO_SET_HOME:
+            // param1 : use current (1=use current location, 0=use specified location)
+            // param5 : latitude
+            // param6 : longitude
+            // param7 : altitude (absolute)
+            result = MAV_RESULT_FAILED; // assume failure
+            if(packet.param1 == 1 || (packet.param5 == 0 && packet.param6 == 0 && packet.param7 == 0)) {
+                if (set_home_to_current_location_and_lock()) {
+                    result = MAV_RESULT_ACCEPTED;
+                }
+            } else {
+                Location new_home_loc;
+                new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
+                new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
+                new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);
+                if (!far_from_EKF_origin(new_home_loc)) {
+                    if (set_home_and_lock(new_home_loc)) {
+                        result = MAV_RESULT_ACCEPTED;
+                    }
+                }
+            }
+            break;
+
         case MAV_CMD_DO_SET_ROI:
             // param1 : regional of interest mode (not supported)
             // param2 : mission index/ target id (not supported)
@@ -1437,11 +1460,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                    packet.time_usec/1000,
                    loc, vel, 10, 0, true);
 
-        if (!ap.home_is_set) {
-            init_home();
-        }
-
-
         // rad/sec
         Vector3f gyros;
         gyros.x = packet.rollspeed;
diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index 723a69ffe2..c0362501ee 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -111,7 +111,7 @@ static bool start_command(const AP_Mission::Mission_Command& cmd)
         break;
 
     case MAV_CMD_DO_SET_HOME:             // 179
-        // unsupported as mission command
+        do_set_home(cmd);
         break;
 
     case MAV_CMD_DO_SET_SERVO:
@@ -864,12 +864,12 @@ static void do_change_speed(const AP_Mission::Mission_Command& cmd)
 
 static void do_set_home(const AP_Mission::Mission_Command& cmd)
 {
-    if(cmd.p1 == 1) {
-        init_home();
+    if(cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) {
+        set_home_to_current_location();
     } else {
-        Location loc = cmd.content.location;
-        ahrs.set_home(loc);
-        set_home_is_set(true);
+        if (!far_from_EKF_origin(cmd.content.location)) {
+            set_home(cmd.content.location);
+        }
     }
 }