Add return value for setTargetCoordinates and loiter

mavsdk_wrapper.cpp

@@ -54,6 +54,8 @@ static Coordinates projected_destination;
 
 static bool do_projection = false;
 static bool is_in_air = false;
+static bool do_reposition_sent = false;
+static bool override_sent = false;
 
 // Logs functions
 
@@ -149,19 +151,27 @@ static int doReposition(float la, float lo, float radius, float y) {
     command.target_sysid = mavlink_passthrough->get_target_sysid();
     command.target_compid = mavlink_passthrough->get_target_compid();
 
-    return doMavlinkCommand(command, "Entering loiter mode failed");
+    int res = doMavlinkCommand(command, "Entering loiter mode failed");
+    do_reposition_sent = false;
+    return res;
 }
 
-static void doReposition_async(float la, float lo, float radius, float y) {
+static int doReposition_async(float la, float lo, float radius, float y) {
+    if (do_reposition_sent) {
+        log_error("Command DO_REPOSITION already in working queue.");
+        return -1;
+    }
+    do_reposition_sent = true;
     std::thread(doReposition, la, lo, radius, y).detach();
+    return 0;
 }
 
-static void updateProjection(int64_t current_lat, int64_t current_lon) {
+static int updateProjection(int64_t current_lat, int64_t current_lon) {
     Coordinates position{ (double)current_lat / 1e7, (double)current_lon / 1e7 };
     projected_destination = getProjectionCoordinates(targeted_destination,
                                                      position);
 
-    doReposition_async(
+    return doReposition_async(
         (float)projected_destination.latitude,
         (float)projected_destination.longitude,
         DEFAULT_RADIUS,
@@ -184,13 +194,21 @@ static int doOverride(float altitude, float airspeed, const char* failure_msg) {
     command.target_sysid = mavlink_passthrough->get_target_sysid();
     command.target_compid = mavlink_passthrough->get_target_compid();
 
-    return doMavlinkCommand(command, failure_msg);
+    int res = doMavlinkCommand(command, failure_msg);
+    override_sent = false;
+    return res;
 }
 
-static void doOverride_async(float altitude, float airspeed) {
+static int doOverride_async(float altitude, float airspeed) {
+    if (override_sent) {
+        log_error("Command DO_REPOSITION already in working queue.");
+        return -1;
+    }
+    override_sent = true;
     std::thread(
         doOverride, altitude, airspeed, "Overriding altitude and speed failed"
     ).detach();
+    return 0;
 }
 
 void updateLogAndProjection(void) {
@@ -344,25 +362,23 @@ int land(void) {
 
 // Flight management functions
 
-void loiter(double latitude, double longitude, float altitude,
+int loiter(double latitude, double longitude, float altitude,
             float radius, float speed) {
     targeted_destination.latitude = latitude;
     targeted_destination.longitude = longitude;
     targeted_radius = radius;
     do_projection = false;
 
-    doReposition_async((float)latitude, (float)longitude, radius, 0);
-    doOverride_async(altitude, speed);
+    return doReposition_async((float)latitude, (float)longitude, radius, 0) && doOverride_async(altitude, speed);
 }
 
-void setTargetCoordinates(double latitude, double longitude, float altitude,
+int setTargetCoordinates(double latitude, double longitude, float altitude,
                           float speed) {
     targeted_destination.latitude = latitude;
     targeted_destination.longitude = longitude;
     do_projection = true;
     
-    updateProjection(current_position[0], current_position[1]);
-    doOverride_async(altitude, speed);
+    return updateProjection(current_position[0], current_position[1]) && doOverride_async(altitude, speed);
 }
 
 // Information functions