From 443d77b07bba99e330308feeebfd450ee50f319c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 6 Aug 2019 02:02:24 -0400 Subject: [PATCH 1/2] logger move non-logged subscriptions to uORB::Subscription --- src/modules/logger/logger.cpp | 86 ++++++++++------------------------- src/modules/logger/logger.h | 17 +++++-- src/modules/logger/util.cpp | 14 ++---- 3 files changed, 42 insertions(+), 75 deletions(-) diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 26d0a65db8cb..16f7005066bc 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -46,12 +46,8 @@ #include #include -#include -#include #include -#include #include -#include #include #include @@ -898,10 +894,7 @@ void Logger::run() } } - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - uORB::SubscriptionData parameter_update_sub(ORB_ID(parameter_update)); - int log_message_sub = orb_subscribe(ORB_ID(log_message)); - orb_set_interval(log_message_sub, 20); + uORB::Subscription parameter_update_sub(ORB_ID(parameter_update)); // mission log topics if enabled (must be added first) int32_t mission_log_mode = 0; @@ -919,12 +912,6 @@ void Logger::run() } } - int manual_control_sp_sub = -1; - - if (_log_mode == LogMode::rc_aux1) { - manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - } - int ntopics = add_topics_from_file(PX4_STORAGEDIR "/etc/logging/logger_topics.txt"); if (ntopics > 0) { @@ -934,15 +921,8 @@ void Logger::run() initialize_configured_topics(); } - int vehicle_command_sub = -1; - - if (_writer.backend() & LogWriter::BackendMavlink) { - vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); - } - //all topics added. Get required message buffer size int max_msg_size = 0; - int ret = 0; for (const auto &subscription : _subscriptions) { //use o_size, because that's what orb_copy will use @@ -1029,8 +1009,7 @@ void Logger::run() while (!should_exit()) { // Start/stop logging (depending on logging mode, by default when arming/disarming) - const bool logging_started = start_stop_logging(vehicle_status_sub, manual_control_sp_sub, - (MissionLogType)mission_log_mode); + const bool logging_started = start_stop_logging((MissionLogType)mission_log_mode); if (logging_started) { #ifdef DBGPRINT @@ -1040,10 +1019,7 @@ void Logger::run() } /* check for logging command from MAVLink (start/stop streaming) */ - if (vehicle_command_sub >= 0) { - handle_vehicle_command_update(vehicle_command_sub); - } - + handle_vehicle_command_update(); if (timer_callback_data.watchdog_triggered) { timer_callback_data.watchdog_triggered = false; @@ -1071,7 +1047,9 @@ void Logger::run() /* Check if parameters have changed */ if (!_should_stop_file_log) { // do not record param changes after disarming - if (parameter_update_sub.update()) { + parameter_update_s param_update; + + if (parameter_update_sub.update(¶m_update)) { write_changed_parameters(LogType::Full); } } @@ -1136,13 +1114,10 @@ void Logger::run() ++sub_idx; } - //check for new logging message(s) - bool log_message_updated = false; - ret = orb_check(log_message_sub, &log_message_updated); + // check for new logging message(s) + log_message_s log_message; - if (ret == 0 && log_message_updated) { - log_message_s log_message; - orb_copy(ORB_ID(log_message), log_message_sub, &log_message); + if (_log_message_sub.update(&log_message)) { const char *message = (const char *)log_message.text; int message_len = strlen(message); @@ -1183,7 +1158,7 @@ void Logger::run() // update buffer statistics for (int i = 0; i < (int)LogType::Count; ++i) { - if (!_statistics[i].dropout_start && _writer.get_buffer_fill_count_file((LogType)i) > _statistics[i].high_water) { + if (!_statistics[i].dropout_start && (_writer.get_buffer_fill_count_file((LogType)i) > _statistics[i].high_water)) { _statistics[i].high_water = _writer.get_buffer_fill_count_file((LogType)i); } } @@ -1279,10 +1254,6 @@ void Logger::run() } } - if (manual_control_sp_sub != -1) { - orb_unsubscribe(manual_control_sp_sub); - } - if (polling_topic_sub >= 0) { orb_unsubscribe(polling_topic_sub); } @@ -1292,10 +1263,6 @@ void Logger::run() _mavlink_log_pub = nullptr; } - if (vehicle_command_sub != -1) { - orb_unsubscribe(vehicle_command_sub); - } - px4_unregister_shutdown_hook(&Logger::request_stop_static); } @@ -1320,21 +1287,20 @@ void Logger::debug_print_buffer(uint32_t &total_bytes, hrt_abstime &timer_start) #endif /* DBGPRINT */ } -bool Logger::start_stop_logging(int vehicle_status_sub, int manual_control_sp_sub, MissionLogType mission_log_type) +bool Logger::start_stop_logging(MissionLogType mission_log_type) { bool bret = false; bool want_start = false; bool want_stop = false; if (_log_mode == LogMode::rc_aux1) { - //aux1-based logging - bool manual_control_setpoint_updated; - int ret = orb_check(manual_control_sp_sub, &manual_control_setpoint_updated); - if (ret == 0 && manual_control_setpoint_updated) { - manual_control_setpoint_s manual_sp; - orb_copy(ORB_ID(manual_control_setpoint), manual_control_sp_sub, &manual_sp); - bool should_start = manual_sp.aux1 > 0.3f || _manually_logging_override; + // aux1-based logging + manual_control_setpoint_s manual_sp; + + if (_manual_control_sp_sub.update(&manual_sp)) { + + bool should_start = ((manual_sp.aux1 > 0.3f) || _manually_logging_override); if (_prev_state != should_start) { _prev_state = should_start; @@ -1350,12 +1316,10 @@ bool Logger::start_stop_logging(int vehicle_status_sub, int manual_control_sp_su } else { // arming-based logging - bool vehicle_status_updated; - int ret = orb_check(vehicle_status_sub, &vehicle_status_updated); + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { - if (ret == 0 && vehicle_status_updated) { - vehicle_status_s vehicle_status; - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) || _manually_logging_override; if (_prev_state != armed && _log_mode != LogMode::boot_until_shutdown) { @@ -1397,16 +1361,14 @@ bool Logger::start_stop_logging(int vehicle_status_sub, int manual_control_sp_su return bret; } -void Logger::handle_vehicle_command_update(int vehicle_command_sub) +void Logger::handle_vehicle_command_update() { - bool command_updated = false; - int ret = orb_check(vehicle_command_sub, &command_updated); + vehicle_command_s command; - if (ret == 0 && command_updated) { - vehicle_command_s command; - orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &command); + if (_vehicle_command_sub.update(&command)) { if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) { + if ((int)(command.param1 + 0.5f) != 0) { ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED); diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 4816e589dcf1..d2dbd3919801 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -39,17 +39,21 @@ #include "util.h" #include #include -#include #include #include #include #include +#include +#include +#include +#include #include +#include extern "C" __EXPORT int logger_main(int argc, char *argv[]); -#define TRY_SUBSCRIBE_INTERVAL 1000*1000 // interval in microseconds at which we try to subscribe to a topic +static constexpr hrt_abstime TRY_SUBSCRIBE_INTERVAL{1000 * 1000}; // interval in microseconds at which we try to subscribe to a topic // if we haven't succeeded before namespace px4 @@ -355,9 +359,9 @@ class Logger : public ModuleBase * @param mission_log_type * @return true if log started */ - bool start_stop_logging(int vehicle_status_sub, int manual_control_sp_sub, MissionLogType mission_log_type); + bool start_stop_logging(MissionLogType mission_log_type); - void handle_vehicle_command_update(int vehicle_command_sub); + void handle_vehicle_command_update(); void ack_vehicle_command(vehicle_command_s *cmd, uint32_t result); /** @@ -408,6 +412,11 @@ class Logger : public ModuleBase hrt_abstime _next_load_print{0}; ///< timestamp when to print the process load PrintLoadReason _print_load_reason {PrintLoadReason::Preflight}; + uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::SubscriptionInterval _log_message_sub{ORB_ID(log_message), 20}; + param_t _sdlog_profile_handle{PARAM_INVALID}; param_t _log_utc_offset{PARAM_INVALID}; param_t _log_dirs_max{PARAM_INVALID}; diff --git a/src/modules/logger/util.cpp b/src/modules/logger/util.cpp index 958369c062d6..832718593d82 100644 --- a/src/modules/logger/util.cpp +++ b/src/modules/logger/util.cpp @@ -39,6 +39,7 @@ #include #include +#include #include #include @@ -72,18 +73,15 @@ bool file_exist(const char *filename) bool get_log_time(struct tm *tt, int utc_offset_sec, bool boot_time) { - int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + uORB::Subscription vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; - if (vehicle_gps_position_sub < 0) { - return false; - } + time_t utc_time_sec; + bool use_clock_time = true; /* Get the latest GPS publication */ vehicle_gps_position_s gps_pos; - time_t utc_time_sec; - bool use_clock_time = true; - if (orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps_pos) == 0) { + if (vehicle_gps_position_sub.copy(&gps_pos)) { utc_time_sec = gps_pos.time_utc_usec / 1e6; if (gps_pos.fix_type >= 2 && utc_time_sec >= GPS_EPOCH_SECS) { @@ -91,8 +89,6 @@ bool get_log_time(struct tm *tt, int utc_offset_sec, bool boot_time) } } - orb_unsubscribe(vehicle_gps_position_sub); - if (use_clock_time) { /* take clock time if there's no fix (yet) */ struct timespec ts = {}; From 8a0ebf068408daca6da176860699a04e4c414241 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 6 Aug 2019 02:07:29 -0400 Subject: [PATCH 2/2] logger use uORB::PublicationQueued for ulog_stream - queue depth is now set by the msg --- msg/ulog_stream.msg | 2 ++ src/modules/logger/log_writer.cpp | 4 ++-- src/modules/logger/log_writer.h | 2 +- src/modules/logger/log_writer_mavlink.cpp | 24 ++++------------------- src/modules/logger/log_writer_mavlink.h | 14 ++++++------- src/modules/logger/logger.cpp | 21 ++++---------------- src/modules/logger/logger.h | 2 +- 7 files changed, 21 insertions(+), 48 deletions(-) diff --git a/msg/ulog_stream.msg b/msg/ulog_stream.msg index e88de59f0dfd..7eaa32778dd1 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 5f68a98e280b..8511f9e92a3d 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 7aa9c1241747..abac8a5d57c9 100644 --- a/src/modules/logger/log_writer.h +++ b/src/modules/logger/log_writer.h @@ -55,7 +55,7 @@ class LogWriter 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 b99eafd0a49d..d0b0c2bc330e 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 5a086ebb8199..6b6f36819808 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 @@ class LogWriterMavlink /** 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 16f7005066bc..ebd4beb1c16a 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 @@ Or if already running: 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 d2dbd3919801..5bfe9f0c6e98 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -117,7 +117,7 @@ class Logger : public ModuleBase }; 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();