Use doReposition and doOverride commands asynchronously

mavsdk_wrapper.cpp

@@ -161,16 +161,20 @@ static int doReposition(float la, float lo, float radius, float y) {
     return doMavlinkCommand(command, "Entering loiter mode failed");
 }
 
-static int updateProjection(double current_lat, double current_lon) {
+static void doReposition_async(float la, float lo, float radius, float y) {
+    std::thread(doReposition, la, lo, radius, y).detach();
+}
+
+static void updateProjection(double current_lat, double current_lon) {
     projected_destination = project(
         targeted_destination,
         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("distance between follower and projection in C %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(
+    doReposition_async(
         (float)projected_destination.latitude,
         (float)projected_destination.longitude,
         DEFAULT_RADIUS,
@@ -183,7 +187,7 @@ static long long int getTimestamp() {
     return std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count();
 }
 
-int updateLogAndProjection(void) {
+void updateLogAndProjection(void) {
     std::ostringstream oss;
     Telemetry::Position position;
     Telemetry::FixedwingMetrics metrics;
@@ -200,11 +204,9 @@ int updateLogAndProjection(void) {
         log(oss.str());
 
         if (projected_destination.latitude != 0 || targeted_destination.latitude != 0) {
-            return updateProjection(position.latitude_deg, position.longitude_deg);
+            updateProjection(position.latitude_deg, position.longitude_deg);
         }
     }
-
-    return 0;
 }
 
 // Connexion management functions
@@ -355,30 +357,37 @@ static int setAltitude(float altitude) {
     return doOverride(altitude, last_override_speed, "Setting altitude failed");
 }
 
-int loiter(double la, double lo, float a, float radius) {
-    int result = doReposition((float)la, (float)lo, radius, 0);
-    result |= setAltitude(a);
-    return result;
+static void setAltitude_async(float altitude) {
+    std::thread(setAltitude, altitude).detach();
+}
+
+void loiter(double la, double lo, float a, float radius) {
+    doReposition_async((float)la, (float)lo, radius, 0);
+    setAltitude_async(a);
 }
 
-int setAirSpeed(float airspeed) {
+static int setAirSpeed(float airspeed) {
     last_override_speed = airspeed;
     return doOverride(last_override_altitude, airspeed,
                       "Setting airspeed failed");
 }
 
-int setTargetCoordinates(double la, double lo, float a) {
-    int result;
-    if (!mavsdk_started)
-        return -1;
+void setAirSpeed_async(float airspeed) {
+    std::thread(setAirSpeed, airspeed).detach();
+}
+
+void setTargetCoordinates(double la, double lo, float a) {
+    if (!mavsdk_started) {
+        log_error("Mavsdk not started");
+        return;
+    }
 
     Telemetry::Position position = telemetry->position();
     targeted_destination.latitude = la;
     targeted_destination.longitude = lo;
     
-    result = updateProjection(position.latitude_deg, position.longitude_deg);
-    result |= setAltitude(a);
-    return result;
+    updateProjection(position.latitude_deg, position.longitude_deg);
+    setAltitude_async(a);
 }
 
 // Information functions