debug distance

mavsdk_wrapper.cpp

@@ -118,6 +118,14 @@ static double bearing(double lat1, double lon1, double lat2, double lon2) {
     return atan2(x, y);
 }
 
+// static double distance(double lat1, double lon1, double lat2, double lon2) {
+//     double la1 = toRad(lat1), la2 = toRad(lat2), lo1 = toRad(lon1), lo2 = toRad(lon2);
+//     double haversine_phi = pow(sin((la2 - la1) / 2), 2);
+//     double sin_lon = sin((lo2 - lo1) / 2);
+//     double h = haversine_phi + cos(la1) * cos(la2) * sin_lon * sin_lon;
+//     return 2 * EARTH_RADIUS * asin(sqrt(h));
+// }
+
 static Coordinates project(Coordinates destination, double bearing) {
     double laRad = toRad(destination.latitude);
     double sinLa = sin(laRad);
@@ -159,6 +167,8 @@ static int updateProjection(double current_lat, double current_lon) {
         bearing(current_lat, current_lon,
                 targeted_destination.latitude, targeted_destination.longitude)
     );
+    // printf("distance between follower and projection %f\n", distance(current_lat, current_lon, targeted_destination.latitude, targeted_destination.longitude));
+    // printf("bearing %f\n", toDeg(bearing(targeted_destination.latitude, targeted_destination.longitude, projected_destination.latitude, projected_destination.longitude)));
 
     return doReposition(
         (float)projected_destination.latitude,