From 2ce2d26d52884f868ecbcb102f38cbc853c2dbf6 Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <holger@steinhaus-home.de>
Date: Tue, 18 Nov 2014 14:58:04 +0100
Subject: [PATCH] UAVCAN: preserve original UAVCAN message timestamps

---
 src/modules/uavcan/sensors/baro.cpp | 6 +-----
 src/modules/uavcan/sensors/gnss.cpp | 2 +-
 src/modules/uavcan/sensors/gnss.hpp | 2 --
 src/modules/uavcan/sensors/mag.cpp  | 3 +--
 4 files changed, 3 insertions(+), 10 deletions(-)

diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp
index 80c5e3828e..8741ae20dd 100644
--- a/src/modules/uavcan/sensors/baro.cpp
+++ b/src/modules/uavcan/sensors/baro.cpp
@@ -91,11 +91,7 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<
 {
 	auto report = ::baro_report();
 
-	report.timestamp = msg.getUtcTimestamp().toUSec();
-	if (report.timestamp == 0) {
-		report.timestamp = msg.getMonotonicTimestamp().toUSec();
-	}
-
+	report.timestamp   = msg.getMonotonicTimestamp().toUSec();
 	report.temperature = msg.static_temperature;
 	report.pressure    = msg.static_pressure / 100.0F;  // Convert to millibar
 
diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
index 24afe6aaf4..a375db37f8 100644
--- a/src/modules/uavcan/sensors/gnss.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -92,7 +92,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 
 	auto report = ::vehicle_gps_position_s();
 
-	report.timestamp_position = hrt_absolute_time();
+	report.timestamp_position = msg.getMonotonicTimestamp().toUSec();
 	report.lat = msg.latitude_deg_1e8 / 10;
 	report.lon = msg.longitude_deg_1e8 / 10;
 	report.alt = msg.height_msl_mm;
diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp
index e8466b4010..2111ff80b6 100644
--- a/src/modules/uavcan/sensors/gnss.hpp
+++ b/src/modules/uavcan/sensors/gnss.hpp
@@ -43,8 +43,6 @@
 
 #pragma once
 
-#include <drivers/drv_hrt.h>
-
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp
index 5cbb96433b..35ebee5426 100644
--- a/src/modules/uavcan/sensors/mag.cpp
+++ b/src/modules/uavcan/sensors/mag.cpp
@@ -37,7 +37,6 @@
 
 #include "mag.hpp"
 
-#include <drivers/drv_hrt.h>
 #include <systemlib/err.h>
 
 static const orb_id_t MAG_TOPICS[3] = {
@@ -140,7 +139,7 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<ua
 {
 	lock();
 	_report.range_ga = 1.3F;   // Arbitrary number, doesn't really mean anything
-	_report.timestamp = hrt_absolute_time();
+	_report.timestamp = msg.getMonotonicTimestamp().toUSec();
 
 	_report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
 	_report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;