AP_Arming: check ahrs and gps differ by less than 10m

libraries/AP_Arming/AP_Arming.cpp

@@ -23,6 +23,7 @@
 #define AP_ARMING_BOARD_VOLTAGE_MIN     4.3f
 #define AP_ARMING_BOARD_VOLTAGE_MAX     5.8f
 #define AP_ARMING_ACCEL_ERROR_THRESHOLD 0.75f
+#define AP_ARMING_AHRS_GPS_ERROR_MAX    10      // accept up to 10m difference between AHRS and GPS
 
 extern const AP_HAL::HAL& hal;
 
@@ -374,6 +375,19 @@ bool AP_Arming::gps_checks(bool report)
             }
             return false;
         }
+
+        // check AHRS and GPS are within 10m of each other
+        Location gps_loc = ahrs.get_gps().location();
+        Location ahrs_loc;
+        if (ahrs.get_position(ahrs_loc)) {
+            float distance = location_3d_diff_NED(gps_loc, ahrs_loc).length();
+            if (distance > AP_ARMING_AHRS_GPS_ERROR_MAX) {
+                if (report) {
+                    gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: GPS and AHRS differ by %4.1fm", (double)distance);
+                }
+                return false;
+            }
+        }
     }
 
     if ((checks_to_perform & ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_GPS_CONFIG)) {