From 086c044f471bf6d79c4072481838f50f2ea08dfd Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Wed, 7 Aug 2024 07:26:12 -0800 Subject: [PATCH] mavlink: log handler rewrite for improved efficiency (#23421) --- src/modules/mavlink/mavlink_log_handler.cpp | 768 +++++++++----------- src/modules/mavlink/mavlink_log_handler.h | 118 +-- 2 files changed, 418 insertions(+), 468 deletions(-) diff --git a/src/modules/mavlink/mavlink_log_handler.cpp b/src/modules/mavlink/mavlink_log_handler.cpp index a75daf90bad7..1afa32382b19 100644 --- a/src/modules/mavlink/mavlink_log_handler.cpp +++ b/src/modules/mavlink/mavlink_log_handler.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,558 +31,498 @@ * ****************************************************************************/ -/// @file mavlink_log_handler.h -/// @author px4dev, Gus Grubba - #include "mavlink_log_handler.h" #include "mavlink_main.h" +#include #include -#include -#include - -#define MOUNTPOINT PX4_STORAGEDIR -static const char *kLogRoot = MOUNTPOINT "/log"; -static const char *kLogData = MOUNTPOINT "/logdata.txt"; -static const char *kTmpData = MOUNTPOINT "/$log$.txt"; +static constexpr int MAX_BYTES_BURST = 256 * 1024; +static const char *kLogListFilePath = PX4_STORAGEDIR "/logdata.txt"; +static const char *kLogListFilePathTemp = PX4_STORAGEDIR "/$log$.txt"; +static const char *kLogDir = PX4_STORAGEDIR "/log"; #ifdef __PX4_NUTTX #define PX4LOG_REGULAR_FILE DTYPE_FILE #define PX4LOG_DIRECTORY DTYPE_DIRECTORY +#define PX4_MAX_FILEPATH CONFIG_PATH_MAX #else +#ifndef PATH_MAX +#define PATH_MAX 1024 // maximum on macOS +#endif #define PX4LOG_REGULAR_FILE DT_REG #define PX4LOG_DIRECTORY DT_DIR +#define PX4_MAX_FILEPATH PATH_MAX #endif -//#define MAVLINK_LOG_HANDLER_VERBOSE - -#ifdef MAVLINK_LOG_HANDLER_VERBOSE -#define PX4LOG_WARN(fmt, ...) warnx(fmt, ##__VA_ARGS__) -#else -#define PX4LOG_WARN(fmt, ...) -#endif +MavlinkLogHandler::MavlinkLogHandler(Mavlink &mavlink) + : _mavlink(mavlink) +{} -//------------------------------------------------------------------- -static bool -stat_file(const char *file, time_t *date = nullptr, uint32_t *size = nullptr) +MavlinkLogHandler::~MavlinkLogHandler() { - struct stat st; - - if (stat(file, &st) == 0) { - if (date) { *date = st.st_mtime; } - - if (size) { *size = st.st_size; } + perf_free(_create_file_elapsed); + perf_free(_listing_elapsed); - return true; + if (_current_entry.fp) { + fclose(_current_entry.fp); } - return false; + unlink(kLogListFilePath); + unlink(kLogListFilePathTemp); } -//------------------------------------------------------------------- -MavlinkLogHandler::MavlinkLogHandler(Mavlink &mavlink) - : _mavlink(mavlink) +void MavlinkLogHandler::send() { + switch (_state) { + case LogHandlerState::Idle: { + state_idle(); + break; + } -} -MavlinkLogHandler::~MavlinkLogHandler() -{ - _close_and_unlink_files(); + case LogHandlerState::Listing: { + state_listing(); + break; + } + + case LogHandlerState::SendingData: { + state_sending_data(); + break; + } + } } -//------------------------------------------------------------------- -void -MavlinkLogHandler::handle_message(const mavlink_message_t *msg) +void MavlinkLogHandler::handle_message(const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_LOG_REQUEST_LIST: - _log_request_list(msg); + handle_log_request_list(msg); break; case MAVLINK_MSG_ID_LOG_REQUEST_DATA: - _log_request_data(msg); + handle_log_request_data(msg); break; - case MAVLINK_MSG_ID_LOG_ERASE: - _log_request_erase(msg); + case MAVLINK_MSG_ID_LOG_REQUEST_END: + handle_log_request_end(msg); break; - case MAVLINK_MSG_ID_LOG_REQUEST_END: - _log_request_end(msg); + case MAVLINK_MSG_ID_LOG_ERASE: + handle_log_erase(msg); break; } } -//------------------------------------------------------------------- -void -MavlinkLogHandler::send() +void MavlinkLogHandler::state_idle() { - //-- An arbitrary count of max bytes in one go (one of the two below but never both) -#define MAX_BYTES_SEND 256 * 1024 - size_t count = 0; - - //-- Log Entries - while (_current_status == LogHandlerState::Listing - && _mavlink.get_free_tx_buf() > MAVLINK_MSG_ID_LOG_ENTRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES - && count < MAX_BYTES_SEND) { - count += _log_send_listing(); - } - - //-- Log Data - while (_current_status == LogHandlerState::SendingData - && _mavlink.get_free_tx_buf() > MAVLINK_MSG_ID_LOG_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES - && count < MAX_BYTES_SEND) { - count += _log_send_data(); + if (_current_entry.fp && _file_send_finished) { + fclose(_current_entry.fp); + _current_entry.fp = nullptr; + + _current_entry.id = 0xffff; + _current_entry.time_utc = 0; + _current_entry.size_bytes = 0; + _current_entry.filepath[0] = 0; + _current_entry.offset = 0; + + _entry_request.id = 0xffff; + _entry_request.start_offset = 0; + _entry_request.byte_count = 0; } } -//------------------------------------------------------------------- -void -MavlinkLogHandler::_log_request_list(const mavlink_message_t *msg) +void MavlinkLogHandler::state_listing() { - mavlink_log_request_list_t request; - mavlink_msg_log_request_list_decode(msg, &request); + static constexpr uint32_t MAVLINK_PACKET_SIZE = MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_LOG_ENTRY_LEN; - //-- Check for re-requests (data loss) or new request - if (_current_status != LogHandlerState::Inactive) { - //-- Is this a new request? - if (request.start == 0) { - _current_status = LogHandlerState::Inactive; - _close_and_unlink_files(); - - } else { - _current_status = LogHandlerState::Idle; - - } + if (_mavlink.get_free_tx_buf() <= MAVLINK_PACKET_SIZE) { + return; } - if (_current_status == LogHandlerState::Inactive) { - //-- Prepare new request - - _reset_list_helper(); - _init_list_helper(); - _current_status = LogHandlerState::Idle; - } + DIR *dp = opendir(kLogDir); - if (_log_count) { - //-- Define (and clamp) range - _next_entry = request.start < _log_count ? request.start : - _log_count - 1; - _last_entry = request.end < _log_count ? request.end : - _log_count - 1; + if (!dp) { + PX4_DEBUG("No logs available"); + return; } - PX4LOG_WARN("\nMavlinkLogHandler::_log_request_list: start: %d last: %d count: %d", - _next_entry, - _last_entry, - _log_count); - //-- Enable streaming - _current_status = LogHandlerState::Listing; -} + FILE *fp = fopen(kLogListFilePath, "r"); -//------------------------------------------------------------------- -void -MavlinkLogHandler::_log_request_data(const mavlink_message_t *msg) -{ - //-- If we haven't listed, we can't do much - if (_current_status == LogHandlerState::Inactive) { - PX4LOG_WARN("MavlinkLogHandler::_log_request_data Log request with no list requested."); + if (!fp) { + PX4_DEBUG("Failed to open log list file"); + closedir(dp); return; } - mavlink_log_request_data_t request; - mavlink_msg_log_request_data_decode(msg, &request); + fseek(fp, _list_request.file_index, SEEK_SET); - //-- Does the requested log exist? - if (request.id >= _log_count) { - PX4LOG_WARN("MavlinkLogHandler::_log_request_data Requested log %" PRIu16 " but we only have %u.", request.id, - _log_count); - return; - } + size_t bytes_sent = 0; - //-- If we were sending log entries, stop it - _current_status = LogHandlerState::Idle; + char line[PX4_MAX_FILEPATH]; - if (_current_log_index != request.id) { - //-- Init send log dataset - _current_log_filename[0] = 0; - _current_log_index = request.id; - uint32_t time_utc = 0; + perf_begin(_listing_elapsed); - if (!_get_entry(_current_log_index, _current_log_size, time_utc, - _current_log_filename, sizeof(_current_log_filename))) { - PX4LOG_WARN("LogListHelper::get_entry failed."); - return; + while (fgets(line, sizeof(line), fp)) { + + if (_list_request.current_id < _list_request.first_id) { + _list_request.current_id++; + continue; } - _open_for_transmit(); - } + // We can send! + uint32_t size_bytes = 0; + uint32_t time_utc = 0; + char filepath[PX4_MAX_FILEPATH]; - _current_log_data_offset = request.ofs; + // If parsed lined successfully, send the entry + if (sscanf(line, "%" PRIu32 " %" PRIu32 " %s", &time_utc, &size_bytes, filepath) != 3) { + PX4_DEBUG("sscanf failed"); + continue; + } - if (_current_log_data_offset >= _current_log_size) { - _current_log_data_remaining = 0; + send_log_entry(time_utc, size_bytes); + bytes_sent += sizeof(mavlink_log_entry_t); + _list_request.current_id++; - } else { - _current_log_data_remaining = _current_log_size - request.ofs; + // Yield if we've exceed mavlink burst or buffer limit + if (_mavlink.get_free_tx_buf() <= MAVLINK_PACKET_SIZE || bytes_sent >= MAX_BYTES_BURST) { + _list_request.file_index = ftell(fp); + fclose(fp); + closedir(dp); + perf_end(_listing_elapsed); + return; + } } - if (_current_log_data_remaining > request.count) { - _current_log_data_remaining = request.count; - } + perf_end(_listing_elapsed); - //-- Enable streaming - _current_status = LogHandlerState::SendingData; -} + fclose(fp); + closedir(dp); -//------------------------------------------------------------------- -void -MavlinkLogHandler::_log_request_erase(const mavlink_message_t * /*msg*/) -{ - /* - mavlink_log_erase_t request; - mavlink_msg_log_erase_decode(msg, &request); - */ - _current_status = LogHandlerState::Inactive; - _close_and_unlink_files(); - - //-- Delete all logs - _delete_all(kLogRoot); + _list_request.current_id = 0; + _list_request.file_index = 0; + _state = LogHandlerState::Idle; } -//------------------------------------------------------------------- -void -MavlinkLogHandler::_log_request_end(const mavlink_message_t * /*msg*/) +void MavlinkLogHandler::state_sending_data() { - PX4LOG_WARN("MavlinkLogHandler::_log_request_end"); + static constexpr uint32_t MAVLINK_PACKET_SIZE = MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_LOG_DATA_LEN; + size_t bytes_sent = 0; - _current_status = LogHandlerState::Inactive; - _close_and_unlink_files(); -} + while (_mavlink.get_free_tx_buf() > MAVLINK_PACKET_SIZE && bytes_sent < MAX_BYTES_BURST) { -//------------------------------------------------------------------- -size_t -MavlinkLogHandler::_log_send_listing() -{ - mavlink_log_entry_t response; - uint32_t size, date; - _get_entry(_next_entry, size, date); - response.size = size; - response.time_utc = date; - response.id = _next_entry; - response.num_logs = _log_count; - response.last_log_num = _last_entry; - mavlink_msg_log_entry_send_struct(_mavlink.get_channel(), &response); - - //-- If we're done listing, flag it. - if (_next_entry >= _last_entry) { - _current_status = LogHandlerState::Idle; - - } else { - _next_entry++; - } + // Only seek if we need to + long int offset = _current_entry.offset - ftell(_current_entry.fp); - PX4LOG_WARN("MavlinkLogHandler::_log_send_listing id: %" PRIu16 " count: %" PRIu16 " last: %" PRIu16 " size: %" PRIu32 - " date: %" PRIu32 " status: %" PRIu32, - response.id, - response.num_logs, - response.last_log_num, - response.size, - response.time_utc, - (uint32_t)_current_status); - return sizeof(response); -} + if (offset && fseek(_current_entry.fp, offset, SEEK_CUR)) { + fclose(_current_entry.fp); + _current_entry.fp = nullptr; + PX4_DEBUG("seek error"); + _state = LogHandlerState::Idle; + } -//------------------------------------------------------------------- -size_t -MavlinkLogHandler::_log_send_data() -{ - mavlink_log_data_t response; - memset(&response, 0, sizeof(response)); - uint32_t len = _current_log_data_remaining; + // Prepare mavlink message + mavlink_log_data_t msg; - if (len > sizeof(response.data)) { - len = sizeof(response.data); - } + if (_current_entry.offset >= _current_entry.size_bytes) { + PX4_DEBUG("Entry offset equal to file size"); + _state = LogHandlerState::Idle; + return; + } - size_t read_size = _get_log_data(len, response.data); - response.ofs = _current_log_data_offset; - response.id = _current_log_index; - response.count = read_size; - mavlink_msg_log_data_send_struct(_mavlink.get_channel(), &response); - _current_log_data_offset += read_size; - _current_log_data_remaining -= read_size; + size_t bytes_to_read = _current_entry.size_bytes - _current_entry.offset; - if (read_size < sizeof(response.data) || _current_log_data_remaining == 0) { - _current_status = LogHandlerState::Idle; - } + if (bytes_to_read > sizeof(msg.data)) { + bytes_to_read = sizeof(msg.data); + } - return sizeof(response); -} + msg.count = fread(msg.data, 1, bytes_to_read, _current_entry.fp); + msg.id = _current_entry.id; + msg.ofs = _current_entry.offset; -//------------------------------------------------------------------- -void MavlinkLogHandler::_close_and_unlink_files() -{ - if (_current_log_filep) { - ::fclose(_current_log_filep); - _reset_list_helper(); - } + mavlink_msg_log_data_send_struct(_mavlink.get_channel(), &msg); + + bytes_sent += MAVLINK_PACKET_SIZE; + _current_entry.offset += msg.count; + + bool chunk_finished = _current_entry.offset >= (_entry_request.byte_count + _entry_request.start_offset); + _file_send_finished = _current_entry.offset >= _current_entry.size_bytes; - // Remove log data files (if any) - unlink(kLogData); - unlink(kTmpData); + if (chunk_finished || _file_send_finished) { + _state = LogHandlerState::Idle; + return; + } + } } -//------------------------------------------------------------------- -bool -MavlinkLogHandler::_get_entry(int idx, uint32_t &size, uint32_t &date, char *filename, int filename_len) +void MavlinkLogHandler::handle_log_request_list(const mavlink_message_t *msg) { - //-- Find log file in log list file created during init() - size = 0; - date = 0; - bool result = false; - //-- Open list of log files - FILE *f = ::fopen(kLogData, "r"); - - if (f) { - //--- Find requested entry - char line[160]; - int count = 0; - - while (fgets(line, sizeof(line), f)) { - //-- Found our "index" - if (count++ == idx) { - char file[160]; - - if (sscanf(line, "%" PRIu32 " %" PRIu32 " %s", &date, &size, file) == 3) { - if (filename && filename_len > 0) { - strncpy(filename, file, filename_len); - filename[filename_len - 1] = 0; // ensure null-termination - } - - result = true; - break; - } - } - } + mavlink_log_request_list_t request; + mavlink_msg_log_request_list_decode(msg, &request); - fclose(f); + if (!create_log_list_file()) { + return; } - return result; + _list_request.first_id = request.start; + _list_request.last_id = request.end == 0xffff ? _num_logs : request.end; + _list_request.current_id = 0; + _list_request.file_index = 0; + _logs_listed = true; + _state = LogHandlerState::Listing; } -//------------------------------------------------------------------- -bool -MavlinkLogHandler::_open_for_transmit() +void MavlinkLogHandler::handle_log_request_data(const mavlink_message_t *msg) { - if (_current_log_filep) { - ::fclose(_current_log_filep); - _current_log_filep = nullptr; + if (!_logs_listed) { + PX4_DEBUG("Logs not yet listed"); + _state = LogHandlerState::Idle; + return; } - _current_log_filep = ::fopen(_current_log_filename, "rb"); + mavlink_log_request_data_t request; + mavlink_msg_log_request_data_decode(msg, &request); - if (!_current_log_filep) { - PX4LOG_WARN("MavlinkLogHandler::open_for_transmit Could not open %s", _current_log_filename); - return false; + if (request.id >= _num_logs) { + PX4_DEBUG("Requested log %" PRIu16 " but we only have %u", request.id, _num_logs); + _state = LogHandlerState::Idle; + return; } - return true; -} + // Handle switching to new request ID + if (request.id != _current_entry.id) { + // Close the old file + if (_current_entry.fp) { + fclose(_current_entry.fp); + _current_entry.fp = nullptr; + } -//------------------------------------------------------------------- -size_t -MavlinkLogHandler::_get_log_data(uint8_t len, uint8_t *buffer) -{ - if (!_current_log_filename[0]) { - return 0; - } + LogEntry entry = {}; - if (!_current_log_filep) { - PX4LOG_WARN("MavlinkLogHandler::get_log_data file not open %s", _current_log_filename); - return 0; - } + if (!log_entry_from_id(request.id, &entry)) { + PX4_DEBUG("Log file ID %u does not exist", request.id); + _state = LogHandlerState::Idle; + return; + } - long int offset = _current_log_data_offset - ftell(_current_log_filep); + entry.fp = fopen(entry.filepath, "rb"); + entry.offset = request.ofs; - if (offset && fseek(_current_log_filep, offset, SEEK_CUR)) { - fclose(_current_log_filep); - _current_log_filep = nullptr; - PX4LOG_WARN("MavlinkLogHandler::get_log_data Seek error in %s", _current_log_filename); - return 0; + if (!entry.fp) { + PX4_DEBUG("Failed to open file %s", entry.filepath); + return; + } + + _current_entry = entry; } - size_t result = fread(buffer, 1, len, _current_log_filep); - return result; -} + // Stop if offset request is larger than file + if (request.ofs >= _current_entry.size_bytes) { + PX4_DEBUG("Request offset %" PRIu32 "greater than file size %" PRIu32, request.ofs, + _current_entry.size_bytes); + _state = LogHandlerState::Idle; + return; + } + _entry_request.id = request.id; + _entry_request.start_offset = request.ofs; + _entry_request.byte_count = request.count; + // Set the offset of the current entry to the requested offset + _current_entry.offset = _entry_request.start_offset; + _file_send_finished = false; + _state = LogHandlerState::SendingData; +} -void -MavlinkLogHandler::_reset_list_helper() +void MavlinkLogHandler::handle_log_request_end(const mavlink_message_t *msg) { - _next_entry = 0; - _last_entry = 0; - _log_count = 0; - _current_log_index = UINT16_MAX; - _current_log_size = 0; - _current_log_data_offset = 0; - _current_log_data_remaining = 0; - _current_log_filep = nullptr; + _state = LogHandlerState::Idle; } -void -MavlinkLogHandler::_init_list_helper() +void MavlinkLogHandler::handle_log_erase(const mavlink_message_t *msg) { - /* + if (_current_entry.fp) { + fclose(_current_entry.fp); + _current_entry.fp = nullptr; + } + + _state = LogHandlerState::Idle; + unlink(kLogListFilePath); + unlink(kLogListFilePathTemp); - When this helper is created, it scans the log directory - and collects all log files found into one file for easy, - subsequent access. - */ + delete_all_logs(kLogDir); +} - _current_log_filename[0] = 0; +bool MavlinkLogHandler::create_log_list_file() +{ + perf_begin(_create_file_elapsed); - // Remove old log data file (if any) - unlink(kLogData); - // Open log directory - DIR *dp = opendir(kLogRoot); + // clean up old file + unlink(kLogListFilePath); + _num_logs = 0; - if (dp == nullptr) { - // No log directory. Nothing to do. - return; + DIR *dp = opendir(kLogDir); + + if (!dp) { + PX4_DEBUG("No logs available"); + return false; } - // Create work file - FILE *f = ::fopen(kTmpData, "w"); + FILE *temp_fp = fopen(kLogListFilePathTemp, "w"); - if (!f) { - PX4LOG_WARN("MavlinkLogHandler::init Error creating %s", kTmpData); + if (!temp_fp) { + PX4_DEBUG("Failed to create temp file"); closedir(dp); - return; + return false; } - // Scan directory and collect log files struct dirent *result = nullptr; - while ((result = readdir(dp))) { - if (result->d_type == PX4LOG_DIRECTORY) { - time_t tt = 0; - char log_path[128]; - int ret = snprintf(log_path, sizeof(log_path), "%s/%s", kLogRoot, result->d_name); - bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_path)); + // Iterate over the log/ directory which contains subdirectories formatted: yyyy-mm-dd + while (1) { + result = readdir(dp); - if (path_is_ok) { - if (_get_session_date(log_path, result->d_name, tt)) { - _scan_logs(f, log_path, tt); - } - } + if (!result) { + // Reached end of directory + break; + } + + if (result->d_type != PX4LOG_DIRECTORY) { + // Skip stray files + continue; + } + + // Skip the '.' and '..' entries + if (strcmp(result->d_name, ".") == 0 || strcmp(result->d_name, "..") == 0) { + continue; + } + + // Open up the sub directory + char dirpath[PX4_MAX_FILEPATH]; + int ret = snprintf(dirpath, sizeof(dirpath), "%s/%s", kLogDir, result->d_name); + + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(dirpath)); + + if (!path_is_ok) { + PX4_DEBUG("Log subdir path error: %s", dirpath); + continue; } + + // Iterate over files inside the subdir and write them to the file + write_entries_to_file(temp_fp, dirpath); } + fclose(temp_fp); closedir(dp); - fclose(f); // Rename temp file to data file - if (rename(kTmpData, kLogData)) { - PX4LOG_WARN("MavlinkLogHandler::init Error renaming %s", kTmpData); - _log_count = 0; + if (rename(kLogListFilePathTemp, kLogListFilePath)) { + PX4_DEBUG("Failed to rename temp file"); + return false; } + + perf_end(_create_file_elapsed); + + return true; } -//------------------------------------------------------------------- -bool -MavlinkLogHandler::_get_session_date(const char *path, const char *dir, time_t &date) +void MavlinkLogHandler::write_entries_to_file(FILE *fp, const char *dir) { - if (strlen(dir) > 4) { - // Always try to get file time first - if (stat_file(path, &date)) { - // Try to prevent taking date if it's around 1970 (use the logic below instead) - if (date > 60 * 60 * 24) { - return true; - } + DIR *dp = opendir(dir); + struct dirent *result = nullptr; + + while (1) { + result = readdir(dp); + + if (!result) { + // Reached end of directory + break; } - // Convert "sess000" to 00:00 Jan 1 1970 (day per session) - if (strncmp(dir, "sess", 4) == 0) { - unsigned u; + if (result->d_type != PX4LOG_REGULAR_FILE) { + // Skip non files + continue; + } - if (sscanf(&dir[4], "%u", &u) == 1) { - date = u * 60 * 60 * 24; - return true; - } + char filepath[PX4_MAX_FILEPATH]; + int ret = snprintf(filepath, sizeof(filepath), "%s/%s", dir, result->d_name); + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(filepath)); + + if (!path_is_ok) { + PX4_DEBUG("Log subdir path error: %s", filepath); + continue; + } + + struct stat filestat; + + if (stat(filepath, &filestat) != 0) { + PX4_DEBUG("stat() failed: %s", filepath); + continue; } + + // Write to file using format: + // [ time ] [ size_bytes ] [ filepath ] + fprintf(fp, "%u %u %s\n", unsigned(filestat.st_mtime), unsigned(filestat.st_size), filepath); + _num_logs++; } - return false; + closedir(dp); } -//------------------------------------------------------------------- -void -MavlinkLogHandler::_scan_logs(FILE *f, const char *dir, time_t &date) +void MavlinkLogHandler::send_log_entry(uint32_t time_utc, uint32_t size_bytes) { - DIR *dp = opendir(dir); + mavlink_log_entry_t msg; + msg.time_utc = time_utc; + msg.size = size_bytes; + msg.id = _list_request.current_id; + msg.num_logs = _num_logs; + msg.last_log_num = _list_request.last_id; + mavlink_msg_log_entry_send_struct(_mavlink.get_channel(), &msg); +} - if (dp) { - struct dirent *result = nullptr; - - while ((result = readdir(dp))) { - if (result->d_type == PX4LOG_REGULAR_FILE) { - time_t ldate = date; - uint32_t size = 0; - char log_file_path[128]; - int ret = snprintf(log_file_path, sizeof(log_file_path), "%s/%s", dir, result->d_name); - bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_file_path)); - - if (path_is_ok) { - if (_get_log_time_size(log_file_path, result->d_name, ldate, size)) { - //-- Write result->out to list file - fprintf(f, "%u %u %s\n", (unsigned)ldate, (unsigned)size, log_file_path); - _log_count++; - } - } - } - } +bool MavlinkLogHandler::log_entry_from_id(uint16_t log_id, LogEntry *entry) +{ + DIR *dp = opendir(kLogDir); + + if (!dp) { + PX4_INFO("No logs available"); + return false; + } + + FILE *fp = fopen(kLogListFilePath, "r"); + if (!fp) { + PX4_DEBUG("Failed to open %s", kLogListFilePath); closedir(dp); + return false; } -} -//------------------------------------------------------------------- -bool -MavlinkLogHandler::_get_log_time_size(const char *path, const char *file, time_t &date, uint32_t &size) -{ - if (file && file[0]) { - if (strstr(file, ".px4log") || strstr(file, ".ulg")) { - // Always try to get file time first - if (stat_file(path, &date, &size)) { - // Try to prevent taking date if it's around 1970 (use the logic below instead) - if (date > 60 * 60 * 24) { - return true; - } - } + bool found_entry = false; + uint16_t current_id = 0; + char line[PX4_MAX_FILEPATH]; - // Convert "log000" to 00:00 (minute per flight in session) - if (strncmp(file, "log", 3) == 0) { - unsigned u; + while (fgets(line, sizeof(line), fp)) { - if (sscanf(&file[3], "%u", &u) == 1) { - date += (u * 60); + if (current_id != log_id) { + current_id++; + continue; + } - if (stat_file(path, nullptr, &size)) { - return true; - } - } - } + if (sscanf(line, "%" PRIu32 " %" PRIu32 " %s", &(entry->time_utc), &(entry->size_bytes), entry->filepath) != 3) { + PX4_DEBUG("sscanf failed"); + continue; } + + entry->id = log_id; + found_entry = true; + break; } - return false; + fclose(fp); + closedir(dp); + + return found_entry; } -//------------------------------------------------------------------- -void -MavlinkLogHandler::_delete_all(const char *dir) +void MavlinkLogHandler::delete_all_logs(const char *dir) { //-- Open log directory DIR *dp = opendir(dir); @@ -600,27 +540,27 @@ MavlinkLogHandler::_delete_all(const char *dir) } if (result->d_type == PX4LOG_DIRECTORY && result->d_name[0] != '.') { - char log_path[128]; - int ret = snprintf(log_path, sizeof(log_path), "%s/%s", dir, result->d_name); - bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_path)); + char filepath[PX4_MAX_FILEPATH]; + int ret = snprintf(filepath, sizeof(filepath), "%s/%s", dir, result->d_name); + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(filepath)); if (path_is_ok) { - _delete_all(log_path); //Recursive call. TODO: consider add protection + delete_all_logs(filepath); - if (rmdir(log_path)) { - PX4LOG_WARN("MavlinkLogHandler::delete_all Error removing %s", log_path); + if (rmdir(filepath)) { + PX4_DEBUG("Error removing %s", filepath); } } } if (result->d_type == PX4LOG_REGULAR_FILE) { - char log_path[128]; - int ret = snprintf(log_path, sizeof(log_path), "%s/%s", dir, result->d_name); - bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_path)); + char filepath[PX4_MAX_FILEPATH]; + int ret = snprintf(filepath, sizeof(filepath), "%s/%s", dir, result->d_name); + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(filepath)); if (path_is_ok) { - if (unlink(log_path)) { - PX4LOG_WARN("MavlinkLogHandler::delete_all Error deleting %s", log_path); + if (unlink(filepath)) { + PX4_DEBUG("Error unlinking %s", filepath); } } } diff --git a/src/modules/mavlink/mavlink_log_handler.h b/src/modules/mavlink/mavlink_log_handler.h index 965c26712f1c..eb521a61ee4f 100644 --- a/src/modules/mavlink/mavlink_log_handler.h +++ b/src/modules/mavlink/mavlink_log_handler.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014, 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,76 +33,86 @@ #pragma once -/// @file mavlink_log_handler.h -/// @author px4dev, Gus Grubba - -#include -#include -#include -#include -#include -#include - +#include #include "mavlink_bridge_header.h" class Mavlink; -// MAVLink LOG_* Message Handler class MavlinkLogHandler { public: MavlinkLogHandler(Mavlink &mavlink); ~MavlinkLogHandler(); - // Handle possible LOG message + void send(); void handle_message(const mavlink_message_t *msg); - /** - * Handle sending of messages. Call this regularly at a fixed frequency. - * @param t current time - */ - void send(); +private: + struct LogEntry { + uint16_t id{0xffff}; + uint32_t time_utc{}; + uint32_t size_bytes{}; + FILE *fp{nullptr}; + char filepath[60]; + uint32_t offset{}; + }; - unsigned get_size(); + struct LogEntryRequest { + uint16_t id{0xffff}; + uint32_t start_offset{}; + uint32_t byte_count{}; + }; + + struct LogListRequest { + uint16_t first_id{0}; + uint16_t last_id{0}; + uint16_t current_id{0}; + int file_index{0}; + }; -private: enum class LogHandlerState { - Inactive, //There is no active action of log handler - Idle, //The log handler is not sending list/data, but list has been sent - Listing, //File list is being send - SendingData //File Data is being send + Idle, + Listing, + SendingData }; - void _log_message(const mavlink_message_t *msg); - void _log_request_list(const mavlink_message_t *msg); - void _log_request_data(const mavlink_message_t *msg); - void _log_request_erase(const mavlink_message_t *msg); - void _log_request_end(const mavlink_message_t *msg); - - void _reset_list_helper(); - void _init_list_helper(); - bool _get_session_date(const char *path, const char *dir, time_t &date); - void _scan_logs(FILE *f, const char *dir, time_t &date); - bool _get_log_time_size(const char *path, const char *file, time_t &date, uint32_t &size); - static void _delete_all(const char *dir); - bool _get_entry(int idx, uint32_t &size, uint32_t &date, char *filename = 0, int filename_len = 0); - bool _open_for_transmit(); - size_t _get_log_data(uint8_t len, uint8_t *buffer); - void _close_and_unlink_files(); - - size_t _log_send_listing(); - size_t _log_send_data(); - - LogHandlerState _current_status{LogHandlerState::Inactive}; + + // mavlink message handlers + void handle_log_request_list(const mavlink_message_t *msg); + void handle_log_request_data(const mavlink_message_t *msg); + void handle_log_request_end(const mavlink_message_t *msg); + void handle_log_erase(const mavlink_message_t *msg); + + // state functions + void state_idle(); + void state_listing(); + void state_sending_data(); + + // Log request list + bool create_log_list_file(); + void write_entries_to_file(FILE *f, const char *dir); + void send_log_entry(uint32_t size, uint32_t time_utc); + + // Log request data + bool log_entry_from_id(uint16_t log_id, LogEntry *entry); + + // Log erase + void delete_all_logs(const char *dir); + + +private: + LogHandlerState _state{LogHandlerState::Idle}; Mavlink &_mavlink; - int _next_entry{0}; - int _last_entry{0}; - int _log_count{0}; + // Log list + LogListRequest _list_request{}; + int _num_logs{0}; + bool _logs_listed{false}; + + // Log data + LogEntry _current_entry{}; + LogEntryRequest _entry_request{}; + bool _file_send_finished{}; - uint16_t _current_log_index{UINT16_MAX}; - uint32_t _current_log_size{0}; - uint32_t _current_log_data_offset{0}; - uint32_t _current_log_data_remaining{0}; - FILE *_current_log_filep{nullptr}; - char _current_log_filename[128]; //TODO: consider to allocate on runtime + perf_counter_t _create_file_elapsed{perf_alloc(PC_ELAPSED, MODULE_NAME": create file")}; + perf_counter_t _listing_elapsed{perf_alloc(PC_ELAPSED, MODULE_NAME": listing")}; };