fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 11:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

events: handle events being generated before a mavlink module started

Browse Source 
- do not print dropped events warnings
- make sure current sequence is still sent with reset flag
This commit is contained in:
Beat Küng
2022-10-11 22:31:20 -04:00
committed by Daniel Agar
parent 3fcdf40a47
commit 6ee2252b8c
2 changed files with 16 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_events.cpp
+14 -4
View File
@@ -136,6 +136,17 @@ void SendProtocol::update(const hrt_abstime &now)
uint16_t buffer_sequence = _buffer.get_latest_sequence();
int num_drops = 0;
if (_first_event && buffer_sequence != _latest_sequence) {
// If events are published before the first mavlink instance starts, they would be flagged as dropped
// events as the buffer initializes the sequence to the global events::initial_event_sequence,
// therefore reset the sequence.
_latest_sequence = buffer_sequence - 1;
_first_event = false;
PX4_DEBUG("Setting initial sequence to %i", _latest_sequence);
// Send sequence - only needed for SITL to ensure we send the sequence reset flag initially
send_current_sequence(now, true);
}
while (_latest_sequence != buffer_sequence) {
// only send if enough tx buffer space available
if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_EVENT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
@@ -164,8 +175,7 @@ void SendProtocol::update(const hrt_abstime &now)
PX4_WARN("Dropped %i events (seq=%i)", num_drops, _latest_sequence);
}
// Send immediately on startup. This is only needed for SITL to ensure we send the sequence reset flag
if (now - _last_current_sequence_sent > current_sequence_interval || _last_current_sequence_sent == 0) {
if (now - _last_current_sequence_sent > current_sequence_interval) {
send_current_sequence(now);
}
}
@@ -216,7 +226,7 @@ void SendProtocol::on_gcs_connected()
send_current_sequence(hrt_absolute_time());
}
void SendProtocol::send_current_sequence(const hrt_abstime &now)
void SendProtocol::send_current_sequence(const hrt_abstime &now, bool force_reset)
{
// only send if enough tx buffer space available
if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
@@ -226,7 +236,7 @@ void SendProtocol::send_current_sequence(const hrt_abstime &now)
_last_current_sequence_sent = now;
mavlink_current_event_sequence_t current_event_seq;
current_event_seq.sequence = _buffer.get_latest_sequence();
current_event_seq.flags = _buffer.size() == 0 ? MAV_EVENT_CURRENT_SEQUENCE_FLAGS_RESET : 0;
current_event_seq.flags = (_buffer.size() == 0 || force_reset) ? MAV_EVENT_CURRENT_SEQUENCE_FLAGS_RESET : 0;
mavlink_msg_current_event_sequence_send_struct(_mavlink.get_channel(), &current_event_seq);
}
src/modules/mavlink/mavlink_events.h
+2 -1
View File
@@ -132,7 +132,7 @@ public:
private:
void send_event(const Event &event) const;
void send_current_sequence(const hrt_abstime &now);
void send_current_sequence(const hrt_abstime &now, bool force_reset = false);
static constexpr hrt_abstime current_sequence_interval{3_s};
@@ -140,6 +140,7 @@ private:
uint16_t _latest_sequence;
hrt_abstime _last_current_sequence_sent{0};
Mavlink &_mavlink;
bool _first_event{true};
};