4578731844

Build Type

push

github

Committed by GitHub

Commit Message

Merge pull request #2012 from mavlink/rename-parent

Run Details

885 of 885 new or added lines in 31 files covered. (100.0%)

7416 of 24250 relevant lines covered (30.58%)

21.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 "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()
{
    mavlink_message_t msg;
    mavlink_msg_log_request_end_pack(
        _system_impl->get_own_system_id(),
        _system_impl->get_own_component_id(),
        &msg,
        _system_impl->get_system_id(),
        MAV_COMP_ID_AUTOPILOT1);
    _system_impl->send_message(msg);
}

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;
    }

    _system_impl->register_timeout_handler(
        [this]() { list_timeout(); }, LIST_TIMEOUT_S, &_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;
    }

    mavlink_message_t msg;
    mavlink_msg_log_request_list_pack(
        _system_impl->get_own_system_id(),
        _system_impl->get_own_component_id(),
        &msg,
        _system_impl->get_system_id(),
        MAV_COMP_ID_AUTOPILOT1,
        index_min,
        index_max);

    _system_impl->send_message(msg);
}

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(); }, LIST_TIMEOUT_S, &_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(); }, DATA_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()
{
    mavlink_message_t msg;
    mavlink_msg_log_erase_pack(
        _system_impl->get_own_system_id(),
        _system_impl->get_own_component_id(),
        &msg,
        _system_impl->get_system_id(),
        MAV_COMP_ID_AUTOPILOT1);
    _system_impl->send_message(msg);

    // 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 << ")";
    mavlink_message_t msg;
    mavlink_msg_log_request_data_pack(
        _system_impl->get_own_system_id(),
        _system_impl->get_own_component_id(),
        &msg,
        _system_impl->get_system_id(),
        MAV_COMP_ID_AUTOPILOT1,
        id,
        start,
        count);
    _system_impl->send_message(msg);
}

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

mavlink / MAVSDK / 4578731844

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