From 8497d3388f354d4a38c47d34f53754b6e8763459 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 20 Mar 2023 16:33:32 +0100 Subject: [PATCH] fix mavlink_ulog: use hrt_abstime instead of float The float inaccuracy was leading to problems on SITL with large timestamps. --- src/modules/mavlink/mavlink_ulog.cpp | 9 ++++----- src/modules/mavlink/mavlink_ulog.h | 4 +++- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_ulog.cpp b/src/modules/mavlink/mavlink_ulog.cpp index 6f4cbd577a..64b9ec981d 100644 --- a/src/modules/mavlink/mavlink_ulog.cpp +++ b/src/modules/mavlink/mavlink_ulog.cpp @@ -46,13 +46,12 @@ bool MavlinkULog::_init = false; MavlinkULog *MavlinkULog::_instance = nullptr; px4_sem_t MavlinkULog::_lock; -const float MavlinkULog::_rate_calculation_delta_t = 0.1f; MavlinkULog::MavlinkULog(int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component) : _target_system(target_system), _target_component(target_component), _max_rate_factor(max_rate_factor), - _max_num_messages(math::max(1, (int)ceilf(_rate_calculation_delta_t *_max_rate_factor * datarate / + _max_num_messages(math::max(1, (int)ceilf((_rate_calculation_delta_t / 1e6f) * _max_rate_factor * datarate / (MAVLINK_MSG_ID_LOGGING_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)))), _current_rate_factor(max_rate_factor) { @@ -63,7 +62,7 @@ MavlinkULog::MavlinkULog(int datarate, float max_rate_factor, uint8_t target_sys _waiting_for_initial_ack = true; _last_sent_time = hrt_absolute_time(); //(ab)use this timestamp during initialization - _next_rate_check = _last_sent_time + _rate_calculation_delta_t * 1.e6f; + _next_rate_check = _last_sent_time + _rate_calculation_delta_t; } MavlinkULog::~MavlinkULog() @@ -189,9 +188,9 @@ int MavlinkULog::handle_update(mavlink_channel_t channel) } _current_num_msgs = 0; - _next_rate_check = t + _rate_calculation_delta_t * 1.e6f; + _next_rate_check = t + _rate_calculation_delta_t; PX4_DEBUG("current rate=%.3f (max=%i msgs in %.3fs)", (double)_current_rate_factor, _max_num_messages, - (double)_rate_calculation_delta_t); + (double)(_rate_calculation_delta_t / 1e6)); } return 0; diff --git a/src/modules/mavlink/mavlink_ulog.h b/src/modules/mavlink/mavlink_ulog.h index 9c26329792..7c96553c6d 100644 --- a/src/modules/mavlink/mavlink_ulog.h +++ b/src/modules/mavlink/mavlink_ulog.h @@ -54,6 +54,8 @@ #include "mavlink_bridge_header.h" +using namespace time_literals; + /** * @class MavlinkULog * ULog streaming class. At most one instance (stream) can exist, assigned to a specific mavlink channel. @@ -120,7 +122,7 @@ private: static px4_sem_t _lock; static bool _init; static MavlinkULog *_instance; - static const float _rate_calculation_delta_t; ///< rate update interval + static constexpr hrt_abstime _rate_calculation_delta_t = 100_ms; ///< rate update interval uORB::SubscriptionData _ulog_stream_sub{ORB_ID(ulog_stream)}; uORB::Publication _ulog_stream_ack_pub{ORB_ID(ulog_stream_ack)};