dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fix mavlink_ulog: use hrt_abstime instead of float 

The float inaccuracy was leading to problems on SITL with large timestamps.
This commit is contained in:
Beat Küng 2023-03-20 16:33:32 +01:00 committed by Daniel Agar
parent 1524bd39b5
commit 8497d3388f
2 changed files with 7 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/modules/mavlink/mavlink_ulog.cpp
View File

@ -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;

4
src/modules/mavlink/mavlink_ulog.h
View File

@ -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_s> _ulog_stream_sub{ORB_ID(ulog_stream)};
uORB::Publication<ulog_stream_ack_s> _ulog_stream_ack_pub{ORB_ID(ulog_stream_ack)};