diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 684f7d1fad..f09e2651e1 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -828,6 +828,12 @@ void Logger::run() _was_armed = armed; if (armed) { + + if (_should_stop_file_log) { // happens on quick arming after disarm + _should_stop_file_log = false; + stop_log_file(); + } + start_log_file(); #ifdef DBGPRINT @@ -836,7 +842,9 @@ void Logger::run() #endif /* DBGPRINT */ } else { - stop_log_file(); + // delayed stop: we measure the process loads and then stop + initialize_load_output(); + _should_stop_file_log = true; } } } @@ -876,6 +884,23 @@ void Logger::run() if (_writer.is_started()) { + hrt_abstime loop_time = hrt_absolute_time(); + + /* check if we need to output the process load */ + if (_next_load_print != 0 && loop_time >= _next_load_print) { + _next_load_print = 0; + + if (_should_stop_file_log) { + _should_stop_file_log = false; + write_load_output(false); + stop_log_file(); + continue; // skip to next loop iteration + + } else { + write_load_output(true); + } + } + bool data_written = false; /* Check if parameters have changed */ @@ -970,10 +995,10 @@ void Logger::run() if (next_subscribe_topic_index != -1) { if (++next_subscribe_topic_index >= _subscriptions.size()) { next_subscribe_topic_index = -1; - next_subscribe_check = hrt_absolute_time() + TRY_SUBSCRIBE_INTERVAL; + next_subscribe_check = loop_time + TRY_SUBSCRIBE_INTERVAL; } - } else if (hrt_absolute_time() > next_subscribe_check) { + } else if (loop_time > next_subscribe_check) { next_subscribe_topic_index = 0; } @@ -1026,6 +1051,8 @@ void Logger::run() } } + stop_log_file(); + hrt_cancel(&timer_call); px4_sem_destroy(&timer_semaphore); @@ -1295,16 +1322,29 @@ void Logger::start_log_file() write_version(); write_formats(); write_parameters(); + write_perf_data(true); write_all_add_logged_msg(); _writer.set_need_reliable_transfer(false); _writer.unselect_write_backend(); _writer.notify(); + /* reset performance counters to get in-flight min and max values in post flight log */ + perf_reset_all(); + _start_time_file = hrt_absolute_time(); + + initialize_load_output(); } void Logger::stop_log_file() { + if (!_writer.is_started(LogWriter::BackendFile)) { + return; + } + + _writer.set_need_reliable_transfer(true); + write_perf_data(false); + _writer.set_need_reliable_transfer(false); _writer.stop_log_file(); } @@ -1323,18 +1363,111 @@ void Logger::start_log_mavlink() write_version(); write_formats(); write_parameters(); + write_perf_data(true); write_all_add_logged_msg(); _writer.set_need_reliable_transfer(false); _writer.unselect_write_backend(); _writer.notify(); + + initialize_load_output(); } void Logger::stop_log_mavlink() { + // don't write perf data since a client does not expect more data after a stop command PX4_INFO("Stop mavlink log"); _writer.stop_log_mavlink(); } +struct perf_callback_data_t { + Logger *logger; + int counter; + bool preflight; + char *buffer; +}; + +void Logger::perf_iterate_callback(perf_counter_t handle, void *user) +{ + perf_callback_data_t *callback_data = (perf_callback_data_t *)user; + const int buffer_length = 256; + char buffer[buffer_length]; + char perf_name[32]; + + perf_print_counter_buffer(buffer, buffer_length, handle); + + if (callback_data->preflight) { + snprintf(perf_name, 32, "perf_counter_preflight-%02i", callback_data->counter); + + } else { + snprintf(perf_name, 32, "perf_counter_postflight-%02i", callback_data->counter); + } + + callback_data->logger->write_info(perf_name, buffer); + ++callback_data->counter; +} + +void Logger::write_perf_data(bool preflight) +{ + perf_callback_data_t callback_data; + callback_data.logger = this; + callback_data.counter = 0; + callback_data.preflight = preflight; + + // write the perf counters + perf_iterate_all(perf_iterate_callback, &callback_data); +} + + +void Logger::print_load_callback(void *user) +{ + perf_callback_data_t *callback_data = (perf_callback_data_t *)user; + char perf_name[32]; + + if (!callback_data->buffer) { + return; + } + + if (callback_data->preflight) { + snprintf(perf_name, 32, "perf_top_preflight-%02i", callback_data->counter); + + } else { + snprintf(perf_name, 32, "perf_top_postflight-%02i", callback_data->counter); + } + + callback_data->logger->write_info(perf_name, callback_data->buffer); + ++callback_data->counter; +} + +void Logger::initialize_load_output() +{ + perf_callback_data_t callback_data; + callback_data.logger = this; + callback_data.counter = 0; + callback_data.buffer = NULL; + char buffer[140]; + hrt_abstime curr_time = hrt_absolute_time(); + init_print_load_s(curr_time, &_load); + // this will not yet print anything + print_load_buffer(curr_time, buffer, sizeof(buffer), print_load_callback, &callback_data, &_load); + _next_load_print = curr_time + 1000000; +} + +void Logger::write_load_output(bool preflight) +{ + perf_callback_data_t callback_data; + char buffer[140]; + callback_data.logger = this; + callback_data.counter = 0; + callback_data.buffer = buffer; + callback_data.preflight = preflight; + hrt_abstime curr_time = hrt_absolute_time(); + _writer.set_need_reliable_transfer(true); + // TODO: maybe we should restrict the output to a selected backend (eg. when file logging is running + // and mavlink log is started, this will be added to the file as well) + print_load_buffer(curr_time, buffer, sizeof(buffer), print_load_callback, &callback_data, &_load); + _writer.set_need_reliable_transfer(false); +} + void Logger::write_formats() { _writer.lock(); diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 6da20955df..f95605e1cf 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -40,6 +40,7 @@ #include #include #include +#include extern "C" __EXPORT int logger_main(int argc, char *argv[]); @@ -177,6 +178,22 @@ private: void write_formats(); + /** + * write performance counters + * @param preflight preflight if true, postflight otherwise + */ + void write_perf_data(bool preflight); + + /** + * callback to write the performance counters + */ + static void perf_iterate_callback(perf_counter_t handle, void *user); + + /** + * callback for print_load_buffer() to print the process load + */ + static void print_load_callback(void *user); + void write_version(); void write_info(const char *name, const char *value); @@ -220,6 +237,17 @@ private: void ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, uint16_t command, uint32_t result); + /** + * initialize the output for the process load, so that ~1 second later it will be written to the log + */ + void initialize_load_output(); + + /** + * write the process load, which was previously initialized with initialize_load_output() + */ + void write_load_output(bool preflight); + + static constexpr size_t MAX_TOPICS_NUM = 64; /**< Maximum number of logged topics */ static constexpr unsigned MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ static constexpr unsigned MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ @@ -257,6 +285,10 @@ private: orb_advert_t _mavlink_log_pub = nullptr; uint16_t _next_topic_id = 0; ///< id of next subscribed ulog topic char *_replay_file_name = nullptr; + bool _should_stop_file_log = false; /**< if true _next_load_print is set and file logging + will be stopped after load printing */ + print_load_s _load; ///< process load data + hrt_abstime _next_load_print = 0; ///< timestamp when to print the process load // control param_t _sdlog_mode_handle;