Allow classic doReposition

mavsdk_wrapper.cpp

@@ -300,31 +300,36 @@ static double bearing(double lat1, double lon1, double lat2, double lon2) {
     return atan2(x, y);
 }
 
-int setTargetCoordinates(double la, double lo, float a) {
-    double b, laRad = toRad(la), loRad = toRad(lo), newLa, newLo;
+int setTargetCoordinates(double la, double lo, float a, bool going_through) {
     int result;
-    float addedDistance = (2 * DEFAULT_RADIUS) / EARTH_RADIUS;
-    Telemetry::Position position = telemetry->position();
-
     if (!mavsdk_started)
         return -1;
 
-    b = bearing(position.latitude_deg, position.longitude_deg, la, lo);
-    newLa = asin(
-      sin(laRad) * cos(addedDistance)
-      + cos(laRad) * sin(addedDistance) * cos(b)
-    );
-    newLo = loRad + atan2(
-      sin(b) * sin(addedDistance) * cos(laRad),
-      cos(addedDistance) - sin(laRad) * sin(newLa)
-    );
+    if (going_through) {
+      double b, laRad = toRad(la), loRad = toRad(lo), newLa, newLo;
+      float addedDistance = (2 * DEFAULT_RADIUS) / EARTH_RADIUS;
+      Telemetry::Position position = telemetry->position();
+
+      b = bearing(position.latitude_deg, position.longitude_deg, la, lo);
+      newLa = asin(
+        sin(laRad) * cos(addedDistance)
+        + cos(laRad) * sin(addedDistance) * cos(b)
+      );
+      newLo = loRad + atan2(
+        sin(b) * sin(addedDistance) * cos(laRad),
+        cos(addedDistance) - sin(laRad) * sin(newLa)
+      );
+
+      result = doReposition(
+          (float)toDeg(newLa),
+          (float)toDeg(newLo),
+          DEFAULT_RADIUS,
+          0
+      );
+    } else {
+      result = doReposition((float)la, (float)lo, DEFAULT_RADIUS, 0);
+    }
 
-    result = doReposition(
-        (float)toDeg(newLa),
-        (float)toDeg(newLo),
-        DEFAULT_RADIUS,
-        0
-    );
     result |= setAltitude(a);
     return result;
 }