AP_Math: added locations_are_same(loc1,loc2) helper

returns true if lat and lng are the same, ignores alt and options

libraries/AP_Math/AP_Math.h

@@ -150,6 +150,11 @@ float wrap_180_cd_float(float angle);
  */
 float wrap_PI(float angle_in_radians);
 
+/*
+ * check if lat and lng match. Ignore altitude and options
+ */
+bool locations_are_same(const struct Location &loc1, const struct Location &loc2);
+
 /*
   print a int32_t lat/long in decimal degrees
  */

libraries/AP_Math/location.cpp

@@ -222,6 +222,13 @@ float wrap_PI(float angle_in_radians)
     return angle_in_radians;
 }
 
+/*
+  return true if lat and lng match. Ignores altitude and options
+ */
+bool locations_are_same(const struct Location &loc1, const struct Location &loc2) {
+    return (loc1.lat == loc2.lat) && (loc1.lng == loc2.lng);
+}
+
 /*
   print a int32_t lat/long in decimal degrees
  */