Commit e78a3ad4 authored by Léo-Paul Géneau's avatar Léo-Paul Géneau 👾

Use cap_status_frequent to know utc time at boot

parent 1eb708b0
......@@ -45,7 +45,7 @@ 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;
static int64_t utc_time_at_boot = 0;
static Coordinates targeted_destination;
static float targeted_radius = DEFAULT_RADIUS;
......@@ -283,18 +283,27 @@ 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 time;
mavlink_msg_cap_status_frequent_decode(&message, &time);
if (abs((long long int)time.utc_time - getTimestamp()) < 1) {
utc_time_at_boot = time.utc_time - time.time_boot_ms;
mavlink_passthrough->subscribe_message_async(MAVLINK_MSG_ID_CAP_STATUS_FREQUENT, nullptr);
} else {
printf("gps time %lu clock time %lld\n", time.utc_time, getTimestamp());
}
});
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;
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 + utc_time_at_boot;
});
do {
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment