Fix telemetry logging when drone is already flying

mavsdk_wrapper.cpp

@@ -34,7 +34,6 @@ static std::future<std::shared_ptr<System>> fut;
 static const float DEFAULT_RADIUS = 100;
 static const float EARTH_RADIUS = 6371000;
 
-static bool takingOff = false;
 static int mavsdk_started = 0;
 static uint64_t init_timestamp;
 static Telemetry::Position origin;
@@ -128,44 +127,29 @@ int start(const char * ip, int port, const char * log_file, int timeout) {
     action = new Action(msystem);
     mavlink_passthrough = new MavlinkPassthrough(msystem);
 
-    telemetry->subscribe_flight_mode([](Telemetry::FlightMode flight_mode) {
-        if (takingOff && flight_mode != Telemetry::FlightMode::Takeoff) {
-            log("Subscribing to raw GPS...");
-            telemetry->subscribe_raw_gps([](Telemetry::RawGps gps) {
-                std::ostringstream oss;
-                oss << (gps.timestamp_us - init_timestamp) / 1000 << ";"
-                    << gps.latitude_deg << ";" << gps.longitude_deg << ";"
-                    << gps.absolute_altitude_m << ";" << telemetry->position().relative_altitude_m << ";"
-                    << telemetry->attitude_euler().yaw_deg << ";"
-                    << gps.velocity_m_s << ";" << telemetry->fixedwing_metrics().climb_rate_m_s;
-                log(oss.str());
-            });          
-        }
-
-        switch(flight_mode) {
-            case Telemetry::FlightMode::Takeoff:
-                  takingOff = true;
-                  log("Taking off...");
-                  break;
-
-            default:
-                if (takingOff) {
-                    takingOff = false;
-                    init_timestamp = telemetry->raw_gps().timestamp_us;
-                }
-        }
-    });
-
     do {
         log("Waiting for first telemetry");
         sleep_for(seconds(1));
         abs_altitude = getAltitude();
     } while(isnan(abs_altitude) || abs_altitude == 0);
+
     origin = telemetry->position();
+    init_timestamp = telemetry->raw_gps().timestamp_us;
 
     log("MAVSDK started...");
     mavsdk_started = 1;
     log("timestamp (ms);latitude (°);longitude (°);AMSL (m);rel altitude (m);yaw (°);ground speed (m/s);climb rate (m/s)");
+
+    log("Subscribing to raw GPS...");
+    telemetry->subscribe_raw_gps([](Telemetry::RawGps gps) {
+        std::ostringstream oss;
+        oss << (gps.timestamp_us - init_timestamp) / 1000 << ";"
+            << gps.latitude_deg << ";" << gps.longitude_deg << ";"
+            << gps.absolute_altitude_m << ";" << telemetry->position().relative_altitude_m << ";"
+            << telemetry->attitude_euler().yaw_deg << ";"
+            << gps.velocity_m_s << ";" << telemetry->fixedwing_metrics().climb_rate_m_s;
+        log(oss.str());
+    });
     return 0;
 }
 
@@ -221,7 +205,7 @@ int takeOffAndWait(void) {
     if (takeOff() < 0)
         return -1;
 
-    while (!takingOff)
+    while (telemetry->flight_mode() != Telemetry::FlightMode::Takeoff)
         sleep_for(seconds(1));
     while (telemetry->flight_mode() == Telemetry::FlightMode::Takeoff)
         sleep_for(seconds(1));