diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp
index 7dc7b93e8d..6f9f5178d4 100644
--- a/libraries/AP_HAL_SITL/sitl_gps.cpp
+++ b/libraries/AP_HAL_SITL/sitl_gps.cpp
@@ -162,7 +162,9 @@ static void gps_time(uint16_t *time_week, uint32_t *time_week_ms)
     const uint32_t epoch = 86400*(10*365 + (1980-1969)/4 + 1 + 6 - 2) - 15;
     uint32_t epoch_seconds = tv.tv_sec - epoch;
     *time_week = epoch_seconds / (86400*7UL);
-    *time_week_ms = (epoch_seconds % (86400*7UL))*1000 + tv.tv_usec/1000;
+    uint32_t t_ms = tv.tv_usec / 1000;
+    // round time to nearest 200ms
+    *time_week_ms = (epoch_seconds % (86400*7UL))*1000 + ((t_ms/200) * 200);
 }
 
 /*