Add timestamp to position array

mavsdk_wrapper.cpp

@@ -170,16 +170,21 @@ static void updateProjection(double current_lat, double current_lon) {
     );
 }
 
+static long long int getTimestamp() {
+    const auto now = std::chrono::system_clock::now();
+    return std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count();
+}
+
 int updateLogAndProjection(void) {
     std::ostringstream oss;
     Telemetry::Position position;
     Telemetry::FixedwingMetrics metrics;
+    long long int timestamp = getTimestamp();
 
     if(mavsdk_started) {
-        const auto now = std::chrono::system_clock::now();
         position = telemetry->position();
         metrics = telemetry->fixedwing_metrics();
-        oss << std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count() << ";"
+        oss << timestamp << ";"
             << position.latitude_deg << ";" << position.longitude_deg << ";"
             << position.absolute_altitude_m << ";" << position.relative_altitude_m << ";"
             << telemetry->attitude_euler().yaw_deg << ";"
@@ -387,12 +392,14 @@ double getInitialLongitude(void) {
 }
 
 double *getPositionArray(void) {
+    double timestamp = (double) getTimestamp();
     Telemetry::Position position = telemetry->position();
     double *positionArray = (double*) malloc(POSITION_ARRAY_SIZE * sizeof(double));
     positionArray[0] = position.latitude_deg;
     positionArray[1] = position.longitude_deg;
     positionArray[2] = (double) position.absolute_altitude_m;
     positionArray[3] = (double) position.relative_altitude_m;
+    positionArray[4] = timestamp;
     return positionArray;
 }