fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 13:48:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix mavlink_ulog: use hrt_abstime instead of float

Browse Source 
The float inaccuracy was leading to problems on SITL with large timestamps.
This commit is contained in:
Beat Küng
2023-03-20 21:47:41 -04:00
committed by Daniel Agar
parent 1524bd39b5
commit 8497d3388f
2 changed files with 7 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_ulog.cpp
+4 -5
View File
@@ -46,13 +46,12 @@
bool MavlinkULog::_init = false; bool MavlinkULog::_init = false;
MavlinkULog *MavlinkULog::_instance = nullptr; MavlinkULog *MavlinkULog::_instance = nullptr;
px4_sem_t MavlinkULog::_lock; 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) MavlinkULog::MavlinkULog(int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component)
: _target_system(target_system), _target_component(target_component), : _target_system(target_system), _target_component(target_component),
_max_rate_factor(max_rate_factor), _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)))), (MAVLINK_MSG_ID_LOGGING_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)))),
_current_rate_factor(max_rate_factor) _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; _waiting_for_initial_ack = true;
_last_sent_time = hrt_absolute_time(); //(ab)use this timestamp during initialization _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() MavlinkULog::~MavlinkULog()
@@ -189,9 +188,9 @@ int MavlinkULog::handle_update(mavlink_channel_t channel)
} }
_current_num_msgs = 0; _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, 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; return 0;
src/modules/mavlink/mavlink_ulog.h
+3 -1
View File
@@ -54,6 +54,8 @@
#include "mavlink_bridge_header.h" #include "mavlink_bridge_header.h"
using namespace time_literals;
/** /**
* @class MavlinkULog * @class MavlinkULog
* ULog streaming class. At most one instance (stream) can exist, assigned to a specific mavlink channel. * 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 px4_sem_t _lock;
static bool _init; static bool _init;
static MavlinkULog *_instance; 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::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)}; uORB::Publication<ulog_stream_ack_s> _ulog_stream_ack_pub{ORB_ID(ulog_stream_ack)};