Fetch timestamps from CAP_FREQUENT_STATUS messages

Correct timestamps provided by the system by fetching timestamps from GPS.

mavsdk_wrapper.cpp

@@ -45,7 +45,19 @@ 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;
+
+enum TIMESTAMPING_STATE { ARRAY_NOT_POPULATED, CHECK_FIRST_TIMESTAMP_VALUE, RUNNING };
+static TIMESTAMPING_STATE timestamp_handler_state = ARRAY_NOT_POPULATED;
+static uint64_t first_timestamp = 0;
+static long long int timestamp_at_boot = 0;
+static long long int timestamp_difference = 0;
+static const int TIMESTAMP_ARRAY_LENGTH = 10;
+static const int TIMESTAMP_SAMPLING_DURANTION_US = 10000000;
+static const int TIMESTAMP_ARRAY_MAX_INTERVAL_MS = 3000;
+static long long int timestamp_array[TIMESTAMP_ARRAY_LENGTH];
+static long long int timestamp_array_sorted[TIMESTAMP_ARRAY_LENGTH];
+static int timestamp_array_index = 0;
+
 
 static Coordinates targeted_destination;
 static float targeted_radius = DEFAULT_RADIUS;
@@ -191,6 +203,10 @@ static long long int getTimestamp() {
     return std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count();
 }
 
+static long long int getCorrectedTimestamp() {
+    return getTimestamp() + timestamp_difference;
+}
+
 static int doOverride(float altitude, float speed, const char* failure_msg) {
     if (!mavsdk_started)
         return -1;
@@ -283,18 +299,78 @@ int start(const char * ip, int port, const char * log_file, int timeout) {
     action = new Action(msystem);
     mavlink_passthrough = new MavlinkPassthrough(msystem);
 
+    mavlink_passthrough->subscribe_message_async(MAVLINK_MSG_ID_CAP_STATUS_FREQUENT, [](const mavlink_message_t& message) {
+        mavlink_cap_status_frequent_t status;
+        mavlink_msg_cap_status_frequent_decode(&message, &status);
+
+        long long int system_timestamp = getTimestamp();
+        long long int timestamp_ms = status.utc_time / 1000;
+        timestamp_array[timestamp_array_index] = timestamp_ms;
+        printf("Value ms: %lld\n", timestamp_ms);
+
+        std::copy(std::begin(timestamp_array), std::end(timestamp_array), std::begin(timestamp_array_sorted));
+        std::sort(std::begin(timestamp_array_sorted), std::end(timestamp_array_sorted));
+        auto pos = std::adjacent_find(std::begin(timestamp_array_sorted), std::end(timestamp_array_sorted));
+
+        switch (timestamp_handler_state) {
+
+        case ARRAY_NOT_POPULATED:
+            printf("populating\n");
+            if (timestamp_array_index == TIMESTAMP_ARRAY_LENGTH - 1) {
+                timestamp_handler_state = CHECK_FIRST_TIMESTAMP_VALUE;
+            }
+            break;
+
+        case CHECK_FIRST_TIMESTAMP_VALUE:
+            if (status.utc_time == 0 || pos != std::end(timestamp_array_sorted)) {
+                printf("it's a zero or array contains duplicates\n");
+                break;
+            }
+            printf("checking first value\n");
+            if (timestamp_array_sorted[TIMESTAMP_ARRAY_LENGTH - 1] - timestamp_array_sorted[0] <= TIMESTAMP_ARRAY_MAX_INTERVAL_MS) {
+                first_timestamp = status.utc_time;
+                timestamp_difference = timestamp_ms - system_timestamp;
+                timestamp_at_boot = 0;
+                timestamp_handler_state = RUNNING;
+            }
+            for (int i = 0; i < TIMESTAMP_ARRAY_LENGTH - 1; i++) {
+              printf("%lld ", timestamp_array[i]);
+            }
+            printf("\n");
+            break;
+
+        case RUNNING:
+            if (status.utc_time == 0 || pos != std::end(timestamp_array_sorted)) {
+                printf("it's a zero or array contains duplicates\n");
+                break;
+            }
+            printf("running\n");
+            if (timestamp_ms - system_timestamp < timestamp_difference && (timestamp_array_sorted[TIMESTAMP_ARRAY_LENGTH - 1] - timestamp_array_sorted[0] <= TIMESTAMP_ARRAY_MAX_INTERVAL_MS)) {
+                timestamp_difference = timestamp_ms - system_timestamp;
+                timestamp_at_boot = 0;
+            }
+            if (status.utc_time - first_timestamp > TIMESTAMP_SAMPLING_DURANTION_US)
+                mavlink_passthrough->subscribe_message_async(MAVLINK_MSG_ID_CAP_STATUS_FREQUENT, nullptr);
+            break;
+        }
+
+        printf("timestamp %lld \n", getCorrectedTimestamp());
+        timestamp_array_index++;
+        timestamp_array_index %= TIMESTAMP_ARRAY_LENGTH;
+    });
+
     mavlink_passthrough->subscribe_message_async(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, [](const mavlink_message_t& message) {
         mavlink_global_position_int_t position;
         mavlink_msg_global_position_int_decode(&message, &position);
 
-        if (timestamp_difference == 0)
-            timestamp_difference = getTimestamp() - position.time_boot_ms;
+        if (timestamp_at_boot == 0)
+            timestamp_at_boot = getCorrectedTimestamp() - position.time_boot_ms;
 
         current_position[0] = position.lat;
         current_position[1] = position.lon;
         current_position[2] = position.alt;
         current_position[3] = position.relative_alt;
-        current_position[4] = position.time_boot_ms + timestamp_difference;
+        current_position[4] = position.time_boot_ms + timestamp_at_boot;
     });
 
     do {