Use timestamp in milliseconds

mavsdk_wrapper.cpp

@@ -48,6 +48,7 @@ static double xy_ratio;
 static double target_latitude;
 static double target_longitude;
 static int mavsdk_started = 0;
+static uint64_t init_timestamp;
 static Telemetry::Position origin;
 
 // Logs functions
@@ -142,14 +143,13 @@ int mavsdk_start(const char * url, const char * log_file, int timeout) {
 
     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 << ";"
+                oss << (gps.timestamp_us - init_timestamp) / 1000 << ";"
                     << gps.latitude_deg << ";" << gps.longitude_deg << ";"
                     << gps.absolute_altitude_m << ";" << position.relative_altitude_m << ";"
                     << telemetry->attitude_euler().yaw_deg << ";"
@@ -165,7 +165,10 @@ int mavsdk_start(const char * url, const char * log_file, int timeout) {
                   break;
 
             default:
-                return;
+                if (takingOff) {
+                    takingOff = false;
+                    init_timestamp = telemetry->raw_gps().timestamp_us;
+                }
         }
     });