Stop using blocking overrides

mavsdk_wrapper.cpp

@@ -45,6 +45,10 @@ static Telemetry::Position origin;
 static Coordinates targeted_destination;
 static Coordinates projected_destination;
 
+static const float DEFAULT_SPEED = 16;
+static float last_override_altitude;
+static float last_override_speed;
+
 // Logs functions
 
 static void log(std::string message) {
@@ -230,6 +234,8 @@ 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;
+    last_override_speed = DEFAULT_SPEED;
 
     log("MAVSDK started...");
     mavsdk_started = 1;
@@ -289,7 +295,8 @@ int takeOffAndWait(void) {
     while (telemetry->flight_mode() == Telemetry::FlightMode::Takeoff)
         sleep_for(seconds(1));
 
-    return doAction(&Action::transition_to_fixedwing, "Transition to fixedwing failed");
+    return doAction(&Action::transition_to_fixedwing,
+                    "Transition to fixedwing failed");
 }
 
 int triggerParachute(void) {
@@ -299,7 +306,8 @@ int triggerParachute(void) {
     log("Landing...");
     MavlinkPassthrough::CommandLong command;
     command.command = MAV_CMD_DO_PARACHUTE;
-    command.param1 = 2; //see https://mavlink.io/en/messages/common.html#PARACHUTE_ACTION
+    //see https://mavlink.io/en/messages/common.html#PARACHUTE_ACTION
+    command.param1 = 2;
     command.target_sysid = mavlink_passthrough->get_target_sysid();
     command.target_compid = mavlink_passthrough->get_target_compid();
     return doMavlinkCommand(command, "Parachute release failed");
@@ -307,19 +315,25 @@ int triggerParachute(void) {
 
 // Flight management functions
 
-static int setAltitude(float altitude) {
+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 | 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, "Setting altitude failed");
+    return doMavlinkCommand(command, failure_msg);
+}
+
+static int setAltitude(float altitude) {
+    last_override_altitude = altitude;
+    return doOverride(altitude, last_override_speed, "Setting altitude failed");
 }
 
 int loiter(double la, double lo, float a, float radius) {
@@ -329,18 +343,9 @@ int loiter(double la, double lo, float a, float radius) {
 }
 
 int setAirSpeed(float airspeed) {
-    if (!mavsdk_started)
-        return -1;
-
-    MavlinkPassthrough::CommandLong command;
-    command.command = MAV_CMD_DO_OVERRIDE;
-    command.param1 = 1 | 2 | 4 | 8;
-    command.param2 = 2 | 4 | 8;
-    command.param4 = airspeed;
-    command.target_sysid = mavlink_passthrough->get_target_sysid();
-    command.target_compid = mavlink_passthrough->get_target_compid();
-
-    return doMavlinkCommand(command, "Setting airspeed failed");
+    last_override_speed = airspeed;
+    return doOverride(last_override_altitude, airspeed,
+                      "Setting airspeed failed");
 }
 
 int setTargetCoordinates(double la, double lo, float a) {
@@ -399,7 +404,8 @@ double *getPositionArray(void) {
 
 float *getSpeedArray(void) {
     Telemetry::FixedwingMetrics metrics = telemetry->fixedwing_metrics();
-    float *speedArray = (float*) malloc(SPEED_ARRAY_SIZE * sizeof(float));
+    float *speedArray =
+        (float*) malloc(SPEED_ARRAY_SIZE * sizeof(float));
     speedArray[0] = getYaw();
     speedArray[1] = metrics.airspeed_m_s;
     speedArray[2] = metrics.climb_rate_m_s;