Start logging once takeoff is done

include/mavsdk_wrapper.h

@@ -26,7 +26,6 @@ int mavsdk_reboot(void);
 
 // Flight state management functions
 int mavsdk_arm(void);
-int mavsdk_land(void);
 int mavsdk_takeOff(void);
 int mavsdk_takeOffAndWait(void);
 int mavsdk_triggerParachute(void);

mavsdk_wrapper.cpp

@@ -40,6 +40,7 @@ static const double MIN_YAW_DIFF = 1;
 static const double MAX_ROLL = 35;
 static const double YAW_VELOCITY_COEF = 0.5;
 
+static bool takingOff = false;
 static bool autocontinue = false;
 static double initialBearing;
 static double previousBearing;
@@ -61,9 +62,9 @@ static void log_error(std::string message) {
 
 template <class Enumeration>
 static void log_error_from_result(std::string message, Enumeration result) {
-  std::ostringstream oss;
-  oss << message << ": " << result;
-  log_error(oss.str());
+    std::ostringstream oss;
+    oss << message << ": " << result;
+    log_error(oss.str());
 }
 
 // Action functions
@@ -139,18 +140,33 @@ int mavsdk_start(const char * url, const char * log_file, int timeout) {
     manual_control = new ManualControl(msystem);
     mavlink_passthrough = new MavlinkPassthrough(msystem);
 
-    log("Subscribing to raw GPS...");
-    telemetry->subscribe_raw_gps([](Telemetry::RawGps gps) {
-        std::ostringstream oss;
-        Telemetry::FixedwingMetrics metrics = telemetry->fixedwing_metrics();
-        Telemetry::Position position = telemetry->position();
+    telemetry->subscribe_flight_mode([](Telemetry::FlightMode flight_mode) {
+        if (takingOff && flight_mode != Telemetry::FlightMode::Takeoff) {
+            takingOff = false;
+            log("Subscribing to raw GPS...");
+            telemetry->subscribe_raw_gps([](Telemetry::RawGps gps) {
+                std::ostringstream oss;
+                Telemetry::FixedwingMetrics metrics = telemetry->fixedwing_metrics();
+                Telemetry::Position position = telemetry->position();
+
+                oss << gps.timestamp_us << ";"
+                    << gps.latitude_deg << ";" << gps.longitude_deg << ";"
+                    << gps.absolute_altitude_m << ";" << position.relative_altitude_m << ";"
+                    << telemetry->attitude_euler().yaw_deg << ";"
+                    << metrics.airspeed_m_s << ";" << metrics.climb_rate_m_s;
+                log(oss.str());
+            });          
+        }
+
+        switch(flight_mode) {
+            case Telemetry::FlightMode::Takeoff:
+                  takingOff = true;
+                  log("Taking off...");
+                  break;
 
-        oss << gps.timestamp_us << ";"
-            << gps.latitude_deg << ";" << gps.longitude_deg << ";"
-            << gps.absolute_altitude_m << ";" << position.relative_altitude_m << ";"
-            << telemetry->attitude_euler().yaw_deg << ";"
-            << metrics.airspeed_m_s << ";" << metrics.climb_rate_m_s;
-        log(oss.str());
+            default:
+                return;
+        }
     });
 
     do {
@@ -208,33 +224,10 @@ int mavsdk_arm(void) {
     return doAction(&Action::arm, "Arming failed");
 }
 
-int mavsdk_land(void) {
-    log("Landing...");
-    if (doAction(&Action::terminate, "Land failed"))
-        return -1;
-
-    // Check if vehicle is still in air
-    while (telemetry->in_air()) {
-        log("Vehicle is landing...");
-        sleep_for(seconds(1));
-    }
-    log("Landed!");
-
-    while (telemetry->armed())
-        sleep_for(seconds(1));
-    log("Finished...");
-
-    return 0;
-}
-
 int mavsdk_takeOff(void) {
     if (doAction(&Action::takeoff, "Takeoff failed"))
         return -1;
 
-    while (telemetry->flight_mode() != Telemetry::FlightMode::Takeoff)
-        sleep_for(seconds(1));
-
-    log("Taking off...");
     return 0;
 }
 
@@ -242,6 +235,8 @@ int mavsdk_takeOffAndWait(void) {
     if (mavsdk_takeOff() < 0)
         return -1;
 
+    while (!takingOff)
+        sleep_for(seconds(1));
     while (telemetry->flight_mode() == Telemetry::FlightMode::Takeoff)
         sleep_for(seconds(1));
     return 0;
@@ -251,12 +246,28 @@ int mavsdk_triggerParachute(void) {
     if (!mavsdk_started)
         return -1;
 
+    log("Landing...");
     MavlinkPassthrough::CommandLong command;
     command.command = MAV_CMD_DO_PARACHUTE;
     command.param1 = 2; //see https://mavlink.io/en/messages/common.html#PARACHUTE_ACTION
     command.target_sysid = mavlink_passthrough->get_target_sysid();
     command.target_compid = mavlink_passthrough->get_target_compid();
-    return doMavlinkCommand(command, "Parachute release failed");
+    if (doMavlinkCommand(command, "Parachute release failed"))
+        return -1;
+
+    // Check if vehicule is still in air
+    while (telemetry->in_air()) {
+	      log("Vehicle is landing...");
+        sleep_for(seconds(1));
+    }
+    log("Landed!");
+
+    while (telemetry->armed())
+        sleep_for(seconds(1));
+    log("Finished...");
+    log_file_fd << std::flush;
+
+    return 0;
 }
 
 // Flight management functions

qjs_wrapper.c

@@ -442,12 +442,6 @@ static JSValue js_mavsdk_arm(JSContext *ctx, JSValueConst this_val,
     return JS_NewInt32(ctx, mavsdk_arm());
 }
 
-static JSValue js_mavsdk_land(JSContext *ctx, JSValueConst thisVal,
-                              int argc, JSValueConst *argv)
-{
-    return JS_NewInt32(ctx, mavsdk_land());
-}
-
 static JSValue js_mavsdk_takeOff(JSContext *ctx, JSValueConst thisVal,
                                  int argc, JSValueConst *argv)
 {
@@ -625,7 +619,6 @@ static const JSCFunctionListEntry js_mavsdk_funcs[] = {
     JS_CFUNC_DEF("stop", 0, js_mavsdk_stop ),
     JS_CFUNC_DEF("reboot", 0, js_mavsdk_reboot ),
     JS_CFUNC_DEF("arm", 0, js_mavsdk_arm ),
-    JS_CFUNC_DEF("land", 0, js_mavsdk_land ),
     JS_CFUNC_DEF("takeOff", 0, js_mavsdk_takeOff ),
     JS_CFUNC_DEF("takeOffAndWait", 0, js_mavsdk_takeOffAndWait ),
     JS_CFUNC_DEF("triggerParachute", 0, js_mavsdk_triggerParachute ),