Multicopters update

See merge request !2

mavsdk_wrapper.cpp

@@ -44,7 +44,6 @@ static const float ADDED_DISTANCE = (2 * DEFAULT_RADIUS) / EARTH_RADIUS;
 static const double COS_ADDED_DISTANCE = cos(ADDED_DISTANCE);
 static const double SIN_ADDED_DISTANCE = sin(ADDED_DISTANCE);
 
-static int mavsdk_started = 0;
 static Telemetry::Position origin;
 static int64_t current_position[POSITION_ARRAY_SIZE] = {0};
 static int64_t timestamp_difference = 0;
@@ -53,13 +52,10 @@ 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;
+static bool do_reposition_sent = false;
+static bool override_sent = false;
 
 // Logs functions
 
@@ -81,11 +77,6 @@ static void log_error_from_result(std::string message, Enumeration result) {
 // Action functions
 
 static int doAction(Action::Result (Action::*toDo)(void) const, std::string failure_message) {
-    if(!mavsdk_started) {
-        log_error("Mavsdk not started");
-        return -1;
-    }
-
     const Action::Result result = (action->*toDo)();
     if(result != Action::Result::Success) {
         log_error_from_result(failure_message, result);
@@ -149,9 +140,6 @@ static Coordinates getProjectionCoordinates(Coordinates destination, Coordinates
 }
 
 static int doReposition(float la, float lo, float radius, float y) {
-    if (!mavsdk_started)
-        return -1;
-
     MavlinkPassthrough::CommandLong command;
     command.command = MAV_CMD_DO_REPOSITION;
     command.param1 = -1; // Ground speed, -1 for default
@@ -163,22 +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) {
-    if (!first_coordinate_entered) {
-      first_coordinate_entered = true;
+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,
@@ -191,41 +184,37 @@ 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;
-
-    if (!first_coordinate_entered) {
-        log_error("Not overriding altitude before user sets coordinates");
-        return -1;
-    }
-
+static int doOverride(float altitude, float airspeed, const char* failure_msg) {
     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");
+    int res = doMavlinkCommand(command, failure_msg);
+    override_sent = false;
+    return res;
 }
 
-static void setAltitude_async(float altitude) {
-    std::thread(setAltitude, altitude).detach();
+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) {
     std::ostringstream oss;
     Telemetry::FixedwingMetrics metrics;
 
-    if(mavsdk_started) {
     metrics = telemetry->fixedwing_metrics();
     oss << current_position[TIMESTAMP] << ";"
         << current_position[LATITUDE] << ";"
@@ -236,7 +225,6 @@ 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]);
@@ -248,8 +236,6 @@ void updateLogAndProjection(void) {
           0
       );
     }
-        }
-    }
 }
 
 // Connexion management functions
@@ -308,6 +294,7 @@ int start(const char * ip, int port, const char * log_file, int timeout) {
     telemetry->subscribe_landed_state([](const Telemetry::LandedState landed_state) {
         if (landed_state == Telemetry::LandedState::InAir) {
             is_in_air = true;
+            doAction(&Action::transition_to_fixedwing, "Transition to fixedwing failed");
         }
     });
 
@@ -318,11 +305,7 @@ 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;
     log("timestamp (ms);latitude (°E7);longitude (°E7);AMSL (mm);rel altitude (mm);yaw (°);air speed (m/s);climb rate (m/s)");
 
     return 0;
@@ -347,10 +330,8 @@ int reboot() {
 // Flight state management functions
 
 int arm(void) {
-    if(!mavsdk_started)
-        return -1;
-
     if (telemetry->armed()) {
+      log("Drone already armed");
       return 0;
     }
 
@@ -365,29 +346,10 @@ int arm(void) {
 
 int takeOff(void) {
     action->set_takeoff_altitude(DEFAULT_OVERRIDE_ALTITUDE);
-    if (doAction(&Action::takeoff, "Takeoff failed"))
-        return -1;
-
-    return 0;
-}
-
-int takeOffAndWait(void) {
-    if (takeOff() < 0)
-        return -1;
-
-    while (telemetry->flight_mode() != Telemetry::FlightMode::Takeoff)
-        sleep_for(seconds(1));
-    while (telemetry->flight_mode() == Telemetry::FlightMode::Takeoff)
-        sleep_for(seconds(1));
-
-    return doAction(&Action::transition_to_fixedwing,
-                    "Transition to fixedwing failed");
+    return doAction(&Action::takeoff, "Takeoff failed");
 }
 
-int triggerParachute(void) {
-    if (!mavsdk_started)
-        return -1;
-
+int land(void) {
     log("Landing...");
     MavlinkPassthrough::CommandLong command;
     command.command = MAV_CMD_DO_PARACHUTE;
@@ -400,38 +362,23 @@ int triggerParachute(void) {
 
 // Flight management functions
 
-void loiter(double la, double lo, float a, float radius) {
-    targeted_destination.latitude = la;
-    targeted_destination.longitude = lo;
+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)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();
+    return doReposition_async((float)latitude, (float)longitude, radius, 0) && doOverride_async(altitude, speed);
 }
 
-void setTargetCoordinates(double la, double lo, float a) {
-    if (!mavsdk_started) {
-        log_error("Mavsdk not started");
-        return;
-    }
-
-    targeted_destination.latitude = la;
-    targeted_destination.longitude = lo;
+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]);
-    setAltitude_async(a);
+    return updateProjection(current_position[0], current_position[1]) && doOverride_async(altitude, speed);
 }
 
 // Information functions
@@ -495,8 +442,8 @@ int gpsIsOk(void) {
     return (fixType != Telemetry::FixType::NoGps) && (fixType != Telemetry::FixType::NoFix);
 }
 
-int healthAllOk(void) {
-    return telemetry->health_all_ok();
+int isReadyToFly(void) {
+    return is_in_air;
 }
 
 int isLanding(void) {