diff options
author | Holger Steinhaus <holger@steinhaus-home.de> | 2014-11-18 14:58:04 +0100 |
---|---|---|
committer | Holger Steinhaus <holger@steinhaus-home.de> | 2014-11-18 14:58:04 +0100 |
commit | 2ce2d26d52884f868ecbcb102f38cbc853c2dbf6 (patch) | |
tree | 7f37f9fc1825504019e8ad7c1a7f954cbf4e5e2a /src | |
parent | e026324784409bd16a561544cb608e7089d500a0 (diff) | |
download | px4-firmware-2ce2d26d52884f868ecbcb102f38cbc853c2dbf6.tar.gz px4-firmware-2ce2d26d52884f868ecbcb102f38cbc853c2dbf6.tar.bz2 px4-firmware-2ce2d26d52884f868ecbcb102f38cbc853c2dbf6.zip |
UAVCAN: preserve original UAVCAN message timestamps
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/uavcan/sensors/baro.cpp | 6 | ||||
-rw-r--r-- | src/modules/uavcan/sensors/gnss.cpp | 2 | ||||
-rw-r--r-- | src/modules/uavcan/sensors/gnss.hpp | 2 | ||||
-rw-r--r-- | 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 80c5e3828..8741ae20d 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 24afe6aaf..a375db37f 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 e8466b401..2111ff80b 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 5cbb96433..35ebee542 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; |