Allow classic doReposition

mavsdk_wrapper.cpp

@@ -300,15 +300,16 @@ 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;
 
+    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)
@@ -325,6 +326,10 @@ int setTargetCoordinates(double la, double lo, float a) {
           DEFAULT_RADIUS,
           0
       );
+    } else {
+      result = doReposition((float)la, (float)lo, DEFAULT_RADIUS, 0);
+    }
+
     result |= setAltitude(a);
     return result;
 }