Fix autocontinue and angles > 180

Fix autocontinue to go straight
Fix angle difference when absolute value is greater than 180

mavsdk_wrapper.cpp

@@ -37,6 +37,7 @@ static std::future<std::shared_ptr<System>> fut;
 static const double EARTH_RADIUS = 6371000.F;
 
 static bool autocontinue = false;
+static double initialBearing;
 static double previousBearing;
 static double xy_ratio;
 static double target_latitude;
@@ -329,8 +330,9 @@ int mavsdk_setTargetCoordinates(double la, double lo, float a) {
 
     target_latitude = la;
     target_longitude = lo;
-    previousBearing = bearing(position.latitude_deg, position.longitude_deg,
-                              la, lo);
+    initialBearing = previousBearing = bearing(position.latitude_deg,
+                                               position.longitude_deg,
+                                               la, lo);
     autocontinue = false;
 
     if (!mavsdk_isInManualMode()) {
@@ -351,19 +353,35 @@ static int setManualControlInput(float x, float y, float z, float r) {
 }
 
 int mavsdk_setManualControlInput(void) {
-    if (autocontinue)
-        return setManualControlInput(0, 0, 0, 0);
+    double b;
 
-    Telemetry::Position position = telemetry->position();
-    double b = bearing(position.latitude_deg, position.longitude_deg,
-                       target_latitude, target_longitude);
-    if (abs(b - previousBearing) > 160) {
-        autocontinue = true;
+    if (autocontinue) {
+        b = initialBearing;
     } else {
-        previousBearing = b;
+        Telemetry::Position position = telemetry->position();
+        b = bearing(position.latitude_deg, position.longitude_deg,
+                    target_latitude, target_longitude);
+/*
+  If there is a difference of more than 160° (180° for a half circle and 20° of imprecision)
+  between the required bearing and the one some milliseconds ago,
+  it means that the target is now behind the drone so the drone just went over the target.
+
+  In this case, we don't follow the target anymore, we simply keep the same trajectory.
+*/
+        if (abs(b - previousBearing) > 160) {
+            autocontinue = true;
+        } else {
+            previousBearing = b;
+        }
     }
 
     float angle_diff = (float)b - mavsdk_getYaw();
+    if (angle_diff < -180) {
+        angle_diff += 360;
+    } else if (angle_diff > 180) {
+        angle_diff -= 360;
+    }
+
     return setManualControlInput(0, (angle_diff > 0 ? 1 : -1) * std::min(abs(angle_diff), 30.0f) / 30, 0, 0);
 }