diff --git a/msg/ulog_stream.msg b/msg/ulog_stream.msg index e88de59f0d..7eaa32778d 100644 --- a/msg/ulog_stream.msg +++ b/msg/ulog_stream.msg @@ -15,3 +15,5 @@ uint8 first_message_offset # offset into data where first message starts. This uint16 sequence # allows determine drops uint8 flags # see FLAGS_* uint8[249] data # ulog data + +uint8 ORB_QUEUE_LENGTH = 14 # TODO: we might be able to reduce this if mavlink polled on the topic diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp index 5f68a98e28..8511f9e92a 100644 --- a/src/modules/logger/log_writer.cpp +++ b/src/modules/logger/log_writer.cpp @@ -38,7 +38,7 @@ namespace px4 namespace logger { -LogWriter::LogWriter(Backend configured_backend, size_t file_buffer_size, unsigned int queue_size) +LogWriter::LogWriter(Backend configured_backend, size_t file_buffer_size) : _backend(configured_backend) { if (configured_backend & BackendFile) { @@ -50,7 +50,7 @@ LogWriter::LogWriter(Backend configured_backend, size_t file_buffer_size, unsign } if (configured_backend & BackendMavlink) { - _log_writer_mavlink_for_write = _log_writer_mavlink = new LogWriterMavlink(queue_size); + _log_writer_mavlink_for_write = _log_writer_mavlink = new LogWriterMavlink(); if (!_log_writer_mavlink) { PX4_ERR("LogWriterMavlink allocation failed"); diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h index 7aa9c12417..abac8a5d57 100644 --- a/src/modules/logger/log_writer.h +++ b/src/modules/logger/log_writer.h @@ -55,7 +55,7 @@ public: static constexpr Backend BackendMavlink = 1 << 1; static constexpr Backend BackendAll = BackendFile | BackendMavlink; - LogWriter(Backend configured_backend, size_t file_buffer_size, unsigned int queue_size); + LogWriter(Backend configured_backend, size_t file_buffer_size); ~LogWriter(); bool init(); diff --git a/src/modules/logger/log_writer_mavlink.cpp b/src/modules/logger/log_writer_mavlink.cpp index b99eafd0a4..d0b0c2bc33 100644 --- a/src/modules/logger/log_writer_mavlink.cpp +++ b/src/modules/logger/log_writer_mavlink.cpp @@ -45,9 +45,7 @@ namespace px4 namespace logger { - -LogWriterMavlink::LogWriterMavlink(unsigned int queue_size) : - _queue_size(queue_size) +LogWriterMavlink::LogWriterMavlink() { _ulog_stream_data.length = 0; } @@ -62,21 +60,10 @@ LogWriterMavlink::~LogWriterMavlink() if (_ulog_stream_ack_sub >= 0) { orb_unsubscribe(_ulog_stream_ack_sub); } - - if (_ulog_stream_pub) { - orb_unadvertise(_ulog_stream_pub); - } } 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)); } @@ -84,9 +71,11 @@ void LogWriterMavlink::start_log() // make sure we don't get any stale ack's by doing an orb_copy ulog_stream_ack_s ack; orb_copy(ORB_ID(ulog_stream_ack), _ulog_stream_ack_sub, &ack); + _ulog_stream_data.sequence = 0; _ulog_stream_data.length = 0; _ulog_stream_data.first_message_offset = 0; + _is_started = true; } @@ -147,12 +136,7 @@ int LogWriterMavlink::publish_message() _ulog_stream_data.flags = _ulog_stream_data.FLAGS_NEED_ACK; } - if (_ulog_stream_pub == nullptr) { - _ulog_stream_pub = orb_advertise_queue(ORB_ID(ulog_stream), &_ulog_stream_data, _queue_size); - - } else { - orb_publish(ORB_ID(ulog_stream), _ulog_stream_pub, &_ulog_stream_data); - } + _ulog_stream_pub.publish(_ulog_stream_data); if (_need_reliable_transfer) { // we need to wait for an ack. Note that this blocks the main logger thread, so if a file logging diff --git a/src/modules/logger/log_writer_mavlink.h b/src/modules/logger/log_writer_mavlink.h index 5a086ebb81..6b6f368198 100644 --- a/src/modules/logger/log_writer_mavlink.h +++ b/src/modules/logger/log_writer_mavlink.h @@ -34,6 +34,7 @@ #pragma once #include +#include #include #include @@ -49,7 +50,7 @@ namespace logger class LogWriterMavlink { public: - LogWriterMavlink(unsigned int queue_size); + LogWriterMavlink(); ~LogWriterMavlink(); bool init(); @@ -75,12 +76,11 @@ private: /** publish message, wait for ack if needed & reset message */ int publish_message(); - ulog_stream_s _ulog_stream_data; - orb_advert_t _ulog_stream_pub = nullptr; - int _ulog_stream_ack_sub = -1; - bool _need_reliable_transfer = false; - bool _is_started = false; - const unsigned int _queue_size; + ulog_stream_s _ulog_stream_data{}; + uORB::PublicationQueued _ulog_stream_pub{ORB_ID(ulog_stream)}; + int _ulog_stream_ack_sub{-1}; + bool _need_reliable_transfer{false}; + bool _is_started{false}; }; } diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 16f7005066..ebd4beb1c1 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -239,8 +239,6 @@ Logger *Logger::instantiate(int argc, char *argv[]) Logger::LogMode log_mode = Logger::LogMode::while_armed; bool error_flag = false; bool log_name_timestamp = false; - unsigned int queue_size = 14; //TODO: we might be able to reduce this if mavlink polled on the topic and/or - // topic sizes get reduced LogWriter::Backend backend = LogWriter::BackendAll; const char *poll_topic = nullptr; @@ -248,7 +246,7 @@ Logger *Logger::instantiate(int argc, char *argv[]) int ch; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "r:b:etfm:q:p:x", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "r:b:etfm:p:x", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'r': { unsigned long r = strtoul(myoptarg, nullptr, 10); @@ -310,15 +308,6 @@ Logger *Logger::instantiate(int argc, char *argv[]) poll_topic = myoptarg; break; - case 'q': - queue_size = strtoul(myoptarg, nullptr, 10); - - if (queue_size == 0) { - queue_size = 1; - } - - break; - case '?': error_flag = true; break; @@ -343,8 +332,7 @@ Logger *Logger::instantiate(int argc, char *argv[]) return nullptr; } - Logger *logger = new Logger(backend, log_buffer_size, log_interval, poll_topic, log_mode, log_name_timestamp, - queue_size); + Logger *logger = new Logger(backend, log_buffer_size, log_interval, poll_topic, log_mode, log_name_timestamp); #if defined(DBGPRINT) && defined(__PX4_NUTTX) struct mallinfo alloc_info = mallinfo(); @@ -373,10 +361,10 @@ Logger *Logger::instantiate(int argc, char *argv[]) Logger::Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_interval, const char *poll_topic_name, - LogMode log_mode, bool log_name_timestamp, unsigned int queue_size) : + LogMode log_mode, bool log_name_timestamp) : _log_mode(log_mode), _log_name_timestamp(log_name_timestamp), - _writer(backend, buffer_size, queue_size), + _writer(backend, buffer_size), _log_interval(log_interval) { _log_utc_offset = param_find("SDLOG_UTC_OFFSET"); @@ -2355,7 +2343,6 @@ $ logger on PRINT_MODULE_USAGE_PARAM_FLAG('t', "Use date/time for naming log directories and files", true); PRINT_MODULE_USAGE_PARAM_INT('r', 280, 0, 8000, "Log rate in Hz, 0 means unlimited rate", true); PRINT_MODULE_USAGE_PARAM_INT('b', 12, 4, 10000, "Log buffer size in KiB", true); - PRINT_MODULE_USAGE_PARAM_INT('q', 14, 1, 100, "uORB queue size for mavlink mode", true); PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "", "Poll on a topic instead of running with fixed rate (Log rate and topic intervals are ignored if this is set)", true); PRINT_MODULE_USAGE_COMMAND_DESCR("on", "start logging now, override arming (logger must be running)"); diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index d2dbd39198..5bfe9f0c6e 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -117,7 +117,7 @@ public: }; Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_interval, const char *poll_topic_name, - LogMode log_mode, bool log_name_timestamp, unsigned int queue_size); + LogMode log_mode, bool log_name_timestamp); ~Logger();