17#include <QtCore/QApplicationStatic>
18#include <QtCore/QTimer>
24 , _timer(new QTimer(this))
27 qCDebug(OnboardLogControllerLog) <<
this;
30 (void) connect(_timer, &QTimer::timeout,
this, &OnboardLogController::_processDownload);
32 _timer->setSingleShot(
false);
39 qCDebug(OnboardLogControllerLog) <<
this;
45 _downloadToDirectory(dir);
48void OnboardLogController::_downloadToDirectory(
const QString &dir)
50 _receivedAllEntries();
52 _downloadData.reset();
55 if (_downloadPath.isEmpty()) {
59 if (!_downloadPath.endsWith(QDir::separator())) {
60 _downloadPath += QDir::separator();
68 _setDownloading(
true);
72void OnboardLogController::_processDownload()
74 if (_requestingLogEntries) {
75 _findMissingEntries();
76 }
else if (_downloadingLogs) {
81void OnboardLogController::_findMissingEntries()
83 const int num_logs = _logEntriesModel->
count();
86 for (
int i = 0; i < num_logs; i++) {
98 }
else if (start >= 0) {
104 _receivedAllEntries();
108 if (_retries++ > 2) {
109 for (
int i = 0; i < num_logs; i++) {
116 _receivedAllEntries();
117 qCWarning(OnboardLogControllerLog) <<
"Too many errors retreiving log list. Giving up.";
128 _requestLogList(
static_cast<uint32_t
>(start),
static_cast<uint32_t
>(end));
131void OnboardLogController::_setActiveVehicle(
Vehicle *vehicle)
133 if (vehicle == _vehicle) {
139 (void) disconnect(_vehicle, &
Vehicle::logEntry,
this, &OnboardLogController::_logEntry);
140 (void) disconnect(_vehicle, &
Vehicle::logData,
this, &OnboardLogController::_logData);
146 (void) connect(_vehicle, &
Vehicle::logEntry,
this, &OnboardLogController::_logEntry);
147 (void) connect(_vehicle, &
Vehicle::logData,
this, &OnboardLogController::_logData);
151void OnboardLogController::_logEntry(uint32_t time_utc, uint32_t size, uint16_t
id, uint16_t num_logs, uint16_t last_log_num)
153 Q_UNUSED(last_log_num);
155 if (!_requestingLogEntries) {
159 if ((_logEntriesModel->
count() == 0) && (num_logs > 0)) {
160 if (_vehicle->
firmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
165 for (
int i = 0; i < num_logs; i++) {
168 _logEntriesModel->
append(entry);
173 if ((size > 0) || (_vehicle->
firmwareType() != MAV_AUTOPILOT_ARDUPILOTMEGA)) {
175 if (id < _logEntriesModel->count()) {
178 entry->
setTime(QDateTime::fromSecsSinceEpoch(time_utc));
182 qCWarning(OnboardLogControllerLog) <<
"Received onboard log entry for out-of-bound index:" << id;
186 _receivedAllEntries();
191 if (_entriesComplete()) {
192 _receivedAllEntries();
194 _timer->start(kTimeOutMs);
198void OnboardLogController::_receivedAllEntries()
204bool OnboardLogController::_entriesComplete()
const
206 const int num_logs = _logEntriesModel->
count();
207 for (
int i = 0; i < num_logs; i++) {
221void OnboardLogController::_logData(uint32_t ofs, uint16_t
id, uint8_t count,
const uint8_t *data)
223 if (!_downloadingLogs || !_downloadData) {
228 if (_downloadData->ID !=
id) {
229 qCWarning(OnboardLogControllerLog) <<
"Received log data for wrong log";
233 if ((ofs % MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) != 0) {
234 qCWarning(OnboardLogControllerLog) <<
"Ignored misaligned incoming packet @" << ofs;
239 if (ofs <= _downloadData->entry->
size()) {
242 if (chunk != _downloadData->current_chunk) {
243 qCWarning(OnboardLogControllerLog) <<
"Ignored packet for out of order chunk actual:expected" << chunk << _downloadData->current_chunk;
248 if (bin >= _downloadData->chunk_table.size()) {
249 qCWarning(OnboardLogControllerLog) <<
"Out of range bin received";
251 _downloadData->chunk_table.setBit(bin);
254 if (_downloadData->file.pos() != ofs) {
255 if (!_downloadData->file.seek(ofs)) {
256 qCWarning(OnboardLogControllerLog) <<
"Error while seeking log file offset";
261 if (_downloadData->file.write(
reinterpret_cast<const char*
>(data), count)) {
262 _downloadData->written += count;
263 _downloadData->rate_bytes += count;
269 _timer->start(kTimeOutMs);
270 if (_logComplete()) {
271 _downloadData->entry->setStatus(tr(
"Downloaded"));
273 }
else if (_chunkComplete()) {
274 _downloadData->advanceChunk();
275 _requestLogData(_downloadData->ID,
277 _downloadData->chunk_table.size() * MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN);
278 }
else if ((bin < (_downloadData->chunk_table.size() - 1)) && _downloadData->chunk_table.at(bin + 1)) {
283 qCWarning(OnboardLogControllerLog) <<
"Error while writing log file chunk";
286 qCWarning(OnboardLogControllerLog) <<
"Received log offset greater than expected";
290 _downloadData->entry->setStatus(tr(
"Error"));
294void OnboardLogController::_findMissingData()
296 if (_logComplete()) {
301 if (_chunkComplete()) {
302 _downloadData->advanceChunk();
309 uint16_t start = 0, end = 0;
310 const int size = _downloadData->chunk_table.size();
311 for (; start < size; start++) {
312 if (!_downloadData->chunk_table.testBit(start)) {
317 for (end = start; end < size; end++) {
318 if (_downloadData->chunk_table.testBit(end)) {
324 const uint32_t len = (end - start) * MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN;
325 _requestLogData(_downloadData->ID, pos, len, _retries);
328void OnboardLogController::_updateDataRate()
330 constexpr uint kSizeUpdateThreshold = 102400;
331 const bool timeThresholdMet = _downloadData->elapsed.elapsed() >= kGUIRateMs;
332 const bool sizeThresholdMet = (_downloadData->written - _downloadData->last_status_written) >= kSizeUpdateThreshold;
334 if (!timeThresholdMet && !sizeThresholdMet) {
339 if (timeThresholdMet) {
341 const qreal rate = _downloadData->rate_bytes / (_downloadData->elapsed.elapsed() / 1000.0);
342 _downloadData->rate_avg = (_downloadData->rate_avg * 0.95) + (rate * 0.05);
343 _downloadData->rate_bytes = 0;
347 _downloadData->elapsed.start();
354 _downloadData->entry->setStatus(status);
355 _downloadData->last_status_written = _downloadData->written;
358bool OnboardLogController::_chunkComplete()
const
360 return _downloadData->chunkEquals(
true);
363bool OnboardLogController::_logComplete()
const
365 return (_chunkComplete() && ((_downloadData->current_chunk + 1) == _downloadData->numChunks()));
368void OnboardLogController::_receivedAllData()
371 if (_prepareLogDownload()) {
372 _requestLogData(_downloadData->ID, 0, _downloadData->chunk_table.size() * MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN);
373 _timer->start(kTimeOutMs);
376 _setDownloading(
false);
380bool OnboardLogController::_prepareLogDownload()
382 _downloadData.reset();
392 const QString ftime = (entry->
time().date().year() >= 2010) ? entry->
time().toString(QStringLiteral(
"yyyy-M-d-hh-mm-ss")) : QStringLiteral(
"UnknownDate");
394 _downloadData = std::make_unique<OnboardLogDownloadData>(entry);
395 _downloadData->filename = QStringLiteral(
"log_") + QString::number(entry->
id()) +
"_" + ftime;
398 const QString loggerParam = QStringLiteral(
"SYS_LOGGER");
401 _downloadData->filename +=
".px4log";
403 _downloadData->filename +=
".ulg";
406 _downloadData->filename +=
".bin";
409 _downloadData->file.setFileName(_downloadPath + _downloadData->filename);
411 if (_downloadData->file.exists()) {
412 uint32_t numDups = 0;
413 const QStringList filename_spl = _downloadData->filename.split(
'.');
416 const QString filename = filename_spl[0] +
'_' + QString::number(numDups) +
'.' + filename_spl[1];
417 _downloadData->file.setFileName(_downloadPath + filename);
418 }
while ( _downloadData->file.exists());
422 if (!_downloadData->file.open(QIODevice::WriteOnly)) {
423 qCWarning(OnboardLogControllerLog) <<
"Failed to create log file:" << _downloadData->filename;
424 }
else if (!_downloadData->file.resize(entry->
size())) {
425 qCWarning(OnboardLogControllerLog) <<
"Failed to allocate space for log file:" << _downloadData->filename;
427 _downloadData->current_chunk = 0;
428 _downloadData->chunk_table = QBitArray(_downloadData->chunkBins(),
false);
429 _downloadData->elapsed.start();
434 if (_downloadData->file.exists()) {
435 (void) _downloadData->file.remove();
438 _downloadData->entry->setStatus(QStringLiteral(
"Error"));
439 _downloadData.reset();
449 _requestLogList(0, 0xffff);
454 const int numLogs = _logEntriesModel->
count();
455 for (
int i = 0; i < numLogs; i++) {
472 _receivedAllEntries();
475 _downloadData->entry->setStatus(QStringLiteral(
"Canceled"));
476 if (_downloadData->file.exists()) {
477 (void) _downloadData->file.remove();
480 _downloadData.reset();
483 _resetSelection(
true);
484 _setDownloading(
false);
489 const int count = _logEntriesModel->
count();
490 for (
int i = 0; i < count; i++) {
499 QSignalBlocker blocker(entry);
509 const int count = _logEntriesModel->
count();
510 for (
int i = 0; i < count; i++) {
524 const int count = _logEntriesModel->
count();
525 for (
int i = 0; i < count; i++) {
535 return (selectable > 0) && (selected == selectable);
545 if (_sortAscending == ascending) {
549 _sortAscending = ascending;
550 _sortEntriesByTimestamp();
554void OnboardLogController::_resetSelection(
bool canceled)
556 const int num_logs = _logEntriesModel->
count();
557 for (
int i = 0; i < num_logs; i++) {
574void OnboardLogController::_sortEntriesByTimestamp()
576 QObjectList sortedEntries = *_logEntriesModel->
objectList();
577 std::stable_sort(sortedEntries.begin(), sortedEntries.end(), [
this](
const QObject *lhsObj,
const QObject *rhsObj) {
578 const QGCOnboardLogEntry *const lhs = qobject_cast<const QGCOnboardLogEntry*>(lhsObj);
579 const QGCOnboardLogEntry *const rhs = qobject_cast<const QGCOnboardLogEntry*>(rhsObj);
590 const bool lhsHasTime = lhs->received() && (lhs->time().toSecsSinceEpoch() > 0);
591 const bool rhsHasTime = rhs->received() && (rhs->time().toSecsSinceEpoch() > 0);
592 if (lhsHasTime != rhsHasTime) {
597 if (lhsHasTime && rhsHasTime) {
598 if (lhs->time() == rhs->time()) {
599 return _sortAscending ? (lhs->id() < rhs->id()) : (lhs->id() > rhs->id());
602 return _sortAscending ? (lhs->time() < rhs->time()) : (lhs->time() > rhs->time());
606 return _sortAscending ? (lhs->id() < rhs->id()) : (lhs->id() > rhs->id());
609 (void) _logEntriesModel->swapObjectList(sortedEntries);
615 qCWarning(OnboardLogControllerLog) <<
"Vehicle Unavailable";
621 qCWarning(OnboardLogControllerLog) <<
"Link Unavailable";
626 (void) mavlink_msg_log_erase_pack_chan(
629 sharedLink->mavlinkChannel(),
636 qCWarning(OnboardLogControllerLog) <<
"Failed to send";
643void OnboardLogController::_requestLogList(uint32_t start, uint32_t end)
646 qCWarning(OnboardLogControllerLog) <<
"Vehicle Unavailable";
652 qCWarning(OnboardLogControllerLog) <<
"Link Unavailable";
657 (void) mavlink_msg_log_request_list_pack_chan(
660 sharedLink->mavlinkChannel(),
669 qCWarning(OnboardLogControllerLog) <<
"Failed to send";
673 qCDebug(OnboardLogControllerLog) <<
"Request onboard log entry list (" << start <<
"through" << end <<
")";
675 _timer->start(kRequestLogListTimeoutMs);
678void OnboardLogController::_requestLogData(uint16_t
id, uint32_t offset, uint32_t count,
int retryCount)
681 qCWarning(OnboardLogControllerLog) <<
"Vehicle Unavailable";
687 qCWarning(OnboardLogControllerLog) <<
"Link Unavailable";
692 qCDebug(OnboardLogControllerLog) <<
"Request log data (id:" <<
id <<
"offset:" << offset <<
"size:" << count <<
"retryCount" << retryCount <<
")";
695 (void) mavlink_msg_log_request_data_pack_chan(
698 sharedLink->mavlinkChannel(),
708 qCWarning(OnboardLogControllerLog) <<
"Failed to send";
712void OnboardLogController::_requestLogEnd()
715 qCWarning(OnboardLogControllerLog) <<
"Vehicle Unavailable";
721 qCWarning(OnboardLogControllerLog) <<
"Link Unavailable";
726 (void) mavlink_msg_log_request_end_pack_chan(
729 sharedLink->mavlinkChannel(),
736 qCWarning(OnboardLogControllerLog) <<
"Failed to send";
740void OnboardLogController::_setDownloading(
bool active)
742 if (_downloadingLogs != active) {
743 _downloadingLogs = active;
749void OnboardLogController::_setListing(
bool active)
751 if (_requestingLogEntries != active) {
752 _requestingLogEntries = active;
755 _sortEntriesByTimestamp();
763 if (_compressLogs != compress) {
764 _compressLogs = compress;
772 qCWarning(OnboardLogControllerLog) <<
"Log compression not yet implemented (decompression-only API)";
781void OnboardLogController::_handleCompressionProgress(qreal progress)
787void OnboardLogController::_handleCompressionFinished(
bool success)
std::shared_ptr< LinkInterface > SharedLinkInterfacePtr
struct __mavlink_message mavlink_message_t
#define QGC_LOGGING_CATEGORY(name, categoryStr)
QVariant rawValue() const
Value after translation.
static int getComponentId()
static MAVLinkProtocol * instance()
static MultiVehicleManager * instance()
void activeVehicleChanged(Vehicle *activeVehicle)
Q_INVOKABLE int selectedCount() const
void downloadingLogsChanged()
void requestingListChanged()
Q_INVOKABLE void selectAll(bool select)
Q_INVOKABLE void eraseAll()
Q_INVOKABLE void download(const QString &path=QString())
Q_INVOKABLE void toggleSortByDate()
Q_INVOKABLE void refresh()
bool allLogsSelected() const
void setCompressLogs(bool compress)
void sortAscendingChanged()
void setSortAscending(bool ascending)
void compressLogsChanged()
Q_INVOKABLE void cancel()
Q_INVOKABLE bool compressLogFile(const QString &logPath)
Compress a single log file.
Q_INVOKABLE void cancelCompression()
Cancel compression.
bool parameterExists(int componentId, const QString ¶mName) const
Fact * getParameter(int componentId, const QString ¶mName)
static constexpr int defaultComponentId
void setStatus(const QString &stat)
void setSelected(bool sel)
void setTime(const QDateTime &date)
void setReceived(bool rec)
void append(QObject *object)
Caller maintains responsibility for object ownership and deletion.
int count() const override final
void clearAndDeleteContents() override final
Clears the list and calls deleteLater on each entry.
QList< QObject * > * objectList()
static SettingsManager * instance()
AppSettings * appSettings() const
WeakLinkInterfacePtr primaryLink() const
void setCommunicationLostEnabled(bool communicationLostEnabled)
VehicleLinkManager * vehicleLinkManager()
void logEntry(uint32_t time_utc, uint32_t size, uint16_t id, uint16_t num_logs, uint16_t last_log_num)
MAV_AUTOPILOT firmwareType() const
bool sendMessageOnLinkThreadSafe(LinkInterface *link, mavlink_message_t message)
int defaultComponentId() const
ParameterManager * parameterManager()
void logData(uint32_t ofs, uint16_t id, uint8_t count, const uint8_t *data)
QString bigSizeToString(quint64 size)
Byte size with unit: B, KB, MB, GB, TB. 1 fractional digit above 1 KB.
static const uint32_t kChunkSize