Initial version of ArduPilot compatible autopilot

- removed PX4 and MAVSDK git submodules

src/az_utils.cpp

@@ -0,0 +1,18 @@
+#include <math.h>
+#include "az_utils.h"
+
+AzUtils::AzUtils() {}
+
+#define EARTH_RADIUS 6371.0 // Earth radius in kilometers
+
+double degrees_to_radians(double degrees) {
+    return degrees * M_PI / 180.0;
+}
+
+double AzUtils::distance(double lat1, double lon1, double lat2, double lon2) {
+    double dlat = degrees_to_radians(lat2 - lat1);
+    double dlon = degrees_to_radians(lon2 - lon1);
+    double a = sin(dlat/2) * sin(dlat/2) + cos(degrees_to_radians(lat1)) * cos(degrees_to_radians(lat2)) * sin(dlon/2) * sin(dlon/2);
+    double c = 2 * atan2(sqrt(a), sqrt(1-a));
+    return EARTH_RADIUS * c;
+}