Copter: set EKF origin from first do-set-home command

ArduCopter/GCS_Mavlink.cpp

@@ -1176,10 +1176,8 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
                 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 (!copter.far_from_EKF_origin(new_home_loc)) {
-                    if (copter.set_home_and_lock(new_home_loc)) {
-                        result = MAV_RESULT_ACCEPTED;
-                    }
+                if (copter.set_home_and_lock(new_home_loc)) {
+                    result = MAV_RESULT_ACCEPTED;
                 }
             }
             break;
@@ -2019,9 +2017,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
             new_home_loc.lat = packet.latitude;
             new_home_loc.lng = packet.longitude;
             new_home_loc.alt = packet.altitude / 10;
-            if (copter.far_from_EKF_origin(new_home_loc)) {
-                break;
-            }
             copter.set_home_and_lock(new_home_loc);
         }
         break;

ArduCopter/commands.cpp

@@ -76,6 +76,18 @@ bool Copter::set_home(const Location& loc)
         return false;
     }
 
+    // set EKF origin to home if it hasn't been set yet and vehicle is disarmed
+    // this is required to allowing flying in AUTO and GUIDED with only an optical flow
+    Location ekf_origin;
+    if (!motors->armed() && !ahrs.get_origin(ekf_origin)) {
+        ahrs.set_origin(loc);
+    }
+
+    // check home is close to EKF origin
+    if (far_from_EKF_origin(loc)) {
+        return false;
+    }
+
     // set ahrs home (used for RTL)
     ahrs.set_home(loc);
 

ArduCopter/commands_logic.cpp

@@ -1104,9 +1104,7 @@ void Copter::do_set_home(const AP_Mission::Mission_Command& cmd)
     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 {
-        if (!far_from_EKF_origin(cmd.content.location)) {
-            set_home(cmd.content.location);
-        }
+        set_home(cmd.content.location);
     }
 }