From d9457ecc38ec3c7de141efa411d06939fe4ff5a9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 May 2012 17:29:52 +1000 Subject: [PATCH] MAVLink: send time of GPS fix accurately when we send a GPS_RAW message, set the usec field to the time we got the fix from the GPS, not the current time. This makes it possible for aerial photo processing to be more accurate, as the usec field with more accurately reflect the planes position/time pair --- ArduCopter/GCS_Mavlink.pde | 4 ++-- ArduPlane/GCS_Mavlink.pde | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index a98af22e98..571b6d843a 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -309,7 +309,7 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan) mavlink_msg_gps_raw_int_send( chan, - micros(), + g_gps->last_fix_time*(uint64_t)1000, fix, g_gps->latitude, // in 1E7 degrees g_gps->longitude, // in 1E7 degrees @@ -323,7 +323,7 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan) #else // MAVLINK10 mavlink_msg_gps_raw_send( chan, - micros(), + g_gps->last_fix_time*(uint64_t)1000, g_gps->status(), g_gps->latitude / 1.0e7, g_gps->longitude / 1.0e7, diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 525c30d71b..8002d0fdb4 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -345,7 +345,7 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan) mavlink_msg_gps_raw_int_send( chan, - micros(), + g_gps->last_fix_time*(uint64_t)1000, fix, g_gps->latitude, // in 1E7 degrees g_gps->longitude, // in 1E7 degrees @@ -359,7 +359,7 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan) #else // MAVLINK10 mavlink_msg_gps_raw_send( chan, - micros(), + g_gps->last_fix_time*(uint64_t)1000, g_gps->status(), g_gps->latitude / 1.0e7, g_gps->longitude / 1.0e7,