APMRover2: Reduce the home position reset when disarm

APMrover2/APMrover2.cpp

@@ -359,6 +359,13 @@ void Rover::one_second_loop(void)
 
     ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
 
+    // update home position if not soft armed and gps position has
+    // changed. Update every 1s at most
+    if (!hal.util->get_soft_armed() &&
+        gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+        update_home();
+    }
+
     // update error mask of sensors and subsystems. The mask uses the
     // MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
     // indicates that the sensor or subsystem is present but not
@@ -429,10 +436,6 @@ void Rover::update_GPS_10Hz(void)
             do_take_picture();
         }
 #endif
-
-        if (!hal.util->get_soft_armed()) {
-            update_home();
-        }
     }
 }
 

APMrover2/commands.cpp

@@ -105,9 +105,11 @@ void Rover::update_home()
     if (home_is_set == HOME_SET_NOT_LOCKED) {
         Location loc;
         if (ahrs.get_position(loc)) {
-            ahrs.set_home(loc);
-            Log_Write_Home_And_Origin();
-            GCS_MAVLINK::send_home_all(gps.location());
+            if (get_distance(loc, ahrs.get_home()) > DISTANCE_HOME_MAX) {
+                ahrs.set_home(loc);
+                Log_Write_Home_And_Origin();
+                GCS_MAVLINK::send_home_all(gps.location());
+            }
         }
     }
     barometer.update_calibration();

APMrover2/defines.h

@@ -110,3 +110,4 @@ enum fs_crash_action {
   FS_CRASH_HOLD_AND_DISARM = 2
 };
 
+#define DISTANCE_HOME_MAX 0.5f  // Distance max to home location before changing it when disarm