diff --git a/src/modules/logger/log_writer_mavlink.cpp b/src/modules/logger/log_writer_mavlink.cpp index c4b988c2d6..c82a8d267d 100644 --- a/src/modules/logger/log_writer_mavlink.cpp +++ b/src/modules/logger/log_writer_mavlink.cpp @@ -69,6 +69,13 @@ LogWriterMavlink::~LogWriterMavlink() void LogWriterMavlink::start_log() { + if (_ulog_stream_pub == nullptr) { + memset(&_ulog_stream_data, 0, sizeof(_ulog_stream_data)); + // advertise before we subscribe: this is a trick to reduce the number of needed file descriptors + // (the advertise temporarily opens a file descriptor) + _ulog_stream_pub = orb_advertise_queue(ORB_ID(ulog_stream), &_ulog_stream_data, _queue_size); + } + if (_ulog_stream_ack_sub == -1) { _ulog_stream_ack_sub = orb_subscribe(ORB_ID(ulog_stream_ack)); } diff --git a/src/modules/mavlink/mavlink_ulog.cpp b/src/modules/mavlink/mavlink_ulog.cpp index 5a0616220b..e47c9ccc8e 100644 --- a/src/modules/mavlink/mavlink_ulog.cpp +++ b/src/modules/mavlink/mavlink_ulog.cpp @@ -143,32 +143,34 @@ int MavlinkULog::handle_update(mavlink_channel_t channel) while (updated && !ret && _current_num_msgs < _max_num_messages) { orb_copy(ORB_ID(ulog_stream), _ulog_stream_sub, &_ulog_data); - if (_ulog_data.flags & ulog_stream_s::FLAGS_NEED_ACK) { - _sent_tries = 1; - _last_sent_time = hrt_absolute_time(); - lock(); - _wait_for_ack_sequence = _ulog_data.sequence; - _ack_received = false; - unlock(); + if (_ulog_data.timestamp > 0) { + if (_ulog_data.flags & ulog_stream_s::FLAGS_NEED_ACK) { + _sent_tries = 1; + _last_sent_time = hrt_absolute_time(); + lock(); + _wait_for_ack_sequence = _ulog_data.sequence; + _ack_received = false; + unlock(); - mavlink_logging_data_acked_t msg; - msg.sequence = _ulog_data.sequence; - msg.length = _ulog_data.length; - msg.first_message_offset = _ulog_data.first_message_offset; - msg.target_system = _target_system; - msg.target_component = _target_component; - memcpy(msg.data, _ulog_data.data, sizeof(msg.data)); - mavlink_msg_logging_data_acked_send_struct(channel, &msg); + mavlink_logging_data_acked_t msg; + msg.sequence = _ulog_data.sequence; + msg.length = _ulog_data.length; + msg.first_message_offset = _ulog_data.first_message_offset; + msg.target_system = _target_system; + msg.target_component = _target_component; + memcpy(msg.data, _ulog_data.data, sizeof(msg.data)); + mavlink_msg_logging_data_acked_send_struct(channel, &msg); - } else { - mavlink_logging_data_t msg; - msg.sequence = _ulog_data.sequence; - msg.length = _ulog_data.length; - msg.first_message_offset = _ulog_data.first_message_offset; - msg.target_system = _target_system; - msg.target_component = _target_component; - memcpy(msg.data, _ulog_data.data, sizeof(msg.data)); - mavlink_msg_logging_data_send_struct(channel, &msg); + } else { + mavlink_logging_data_t msg; + msg.sequence = _ulog_data.sequence; + msg.length = _ulog_data.length; + msg.first_message_offset = _ulog_data.first_message_offset; + msg.target_system = _target_system; + msg.target_component = _target_component; + memcpy(msg.data, _ulog_data.data, sizeof(msg.data)); + mavlink_msg_logging_data_send_struct(channel, &msg); + } } ++_current_num_msgs;