Require speed to set coordinates

mavsdk_wrapper.cpp

@@ -53,12 +53,7 @@ static Coordinates targeted_destination;
 static float targeted_radius = DEFAULT_RADIUS;
 static Coordinates projected_destination;
 
-static const float DEFAULT_SPEED = 17;
-static float last_override_altitude;
-static float last_override_speed;
-
 static bool do_projection = false;
-static bool first_coordinate_entered = false;
 static bool is_in_air = false;
 
 // Logs functions
@@ -167,9 +162,6 @@ static int doReposition(float la, float lo, float radius, float y) {
 }
 
 static void doReposition_async(float la, float lo, float radius, float y) {
-    if (!first_coordinate_entered) {
-      first_coordinate_entered = true;
-    }
     std::thread(doReposition, la, lo, radius, y).detach();
 }
 
@@ -191,34 +183,26 @@ 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) {
+static int doOverride(float altitude, float airspeed, const char* failure_msg) {
     if (!mavsdk_started)
         return -1;
 
-    if (!first_coordinate_entered) {
-        log_error("Not overriding altitude before user sets coordinates");
-        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.param4 = airspeed;
     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();
+static void doOverride_async(float altitude, float airspeed) {
+    std::thread(
+      doOverride, altitude, airspeed, "Overriding altitude and speed failed"
+    ).detach();
 }
 
 void updateLogAndProjection(void) {
@@ -236,18 +220,16 @@ void updateLogAndProjection(void) {
             << metrics.airspeed_m_s << ";" << metrics.climb_rate_m_s;
         log(oss.str());
 
-        if (first_coordinate_entered) {
-          if (do_projection) {
-              updateProjection(current_position[LATITUDE],
-                               current_position[LONGITUDE]);
-          } else {
-            doReposition_async(
-                (float)targeted_destination.latitude,
-                (float)targeted_destination.longitude,
-                targeted_radius,
-                0
-            ); 
-          }
+        if (do_projection) {
+            updateProjection(current_position[LATITUDE],
+                             current_position[LONGITUDE]);
+        } else {
+          doReposition_async(
+              (float)targeted_destination.latitude,
+              (float)targeted_destination.longitude,
+              targeted_radius,
+              0
+          );
         }
     }
 }
@@ -318,8 +300,6 @@ int start(const char * ip, int port, const char * log_file, int timeout) {
     } while(isnan(abs_altitude) || abs_altitude == 0);
 
     origin = telemetry->position();
-    last_override_altitude = origin.absolute_altitude_m + DEFAULT_OVERRIDE_ALTITUDE;
-    last_override_speed = DEFAULT_SPEED;
 
     log("MAVSDK started...");
     mavsdk_started = 1;
@@ -400,38 +380,30 @@ int triggerParachute(void) {
 
 // Flight management functions
 
-void loiter(double la, double lo, float a, float radius) {
-    targeted_destination.latitude = la;
-    targeted_destination.longitude = lo;
+void 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)la, (float)lo, radius, 0);
-    setAltitude_async(a);
-}
-
-static int setAirSpeed(float airspeed) {
-    last_override_speed = airspeed;
-    return doOverride(last_override_altitude, airspeed,
-                      "Setting airspeed failed");
-}
-
-void setAirSpeed_async(float airspeed) {
-    std::thread(setAirSpeed, airspeed).detach();
+    doReposition_async((float)latitude, (float)longitude, radius, 0);
+    doOverride_async(altitude, speed);
 }
 
-void setTargetCoordinates(double la, double lo, float a) {
+void setTargetCoordinates(double latitude, double longitude, float altitude,
+                          float speed) {
     if (!mavsdk_started) {
         log_error("Mavsdk not started");
         return;
     }
 
-    targeted_destination.latitude = la;
-    targeted_destination.longitude = lo;
+    targeted_destination.latitude = latitude;
+    targeted_destination.longitude = longitude;
     do_projection = true;
     
     updateProjection(current_position[0], current_position[1]);
-    setAltitude_async(a);
+    doOverride_async(altitude, speed);
 }
 
 // Information functions