6568658631

Committed 19 Oct 2023 01:31AM UTC coverage: 31.23% (+0.02%) from 31.215%

Build # 6568658631

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2155 from mavlink/pr-static-fixes

Threading fixes, MAVLink sequence number cleanup

Run Details

1386 of 1386 new or added lines in 46 files covered. (100.0%)

7906 of 25315 relevant lines covered (31.23%)

23.54 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

0.0

/src/mavsdk/plugins/log_files/log_files_impl.cpp

#include "log_files_impl.h"
#include "mavlink_address.h"
#include "mavsdk_impl.h"
#include "filesystem_include.h"
#include "unused.h"

#include <algorithm>
#include <cmath>
#include <ctime>
#include <cstring>

namespace mavsdk {

LogFilesImpl::LogFilesImpl(System& system) : PluginImplBase(system)
{
    _system_impl->register_plugin(this);
}

LogFilesImpl::LogFilesImpl(std::shared_ptr<System> system) : PluginImplBase(std::move(system))
{
    _system_impl->register_plugin(this);
}

LogFilesImpl::~LogFilesImpl()
{
    _system_impl->unregister_plugin(this);
}

void LogFilesImpl::init()
{
    _system_impl->register_mavlink_message_handler(
        MAVLINK_MSG_ID_LOG_ENTRY,
        [this](const mavlink_message_t& message) { process_log_entry(message); },
        this);

    _system_impl->register_mavlink_message_handler(
        MAVLINK_MSG_ID_LOG_DATA,
        [this](const mavlink_message_t& message) { process_log_data(message); },
        this);

    // Cancel any log file downloads that might still be going on.
    request_end();
}

void LogFilesImpl::deinit()
{
    {
        std::lock_guard<std::mutex> lock(_entries.mutex);
        _system_impl->unregister_timeout_handler(_entries.cookie);
    }

    {
        std::lock_guard<std::mutex> lock(_data.mutex);
        _system_impl->unregister_timeout_handler(_data.cookie);
    }
    _system_impl->unregister_all_mavlink_message_handlers(this);
}

void LogFilesImpl::enable() {}

void LogFilesImpl::disable() {}

void LogFilesImpl::request_end()
{
    _system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
        mavlink_message_t message;
        mavlink_msg_log_request_end_pack_chan(
            mavlink_address.system_id,
            mavlink_address.component_id,
            channel,
            &message,
            _system_impl->get_system_id(),
            MAV_COMP_ID_AUTOPILOT1);
        return message;
    });
}

std::pair<LogFiles::Result, std::vector<LogFiles::Entry>> LogFilesImpl::get_entries()
{
    auto prom =
        std::make_shared<std::promise<std::pair<LogFiles::Result, std::vector<LogFiles::Entry>>>>();
    auto future_result = prom->get_future();

    get_entries_async([prom](LogFiles::Result result, std::vector<LogFiles::Entry> entries) {
        prom->set_value(std::make_pair<>(result, entries));
    });
    return future_result.get();
}

void LogFilesImpl::get_entries_async(LogFiles::GetEntriesCallback callback)
{
    {
        std::lock_guard<std::mutex> lock(_entries.mutex);
        _entries.entry_map.clear();
        _entries.callback = callback;
        _entries.max_list_id = 0;
        _entries.retries = 0;
    }

    // This first step can take a moment, on PX4 with 100+ logs I see about 2-3s.
    _system_impl->register_timeout_handler(
        [this]() { list_timeout(); }, _system_impl->timeout_s() * 10.0, &_entries.cookie);

    request_list_entry(-1);
}

void LogFilesImpl::request_list_entry(int entry_id)
{
    // all
    uint16_t index_min = 0;
    uint16_t index_max = 0xFFFF;

    if (entry_id >= 0) {
        index_min = entry_id;
        index_max = entry_id;
    }

    _system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
        mavlink_message_t message;
        mavlink_msg_log_request_list_pack_chan(
            mavlink_address.system_id,
            mavlink_address.component_id,
            channel,
            &message,
            _system_impl->get_system_id(),
            MAV_COMP_ID_AUTOPILOT1,
            index_min,
            index_max);
        return message;
    });
}

void LogFilesImpl::process_log_entry(const mavlink_message_t& message)
{
    mavlink_log_entry_t log_entry;
    mavlink_msg_log_entry_decode(&message, &log_entry);

    // Catch case where there are no log files to be found.
    if (log_entry.num_logs == 0 && log_entry.id == 0) {
        std::lock_guard<std::mutex> lock(_entries.mutex);
        _system_impl->unregister_timeout_handler(_entries.cookie);
        if (_entries.callback) {
            const auto tmp_callback = _entries.callback;
            std::vector<LogFiles::Entry> empty_list{};
            _system_impl->call_user_callback([tmp_callback, empty_list]() {
                tmp_callback(LogFiles::Result::NoLogfiles, empty_list);
            });
        }
        return;
    }

    LogFiles::Entry new_entry;
    new_entry.id = log_entry.id;

    // Convert milliseconds to ISO 8601 date string in UTC.
    char buf[sizeof "2018-08-31T20:50:42Z"];
    const time_t time_utc = log_entry.time_utc;
    strftime(buf, sizeof buf, "%FT%TZ", gmtime(&time_utc));

    new_entry.date = buf;
    new_entry.size_bytes = log_entry.size;
    {
        std::lock_guard<std::mutex> lock(_entries.mutex);
        _entries.entry_map[new_entry.id] = new_entry;
        _entries.max_list_id = log_entry.num_logs;
        _system_impl->refresh_timeout_handler(_entries.cookie);
    }
}

void LogFilesImpl::list_timeout()
{
    std::lock_guard<std::mutex> lock(_entries.mutex);
    if (_entries.entry_map.size() == 0) {
        LogWarn() << "No entries received";
    } else if (_entries.entry_map.size() == _entries.max_list_id) {
        LogDebug() << "Received all entries";
        // Copy map entries into list to return;
        std::vector<LogFiles::Entry> entry_list{};
        for (unsigned i = 0; i < _entries.max_list_id; ++i) {
            entry_list.push_back(_entries.entry_map[i]);
        }
        if (_entries.callback) {
            const auto tmp_callback = _entries.callback;
            _system_impl->call_user_callback([tmp_callback, entry_list]() {
                tmp_callback(LogFiles::Result::Success, entry_list);
            });
        }
    } else {
        if (_entries.retries > 3) {
            LogWarn() << "Too many log entry retries, giving up.";
            if (_entries.callback) {
                const auto tmp_callback = _entries.callback;
                _system_impl->call_user_callback([tmp_callback]() {
                    std::vector<LogFiles::Entry> empty_vector{};
                    tmp_callback(LogFiles::Result::Timeout, empty_vector);
                });
            }
        } else {
            for (unsigned i = 0; i < _entries.max_list_id; ++i) {
                auto it = _entries.entry_map.find(i);
                if (it == _entries.entry_map.end()) {
                    LogDebug() << "Requesting log entry " << i << " again";
                    request_list_entry(int(i));
                }
            }
            _system_impl->register_timeout_handler(
                [this]() { list_timeout(); }, _system_impl->timeout_s() * 10.0, &_entries.cookie);
            _entries.retries++;
        }
    }
}

std::pair<LogFiles::Result, LogFiles::ProgressData>
LogFilesImpl::download_log_file(LogFiles::Entry entry, const std::string& file_path)
{
    auto prom =
        std::make_shared<std::promise<std::pair<LogFiles::Result, LogFiles::ProgressData>>>();
    auto future_result = prom->get_future();

    download_log_file_async(
        entry, file_path, [prom](LogFiles::Result result, LogFiles::ProgressData progress) {
            UNUSED(progress);
            if (result != LogFiles::Result::Next) {
                prom->set_value(std::make_pair(result, progress));
            }
        });
    return future_result.get();
}

void LogFilesImpl::download_log_file_async(
    LogFiles::Entry entry, const std::string& file_path, LogFiles::DownloadLogFileCallback callback)
{
    unsigned bytes_to_get;
    {
        std::lock_guard<std::mutex> lock(_entries.mutex);

        auto it = _entries.entry_map.find(entry.id);
        if (it == _entries.entry_map.end()) {
            LogErr() << "Log entry id " << entry.id << " not found";
            if (callback) {
                const auto tmp_callback = callback;
                _system_impl->call_user_callback([tmp_callback]() {
                    LogFiles::ProgressData progress;
                    progress.progress = 0.0f;
                    tmp_callback(LogFiles::Result::InvalidArgument, progress);
                });
            }
            return;
        }

        bytes_to_get = _entries.entry_map[entry.id].size_bytes;
    }

    {
        std::lock_guard<std::mutex> lock(_data.mutex);

        if (is_directory(file_path)) {
            if (callback) {
                const auto tmp_callback = callback;
                _system_impl->call_user_callback([tmp_callback]() {
                    LogFiles::ProgressData progress;
                    progress.progress = NAN;
                    LogErr()
                        << "Invalid path! The path must point to an unexisting file, and it points to a directory!";
                    tmp_callback(LogFiles::Result::InvalidArgument, progress);
                });
            }
            return;
        }

        if (file_exists(file_path)) {
            if (callback) {
                const auto tmp_callback = callback;
                _system_impl->call_user_callback([tmp_callback]() {
                    LogFiles::ProgressData progress;
                    progress.progress = NAN;
                    LogErr() << "Target log file already exists!";
                    tmp_callback(LogFiles::Result::InvalidArgument, progress);
                });
            }
            return;
        }

        if (!start_logfile(file_path)) {
            if (callback) {
                const auto tmp_callback = callback;
                _system_impl->call_user_callback([tmp_callback]() {
                    LogFiles::ProgressData progress;
                    progress.progress = NAN;
                    tmp_callback(LogFiles::Result::FileOpenFailed, progress);
                });
            }
            return;
        }

        _data.id = entry.id;
        _data.callback = callback;
        _data.time_started = _time.steady_time();
        _data.bytes_to_get = bytes_to_get;
        _data.part_start = 0;
        const auto part_size = determine_part_end() - _data.part_start;
        _data.bytes.resize(part_size);
        _data.chunks_received.resize(
            part_size / MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN +
            ((part_size % MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) != 0));

        _system_impl->register_timeout_handler(
            [this]() { LogFilesImpl::data_timeout(); }, _system_impl->timeout_s(), &_data.cookie);

        request_log_data(_data.id, _data.part_start, _data.bytes.size());

        if (_data.callback) {
            const auto tmp_callback = _data.callback;
            _system_impl->call_user_callback([tmp_callback]() {
                LogFiles::ProgressData progress;
                progress.progress = 0.0f;
                tmp_callback(LogFiles::Result::Next, progress);
            });
        }
    }
}

LogFiles::Result LogFilesImpl::erase_all_log_files()
{
    _system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
        mavlink_message_t message;
        mavlink_msg_log_erase_pack_chan(
            mavlink_address.system_id,
            mavlink_address.component_id,
            channel,
            &message,
            _system_impl->get_system_id(),
            MAV_COMP_ID_AUTOPILOT1);
        return message;
    });

    // TODO: find a good way to know about the success or failure of the operation
    return LogFiles::Result::Success;
}

std::size_t LogFilesImpl::determine_part_end()
{
    // Assumes to have the lock for _data.mutex.

    return std::min(
        _data.part_start + PART_SIZE * MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN,
        std::size_t(_data.bytes_to_get));
}

void LogFilesImpl::process_log_data(const mavlink_message_t& message)
{
    mavlink_log_data_t log_data;
    mavlink_msg_log_data_decode(&message, &log_data);

#if 0
    // To test retransmission
    static unsigned counter = 0;
    if (counter < 10 && (log_data.ofs == 1800 || log_data.ofs == 3600)) {
        ++counter;
        return;
    }
#endif

    std::lock_guard<std::mutex> lock(_data.mutex);

    _system_impl->refresh_timeout_handler(_data.cookie);

    if (log_data.count > MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) {
        LogErr() << "Ignoring wrong count";
        return;
    }

    if (log_data.ofs < _data.part_start ||
        log_data.ofs + log_data.count > _data.part_start + _data.bytes.size()) {
        LogErr() << "Ignoring wrong offset";
        return;
    }

    std::memcpy(&_data.bytes[log_data.ofs - _data.part_start], log_data.data, log_data.count);
    _data.chunks_received[(log_data.ofs - _data.part_start) / MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN] =
        true;

    if (log_data.ofs + log_data.count - _data.part_start == _data.bytes.size() ||
        _data.rerequesting) {
        // We received last message of this part.
        _data.rerequesting = true;
        check_part();
    }
}

void LogFilesImpl::report_progress(unsigned transferred, unsigned total)
{
    float progress = float(transferred) / float(total);

    if (_data.callback) {
        const auto tmp_callback = _data.callback;
        _system_impl->call_user_callback([tmp_callback, progress]() {
            LogFiles::ProgressData progress_data;
            progress_data.progress = progress;

            tmp_callback(LogFiles::Result::Next, progress_data);
        });
    }
}

void LogFilesImpl::check_part()
{
    // Assumes to have the lock for _data.mutex.

    auto first_missing =
        std::find(_data.chunks_received.begin(), _data.chunks_received.end(), false);
    if (first_missing != _data.chunks_received.end()) {
        auto first_missing_index = std::distance(_data.chunks_received.begin(), first_missing);

        auto first_not_missing = std::find(first_missing, _data.chunks_received.end(), true);

        auto first_not_missing_index =
            std::distance(_data.chunks_received.begin(), first_not_missing);

        request_log_data(
            _data.id,
            _data.part_start + first_missing_index * MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN,
            (first_not_missing_index - first_missing_index) * MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN);
    } else {
        _data.rerequesting = false;

        write_part_to_disk();

        report_progress(_data.part_start + _data.bytes.size(), _data.bytes_to_get);

        const float kib_s = float(_data.part_start + _data.bytes.size()) /
                            float(_time.elapsed_since_s(_data.time_started)) / 1024.0f;

        LogDebug() << _data.part_start + _data.bytes.size() << " B of " << _data.bytes_to_get
                   << " B (" << kib_s << " kiB/s)";

        if (_data.part_start + _data.bytes.size() == _data.bytes_to_get) {
            _system_impl->unregister_timeout_handler(_data.cookie);

            finish_logfile();

            if (_data.callback) {
                const auto tmp_callback = _data.callback;
                _system_impl->call_user_callback([tmp_callback]() {
                    LogFiles::ProgressData progress_data;
                    progress_data.progress = 1.0f;
                    tmp_callback(LogFiles::Result::Success, progress_data);
                });
            }

            reset_data();
        } else {
            _data.part_start = _data.part_start + _data.bytes.size();

            const auto part_size = determine_part_end() - _data.part_start;
            _data.bytes.resize(part_size);
            _data.chunks_received.resize(
                part_size / MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN +
                ((part_size % MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) != 0));
            std::fill(_data.chunks_received.begin(), _data.chunks_received.end(), false);

            request_log_data(_data.id, _data.part_start, _data.bytes.size());
        }
    }
}

void LogFilesImpl::request_log_data(unsigned id, unsigned start, unsigned count)
{
    // LogDebug() << "requesting: " << start << ".." << start+count << " (" << id << ")";
    _system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
        mavlink_message_t message;
        mavlink_msg_log_request_data_pack_chan(
            mavlink_address.system_id,
            mavlink_address.component_id,
            channel,
            &message,
            _system_impl->get_system_id(),
            MAV_COMP_ID_AUTOPILOT1,
            id,
            start,
            count);
        return message;
    });
}

void LogFilesImpl::data_timeout()
{
    {
        std::lock_guard<std::mutex> lock(_data.mutex);
        _system_impl->register_timeout_handler(
            [this]() { LogFilesImpl::data_timeout(); }, _system_impl->timeout_s(), &_data.cookie);
        _data.rerequesting = true;
        check_part();
    }
}

bool LogFilesImpl::is_directory(const std::string& path) const
{
    fs::path file_path(path);
    std::error_code ignored;
    return fs::is_directory(file_path, ignored);
}

bool LogFilesImpl::file_exists(const std::string& path) const
{
    fs::path file_path(path);
    std::error_code ignored;
    return fs::exists(file_path, ignored);
}

bool LogFilesImpl::start_logfile(const std::string& path)
{
    // Assumes to have the lock for _data.mutex.
    // Assumes that the path is valid and points to a file (not a directory)

    _data.file.open(path, std::ios::out | std::ios::binary);

    return ((_data.file.rdstate() & std::ofstream::failbit) == 0);
}

void LogFilesImpl::write_part_to_disk()
{
    // Assumes to have the lock for _data.mutex.

    _data.file.write(reinterpret_cast<char*>(_data.bytes.data()), _data.bytes.size());
}

void LogFilesImpl::finish_logfile()
{
    // Assumes to have the lock for _data.mutex.

    _data.file.close();
}

void LogFilesImpl::reset_data()
{
    // Assumes to have the lock for _data.mutex.
    _data.id = 0;
    _data.bytes_to_get = 0;
    _data.bytes.clear();
    _data.chunks_received.clear();
    _data.part_start = 0;
    _data.retries = 0;
    _data.rerequesting = false;
    _data.last_ofs_rerequested = -1;
    _data.callback = nullptr;
}

} // namespace mavsdk

1	#include "log_files_impl.h"
2	#include "mavlink_address.h"
3	#include "mavsdk_impl.h"
4	#include "filesystem_include.h"
5	#include "unused.h"
6
7	#include <algorithm>
8	#include <cmath>
9	#include <ctime>
10	#include <cstring>
11
12	namespace mavsdk {
13
14	LogFilesImpl::LogFilesImpl(System& system) : PluginImplBase(system)	×
15	{
16	_system_impl->register_plugin(this);	×
17	}	×
18
19	LogFilesImpl::LogFilesImpl(std::shared_ptr<System> system) : PluginImplBase(std::move(system))	×
20	{
21	_system_impl->register_plugin(this);	×
22	}	×
23
24	LogFilesImpl::~LogFilesImpl()	×
25	{
26	_system_impl->unregister_plugin(this);	×
27	}	×
28
29	void LogFilesImpl::init()	×
30	{
31	_system_impl->register_mavlink_message_handler(	×
32	MAVLINK_MSG_ID_LOG_ENTRY,
33	[this](const mavlink_message_t& message) { process_log_entry(message); },	×
34	this);
35
36	_system_impl->register_mavlink_message_handler(	×
37	MAVLINK_MSG_ID_LOG_DATA,
38	[this](const mavlink_message_t& message) { process_log_data(message); },	×
39	this);
40
41	// Cancel any log file downloads that might still be going on.
42	request_end();	×
43	}	×
44
45	void LogFilesImpl::deinit()	×
46	{
47	{
48	std::lock_guard<std::mutex> lock(_entries.mutex);	×
49	_system_impl->unregister_timeout_handler(_entries.cookie);	×
50	}
51
52	{
53	std::lock_guard<std::mutex> lock(_data.mutex);	×
54	_system_impl->unregister_timeout_handler(_data.cookie);	×
55	}
56	_system_impl->unregister_all_mavlink_message_handlers(this);	×
57	}	×
58
59	void LogFilesImpl::enable() {}	×
60
61	void LogFilesImpl::disable() {}	×
62
63	void LogFilesImpl::request_end()	×
64	{
65	_system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
66	mavlink_message_t message;
67	mavlink_msg_log_request_end_pack_chan(	×
68	mavlink_address.system_id,	×
69	mavlink_address.component_id,	×
70	channel,
71	&message,
72	_system_impl->get_system_id(),	×
73	MAV_COMP_ID_AUTOPILOT1);
74	return message;	×
75	});
76	}	×
77
78	std::pair<LogFiles::Result, std::vector<LogFiles::Entry>> LogFilesImpl::get_entries()	×
79	{
80	auto prom =	×
81	std::make_shared<std::promise<std::pair<LogFiles::Result, std::vector<LogFiles::Entry>>>>();	×
82	auto future_result = prom->get_future();	×
83
84	get_entries_async([prom](LogFiles::Result result, std::vector<LogFiles::Entry> entries) {	×
85	prom->set_value(std::make_pair<>(result, entries));	×
86	});	×
87	return future_result.get();	×
88	}
89
90	void LogFilesImpl::get_entries_async(LogFiles::GetEntriesCallback callback)	×
91	{
92	{
93	std::lock_guard<std::mutex> lock(_entries.mutex);	×
94	_entries.entry_map.clear();	×
95	_entries.callback = callback;	×
96	_entries.max_list_id = 0;	×
97	_entries.retries = 0;	×
98	}
99
100	// This first step can take a moment, on PX4 with 100+ logs I see about 2-3s.
101	_system_impl->register_timeout_handler(	×
102	[this]() { list_timeout(); }, _system_impl->timeout_s() * 10.0, &_entries.cookie);	×
103
104	request_list_entry(-1);	×
105	}	×
106
107	void LogFilesImpl::request_list_entry(int entry_id)	×
108	{
109	// all
110	uint16_t index_min = 0;	×
111	uint16_t index_max = 0xFFFF;	×
112
113	if (entry_id >= 0) {	×
114	index_min = entry_id;	×
115	index_max = entry_id;	×
116	}
117
118	_system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
119	mavlink_message_t message;
120	mavlink_msg_log_request_list_pack_chan(	×
121	mavlink_address.system_id,	×
122	mavlink_address.component_id,	×
123	channel,
124	&message,
125	_system_impl->get_system_id(),	×
126	MAV_COMP_ID_AUTOPILOT1,
127	index_min,	×
128	index_max);	×
129	return message;	×
130	});
131	}	×
132
133	void LogFilesImpl::process_log_entry(const mavlink_message_t& message)	×
134	{
135	mavlink_log_entry_t log_entry;	×
136	mavlink_msg_log_entry_decode(&message, &log_entry);	×
137
138	// Catch case where there are no log files to be found.
139	if (log_entry.num_logs == 0 && log_entry.id == 0) {	×
140	std::lock_guard<std::mutex> lock(_entries.mutex);	×
141	_system_impl->unregister_timeout_handler(_entries.cookie);	×
142	if (_entries.callback) {	×
143	const auto tmp_callback = _entries.callback;	×
144	std::vector<LogFiles::Entry> empty_list{};	×
145	_system_impl->call_user_callback([tmp_callback, empty_list]() {	×
146	tmp_callback(LogFiles::Result::NoLogfiles, empty_list);
147	});
148	}
149	return;	×
150	}
151
152	LogFiles::Entry new_entry;	×
153	new_entry.id = log_entry.id;	×
154
155	// Convert milliseconds to ISO 8601 date string in UTC.
156	char buf[sizeof "2018-08-31T20:50:42Z"];	×
157	const time_t time_utc = log_entry.time_utc;	×
158	strftime(buf, sizeof buf, "%FT%TZ", gmtime(&time_utc));	×
159
160	new_entry.date = buf;	×
161	new_entry.size_bytes = log_entry.size;	×
162	{
163	std::lock_guard<std::mutex> lock(_entries.mutex);	×
164	_entries.entry_map[new_entry.id] = new_entry;	×
165	_entries.max_list_id = log_entry.num_logs;	×
166	_system_impl->refresh_timeout_handler(_entries.cookie);	×
167	}
168	}
169
170	void LogFilesImpl::list_timeout()	×
171	{
172	std::lock_guard<std::mutex> lock(_entries.mutex);	×
173	if (_entries.entry_map.size() == 0) {	×
174	LogWarn() << "No entries received";	×
175	} else if (_entries.entry_map.size() == _entries.max_list_id) {	×
176	LogDebug() << "Received all entries";	×
177	// Copy map entries into list to return;
178	std::vector<LogFiles::Entry> entry_list{};	×
179	for (unsigned i = 0; i < _entries.max_list_id; ++i) {	×
180	entry_list.push_back(_entries.entry_map[i]);	×
181	}
182	if (_entries.callback) {	×
183	const auto tmp_callback = _entries.callback;	×
184	_system_impl->call_user_callback([tmp_callback, entry_list]() {	×
185	tmp_callback(LogFiles::Result::Success, entry_list);
186	});
187	}
188	} else {
189	if (_entries.retries > 3) {	×
190	LogWarn() << "Too many log entry retries, giving up.";	×
191	if (_entries.callback) {	×
192	const auto tmp_callback = _entries.callback;	×
193	_system_impl->call_user_callback([tmp_callback]() {	×
194	std::vector<LogFiles::Entry> empty_vector{};
195	tmp_callback(LogFiles::Result::Timeout, empty_vector);
196	});
197	}
198	} else {
199	for (unsigned i = 0; i < _entries.max_list_id; ++i) {	×
200	auto it = _entries.entry_map.find(i);	×
201	if (it == _entries.entry_map.end()) {	×
202	LogDebug() << "Requesting log entry " << i << " again";	×
203	request_list_entry(int(i));	×
204	}
205	}
206	_system_impl->register_timeout_handler(	×
207	[this]() { list_timeout(); }, _system_impl->timeout_s() * 10.0, &_entries.cookie);	×
208	_entries.retries++;	×
209	}
210	}
211	}	×
212
213	std::pair<LogFiles::Result, LogFiles::ProgressData>
214	LogFilesImpl::download_log_file(LogFiles::Entry entry, const std::string& file_path)	×
215	{
216	auto prom =	×
217	std::make_shared<std::promise<std::pair<LogFiles::Result, LogFiles::ProgressData>>>();	×
218	auto future_result = prom->get_future();	×
219
220	download_log_file_async(	×
221	entry, file_path, [prom](LogFiles::Result result, LogFiles::ProgressData progress) {	×
222	UNUSED(progress);	×
223	if (result != LogFiles::Result::Next) {	×
224	prom->set_value(std::make_pair(result, progress));	×
225	}
226	});	×
227	return future_result.get();	×
228	}
229
230	void LogFilesImpl::download_log_file_async(	×
231	LogFiles::Entry entry, const std::string& file_path, LogFiles::DownloadLogFileCallback callback)
232	{
233	unsigned bytes_to_get;
234	{
235	std::lock_guard<std::mutex> lock(_entries.mutex);	×
236
237	auto it = _entries.entry_map.find(entry.id);	×
238	if (it == _entries.entry_map.end()) {	×
239	LogErr() << "Log entry id " << entry.id << " not found";	×
240	if (callback) {	×
241	const auto tmp_callback = callback;	×
242	_system_impl->call_user_callback([tmp_callback]() {	×
243	LogFiles::ProgressData progress;
244	progress.progress = 0.0f;
245	tmp_callback(LogFiles::Result::InvalidArgument, progress);
246	});
247	}
248	return;	×
249	}
250
251	bytes_to_get = _entries.entry_map[entry.id].size_bytes;	×
252	}
253
254	{
255	std::lock_guard<std::mutex> lock(_data.mutex);	×
256
257	if (is_directory(file_path)) {	×
258	if (callback) {	×
259	const auto tmp_callback = callback;	×
260	_system_impl->call_user_callback([tmp_callback]() {	×
261	LogFiles::ProgressData progress;
262	progress.progress = NAN;
263	LogErr()
264	<< "Invalid path! The path must point to an unexisting file, and it points to a directory!";
265	tmp_callback(LogFiles::Result::InvalidArgument, progress);
266	});
267	}
268	return;	×
269	}
270
271	if (file_exists(file_path)) {	×
272	if (callback) {	×
273	const auto tmp_callback = callback;	×
274	_system_impl->call_user_callback([tmp_callback]() {	×
275	LogFiles::ProgressData progress;
276	progress.progress = NAN;
277	LogErr() << "Target log file already exists!";
278	tmp_callback(LogFiles::Result::InvalidArgument, progress);
279	});
280	}
281	return;	×
282	}
283
284	if (!start_logfile(file_path)) {	×
285	if (callback) {	×
286	const auto tmp_callback = callback;	×
287	_system_impl->call_user_callback([tmp_callback]() {	×
288	LogFiles::ProgressData progress;
289	progress.progress = NAN;
290	tmp_callback(LogFiles::Result::FileOpenFailed, progress);
291	});
292	}
293	return;	×
294	}
295
296	_data.id = entry.id;	×
297	_data.callback = callback;	×
298	_data.time_started = _time.steady_time();	×
299	_data.bytes_to_get = bytes_to_get;	×
300	_data.part_start = 0;	×
301	const auto part_size = determine_part_end() - _data.part_start;	×
302	_data.bytes.resize(part_size);	×
303	_data.chunks_received.resize(	×
304	part_size / MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN +	×
305	((part_size % MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) != 0));	×
306
307	_system_impl->register_timeout_handler(	×
308	[this]() { LogFilesImpl::data_timeout(); }, _system_impl->timeout_s(), &_data.cookie);	×
309
310	request_log_data(_data.id, _data.part_start, _data.bytes.size());	×
311
312	if (_data.callback) {	×
313	const auto tmp_callback = _data.callback;	×
314	_system_impl->call_user_callback([tmp_callback]() {	×
315	LogFiles::ProgressData progress;
316	progress.progress = 0.0f;
317	tmp_callback(LogFiles::Result::Next, progress);
318	});
319	}
320	}
321	}
322
323	LogFiles::Result LogFilesImpl::erase_all_log_files()	×
324	{
325	_system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
326	mavlink_message_t message;
327	mavlink_msg_log_erase_pack_chan(	×
328	mavlink_address.system_id,	×
329	mavlink_address.component_id,	×
330	channel,
331	&message,
332	_system_impl->get_system_id(),	×
333	MAV_COMP_ID_AUTOPILOT1);
334	return message;	×
335	});
336
337	// TODO: find a good way to know about the success or failure of the operation
338	return LogFiles::Result::Success;	×
339	}
340
341	std::size_t LogFilesImpl::determine_part_end()	×
342	{
343	// Assumes to have the lock for _data.mutex.
344
345	return std::min(
346	_data.part_start + PART_SIZE * MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN,	×
347	std::size_t(_data.bytes_to_get));	×
348	}
349
350	void LogFilesImpl::process_log_data(const mavlink_message_t& message)	×
351	{
352	mavlink_log_data_t log_data;	×
353	mavlink_msg_log_data_decode(&message, &log_data);	×
354
355	#if 0
356	// To test retransmission
357	static unsigned counter = 0;
358	if (counter < 10 && (log_data.ofs == 1800 \|\| log_data.ofs == 3600)) {
359	++counter;
360	return;
361	}
362	#endif
363
364	std::lock_guard<std::mutex> lock(_data.mutex);	×
365
366	_system_impl->refresh_timeout_handler(_data.cookie);	×
367
368	if (log_data.count > MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) {	×
369	LogErr() << "Ignoring wrong count";	×
370	return;	×
371	}
372
373	if (log_data.ofs < _data.part_start \|\|	×
374	log_data.ofs + log_data.count > _data.part_start + _data.bytes.size()) {	×
375	LogErr() << "Ignoring wrong offset";	×
376	return;	×
377	}
378
379	std::memcpy(&_data.bytes[log_data.ofs - _data.part_start], log_data.data, log_data.count);	×
380	_data.chunks_received[(log_data.ofs - _data.part_start) / MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN] =	×
381	true;	×
382
383	if (log_data.ofs + log_data.count - _data.part_start == _data.bytes.size() \|\|	×
384	_data.rerequesting) {	×
385	// We received last message of this part.
386	_data.rerequesting = true;	×
387	check_part();	×
388	}
389	}
390
391	void LogFilesImpl::report_progress(unsigned transferred, unsigned total)	×
392	{
393	float progress = float(transferred) / float(total);	×
394
395	if (_data.callback) {	×
396	const auto tmp_callback = _data.callback;	×
397	_system_impl->call_user_callback([tmp_callback, progress]() {	×
398	LogFiles::ProgressData progress_data;
399	progress_data.progress = progress;
400
401	tmp_callback(LogFiles::Result::Next, progress_data);
402	});
403	}
404	}	×
405
406	void LogFilesImpl::check_part()	×
407	{
408	// Assumes to have the lock for _data.mutex.
409
410	auto first_missing =	×
411	std::find(_data.chunks_received.begin(), _data.chunks_received.end(), false);	×
412	if (first_missing != _data.chunks_received.end()) {	×
413	auto first_missing_index = std::distance(_data.chunks_received.begin(), first_missing);	×
414
415	auto first_not_missing = std::find(first_missing, _data.chunks_received.end(), true);	×
416
417	auto first_not_missing_index =
418	std::distance(_data.chunks_received.begin(), first_not_missing);	×
419
420	request_log_data(	×
421	_data.id,
422	_data.part_start + first_missing_index * MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN,	×
423	(first_not_missing_index - first_missing_index) * MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN);	×
424	} else {
425	_data.rerequesting = false;	×
426
427	write_part_to_disk();	×
428
429	report_progress(_data.part_start + _data.bytes.size(), _data.bytes_to_get);	×
430
431	const float kib_s = float(_data.part_start + _data.bytes.size()) /	×
432	float(_time.elapsed_since_s(_data.time_started)) / 1024.0f;	×
433
434	LogDebug() << _data.part_start + _data.bytes.size() << " B of " << _data.bytes_to_get	×
435	<< " B (" << kib_s << " kiB/s)";	×
436
437	if (_data.part_start + _data.bytes.size() == _data.bytes_to_get) {	×
438	_system_impl->unregister_timeout_handler(_data.cookie);	×
439
440	finish_logfile();	×
441
442	if (_data.callback) {	×
443	const auto tmp_callback = _data.callback;	×
444	_system_impl->call_user_callback([tmp_callback]() {	×
445	LogFiles::ProgressData progress_data;
446	progress_data.progress = 1.0f;
447	tmp_callback(LogFiles::Result::Success, progress_data);
448	});
449	}
450
451	reset_data();	×
452	} else {
453	_data.part_start = _data.part_start + _data.bytes.size();	×
454
455	const auto part_size = determine_part_end() - _data.part_start;	×
456	_data.bytes.resize(part_size);	×
457	_data.chunks_received.resize(	×
458	part_size / MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN +	×
459	((part_size % MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) != 0));	×
460	std::fill(_data.chunks_received.begin(), _data.chunks_received.end(), false);	×
461
462	request_log_data(_data.id, _data.part_start, _data.bytes.size());	×
463	}
464	}
465	}	×
466
467	void LogFilesImpl::request_log_data(unsigned id, unsigned start, unsigned count)	×
468	{
469	// LogDebug() << "requesting: " << start << ".." << start+count << " (" << id << ")";
470	_system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
471	mavlink_message_t message;
472	mavlink_msg_log_request_data_pack_chan(	×
473	mavlink_address.system_id,	×
474	mavlink_address.component_id,	×
475	channel,
476	&message,
477	_system_impl->get_system_id(),	×
478	MAV_COMP_ID_AUTOPILOT1,
479	id,	×
480	start,	×
481	count);	×
482	return message;	×
483	});
484	}	×
485
486	void LogFilesImpl::data_timeout()	×
487	{
488	{
489	std::lock_guard<std::mutex> lock(_data.mutex);	×
490	_system_impl->register_timeout_handler(	×
491	[this]() { LogFilesImpl::data_timeout(); }, _system_impl->timeout_s(), &_data.cookie);	×
492	_data.rerequesting = true;	×
493	check_part();	×
494	}
495	}	×
496
497	bool LogFilesImpl::is_directory(const std::string& path) const	×
498	{
499	fs::path file_path(path);	×
500	std::error_code ignored;	×
501	return fs::is_directory(file_path, ignored);	×
502	}
503
504	bool LogFilesImpl::file_exists(const std::string& path) const	×
505	{
506	fs::path file_path(path);	×
507	std::error_code ignored;	×
508	return fs::exists(file_path, ignored);	×
509	}
510
511	bool LogFilesImpl::start_logfile(const std::string& path)	×
512	{
513	// Assumes to have the lock for _data.mutex.
514	// Assumes that the path is valid and points to a file (not a directory)
515
516	_data.file.open(path, std::ios::out \| std::ios::binary);	×
517
518	return ((_data.file.rdstate() & std::ofstream::failbit) == 0);	×
519	}
520
521	void LogFilesImpl::write_part_to_disk()	×
522	{
523	// Assumes to have the lock for _data.mutex.
524
525	_data.file.write(reinterpret_cast<char*>(_data.bytes.data()), _data.bytes.size());	×
526	}	×
527
528	void LogFilesImpl::finish_logfile()	×
529	{
530	// Assumes to have the lock for _data.mutex.
531
532	_data.file.close();	×
533	}	×
534
535	void LogFilesImpl::reset_data()	×
536	{
537	// Assumes to have the lock for _data.mutex.
538	_data.id = 0;	×
539	_data.bytes_to_get = 0;	×
540	_data.bytes.clear();	×
541	_data.chunks_received.clear();	×
542	_data.part_start = 0;	×
543	_data.retries = 0;	×
544	_data.rerequesting = false;	×
545	_data.last_ofs_rerequested = -1;	×
546	_data.callback = nullptr;	×
547	}	×
548
549	} // namespace mavsdk

mavlink / MAVSDK / 6568658631

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous