From e2b801a3239df53c743e343caf5512ab772eeeb7 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Sat, 26 Sep 2015 20:12:15 +0300
Subject: [PATCH] Fixes 2911

---
 src/modules/uavcan/sensors/baro.cpp |  9 ++++++++-
 src/modules/uavcan/sensors/gnss.cpp | 10 +++++++++-
 src/modules/uavcan/sensors/mag.cpp  |  9 ++++++++-
 3 files changed, 25 insertions(+), 3 deletions(-)

diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp
index 1986de4247..694a3988a1 100644
--- a/src/modules/uavcan/sensors/baro.cpp
+++ b/src/modules/uavcan/sensors/baro.cpp
@@ -159,7 +159,14 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
 {
 	baro_report report;
 
-	report.timestamp   = msg.getMonotonicTimestamp().toUSec();
+	/*
+	 * FIXME HACK
+	 * This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT.
+	 * It stopped working when the time sync feature has been introduced, because it caused libuavcan
+	 * to use an independent time source (based on hardware TIM5) instead of HRT.
+	 * The proper solution is to be developed.
+	 */
+	report.timestamp   = hrt_absolute_time();
 	report.temperature = last_temperature_kelvin - 273.15F;
 	report.pressure    = msg.static_pressure / 100.0F;  // Convert to millibar
 	report.error_count = 0;
diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
index 0830aba4ee..d80b4b425d 100644
--- a/src/modules/uavcan/sensors/gnss.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -96,7 +96,15 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 
 	auto report = ::vehicle_gps_position_s();
 
-	report.timestamp_position = msg.getMonotonicTimestamp().toUSec();
+	/*
+	 * FIXME HACK
+	 * There used to be the following line of code:
+	 * 	report.timestamp_position = msg.getMonotonicTimestamp().toUSec();
+	 * It stopped working when the time sync feature has been introduced, because it caused libuavcan
+	 * to use an independent time source (based on hardware TIM5) instead of HRT.
+	 * The proper solution is to be developed.
+	 */
+	report.timestamp_position = hrt_absolute_time();
 	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/mag.cpp b/src/modules/uavcan/sensors/mag.cpp
index d7110087ce..e184ff12fc 100644
--- a/src/modules/uavcan/sensors/mag.cpp
+++ b/src/modules/uavcan/sensors/mag.cpp
@@ -146,7 +146,14 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<ua
 {
 	lock();
 	_report.range_ga = 1.3F;   // Arbitrary number, doesn't really mean anything
-	_report.timestamp = msg.getMonotonicTimestamp().toUSec();
+	/*
+	 * FIXME HACK
+	 * This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT.
+	 * It stopped working when the time sync feature has been introduced, because it caused libuavcan
+	 * to use an independent time source (based on hardware TIM5) instead of HRT.
+	 * The proper solution is to be developed.
+	 */
+	_report.timestamp = hrt_absolute_time();
 
 	_report.x = (msg.magnetic_field_ga[0] - _scale.x_offset) * _scale.x_scale;
 	_report.y = (msg.magnetic_field_ga[1] - _scale.y_offset) * _scale.y_scale;