Fix loiter

Stop projecting destination

mavsdk_wrapper.cpp

@@ -48,12 +48,15 @@ static int64_t current_position[POSITION_ARRAY_SIZE] = {0};
 static int64_t timestamp_difference = 0;
 
 static Coordinates targeted_destination;
+static float targeted_radius = DEFAULT_RADIUS;
 static Coordinates projected_destination;
 
 static const float DEFAULT_SPEED = 16;
 static float last_override_altitude;
 static float last_override_speed;
 
+static bool do_projection = false;
+
 // Logs functions
 
 static void log(std::string message) {
@@ -181,6 +184,31 @@ static long long int getTimestamp() {
     return std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count();
 }
 
+static int doOverride(float altitude, float speed, const char* failure_msg) {
+    if (!mavsdk_started)
+        return -1;
+
+    MavlinkPassthrough::CommandLong command;
+    command.command = MAV_CMD_DO_OVERRIDE;
+    command.param1 = 1 | 2 | 4 | 8;
+    command.param2 = 1 | 2 | 4 | 8;
+    command.param3 = altitude;
+    command.param4 = speed;
+    command.target_sysid = mavlink_passthrough->get_target_sysid();
+    command.target_compid = mavlink_passthrough->get_target_compid();
+
+    return doMavlinkCommand(command, failure_msg);
+}
+
+static int setAltitude(float altitude) {
+    last_override_altitude = altitude;
+    return doOverride(altitude, last_override_speed, "Setting altitude failed");
+}
+
+static void setAltitude_async(float altitude) {
+    std::thread(setAltitude, altitude).detach();
+}
+
 void updateLogAndProjection(void) {
     std::ostringstream oss;
     Telemetry::FixedwingMetrics metrics;
@@ -194,8 +222,15 @@ void updateLogAndProjection(void) {
             << metrics.airspeed_m_s << ";" << metrics.climb_rate_m_s;
         log(oss.str());
 
-        if (projected_destination.latitude != 0 || targeted_destination.latitude != 0) {
+        if (do_projection) {
             updateProjection(current_position[0], current_position[1]);
+        } else {
+          doReposition_async(
+              (float)targeted_destination.latitude,
+              (float)targeted_destination.longitude,
+              targeted_radius,
+              0
+          ); 
         }
     }
 }
@@ -341,32 +376,12 @@ int triggerParachute(void) {
 
 // Flight management functions
 
-static int doOverride(float altitude, float speed, const char* failure_msg) {
-    if (!mavsdk_started)
-        return -1;
-
-    MavlinkPassthrough::CommandLong command;
-    command.command = MAV_CMD_DO_OVERRIDE;
-    command.param1 = 1 | 2 | 4 | 8;
-    command.param2 = 1 | 2 | 4 | 8;
-    command.param3 = altitude;
-    command.param4 = speed;
-    command.target_sysid = mavlink_passthrough->get_target_sysid();
-    command.target_compid = mavlink_passthrough->get_target_compid();
-
-    return doMavlinkCommand(command, failure_msg);
-}
-
-static int setAltitude(float altitude) {
-    last_override_altitude = altitude;
-    return doOverride(altitude, last_override_speed, "Setting altitude failed");
-}
-
-static void setAltitude_async(float altitude) {
-    std::thread(setAltitude, altitude).detach();
-}
-
 void loiter(double la, double lo, float a, float radius) {
+    targeted_destination.latitude = la;
+    targeted_destination.longitude = lo;
+    targeted_radius = radius;
+    do_projection = false;
+
     doReposition_async((float)la, (float)lo, radius, 0);
     setAltitude_async(a);
 }
@@ -389,6 +404,7 @@ void setTargetCoordinates(double la, double lo, float a) {
 
     targeted_destination.latitude = la;
     targeted_destination.longitude = lo;
+    do_projection = true;
     
     updateProjection(current_position[0], current_position[1]);
     setAltitude_async(a);