11767930807

Committed 10 Nov 2024 07:33PM UTC coverage: 38.608% (+0.7%) from 37.921%

Build # 11767930807

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2394 from mavlink/pr-consolidate-ci

Consolidate CI

Run Details

12030 of 31159 relevant lines covered (38.61%)

243.33 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 <algorithm>
#include <cmath>
#include <ctime>
#include <filesystem>

namespace mavsdk {

namespace fs = std::filesystem;

LogData::LogData(
    const LogFiles::Entry& e, const std::string& filepath, LogFiles::DownloadLogFileCallback cb) :
    entry(e),
    file_path(filepath),
    user_callback(cb)
{
    // Reset the table
    chunk_bin_table = std::vector<bool>(bins_in_chunk(), false);
    file.open(file_path, std::ios::out | std::ios::binary);
}

bool LogData::file_is_open()
{
    // TODO: any other checks?
    return !(file.rdstate() & std::ofstream::failbit); // Ensure file is writable
}

uint32_t LogData::total_chunks() const
{
    uint32_t partial_chunk = entry.size_bytes % CHUNK_SIZE;
    uint32_t add_one_chunk = partial_chunk ? 1 : 0;
    return entry.size_bytes / CHUNK_SIZE + add_one_chunk;
}

uint32_t LogData::bins_in_chunk() const
{
    uint32_t size = current_chunk_size();
    uint32_t add_one_bin = size % MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN ? 1 : 0;
    return size / MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN + add_one_bin;
}

uint32_t LogData::current_chunk_size() const
{
    uint32_t chunk_size = CHUNK_SIZE;
    uint32_t chunk_remainder = entry.size_bytes % CHUNK_SIZE;
    bool is_last_chunk = (current_chunk == total_chunks() - 1);
    // Last chunk might be less than CHUNK_SIZE
    if (is_last_chunk && chunk_remainder) {
        chunk_size = entry.size_bytes % CHUNK_SIZE;
    }

    return chunk_size;
}

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

    {
        std::lock_guard<std::mutex> lock(_download_data_mutex);
        _system_impl->unregister_timeout_handler(_download_data.timeout_cookie);
    }
    _system_impl->unregister_all_mavlink_message_handlers(this);
}

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

    auto result = future_result.get();

    _entries_user_callback = nullptr;

    return result;
}

void LogFilesImpl::get_entries_async(LogFiles::GetEntriesCallback callback)
{
    std::lock_guard<std::mutex> lock(_entries_mutex);
    _log_entries.clear();
    _entries_user_callback = callback;
    _total_entries = 0;

    _entries_timeout_cookie = _system_impl->register_timeout_handler(
        [this]() { entries_timeout(); }, _system_impl->timeout_s() * 10.0);

    request_log_list(0, 0xFFFF);
}

void LogFilesImpl::request_log_list(uint16_t index_min, uint16_t index_max)
{
    _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 msg;
    mavlink_msg_log_entry_decode(&message, &msg);

    std::lock_guard<std::mutex> lock(_entries_mutex);

    _system_impl->refresh_timeout_handler(_entries_timeout_cookie);

    // Bad data handling
    if (msg.num_logs == 0 || msg.id >= msg.num_logs) {
        LogWarn() << "No logs available";

        _system_impl->unregister_timeout_handler(_entries_timeout_cookie);

        const auto cb = _entries_user_callback;
        if (cb) {
            _system_impl->call_user_callback(
                [cb]() { cb(LogFiles::Result::NoLogfiles, std::vector<LogFiles::Entry>()); });
        }
        return;
    }

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

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

    new_entry.date = buf;
    new_entry.size_bytes = msg.size;

    _log_entries[new_entry.id] = new_entry;
    _total_entries = msg.num_logs;

    // Check if all entries are received
    if (_log_entries.size() == _total_entries) {
        _system_impl->unregister_timeout_handler(_entries_timeout_cookie);

        // Copy map entries into list to return
        std::vector<LogFiles::Entry> entry_list{};
        for (unsigned i = 0; i < _log_entries.size(); i++) {
            entry_list.push_back(_log_entries[i]);
        }

        const auto cb = _entries_user_callback;
        if (cb) {
            _system_impl->call_user_callback(
                [cb, entry_list]() { cb(LogFiles::Result::Success, entry_list); });
        }
    }
}

void LogFilesImpl::entries_timeout()
{
    std::lock_guard<std::mutex> lock(_entries_mutex);

    const auto cb = _entries_user_callback;
    if (cb) {
        _system_impl->call_user_callback([cb]() {
            LogDebug() << "Request entries timeout!";
            cb(LogFiles::Result::Timeout, std::vector<LogFiles::Entry>());
        });
    }
}

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) {
            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)
{
    std::lock_guard<std::mutex> lock(_entries_mutex);

    auto entry_it = _log_entries.find(entry.id);
    bool error = entry_it == _log_entries.end() || fs::is_directory(fs::path(file_path)) ||
                 fs::exists(file_path);

    if (error) {
        LogErr() << "error: download_log_file_async failed";

        if (callback) {
            _system_impl->call_user_callback([callback]() {
                callback(LogFiles::Result::InvalidArgument, LogFiles::ProgressData());
            });
        }
        return;
    }

    _download_data = LogData(entry_it->second, file_path, callback);

    if (!_download_data.file_is_open()) {
        if (callback) {
            _system_impl->call_user_callback([callback]() {
                callback(LogFiles::Result::FileOpenFailed, LogFiles::ProgressData());
            });
        }
        return;
    }

    _download_data.timeout_cookie = _system_impl->register_timeout_handler(
        [this]() { LogFilesImpl::data_timeout(); }, _system_impl->timeout_s() * 1.0);

    // Request the first chunk
    request_log_data(_download_data.entry.id, 0, _download_data.current_chunk_size());
}

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

    std::lock_guard<std::mutex> lock(_download_data_mutex);

    _system_impl->refresh_timeout_handler(_download_data.timeout_cookie);

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

    if (msg.id != _download_data.entry.id) {
        LogErr() << "Ignoring wrong ID: actual/expected: " << msg.id << "/"
                 << _download_data.entry.id;
        return;
    }

    if (msg.ofs > _download_data.entry.size_bytes) {
        LogErr() << "Offset greater than file size: offset/size: " << msg.ofs << "/"
                 << _download_data.entry.size_bytes;
        return;
    }

    if (msg.ofs % MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) {
        LogErr() << "Ignoring misaligned offset: " << msg.ofs;
        return;
    }

    // Calculate current chunk
    const uint32_t chunk = msg.ofs / CHUNK_SIZE;

    if (chunk != _download_data.current_chunk) {
        // Quietly ignore packets for out of order chunks
        // LogErr() << "Ignored packet for out of order chunk actual/expected: " << chunk << "/"
        //          << _download_data.current_chunk;
        return;
    }

    // Calculate current bin within the chunk
    const uint16_t bin = (msg.ofs - chunk * CHUNK_SIZE) / MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN;

    if (bin >= _download_data.chunk_bin_table.size()) {
        LogErr() << "Out of range bin received: bin/size: " << bin << "/"
                 << _download_data.chunk_bin_table.size();
        return;
    }

    if (_download_data.file.tellp() != msg.ofs) {
        if (!_download_data.file.seekp(msg.ofs)) {
            LogErr() << "Error while seeking to log file offset";
            return;
        }
    }

    _download_data.file.write((char*)msg.data, msg.count);

    // Quietly ignore duplicate packets -- we don't want to record the bytes_written twice
    if (!_download_data.chunk_bin_table[bin]) {
        _download_data.chunk_bytes_written += msg.count;
        _download_data.chunk_bin_table[bin] = true;
    }

    bool chunk_complete = _download_data.chunk_bytes_written == _download_data.current_chunk_size();

    if (chunk_complete) {
        auto result = LogFiles::Result::Next;

        _download_data.current_chunk++;
        _download_data.total_bytes_written += _download_data.chunk_bytes_written;
        _download_data.chunk_bytes_written = 0;
        _download_data.chunk_bin_table = std::vector<bool>(_download_data.bins_in_chunk(), false);

        bool log_complete = _download_data.total_bytes_written == _download_data.entry.size_bytes;

        if (log_complete) {
            result = LogFiles::Result::Success;
            _download_data.file.close();
            _system_impl->unregister_timeout_handler(_download_data.timeout_cookie);

        } else {
            // Request the next chunk
            request_log_data(
                _download_data.entry.id,
                _download_data.current_chunk * CHUNK_SIZE,
                _download_data.current_chunk_size());
        }

        LogFiles::ProgressData progress_data;
        progress_data.progress =
            (float)_download_data.total_bytes_written / (float)_download_data.entry.size_bytes;

        // Update progress
        const auto cb = _download_data.user_callback;
        if (cb) {
            _system_impl->call_user_callback(
                [cb, progress_data, result]() { cb(result, progress_data); });
        }
    }
}

void LogFilesImpl::data_timeout()
{
    std::lock_guard<std::mutex> lock(_download_data_mutex);

    LogErr() << "Timeout!";
    LogErr() << "Requesting missing chunk:\t" << _download_data.current_chunk << "/"
             << _download_data.total_chunks();

    // Reset chunk data
    _download_data.chunk_bytes_written = 0;
    _download_data.chunk_bin_table = std::vector<bool>(_download_data.bins_in_chunk(), false);

    request_log_data(
        _download_data.entry.id,
        _download_data.current_chunk * CHUNK_SIZE,
        _download_data.current_chunk_size());

    _download_data.timeout_cookie = _system_impl->register_timeout_handler(
        [this]() { LogFilesImpl::data_timeout(); }, _system_impl->timeout_s() * 1.0);
}

void LogFilesImpl::request_log_data(unsigned id, unsigned start, unsigned count)
{
    _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;
    });
}

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

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

} // namespace mavsdk

1	#include "log_files_impl.h"
2	#include "mavlink_address.h"
3	#include "mavsdk_impl.h"
4
5	#include <algorithm>
6	#include <cmath>
7	#include <ctime>
8	#include <filesystem>
9
10	namespace mavsdk {
11
12	namespace fs = std::filesystem;
13
14	LogData::LogData(	×
15	const LogFiles::Entry& e, const std::string& filepath, LogFiles::DownloadLogFileCallback cb) :	×
16	entry(e),	×
17	file_path(filepath),	×
18	user_callback(cb)	×
19	{
20	// Reset the table
21	chunk_bin_table = std::vector<bool>(bins_in_chunk(), false);	×
22	file.open(file_path, std::ios::out \| std::ios::binary);	×
23	}	×
24
25	bool LogData::file_is_open()	×
26	{
27	// TODO: any other checks?
28	return !(file.rdstate() & std::ofstream::failbit); // Ensure file is writable	×
29	}
30
31	uint32_t LogData::total_chunks() const	×
32	{
33	uint32_t partial_chunk = entry.size_bytes % CHUNK_SIZE;	×
34	uint32_t add_one_chunk = partial_chunk ? 1 : 0;	×
35	return entry.size_bytes / CHUNK_SIZE + add_one_chunk;	×
36	}
37
38	uint32_t LogData::bins_in_chunk() const	×
39	{
40	uint32_t size = current_chunk_size();	×
41	uint32_t add_one_bin = size % MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN ? 1 : 0;	×
42	return size / MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN + add_one_bin;	×
43	}
44
45	uint32_t LogData::current_chunk_size() const	×
46	{
47	uint32_t chunk_size = CHUNK_SIZE;	×
48	uint32_t chunk_remainder = entry.size_bytes % CHUNK_SIZE;	×
49	bool is_last_chunk = (current_chunk == total_chunks() - 1);	×
50	// Last chunk might be less than CHUNK_SIZE
51	if (is_last_chunk && chunk_remainder) {	×
52	chunk_size = entry.size_bytes % CHUNK_SIZE;	×
53	}
54
55	return chunk_size;	×
56	}
57
58	LogFilesImpl::LogFilesImpl(System& system) : PluginImplBase(system)	×
59	{
60	_system_impl->register_plugin(this);	×
61	}	×
62
63	LogFilesImpl::LogFilesImpl(std::shared_ptr<System> system) : PluginImplBase(std::move(system))	×
64	{
65	_system_impl->register_plugin(this);	×
66	}	×
67
68	LogFilesImpl::~LogFilesImpl()	×
69	{
70	_system_impl->unregister_plugin(this);	×
71	}	×
72
73	void LogFilesImpl::init()	×
74	{
75	_system_impl->register_mavlink_message_handler(	×
76	MAVLINK_MSG_ID_LOG_ENTRY,
77	[this](const mavlink_message_t& message) { process_log_entry(message); },	×
78	this);
79
80	_system_impl->register_mavlink_message_handler(	×
81	MAVLINK_MSG_ID_LOG_DATA,
82	[this](const mavlink_message_t& message) { process_log_data(message); },	×
83	this);
84
85	// Cancel any log file downloads that might still be going on.
86	request_end();	×
87	}	×
88
89	void LogFilesImpl::deinit()	×
90	{
91	{
92	std::lock_guard<std::mutex> lock(_entries_mutex);	×
93	_system_impl->unregister_timeout_handler(_entries_timeout_cookie);	×
94	}	×
95
96	{
97	std::lock_guard<std::mutex> lock(_download_data_mutex);	×
98	_system_impl->unregister_timeout_handler(_download_data.timeout_cookie);	×
99	}	×
100	_system_impl->unregister_all_mavlink_message_handlers(this);	×
101	}	×
102
103	std::pair<LogFiles::Result, std::vector<LogFiles::Entry>> LogFilesImpl::get_entries()	×
104	{
105	auto prom =	×
106	std::make_shared<std::promise<std::pair<LogFiles::Result, std::vector<LogFiles::Entry>>>>();	×
107	auto future_result = prom->get_future();	×
108
109	get_entries_async([prom](LogFiles::Result result, std::vector<LogFiles::Entry> entries) {	×
110	prom->set_value(std::make_pair<>(result, entries));	×
111	});	×
112
113	auto result = future_result.get();	×
114
115	_entries_user_callback = nullptr;	×
116
117	return result;	×
118	}	×
119
120	void LogFilesImpl::get_entries_async(LogFiles::GetEntriesCallback callback)	×
121	{
122	std::lock_guard<std::mutex> lock(_entries_mutex);	×
123	_log_entries.clear();	×
124	_entries_user_callback = callback;	×
125	_total_entries = 0;	×
126
127	_entries_timeout_cookie = _system_impl->register_timeout_handler(	×
128	[this]() { entries_timeout(); }, _system_impl->timeout_s() * 10.0);	×
129
130	request_log_list(0, 0xFFFF);	×
131	}	×
132
133	void LogFilesImpl::request_log_list(uint16_t index_min, uint16_t index_max)	×
134	{
135	_system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
136	mavlink_message_t message;
137	mavlink_msg_log_request_list_pack_chan(	×
138	mavlink_address.system_id,	×
139	mavlink_address.component_id,	×
140	channel,
141	&message,
142	_system_impl->get_system_id(),	×
143	MAV_COMP_ID_AUTOPILOT1,
144	index_min,	×
145	index_max);	×
146	return message;	×
147	});
148	}	×
149
150	void LogFilesImpl::process_log_entry(const mavlink_message_t& message)	×
151	{
152	mavlink_log_entry_t msg;	×
153	mavlink_msg_log_entry_decode(&message, &msg);	×
154
155	std::lock_guard<std::mutex> lock(_entries_mutex);	×
156
157	_system_impl->refresh_timeout_handler(_entries_timeout_cookie);	×
158
159	// Bad data handling
160	if (msg.num_logs == 0 \|\| msg.id >= msg.num_logs) {	×
161	LogWarn() << "No logs available";	×
162
163	_system_impl->unregister_timeout_handler(_entries_timeout_cookie);	×
164
165	const auto cb = _entries_user_callback;	×
166	if (cb) {	×
167	_system_impl->call_user_callback(	×
168	[cb]() { cb(LogFiles::Result::NoLogfiles, std::vector<LogFiles::Entry>()); });
169	}
170	return;	×
171	}	×
172
173	LogFiles::Entry new_entry;	×
174	new_entry.id = msg.id;	×
175
176	// Convert milliseconds to ISO 8601 date string in UTC.
177	char buf[sizeof "2018-08-31T20:50:42Z"];	×
178	const time_t time_utc = msg.time_utc;	×
179	strftime(buf, sizeof buf, "%FT%TZ", gmtime(&time_utc));	×
180
181	new_entry.date = buf;	×
182	new_entry.size_bytes = msg.size;	×
183
184	_log_entries[new_entry.id] = new_entry;	×
185	_total_entries = msg.num_logs;	×
186
187	// Check if all entries are received
188	if (_log_entries.size() == _total_entries) {	×
189	_system_impl->unregister_timeout_handler(_entries_timeout_cookie);	×
190
191	// Copy map entries into list to return
192	std::vector<LogFiles::Entry> entry_list{};	×
193	for (unsigned i = 0; i < _log_entries.size(); i++) {	×
194	entry_list.push_back(_log_entries[i]);	×
195	}
196
197	const auto cb = _entries_user_callback;	×
198	if (cb) {	×
199	_system_impl->call_user_callback(	×
200	[cb, entry_list]() { cb(LogFiles::Result::Success, entry_list); });
201	}
202	}	×
203	}	×
204
205	void LogFilesImpl::entries_timeout()	×
206	{
207	std::lock_guard<std::mutex> lock(_entries_mutex);	×
208
209	const auto cb = _entries_user_callback;	×
210	if (cb) {	×
211	_system_impl->call_user_callback([cb]() {	×
212	LogDebug() << "Request entries timeout!";
213	cb(LogFiles::Result::Timeout, std::vector<LogFiles::Entry>());
214	});
215	}
216	}	×
217
218	std::pair<LogFiles::Result, LogFiles::ProgressData>
219	LogFilesImpl::download_log_file(LogFiles::Entry entry, const std::string& file_path)	×
220	{
221	auto prom =	×
222	std::make_shared<std::promise<std::pair<LogFiles::Result, LogFiles::ProgressData>>>();	×
223	auto future_result = prom->get_future();	×
224
225	download_log_file_async(	×
226	entry, file_path, [prom](LogFiles::Result result, LogFiles::ProgressData progress) {	×
227	if (result != LogFiles::Result::Next) {	×
228	prom->set_value(std::make_pair(result, progress));	×
229	}
230	});	×
231
232	return future_result.get();	×
233	}	×
234
235	void LogFilesImpl::download_log_file_async(	×
236	LogFiles::Entry entry, const std::string& file_path, LogFiles::DownloadLogFileCallback callback)
237	{
238	std::lock_guard<std::mutex> lock(_entries_mutex);	×
239
240	auto entry_it = _log_entries.find(entry.id);	×
241	bool error = entry_it == _log_entries.end() \|\| fs::is_directory(fs::path(file_path)) \|\|	×
242	fs::exists(file_path);	×
243
244	if (error) {	×
245	LogErr() << "error: download_log_file_async failed";	×
246
247	if (callback) {	×
248	_system_impl->call_user_callback([callback]() {	×
249	callback(LogFiles::Result::InvalidArgument, LogFiles::ProgressData());
250	});
251	}
252	return;	×
253	}
254
255	_download_data = LogData(entry_it->second, file_path, callback);	×
256
257	if (!_download_data.file_is_open()) {	×
258	if (callback) {	×
259	_system_impl->call_user_callback([callback]() {	×
260	callback(LogFiles::Result::FileOpenFailed, LogFiles::ProgressData());
261	});
262	}
263	return;	×
264	}
265
266	_download_data.timeout_cookie = _system_impl->register_timeout_handler(	×
267	[this]() { LogFilesImpl::data_timeout(); }, _system_impl->timeout_s() * 1.0);	×
268
269	// Request the first chunk
270	request_log_data(_download_data.entry.id, 0, _download_data.current_chunk_size());	×
271	}	×
272
273	void LogFilesImpl::process_log_data(const mavlink_message_t& message)	×
274	{
275	mavlink_log_data_t msg;	×
276	mavlink_msg_log_data_decode(&message, &msg);	×
277
278	std::lock_guard<std::mutex> lock(_download_data_mutex);	×
279
280	_system_impl->refresh_timeout_handler(_download_data.timeout_cookie);	×
281
282	if (msg.count > MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) {	×
283	LogErr() << "Ignoring wrong count" << msg.count;	×
284	return;	×
285	}
286
287	if (msg.id != _download_data.entry.id) {	×
288	LogErr() << "Ignoring wrong ID: actual/expected: " << msg.id << "/"	×
289	<< _download_data.entry.id;	×
290	return;	×
291	}
292
293	if (msg.ofs > _download_data.entry.size_bytes) {	×
294	LogErr() << "Offset greater than file size: offset/size: " << msg.ofs << "/"	×
295	<< _download_data.entry.size_bytes;	×
296	return;	×
297	}
298
299	if (msg.ofs % MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) {	×
300	LogErr() << "Ignoring misaligned offset: " << msg.ofs;	×
301	return;	×
302	}
303
304	// Calculate current chunk
305	const uint32_t chunk = msg.ofs / CHUNK_SIZE;	×
306
307	if (chunk != _download_data.current_chunk) {	×
308	// Quietly ignore packets for out of order chunks
309	// LogErr() << "Ignored packet for out of order chunk actual/expected: " << chunk << "/"
310	// << _download_data.current_chunk;
311	return;	×
312	}
313
314	// Calculate current bin within the chunk
315	const uint16_t bin = (msg.ofs - chunk * CHUNK_SIZE) / MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN;	×
316
317	if (bin >= _download_data.chunk_bin_table.size()) {	×
318	LogErr() << "Out of range bin received: bin/size: " << bin << "/"	×
319	<< _download_data.chunk_bin_table.size();	×
320	return;	×
321	}
322
323	if (_download_data.file.tellp() != msg.ofs) {	×
324	if (!_download_data.file.seekp(msg.ofs)) {	×
325	LogErr() << "Error while seeking to log file offset";	×
326	return;	×
327	}
328	}
329
330	_download_data.file.write((char*)msg.data, msg.count);	×
331
332	// Quietly ignore duplicate packets -- we don't want to record the bytes_written twice
333	if (!_download_data.chunk_bin_table[bin]) {	×
334	_download_data.chunk_bytes_written += msg.count;	×
335	_download_data.chunk_bin_table[bin] = true;	×
336	}
337
338	bool chunk_complete = _download_data.chunk_bytes_written == _download_data.current_chunk_size();	×
339
340	if (chunk_complete) {	×
341	auto result = LogFiles::Result::Next;	×
342
343	_download_data.current_chunk++;	×
344	_download_data.total_bytes_written += _download_data.chunk_bytes_written;	×
345	_download_data.chunk_bytes_written = 0;	×
346	_download_data.chunk_bin_table = std::vector<bool>(_download_data.bins_in_chunk(), false);	×
347
348	bool log_complete = _download_data.total_bytes_written == _download_data.entry.size_bytes;	×
349
350	if (log_complete) {	×
351	result = LogFiles::Result::Success;	×
352	_download_data.file.close();	×
353	_system_impl->unregister_timeout_handler(_download_data.timeout_cookie);	×
354
355	} else {
356	// Request the next chunk
357	request_log_data(	×
358	_download_data.entry.id,
359	_download_data.current_chunk * CHUNK_SIZE,	×
360	_download_data.current_chunk_size());
361	}
362
363	LogFiles::ProgressData progress_data;	×
364	progress_data.progress =	×
365	(float)_download_data.total_bytes_written / (float)_download_data.entry.size_bytes;	×
366
367	// Update progress
368	const auto cb = _download_data.user_callback;	×
369	if (cb) {	×
370	_system_impl->call_user_callback(	×
371	[cb, progress_data, result]() { cb(result, progress_data); });
372	}
373	}	×
374	}	×
375
376	void LogFilesImpl::data_timeout()	×
377	{
378	std::lock_guard<std::mutex> lock(_download_data_mutex);	×
379
380	LogErr() << "Timeout!";	×
381	LogErr() << "Requesting missing chunk:\t" << _download_data.current_chunk << "/"	×
382	<< _download_data.total_chunks();	×
383
384	// Reset chunk data
385	_download_data.chunk_bytes_written = 0;	×
386	_download_data.chunk_bin_table = std::vector<bool>(_download_data.bins_in_chunk(), false);	×
387
388	request_log_data(	×
389	_download_data.entry.id,
390	_download_data.current_chunk * CHUNK_SIZE,	×
391	_download_data.current_chunk_size());
392
393	_download_data.timeout_cookie = _system_impl->register_timeout_handler(	×
394	[this]() { LogFilesImpl::data_timeout(); }, _system_impl->timeout_s() * 1.0);	×
395	}	×
396
397	void LogFilesImpl::request_log_data(unsigned id, unsigned start, unsigned count)	×
398	{
399	_system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
400	mavlink_message_t message;
401	mavlink_msg_log_request_data_pack_chan(	×
402	mavlink_address.system_id,	×
403	mavlink_address.component_id,	×
404	channel,
405	&message,
406	_system_impl->get_system_id(),	×
407	MAV_COMP_ID_AUTOPILOT1,
408	id,	×
409	start,	×
410	count);	×
411	return message;	×
412	});
413	}	×
414
415	LogFiles::Result LogFilesImpl::erase_all_log_files()	×
416	{
417	_system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
418	mavlink_message_t message;
419	mavlink_msg_log_erase_pack_chan(	×
420	mavlink_address.system_id,	×
421	mavlink_address.component_id,	×
422	channel,
423	&message,
424	_system_impl->get_system_id(),	×
425	MAV_COMP_ID_AUTOPILOT1);
426	return message;	×
427	});
428
429	// TODO: find a good way to know about the success or failure of the operation
430	return LogFiles::Result::Success;	×
431	}
432
433	void LogFilesImpl::request_end()	×
434	{
435	_system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
436	mavlink_message_t message;
437	mavlink_msg_log_request_end_pack_chan(	×
438	mavlink_address.system_id,	×
439	mavlink_address.component_id,	×
440	channel,
441	&message,
442	_system_impl->get_system_id(),	×
443	MAV_COMP_ID_AUTOPILOT1);
444	return message;	×
445	});
446	}	×
447
448	} // namespace mavsdk

mavlink / MAVSDK / 11767930807

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