From 627785c0fa67a863a7fcb61b95c3e64f60163948 Mon Sep 17 00:00:00 2001 From: RyanCraighead <64346321+RyanCraighead@users.noreply.github.com> Date: Tue, 12 May 2026 01:36:20 -0400 Subject: [PATCH] UAS: Add MAVFTP parameter and mission support Adds a MAVFTP client for ArduPilot virtual file transfers. Downloads packed parameters, uploads changed parameters through @PARAM/param.pck from the existing Write path, and uploads/downloads missions through @MISSION/mission.dat, with fallback to existing MAVLink protocols. Adds focused CMake unit coverage for packed parameter files, parameter upload encoding, mission.dat files, and MAVFTP request/response payloads. Relates #1252 --- CMakeLists.txt | 31 +++ apm_planner.pro | 6 + src/uas/MAVFTPFileFormats.cc | 455 ++++++++++++++++++++++++++++++ src/uas/MAVFTPFileFormats.h | 48 ++++ src/uas/MAVFTPManager.cc | 496 +++++++++++++++++++++++++++++++++ src/uas/MAVFTPManager.h | 117 ++++++++ src/uas/MAVFTPProtocol.cc | 158 +++++++++++ src/uas/MAVFTPProtocol.h | 81 ++++++ src/uas/UAS.cc | 219 +++++++++++---- src/uas/UAS.h | 72 +++-- src/uas/UASInterface.h | 5 + src/uas/UASWaypointManager.cc | 240 +++++++++++++++- src/uas/UASWaypointManager.h | 13 + src/ui/QGCParamWidget.cc | 51 ++++ src/ui/QGCParamWidget.h | 2 + tests/mavftp/MAVFTPUnitTest.cc | 410 +++++++++++++++++++++++++++ 16 files changed, 2323 insertions(+), 81 deletions(-) create mode 100644 src/uas/MAVFTPFileFormats.cc create mode 100644 src/uas/MAVFTPFileFormats.h create mode 100644 src/uas/MAVFTPManager.cc create mode 100644 src/uas/MAVFTPManager.h create mode 100644 src/uas/MAVFTPProtocol.cc create mode 100644 src/uas/MAVFTPProtocol.h create mode 100644 tests/mavftp/MAVFTPUnitTest.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index e1194fe146..9025f23225 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -126,6 +126,12 @@ set(SOURCES src/uas/LogDownloadDialog.cc src/uas/LogDownloadDialog.h src/uas/LogDownloadDialog.ui + src/uas/MAVFTPFileFormats.cc + src/uas/MAVFTPFileFormats.h + src/uas/MAVFTPManager.cc + src/uas/MAVFTPManager.h + src/uas/MAVFTPProtocol.cc + src/uas/MAVFTPProtocol.h src/uas/PxQuadMAV.cc src/uas/PxQuadMAV.h src/uas/QGCMAVLinkUASFactory.cc @@ -785,6 +791,31 @@ target_compile_definitions(apmplanner2 PRIVATE NOMINMAX ) +option(BUILD_MAVFTP_TESTS "Build MAVFTP unit tests" ON) +if(BUILD_MAVFTP_TESTS) + enable_testing() + add_executable(mavftp_unit_tests + tests/mavftp/MAVFTPUnitTest.cc + src/uas/MAVFTPFileFormats.cc + src/uas/MAVFTPFileFormats.h + src/uas/MAVFTPProtocol.cc + src/uas/MAVFTPProtocol.h + ) + target_include_directories(mavftp_unit_tests PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src/comm + ${CMAKE_CURRENT_SOURCE_DIR}/src/uas + ${MAVLINKPATH} + ${MAVLINKPATH}/${MAVLINK_CONF} + ) + target_compile_definitions(mavftp_unit_tests PRIVATE + QGC_USE_${MAVLINK_CONF}_MESSAGES + ) + target_link_libraries(mavftp_unit_tests + Qt5::Test + ) + add_test(NAME mavftp_unit_tests COMMAND mavftp_unit_tests) +endif() + # Platform-specific properties and post-build if(APPLE) set(MACOSX_BUNDLE_ICON_FILE "icons.icns") diff --git a/apm_planner.pro b/apm_planner.pro index 991a5360cf..1ffc804225 100644 --- a/apm_planner.pro +++ b/apm_planner.pro @@ -477,6 +477,9 @@ HEADERS += \ src/QGCCore.h \ src/uas/UASInterface.h \ src/uas/UAS.h \ + src/uas/MAVFTPFileFormats.h \ + src/uas/MAVFTPManager.h \ + src/uas/MAVFTPProtocol.h \ src/uas/UASManager.h \ src/comm/LinkManager.h \ src/comm/LinkInterface.h \ @@ -709,6 +712,9 @@ SOURCES += src/main.cc \ src/QGCCore.cc \ src/uas/UASManager.cc \ src/uas/UAS.cc \ + src/uas/MAVFTPFileFormats.cc \ + src/uas/MAVFTPManager.cc \ + src/uas/MAVFTPProtocol.cc \ src/comm/LinkManager.cc \ src/comm/LinkInterface.cpp \ src/comm/QGCFlightGearLink.cc \ diff --git a/src/uas/MAVFTPFileFormats.cc b/src/uas/MAVFTPFileFormats.cc new file mode 100644 index 0000000000..370b792416 --- /dev/null +++ b/src/uas/MAVFTPFileFormats.cc @@ -0,0 +1,455 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of QGroundControl/APM Planner. + +QGROUNDCONTROL is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +======================================================================*/ + +#include "MAVFTPFileFormats.h" + +#include +#include + +#include +#include + +namespace { + +const quint16 kParamMagicStandard = 0x671B; +const quint16 kParamMagicWithDefaults = 0x671C; +const quint16 kMissionMagic = 0x763d; +const int kMissionHeaderLength = 10; + +enum ApParamType +{ + AP_PARAM_NONE = 0, + AP_PARAM_INT8 = 1, + AP_PARAM_INT16 = 2, + AP_PARAM_INT32 = 3, + AP_PARAM_FLOAT = 4 +}; + +void setError(QString* errorString, const QString& error) +{ + if (errorString) { + *errorString = error; + } +} + +quint16 readUInt16(const QByteArray& data, int offset) +{ + const uchar* bytes = reinterpret_cast(data.constData() + offset); + return static_cast(bytes[0]) | + (static_cast(bytes[1]) << 8); +} + +qint16 readInt16(const QByteArray& data, int offset) +{ + return static_cast(readUInt16(data, offset)); +} + +quint32 readUInt32(const QByteArray& data, int offset) +{ + const uchar* bytes = reinterpret_cast(data.constData() + offset); + return static_cast(bytes[0]) | + (static_cast(bytes[1]) << 8) | + (static_cast(bytes[2]) << 16) | + (static_cast(bytes[3]) << 24); +} + +qint32 readInt32(const QByteArray& data, int offset) +{ + const quint32 raw = readUInt32(data, offset); + qint32 value = 0; + memcpy(&value, &raw, sizeof(value)); + return value; +} + +float readFloat(const QByteArray& data, int offset) +{ + const quint32 raw = readUInt32(data, offset); + float value = 0.0f; + memcpy(&value, &raw, sizeof(value)); + return value; +} + +void appendUInt16(QByteArray* data, quint16 value) +{ + data->append(static_cast(value & 0xff)); + data->append(static_cast((value >> 8) & 0xff)); +} + +void writeUInt16(QByteArray* data, int offset, quint16 value) +{ + (*data)[offset] = static_cast(value & 0xff); + (*data)[offset + 1] = static_cast((value >> 8) & 0xff); +} + +void appendUInt32(QByteArray* data, quint32 value) +{ + data->append(static_cast(value & 0xff)); + data->append(static_cast((value >> 8) & 0xff)); + data->append(static_cast((value >> 16) & 0xff)); + data->append(static_cast((value >> 24) & 0xff)); +} + +void appendFloat(QByteArray* data, float value) +{ + quint32 raw = 0; + memcpy(&raw, &value, sizeof(raw)); + appendUInt32(data, raw); +} + +int packedParamTypeForValue(const QVariant& value) +{ + switch (static_cast(value.type())) { + case QMetaType::QChar: + return AP_PARAM_INT8; + case QMetaType::Int: + return AP_PARAM_INT32; + case QMetaType::UInt: + if (value.toUInt() > static_cast(std::numeric_limits::max())) { + return AP_PARAM_NONE; + } + return AP_PARAM_INT32; + case QMetaType::Double: + case QMetaType::Float: + return AP_PARAM_FLOAT; + default: + return AP_PARAM_NONE; + } +} + +bool appendPackedParamValue(QByteArray* data, int paramType, const QVariant& value) +{ + switch (paramType) { + case AP_PARAM_INT8: + data->append(static_cast(value.type() == QVariant::Char ? value.toChar().toLatin1() : value.toInt())); + return true; + case AP_PARAM_INT32: + appendUInt32(data, static_cast(static_cast(value.toInt()))); + return true; + case AP_PARAM_FLOAT: + appendFloat(data, value.toFloat()); + return true; + default: + return false; + } +} + +bool readPackedParamValue(const QByteArray& data, int* offset, int paramType, QVariant* value) +{ + switch (paramType) { + case AP_PARAM_INT8: + if (*offset + 1 > data.size()) { + return false; + } + *value = QVariant(static_cast(static_cast(static_cast(data.at(*offset))))); + *offset += 1; + return true; + case AP_PARAM_INT16: + if (*offset + 2 > data.size()) { + return false; + } + *value = QVariant(static_cast(readInt16(data, *offset))); + *offset += 2; + return true; + case AP_PARAM_INT32: + if (*offset + 4 > data.size()) { + return false; + } + *value = QVariant(static_cast(readInt32(data, *offset))); + *offset += 4; + return true; + case AP_PARAM_FLOAT: + if (*offset + 4 > data.size()) { + return false; + } + *value = QVariant(static_cast(readFloat(data, *offset))); + *offset += 4; + return true; + default: + return false; + } +} + +void appendMissionItem(QByteArray* data, const mavlink_mission_item_int_t& item) +{ + appendFloat(data, item.param1); + appendFloat(data, item.param2); + appendFloat(data, item.param3); + appendFloat(data, item.param4); + appendUInt32(data, static_cast(item.x)); + appendUInt32(data, static_cast(item.y)); + appendFloat(data, item.z); + appendUInt16(data, item.seq); + appendUInt16(data, item.command); + data->append(static_cast(item.target_system)); + data->append(static_cast(item.target_component)); + data->append(static_cast(item.frame)); + data->append(static_cast(item.current)); + data->append(static_cast(item.autocontinue)); + data->append(static_cast(item.mission_type)); +} + +mavlink_mission_item_int_t readMissionItem(const QByteArray& data, int offset) +{ + mavlink_mission_item_int_t item; + memset(&item, 0, sizeof(item)); + item.param1 = readFloat(data, offset); + item.param2 = readFloat(data, offset + 4); + item.param3 = readFloat(data, offset + 8); + item.param4 = readFloat(data, offset + 12); + item.x = readInt32(data, offset + 16); + item.y = readInt32(data, offset + 20); + item.z = readFloat(data, offset + 24); + item.seq = readUInt16(data, offset + 28); + item.command = readUInt16(data, offset + 30); + item.target_system = static_cast(data.at(offset + 32)); + item.target_component = static_cast(data.at(offset + 33)); + item.frame = static_cast(data.at(offset + 34)); + item.current = static_cast(data.at(offset + 35)); + item.autocontinue = static_cast(data.at(offset + 36)); + item.mission_type = static_cast(data.at(offset + 37)); + return item; +} + +} // namespace + +namespace MAVFTPFileFormats +{ + +QString parameterDownloadPath() +{ + return QStringLiteral("@PARAM/param.pck?withdefaults=1"); +} + +QString parameterUploadPath() +{ + return QStringLiteral("@PARAM/param.pck"); +} + +QString missionPath() +{ + return QStringLiteral("@MISSION/mission.dat"); +} + +bool parseParameterFile(const QByteArray& data, QList* parameters, QString* errorString) +{ + if (parameters) { + parameters->clear(); + } + if (data.size() < 6) { + setError(errorString, QStringLiteral("parameter file is too small")); + return false; + } + + const quint16 magic = readUInt16(data, 0); + const int paramCount = readUInt16(data, 2); + const int totalParamCount = readUInt16(data, 4); + if (magic != kParamMagicStandard && magic != kParamMagicWithDefaults) { + setError(errorString, QStringLiteral("parameter file has invalid magic 0x%1").arg(magic, 4, 16, QLatin1Char('0'))); + return false; + } + if (paramCount != totalParamCount) { + setError(errorString, QStringLiteral("parameter file is partial (%1 of %2 parameters)").arg(paramCount).arg(totalParamCount)); + return false; + } + + QList parsed; + QByteArray previousName; + int offset = 6; + int paramIndex = 0; + while (paramIndex < paramCount) { + while (offset < data.size() && data.at(offset) == '\0') { + offset++; + } + + if (offset + 2 > data.size()) { + setError(errorString, QStringLiteral("unexpected end of file after %1 parameters").arg(paramIndex)); + return false; + } + + const uchar typeAndFlags = static_cast(data.at(offset++)); + const int paramType = typeAndFlags & 0x0f; + const int flags = (typeAndFlags >> 4) & 0x0f; + const bool hasDefault = (flags & 0x01) == 0x01; + + const uchar nameByte = static_cast(data.at(offset++)); + const int commonLength = nameByte & 0x0f; + const int nameLength = ((nameByte >> 4) & 0x0f) + 1; + if (commonLength > previousName.size() || commonLength + nameLength > MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN) { + setError(errorString, QStringLiteral("invalid parameter name prefix at index %1").arg(paramIndex)); + return false; + } + if (offset + nameLength > data.size()) { + setError(errorString, QStringLiteral("unexpected end of file while reading parameter name at index %1").arg(paramIndex)); + return false; + } + + QByteArray paramNameBytes = previousName.left(commonLength); + paramNameBytes.append(data.constData() + offset, nameLength); + offset += nameLength; + previousName = paramNameBytes; + + QVariant paramValue; + if (!readPackedParamValue(data, &offset, paramType, ¶mValue)) { + setError(errorString, QStringLiteral("invalid or truncated parameter value at index %1").arg(paramIndex)); + return false; + } + + QVariant defaultValue; + if (hasDefault && !readPackedParamValue(data, &offset, paramType, &defaultValue)) { + setError(errorString, QStringLiteral("invalid or truncated default value at index %1").arg(paramIndex)); + return false; + } + + ParameterValue parameter; + parameter.name = QString::fromLatin1(paramNameBytes.constData(), paramNameBytes.size()); + parameter.value = paramValue; + parameter.packedType = paramType; + parameter.hasDefault = hasDefault; + parameter.defaultValue = defaultValue; + parsed.append(parameter); + paramIndex++; + } + + if (parameters) { + *parameters = parsed; + } + return true; +} + +bool encodeParameterUploadFile(const QMap& parameters, QByteArray* data, QString* errorString) +{ + if (!data) { + setError(errorString, QStringLiteral("missing output buffer")); + return false; + } + data->clear(); + if (parameters.size() > std::numeric_limits::max()) { + setError(errorString, QStringLiteral("too many parameters for MAVFTP upload")); + return false; + } + + appendUInt16(data, kParamMagicStandard); + appendUInt16(data, static_cast(parameters.count())); + appendUInt16(data, 0); + + QByteArray previousName; + for (QMap::const_iterator it = parameters.constBegin(); it != parameters.constEnd(); ++it) { + const QByteArray name = it.key().toLatin1(); + if (name.isEmpty() || name.size() > MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN) { + setError(errorString, QStringLiteral("invalid MAVFTP parameter name '%1'").arg(it.key())); + return false; + } + + int commonLength = 0; + const int commonLimit = qMin(qMin(previousName.size(), name.size()), 15); + while (commonLength < commonLimit && previousName.at(commonLength) == name.at(commonLength)) { + commonLength++; + } + + const int nameLength = name.size() - commonLength; + if (nameLength <= 0 || nameLength > 16) { + setError(errorString, QStringLiteral("invalid MAVFTP parameter name compression for '%1'").arg(it.key())); + return false; + } + + const int paramType = packedParamTypeForValue(it.value()); + if (paramType == AP_PARAM_NONE) { + setError(errorString, QStringLiteral("unsupported MAVFTP parameter type for '%1'").arg(it.key())); + return false; + } + + data->append(static_cast(paramType)); + data->append(static_cast(commonLength | ((nameLength - 1) << 4))); + data->append(name.constData() + commonLength, nameLength); + if (!appendPackedParamValue(data, paramType, it.value())) { + setError(errorString, QStringLiteral("failed to encode MAVFTP parameter '%1'").arg(it.key())); + return false; + } + + previousName = name; + if (data->size() > std::numeric_limits::max()) { + setError(errorString, QStringLiteral("MAVFTP parameter file exceeds 65535 bytes")); + return false; + } + } + + writeUInt16(data, 4, static_cast(data->size())); + return true; +} + +bool parseMissionFile(const QByteArray& data, QList* items, QString* errorString) +{ + if (items) { + items->clear(); + } + if (data.size() < kMissionHeaderLength) { + setError(errorString, QStringLiteral("mission file is too short")); + return false; + } + + const quint16 magic = readUInt16(data, 0); + const quint16 dataType = readUInt16(data, 2); + const quint16 start = readUInt16(data, 6); + const quint16 itemCount = readUInt16(data, 8); + const int expectedSize = kMissionHeaderLength + itemCount * MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN; + + if (magic != kMissionMagic) { + setError(errorString, QStringLiteral("invalid mission file magic 0x%1").arg(magic, 4, 16, QLatin1Char('0'))); + return false; + } + if (dataType != MAV_MISSION_TYPE_MISSION) { + setError(errorString, QStringLiteral("unsupported mission type %1").arg(dataType)); + return false; + } + if (start != 0) { + setError(errorString, QStringLiteral("partial mission file starts at %1").arg(start)); + return false; + } + if (data.size() < expectedSize) { + setError(errorString, QStringLiteral("mission file has %1 bytes, expected at least %2").arg(data.size()).arg(expectedSize)); + return false; + } + + QList parsed; + for (int i = 0; i < itemCount; i++) { + parsed.append(readMissionItem(data, kMissionHeaderLength + i * MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN)); + } + + if (items) { + *items = parsed; + } + return true; +} + +QByteArray encodeMissionFile(const QList& items) +{ + QByteArray data; + const quint16 itemCount = static_cast(items.count()); + data.reserve(kMissionHeaderLength + itemCount * MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); + + appendUInt16(&data, kMissionMagic); + appendUInt16(&data, MAV_MISSION_TYPE_MISSION); + appendUInt16(&data, 0); + appendUInt16(&data, 0); + appendUInt16(&data, itemCount); + + foreach (const mavlink_mission_item_int_t& item, items) { + appendMissionItem(&data, item); + } + + return data; +} + +} // namespace MAVFTPFileFormats diff --git a/src/uas/MAVFTPFileFormats.h b/src/uas/MAVFTPFileFormats.h new file mode 100644 index 0000000000..0d7d904847 --- /dev/null +++ b/src/uas/MAVFTPFileFormats.h @@ -0,0 +1,48 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of QGroundControl/APM Planner. + +QGROUNDCONTROL is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +======================================================================*/ + +#pragma once + +#include +#include +#include +#include +#include + +#include "QGCMAVLink.h" + +namespace MAVFTPFileFormats +{ + +struct ParameterValue +{ + QString name; + QVariant value; + int packedType; + bool hasDefault; + QVariant defaultValue; +}; + +QString parameterDownloadPath(); +QString parameterUploadPath(); +QString missionPath(); + +bool parseParameterFile(const QByteArray& data, QList* parameters, QString* errorString); +bool encodeParameterUploadFile(const QMap& parameters, QByteArray* data, QString* errorString); + +bool parseMissionFile(const QByteArray& data, QList* items, QString* errorString); +QByteArray encodeMissionFile(const QList& items); + +} // namespace MAVFTPFileFormats diff --git a/src/uas/MAVFTPManager.cc b/src/uas/MAVFTPManager.cc new file mode 100644 index 0000000000..27f86befe3 --- /dev/null +++ b/src/uas/MAVFTPManager.cc @@ -0,0 +1,496 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of QGroundControl/APM Planner. + +QGROUNDCONTROL is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +======================================================================*/ + +#include "MAVFTPManager.h" + +#include "LinkInterface.h" +#include "MAVFTPFileFormats.h" +#include "MAVFTPProtocol.h" +#include "UAS.h" +#include "logging.h" + +#include +#include + +#include + +MAVFTPManager::MAVFTPManager(UAS* uas) : + QObject(uas), + _uas(uas), + _link(nullptr), + _state(Idle), + _transferType(NoTransfer), + _targetComponent(MAV_COMP_ID_PRIMARY), + _session(0), + _sequence(0), + _lastSequence(0), + _offset(0), + _lastOpcode(0), + _lastSize(0), + _lastOffset(0), + _retryCount(0) +{ + _timer.setInterval(kTimeoutMs); + _timer.setSingleShot(true); + connect(&_timer, SIGNAL(timeout()), this, SLOT(timeout())); +} + +bool MAVFTPManager::isBusy() const +{ + return _state != Idle; +} + +bool MAVFTPManager::downloadParameterFile() +{ + return downloadFile(MAVFTPFileFormats::parameterDownloadPath(), MAV_COMP_ID_PRIMARY); +} + +bool MAVFTPManager::downloadFile(const QString& remotePath, uint8_t targetComponent) +{ + if (_state != Idle) { + return false; + } + + if (remotePath.isEmpty()) { + return false; + } + + _link = activeLink(); + if (!_link) { + return false; + } + + const QByteArray path = remotePath.toLatin1(); + if (path.size() > kMaxDataLength) { + return false; + } + + _targetComponent = targetComponent; + _session = 0; + _offset = 0; + _remotePath = remotePath; + _download.clear(); + _upload.clear(); + _transferType = DownloadTransfer; + _state = Opening; + + QLOG_DEBUG() << "Starting MAVFTP download of" << remotePath; + if (!sendRequest(MAVFTPProtocol::OpOpenFileRO, static_cast(path.size()), 0, path)) { + reset(); + return false; + } + + return true; +} + +bool MAVFTPManager::uploadFile(const QString& remotePath, const QByteArray& data, uint8_t targetComponent) +{ + if (_state != Idle) { + return false; + } + + if (remotePath.isEmpty()) { + return false; + } + + _link = activeLink(); + if (!_link) { + return false; + } + + const QByteArray path = remotePath.toLatin1(); + if (path.size() > kMaxDataLength) { + return false; + } + + _targetComponent = targetComponent; + _session = 0; + _offset = 0; + _remotePath = remotePath; + _download.clear(); + _upload = data; + _transferType = UploadTransfer; + _state = Creating; + + QLOG_DEBUG() << "Starting MAVFTP upload of" << remotePath << "with" << data.size() << "bytes"; + if (!sendRequest(MAVFTPProtocol::OpCreateFile, static_cast(path.size()), 0, path)) { + reset(); + return false; + } + + return true; +} + +bool MAVFTPManager::handleMessage(LinkInterface* link, const mavlink_message_t& message) +{ + if (_state == Idle || link != _link || message.msgid != MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { + return false; + } + if (message.sysid != _uas->getUASID() || message.compid != _targetComponent) { + return false; + } + + Response response; + if (!decodeResponse(message, &response)) { + return false; + } + + const uint8_t expectedRequestOpcode = + _state == Opening ? MAVFTPProtocol::OpOpenFileRO : + _state == Reading ? MAVFTPProtocol::OpReadFile : + _state == Creating ? MAVFTPProtocol::OpCreateFile : + _state == Writing ? MAVFTPProtocol::OpWriteFile : + _state == Closing ? MAVFTPProtocol::OpTerminateSession : 0; + if (response.requestOpcode != expectedRequestOpcode) { + return false; + } + if (response.sequence != expectedResponseSequence()) { + return false; + } + if ((_state == Reading || _state == Writing || _state == Closing) && response.session != _session) { + return false; + } + if (_state == Reading && response.opcode == MAVFTPProtocol::OpAck && response.offset < _offset) { + return false; + } + _timer.stop(); + _retryCount = 0; + + switch (_state) { + case Opening: + handleOpenResponse(response); + break; + case Reading: + handleReadResponse(response); + break; + case Creating: + handleCreateResponse(response); + break; + case Writing: + handleWriteResponse(response); + break; + case Closing: + handleTerminateResponse(response); + break; + case Idle: + break; + } + + return true; +} + +void MAVFTPManager::cancel() +{ + if (_state == Idle) { + return; + } + + sendRequest(MAVFTPProtocol::OpTerminateSession, 0, 0, QByteArray()); + finish(QStringLiteral("MAVFTP transfer cancelled")); +} + +void MAVFTPManager::timeout() +{ + if (_state == Idle) { + return; + } + + if (_retryCount < kMaxRetries) { + _retryCount++; + if (!sendRequest(_lastOpcode, _lastSize, _lastOffset, _lastData, true)) { + finish(QStringLiteral("MAVFTP request could not be sent")); + } + return; + } + + finish(QStringLiteral("MAVFTP request timed out")); +} + +LinkInterface* MAVFTPManager::activeLink() const +{ + if (!_uas || !_uas->getLinks()) { + return nullptr; + } + + foreach (LinkInterface* link, *_uas->getLinks()) { + if (link && link->isConnected()) { + return link; + } + } + + return nullptr; +} + +bool MAVFTPManager::sendRequest(uint8_t opcode, uint8_t size, quint32 offset, const QByteArray& data, bool retrying) +{ + if (!_uas || !_link || !_link->isConnected()) { + return false; + } + + const quint16 requestSequence = retrying ? _lastSequence : _sequence; + if (!retrying) { + _sequence = static_cast(_sequence + 2); + } + + uint8_t payload[MAVFTPProtocol::PayloadLength]; + QString encodeError; + if (!MAVFTPProtocol::encodePayload(requestSequence, _session, opcode, size, offset, data, + payload, MAVFTPProtocol::PayloadLength, &encodeError)) { + QLOG_WARN() << encodeError; + return false; + } + + mavlink_message_t message; + UASInterface* uasInterface = static_cast(_uas); + mavlink_msg_file_transfer_protocol_pack(static_cast(uasInterface->getSystemId()), + static_cast(uasInterface->getComponentId()), + &message, + 0, + static_cast(_uas->getUASID()), + _targetComponent, + payload); + _uas->sendMessage(_link, message); + + if (!retrying) { + _lastSequence = requestSequence; + _lastOpcode = opcode; + _lastSize = size; + _lastOffset = offset; + _lastData = data; + _retryCount = 0; + } + + _timer.start(); + return true; +} + +bool MAVFTPManager::decodeResponse(const mavlink_message_t& message, Response* response) const +{ + mavlink_file_transfer_protocol_t packet; + mavlink_msg_file_transfer_protocol_decode(&message, &packet); + + UASInterface* uasInterface = static_cast(_uas); + const uint8_t gcsSystemId = static_cast(uasInterface->getSystemId()); + const uint8_t gcsComponentId = static_cast(uasInterface->getComponentId()); + if (packet.target_system != 0 && packet.target_system != gcsSystemId) { + return false; + } + if (packet.target_component != 0 && packet.target_component != gcsComponentId) { + return false; + } + + QString decodeError; + if (!MAVFTPProtocol::decodePayload(packet.payload, MAVFTPProtocol::PayloadLength, response, &decodeError)) { + QLOG_WARN() << decodeError; + return false; + } + return true; +} + +void MAVFTPManager::handleOpenResponse(const Response& response) +{ + if (response.requestOpcode != MAVFTPProtocol::OpOpenFileRO) { + return; + } + + if (response.opcode != MAVFTPProtocol::OpAck) { + const uint8_t errorCode = MAVFTPProtocol::responseErrorCode(response); + finish(QStringLiteral("MAVFTP open failed: %1").arg(MAVFTPProtocol::errorString(errorCode))); + return; + } + + _session = response.session; + _offset = 0; + _state = Reading; + if (!sendReadRequest()) { + finish(QStringLiteral("MAVFTP read request could not be sent")); + } +} + +void MAVFTPManager::handleReadResponse(const Response& response) +{ + if (response.requestOpcode != MAVFTPProtocol::OpReadFile) { + return; + } + + if (response.opcode == MAVFTPProtocol::OpNack) { + const uint8_t errorCode = MAVFTPProtocol::responseErrorCode(response); + if (errorCode == MAVFTPProtocol::ErrEndOfFile || errorCode == MAVFTPProtocol::ErrNone) { + if (!sendTerminateRequest()) { + finish(QStringLiteral("MAVFTP close request could not be sent")); + } + return; + } + + finish(QStringLiteral("MAVFTP read failed: %1").arg(MAVFTPProtocol::errorString(errorCode))); + return; + } + + if (response.opcode != MAVFTPProtocol::OpAck || response.offset != _offset) { + finish(QStringLiteral("MAVFTP received an unexpected read response")); + return; + } + + _download.append(response.data.constData(), response.data.size()); + _offset += response.data.size(); + + if (response.size == 0 || response.size < _lastSize) { + if (!sendTerminateRequest()) { + finish(QStringLiteral("MAVFTP close request could not be sent")); + } + return; + } + + if (!sendReadRequest()) { + finish(QStringLiteral("MAVFTP read request could not be sent")); + } +} + +void MAVFTPManager::handleCreateResponse(const Response& response) +{ + if (response.requestOpcode != MAVFTPProtocol::OpCreateFile) { + return; + } + + if (response.opcode != MAVFTPProtocol::OpAck) { + const uint8_t errorCode = MAVFTPProtocol::responseErrorCode(response); + finish(QStringLiteral("MAVFTP create failed: %1").arg(MAVFTPProtocol::errorString(errorCode))); + return; + } + + _session = response.session; + _offset = 0; + _state = Writing; + + if (_upload.isEmpty()) { + if (!sendTerminateRequest()) { + finish(QStringLiteral("MAVFTP close request could not be sent")); + } + return; + } + + if (!sendWriteRequest()) { + finish(QStringLiteral("MAVFTP write request could not be sent")); + } +} + +void MAVFTPManager::handleWriteResponse(const Response& response) +{ + if (response.requestOpcode != MAVFTPProtocol::OpWriteFile) { + return; + } + + if (response.opcode == MAVFTPProtocol::OpNack) { + const uint8_t errorCode = MAVFTPProtocol::responseErrorCode(response); + finish(QStringLiteral("MAVFTP write failed: %1").arg(MAVFTPProtocol::errorString(errorCode))); + return; + } + + if (response.opcode != MAVFTPProtocol::OpAck || response.offset != _offset) { + finish(QStringLiteral("MAVFTP received an unexpected write response")); + return; + } + + _offset += _lastSize; + if (_offset >= static_cast(_upload.size())) { + if (!sendTerminateRequest()) { + finish(QStringLiteral("MAVFTP close request could not be sent")); + } + return; + } + + if (!sendWriteRequest()) { + finish(QStringLiteral("MAVFTP write request could not be sent")); + } +} + +void MAVFTPManager::handleTerminateResponse(const Response& response) +{ + if (response.requestOpcode != MAVFTPProtocol::OpTerminateSession) { + return; + } + + if (response.opcode != MAVFTPProtocol::OpAck) { + const uint8_t errorCode = MAVFTPProtocol::responseErrorCode(response); + finish(QStringLiteral("MAVFTP close failed: %1").arg(MAVFTPProtocol::errorString(errorCode))); + return; + } + + finish(QString()); +} + +bool MAVFTPManager::sendReadRequest() +{ + return sendRequest(MAVFTPProtocol::OpReadFile, static_cast(kMaxDataLength), _offset, QByteArray()); +} + +bool MAVFTPManager::sendWriteRequest() +{ + const int remaining = _upload.size() - static_cast(_offset); + const int writeSize = qMin(static_cast(kMaxDataLength), remaining); + if (writeSize <= 0) { + return false; + } + + const QByteArray data = _upload.mid(static_cast(_offset), writeSize); + return sendRequest(MAVFTPProtocol::OpWriteFile, static_cast(data.size()), _offset, data); +} + +bool MAVFTPManager::sendTerminateRequest() +{ + _state = Closing; + return sendRequest(MAVFTPProtocol::OpTerminateSession, 0, 0, QByteArray()); +} + +void MAVFTPManager::finish(const QString& errorString) +{ + const State completedState = _state; + const TransferType completedTransferType = _transferType; + const QString remotePath = _remotePath; + const QByteArray data = _download; + reset(); + + if (completedTransferType == DownloadTransfer && (completedState == Opening || completedState == Reading || completedState == Closing)) { + emit fileDownloadComplete(remotePath, data, errorString); + if (remotePath.startsWith(QStringLiteral("@PARAM/param.pck"))) { + emit downloadComplete(data, errorString); + } + } else if (completedTransferType == UploadTransfer && (completedState == Creating || completedState == Writing || completedState == Closing)) { + emit fileUploadComplete(remotePath, errorString); + } +} + +void MAVFTPManager::reset() +{ + _timer.stop(); + _state = Idle; + _transferType = NoTransfer; + _link = nullptr; + _offset = 0; + _lastSequence = 0; + _remotePath.clear(); + _download.clear(); + _upload.clear(); + _lastOpcode = 0; + _lastSize = 0; + _lastOffset = 0; + _lastData.clear(); + _retryCount = 0; +} + +quint16 MAVFTPManager::expectedResponseSequence() const +{ + return MAVFTPProtocol::expectedResponseSequence(_lastSequence); +} diff --git a/src/uas/MAVFTPManager.h b/src/uas/MAVFTPManager.h new file mode 100644 index 0000000000..f656451e22 --- /dev/null +++ b/src/uas/MAVFTPManager.h @@ -0,0 +1,117 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of QGroundControl/APM Planner. + +QGROUNDCONTROL is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +======================================================================*/ + +#pragma once + +#include +#include +#include +#include + +#include "MAVFTPProtocol.h" +#include "QGCMAVLink.h" + +#include + +class LinkInterface; +class UAS; + +/** + * @brief Minimal MAVLink FTP client used for ArduPilot virtual file access. + */ +class MAVFTPManager : public QObject +{ + Q_OBJECT +public: + explicit MAVFTPManager(UAS* uas); + + bool isBusy() const; + bool downloadParameterFile(); + bool downloadFile(const QString& remotePath, uint8_t targetComponent = MAV_COMP_ID_PRIMARY); + bool uploadFile(const QString& remotePath, const QByteArray& data, uint8_t targetComponent = MAV_COMP_ID_PRIMARY); + bool handleMessage(LinkInterface* link, const mavlink_message_t& message); + void cancel(); + +signals: + void downloadComplete(const QByteArray& data, const QString& errorString); + void fileDownloadComplete(const QString& remotePath, const QByteArray& data, const QString& errorString); + void fileUploadComplete(const QString& remotePath, const QString& errorString); + +private slots: + void timeout(); + +private: + enum State + { + Idle, + Opening, + Reading, + Creating, + Writing, + Closing + }; + + enum TransferType + { + NoTransfer, + DownloadTransfer, + UploadTransfer + }; + + typedef MAVFTPProtocol::Packet Response; + + LinkInterface* activeLink() const; + bool sendRequest(uint8_t opcode, uint8_t size, quint32 offset, const QByteArray& data, bool retrying = false); + bool decodeResponse(const mavlink_message_t& message, Response* response) const; + void handleOpenResponse(const Response& response); + void handleReadResponse(const Response& response); + void handleCreateResponse(const Response& response); + void handleWriteResponse(const Response& response); + void handleTerminateResponse(const Response& response); + bool sendReadRequest(); + bool sendWriteRequest(); + bool sendTerminateRequest(); + void finish(const QString& errorString); + void reset(); + + quint16 expectedResponseSequence() const; + + enum + { + kMaxDataLength = MAVFTPProtocol::MaxDataLength, + kTimeoutMs = 1000, + kMaxRetries = 3 + }; + + UAS* _uas; + LinkInterface* _link; + QTimer _timer; + State _state; + TransferType _transferType; + uint8_t _targetComponent; + uint8_t _session; + quint16 _sequence; + quint16 _lastSequence; + quint32 _offset; + QString _remotePath; + QByteArray _download; + QByteArray _upload; + + uint8_t _lastOpcode; + uint8_t _lastSize; + quint32 _lastOffset; + QByteArray _lastData; + int _retryCount; +}; diff --git a/src/uas/MAVFTPProtocol.cc b/src/uas/MAVFTPProtocol.cc new file mode 100644 index 0000000000..c648d85c75 --- /dev/null +++ b/src/uas/MAVFTPProtocol.cc @@ -0,0 +1,158 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of QGroundControl/APM Planner. + +QGROUNDCONTROL is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +======================================================================*/ + +#include "MAVFTPProtocol.h" + +#include + +namespace { + +void setError(QString* errorString, const QString& error) +{ + if (errorString) { + *errorString = error; + } +} + +void writeUInt16(uint8_t* bytes, quint16 value) +{ + bytes[0] = static_cast(value & 0xff); + bytes[1] = static_cast((value >> 8) & 0xff); +} + +void writeUInt32(uint8_t* bytes, quint32 value) +{ + bytes[0] = static_cast(value & 0xff); + bytes[1] = static_cast((value >> 8) & 0xff); + bytes[2] = static_cast((value >> 16) & 0xff); + bytes[3] = static_cast((value >> 24) & 0xff); +} + +quint16 readUInt16(const uint8_t* bytes) +{ + return static_cast(bytes[0]) | + (static_cast(bytes[1]) << 8); +} + +quint32 readUInt32(const uint8_t* bytes) +{ + return static_cast(bytes[0]) | + (static_cast(bytes[1]) << 8) | + (static_cast(bytes[2]) << 16) | + (static_cast(bytes[3]) << 24); +} + +} // namespace + +namespace MAVFTPProtocol +{ + +bool encodePayload(quint16 sequence, uint8_t session, uint8_t opcode, uint8_t size, quint32 offset, + const QByteArray& data, uint8_t* payload, int payloadLength, QString* errorString) +{ + if (!payload || payloadLength != PayloadLength) { + setError(errorString, QStringLiteral("invalid MAVFTP payload buffer")); + return false; + } + if (data.size() > MaxDataLength || data.size() > size) { + setError(errorString, QStringLiteral("MAVFTP payload data is too large")); + return false; + } + + memset(payload, 0, payloadLength); + writeUInt16(&payload[0], sequence); + payload[2] = session; + payload[3] = opcode; + payload[4] = size; + payload[5] = 0; + payload[6] = 0; + payload[7] = 0; + writeUInt32(&payload[8], offset); + + if (!data.isEmpty()) { + memcpy(&payload[HeaderLength], data.constData(), data.size()); + } + return true; +} + +bool decodePayload(const uint8_t* payload, int payloadLength, Packet* packet, QString* errorString) +{ + if (!payload || payloadLength != PayloadLength || !packet) { + setError(errorString, QStringLiteral("invalid MAVFTP payload")); + return false; + } + + Packet decoded; + decoded.sequence = readUInt16(&payload[0]); + decoded.session = payload[2]; + decoded.opcode = payload[3]; + decoded.size = payload[4]; + decoded.requestOpcode = payload[5]; + decoded.burstComplete = payload[6]; + decoded.offset = readUInt32(&payload[8]); + if (decoded.size > MaxDataLength) { + setError(errorString, QStringLiteral("MAVFTP response data is too large")); + return false; + } + + decoded.data = QByteArray(reinterpret_cast(&payload[HeaderLength]), decoded.size); + *packet = decoded; + return true; +} + +quint16 expectedResponseSequence(quint16 requestSequence) +{ + return static_cast(requestSequence + 1); +} + +uint8_t responseErrorCode(const Packet& packet) +{ + if (packet.data.isEmpty()) { + return static_cast(ErrFail); + } + return static_cast(static_cast(packet.data.at(0))); +} + +QString errorString(uint8_t errorCode) +{ + switch (errorCode) { + case ErrNone: + return QStringLiteral("no error"); + case ErrFail: + return QStringLiteral("generic failure"); + case ErrFailErrno: + return QStringLiteral("system error"); + case ErrInvalidDataSize: + return QStringLiteral("invalid data size"); + case ErrInvalidSession: + return QStringLiteral("invalid session"); + case ErrNoSessionsAvailable: + return QStringLiteral("no sessions available"); + case ErrEndOfFile: + return QStringLiteral("end of file"); + case ErrUnknownCommand: + return QStringLiteral("unknown command"); + case ErrFileExists: + return QStringLiteral("file exists"); + case ErrFileProtected: + return QStringLiteral("file protected"); + case ErrFileNotFound: + return QStringLiteral("file not found"); + default: + return QStringLiteral("unknown error %1").arg(errorCode); + } +} + +} // namespace MAVFTPProtocol diff --git a/src/uas/MAVFTPProtocol.h b/src/uas/MAVFTPProtocol.h new file mode 100644 index 0000000000..55e5ad8049 --- /dev/null +++ b/src/uas/MAVFTPProtocol.h @@ -0,0 +1,81 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of QGroundControl/APM Planner. + +QGROUNDCONTROL is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +======================================================================*/ + +#pragma once + +#include +#include + +#include "QGCMAVLink.h" + +#include + +namespace MAVFTPProtocol +{ + +enum Opcode +{ + OpTerminateSession = 1, + OpOpenFileRO = 4, + OpReadFile = 5, + OpCreateFile = 6, + OpWriteFile = 7, + OpAck = 128, + OpNack = 129 +}; + +enum ErrorCode +{ + ErrNone = 0, + ErrFail = 1, + ErrFailErrno = 2, + ErrInvalidDataSize = 3, + ErrInvalidSession = 4, + ErrNoSessionsAvailable = 5, + ErrEndOfFile = 6, + ErrUnknownCommand = 7, + ErrFileExists = 8, + ErrFileProtected = 9, + ErrFileNotFound = 10 +}; + +enum +{ + HeaderLength = 12, + PayloadLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN, + MaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - HeaderLength +}; + +struct Packet +{ + quint16 sequence; + uint8_t session; + uint8_t opcode; + uint8_t size; + uint8_t requestOpcode; + uint8_t burstComplete; + quint32 offset; + QByteArray data; +}; + +bool encodePayload(quint16 sequence, uint8_t session, uint8_t opcode, uint8_t size, quint32 offset, + const QByteArray& data, uint8_t* payload, int payloadLength, QString* errorString); +bool decodePayload(const uint8_t* payload, int payloadLength, Packet* packet, QString* errorString); + +quint16 expectedResponseSequence(quint16 requestSequence); +uint8_t responseErrorCode(const Packet& packet); +QString errorString(uint8_t errorCode); + +} // namespace MAVFTPProtocol diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 05e8c9b000..72840e0c0e 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -9,10 +9,12 @@ * */ -#include "logging.h" -#include "UAS.h" -#include "LinkInterface.h" -#include "UASManager.h" +#include "logging.h" +#include "UAS.h" +#include "LinkInterface.h" +#include "MAVFTPFileFormats.h" +#include "MAVFTPManager.h" +#include "UASManager.h" #include "QGC.h" #include "GAudioOutput.h" #include "QGCMAVLink.h" @@ -26,22 +28,23 @@ #include #include #include -#include - -#include -#include +#include + +#include +#include +#include +#include #ifdef QGC_PROTOBUF_ENABLED #include #endif -const double UAS::lipoFull = 4.2f; ///< 100% charged voltage -const double UAS::lipoEmpty = 3.5f; ///< Discharged voltage - - -/** -* Gets the settings from the previous UAS (name, airframe, autopilot, battery specs) +const double UAS::lipoFull = 4.2f; ///< 100% charged voltage +const double UAS::lipoEmpty = 3.5f; ///< Discharged voltage + +/** +* Gets the settings from the previous UAS (name, airframe, autopilot, battery specs) * by calling readSettings. This means the new UAS will have the same settings * as the previous one created unless one calls deleteSettings in the code after * creating the UAS. @@ -145,11 +148,12 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), blockHomePositionChanges(false), receivedMode(false), - - paramsOnceRequested(false), - paramManager(nullptr), - - simulation(nullptr), + + paramsOnceRequested(false), + paramManager(nullptr), + mavftpManager(nullptr), + + simulation(nullptr), p_protocol(protocol), // The protected members. @@ -178,10 +182,20 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), emit disarmed(); emit armingChanged(false); - systemId = QGC::MavlinkID(); - componentId = QGC::defaultComponentId; - - m_heartbeatsEnabled = MainWindow::instance()->heartbeatEnabled(); //Default to sending heartbeats + systemId = QGC::MavlinkID(); + componentId = QGC::defaultComponentId; + + mavftpManager = new MAVFTPManager(this); + connect(mavftpManager, SIGNAL(downloadComplete(QByteArray,QString)), + this, SLOT(mavftpParameterDownloadComplete(QByteArray,QString))); + connect(mavftpManager, SIGNAL(fileUploadComplete(QString,QString)), + this, SLOT(mavftpFileUploadComplete(QString,QString))); + connect(mavftpManager, SIGNAL(fileDownloadComplete(QString,QByteArray,QString)), + &waypointManager, SLOT(mavftpMissionDownloadComplete(QString,QByteArray,QString))); + connect(mavftpManager, SIGNAL(fileUploadComplete(QString,QString)), + &waypointManager, SLOT(mavftpMissionUploadComplete(QString,QString))); + + m_heartbeatsEnabled = MainWindow::instance()->heartbeatEnabled(); //Default to sending heartbeats QTimer *heartbeattimer = new QTimer(this); connect(heartbeattimer,SIGNAL(timeout()),this,SLOT(sendHeartbeat())); heartbeattimer->start(MAVLINK_HEARTBEAT_DEFAULT_RATE * 1000); @@ -1025,11 +1039,18 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) processParamValueMsg(message, parameterName,rawValue,paramVal); - } - break; - case MAVLINK_MSG_ID_COMMAND_ACK: - { - mavlink_command_ack_t ack; + } + break; + case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: + { + if (mavftpManager) { + mavftpManager->handleMessage(link, message); + } + } + break; + case MAVLINK_MSG_ID_COMMAND_ACK: + { + mavlink_command_ack_t ack; mavlink_msg_command_ack_decode(&message, &ack); switch (ack.result) { @@ -2340,26 +2361,67 @@ int UAS::getCommunicationStatus() const return commStatus; } -void UAS::requestParameters() -{ - mavlink_message_t msg; - mavlink_msg_param_request_list_pack(systemId, componentId, &msg, this->getUASID(), MAV_COMP_ID_PRIMARY); - sendMessage(msg); +void UAS::requestParameters() +{ + if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA && mavftpManager && mavftpManager->downloadParameterFile()) { + QLOG_DEBUG() << __FILE__ << __LINE__ << "LOADING PARAM LIST VIA MAVFTP"; + return; + } + + requestParametersViaMavlink(); +} + +void UAS::requestParametersViaMavlink() +{ + mavlink_message_t msg; + mavlink_msg_param_request_list_pack(systemId, componentId, &msg, this->getUASID(), MAV_COMP_ID_PRIMARY); + sendMessage(msg); QLOG_DEBUG() << __FILE__ << __LINE__ << "LOADING PARAM LIST"; } -void UAS::writeParametersToStorage() -{ - mavlink_message_t msg; - mavlink_msg_command_long_pack(systemId, componentId, &msg, uasId, 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0); - QLOG_DEBUG() << "SENT COMMAND" << MAV_CMD_PREFLIGHT_STORAGE; - sendMessage(msg); -} - -void UAS::readParametersFromStorage() -{ - mavlink_message_t msg; - mavlink_msg_command_long_pack(systemId, componentId, &msg, uasId, 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 0, -1, -1, -1, 0, 0, 0); +void UAS::writeParametersToStorage() +{ + mavlink_message_t msg; + mavlink_msg_command_long_pack(systemId, componentId, &msg, uasId, 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0); + QLOG_DEBUG() << "SENT COMMAND" << MAV_CMD_PREFLIGHT_STORAGE; + sendMessage(msg); +} + +bool UAS::uploadParametersViaMavftp(const QMap*>& changedParameters) +{ + if (getAutopilotType() != MAV_AUTOPILOT_ARDUPILOTMEGA || !mavftpManager || mavftpManager->isBusy()) { + return false; + } + + const QMap* primaryParams = changedParameters.value(MAV_COMP_ID_PRIMARY, nullptr); + if (!primaryParams || primaryParams->isEmpty()) { + return false; + } + if (primaryParams->count() > std::numeric_limits::max()) { + return false; + } + + foreach (int component, changedParameters.keys()) { + const QMap* componentParams = changedParameters.value(component); + if (component != MAV_COMP_ID_PRIMARY && componentParams && !componentParams->isEmpty()) { + return false; + } + } + + QByteArray data; + QString encodeError; + if (!MAVFTPFileFormats::encodeParameterUploadFile(*primaryParams, &data, &encodeError)) { + QLOG_WARN() << "MAVFTP parameter upload file encode failed:" << encodeError; + return false; + } + + return mavftpManager->uploadFile(MAVFTPFileFormats::parameterUploadPath(), data, MAV_COMP_ID_PRIMARY); +} + +void UAS::readParametersFromStorage() +{ + mavlink_message_t msg; + mavlink_msg_command_long_pack(systemId, componentId, &msg, uasId, 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 0, -1, -1, -1, 0, 0, 0); sendMessage(msg); } @@ -2825,13 +2887,66 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, break; default: QLOG_ERROR() << "INVALID DATA TYPE USED AS PARAMETER VALUE: " << rawValue.param_type; - } //switch (value.param_type) -} - -/** -* Request parameter, use parameter name to request it. -*/ -void UAS::requestParameter(int component, int id) + } //switch (value.param_type) +} + +void UAS::processMavftpParamValue(int compId, int paramCount, int paramIndex, const QString& paramName, const QVariant& param) +{ + if (!parameters.contains(compId)) { + parameters.insert(compId, new QMap()); + } + + if (parameters.value(compId)->contains(paramName)) { + parameters.value(compId)->remove(paramName); + } + + parameters.value(compId)->insert(paramName, param); + emit parameterChanged(uasId, compId, paramName, param); + emit parameterChanged(uasId, compId, paramCount, paramIndex, paramName, param); +} + +bool UAS::processMavftpParameterFile(const QByteArray& data, QString* errorString) +{ + QList parameters; + if (!MAVFTPFileFormats::parseParameterFile(data, ¶meters, errorString)) { + return false; + } + + for (int i = 0; i < parameters.count(); i++) { + const MAVFTPFileFormats::ParameterValue& parameter = parameters.at(i); + processMavftpParamValue(MAV_COMP_ID_PRIMARY, parameters.count(), i, parameter.name, parameter.value); + } + + QLOG_DEBUG() << "Loaded" << parameters.count() << "parameters from MAVFTP packed parameter file"; + return true; +} + +void UAS::mavftpParameterDownloadComplete(const QByteArray& data, const QString& errorString) +{ + if (!errorString.isEmpty()) { + QLOG_WARN() << "MAVFTP parameter download failed, falling back to PARAM_REQUEST_LIST:" << errorString; + requestParametersViaMavlink(); + return; + } + + QString parseError; + if (!processMavftpParameterFile(data, &parseError)) { + QLOG_WARN() << "MAVFTP parameter parse failed, falling back to PARAM_REQUEST_LIST:" << parseError; + requestParametersViaMavlink(); + } +} + +void UAS::mavftpFileUploadComplete(const QString& remotePath, const QString& errorString) +{ + if (remotePath == MAVFTPFileFormats::parameterUploadPath()) { + emit mavftpParameterUploadComplete(errorString); + } +} + +/** +* Request parameter, use parameter name to request it. +*/ +void UAS::requestParameter(int component, int id) { // Request parameter, use parameter name to request it mavlink_message_t msg; diff --git a/src/uas/UAS.h b/src/uas/UAS.h index dc45377783..fd204b77ca 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -34,10 +34,13 @@ This file is part of the QGROUNDCONTROL project #include "UASInterface.h" #include "QGCHilLink.h" -#include - -#include - +#include + +#include +#include + +class MAVFTPManager; + /** * @brief A generic MAVLINK-connected MAV/UAV * @@ -414,12 +417,17 @@ class UAS : public UASInterface friend class UASWaypointManager; - MAVLinkProtocol* getMavLinkProtocol() const - { - return p_protocol; - } - -protected: //COMMENTS FOR TEST UNIT + MAVLinkProtocol* getMavLinkProtocol() const + { + return p_protocol; + } + + MAVFTPManager* getMavftpManager() const + { + return mavftpManager; + } + +protected: //COMMENTS FOR TEST UNIT bool m_heartbeatsEnabled; /// LINK ID AND STATUS int uasId; ///< Unique system ID @@ -551,12 +559,13 @@ class UAS : public UASInterface bool blockHomePositionChanges; ///< Block changes to the home position bool receivedMode; ///< True if mode was retrieved from current conenction to UAS - /// PARAMETERS - QMap* > parameters; ///< All parameters - bool paramsOnceRequested; ///< If the parameter list has been read at least once - QGCUASParamManager* paramManager; ///< Parameter manager class - - /// SIMULATION + /// PARAMETERS + QMap* > parameters; ///< All parameters + bool paramsOnceRequested; ///< If the parameter list has been read at least once + QGCUASParamManager* paramManager; ///< Parameter manager class + MAVFTPManager* mavftpManager; ///< MAVLink FTP helper for ArduPilot virtual file access + + /// SIMULATION QGCHilLink* simulation; ///< Hardware in the loop simulation link MAVLinkProtocol* p_protocol = nullptr; @@ -941,13 +950,15 @@ public slots: /** @brief Request a single parameter by index */ void requestParameter(int component, int id); - /** @brief Set a system parameter */ - void setParameter(const int compId, const QString& paramId, const QVariant& value); - - /** @brief Write parameters to permanent storage */ - void writeParametersToStorage(); - /** @brief Read parameters from permanent storage */ - void readParametersFromStorage(); + /** @brief Set a system parameter */ + void setParameter(const int compId, const QString& paramId, const QVariant& value); + + /** @brief Write parameters to permanent storage */ + void writeParametersToStorage(); + /** @brief Upload changed parameters through ArduPilot MAVFTP parameter file */ + bool uploadParametersViaMavftp(const QMap*>& changedParameters); + /** @brief Read parameters from permanent storage */ + void readParametersFromStorage(); /** @brief Get the names of all parameters */ QList getParameterNames(int component); @@ -1054,7 +1065,10 @@ public slots: /** @brief convert Joystick input ([-1.0, +1.0]) to RC PPM value ([1000, 2000]) for channel */ uint16_t scaleJoystickToRC(double pct, int channel) const; - virtual void processParamValueMsg(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue); + virtual void processParamValueMsg(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue); + void processMavftpParamValue(int compId, int paramCount, int paramIndex, const QString& paramName, const QVariant& param); + bool processMavftpParameterFile(const QByteArray& data, QString* errorString); + void requestParametersViaMavlink(); int componentID[256]; bool componentMulti[256]; @@ -1073,10 +1087,12 @@ public slots: QTimer m_parameterSendTimer; -protected slots: - void requestNextParamFromQueue(); - - /** @brief Write settings to disk */ +protected slots: + void requestNextParamFromQueue(); + void mavftpParameterDownloadComplete(const QByteArray& data, const QString& errorString); + void mavftpFileUploadComplete(const QString& remotePath, const QString& errorString); + + /** @brief Write settings to disk */ void writeSettings(); /** @brief Read settings from disk */ void readSettings(); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index fc480dd492..94aaf14ebe 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -36,7 +36,9 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include #include +#include #include "LinkInterface.h" #include "ProtocolInterface.h" @@ -358,6 +360,8 @@ public slots: virtual void requestParameter(int component, const QString& parameter) = 0; /** @brief Write parameter to permanent storage */ virtual void writeParametersToStorage() = 0; + /** @brief Upload changed parameters through ArduPilot MAVFTP parameter file */ + virtual bool uploadParametersViaMavftp(const QMap*>& changedParameters) = 0; /** @brief Read parameter from permanent storage */ virtual void readParametersFromStorage() = 0; /** @brief Set a system parameter @@ -547,6 +551,7 @@ public slots: void autoModeChanged(bool autoMode); void parameterChanged(int uas, int component, QString parameterName, QVariant value); void parameterChanged(int uas, int component, int parameterCount, int parameterId, QString parameterName, QVariant value); + void mavftpParameterUploadComplete(QString errorString); void patternDetected(int uasId, QString patternPath, float confidence, bool detected); void letterDetected(int uasId, QString letter, float confidence, bool detected); /** diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 17a77fac08..b4edc3cf7a 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -31,10 +31,14 @@ This file is part of the QGROUNDCONTROL project #include "logging.h" #include "UASWaypointManager.h" +#include "MAVFTPFileFormats.h" +#include "MAVFTPManager.h" #include "UAS.h" #include "configuration.h" #include "MainWindow.h" +#include + #define PROTOCOL_TIMEOUT_MS 2000 ///< maximum time to wait for pending messages until timeout #define PROTOCOL_DELAY_MS 20 ///< minimum delay between sent messages #define PROTOCOL_MAX_RETRIES 5 ///< maximum number of send retries (after timeout) @@ -56,7 +60,10 @@ UASWaypointManager::UASWaypointManager(UAS* _uas) uasid(0), m_defaultAcceptanceRadius(5.0), m_defaultRelativeAlt(0.0), - waypointIDHandled(65534) // nobody will have a waypoint list with 65534 waypoints. + waypointIDHandled(65534), // nobody will have a waypoint list with 65534 waypoints. + mavftpReadActive(false), + mavftpWriteActive(false), + mavftpReadToEdit(false) { if (uas) { @@ -849,6 +856,15 @@ int UASWaypointManager::getMissionFrameIndexOf(Waypoint* wp) * @param readToEdit If true, incoming waypoints will be copied both to "edit"-tab and "view"-tab. Otherwise, only to "view"-tab. */ void UASWaypointManager::readWaypoints(bool readToEdit) +{ + if (tryReadWaypointsViaMavftp(readToEdit)) { + return; + } + + readWaypointsViaMavlink(readToEdit); +} + +void UASWaypointManager::readWaypointsViaMavlink(bool readToEdit) { read_to_edit = readToEdit; emit readGlobalWPFromUAS(true); @@ -882,6 +898,219 @@ void UASWaypointManager::readWaypoints(bool readToEdit) } } + +bool UASWaypointManager::tryReadWaypointsViaMavftp(bool readToEdit) +{ + if (!uas || uas->getAutopilotType() != MAV_AUTOPILOT_ARDUPILOTMEGA || current_state != WP_IDLE) { + return false; + } + + MAVFTPManager* ftp = uas->getMavftpManager(); + if (!ftp || ftp->isBusy()) { + return false; + } + + read_to_edit = readToEdit; + mavftpReadActive = true; + mavftpReadToEdit = readToEdit; + current_state = WP_GETLIST; + current_wp_id = 0; + current_count = 0; + current_partner_systemid = uasid; + current_partner_compid = MAV_COMP_ID_PRIMARY; + + emit readGlobalWPFromUAS(true); + qDeleteAll(waypointsViewOnly); + waypointsViewOnly.clear(); + emit waypointViewOnlyListChanged(); + emit updateStatusString(QStringLiteral("Requesting waypoint list via MAVFTP...")); + + if (!ftp->downloadFile(MAVFTPFileFormats::missionPath(), MAV_COMP_ID_PRIMARY)) { + mavftpReadActive = false; + current_state = WP_IDLE; + current_partner_systemid = 0; + current_partner_compid = MAV_COMP_ID_PRIMARY; + emit readGlobalWPFromUAS(false); + return false; + } + + return true; +} + +bool UASWaypointManager::tryWriteWaypointsViaMavftp() +{ + if (!uas || uas->getAutopilotType() != MAV_AUTOPILOT_ARDUPILOTMEGA || current_state != WP_IDLE) { + return false; + } + + MAVFTPManager* ftp = uas->getMavftpManager(); + if (!ftp || ftp->isBusy()) { + return false; + } + + const QByteArray missionData = buildMavftpMissionData(); + mavftpWriteActive = true; + current_count = waypointsEditable.count(); + current_state = WP_SENDLIST; + current_wp_id = 0; + current_partner_systemid = uasid; + current_partner_compid = MAV_COMP_ID_PRIMARY; + + emit updateStatusString(QStringLiteral("Uploading waypoint list via MAVFTP...")); + if (!ftp->uploadFile(MAVFTPFileFormats::missionPath(), missionData, MAV_COMP_ID_PRIMARY)) { + mavftpWriteActive = false; + current_state = WP_IDLE; + current_count = 0; + current_partner_systemid = 0; + current_partner_compid = MAV_COMP_ID_PRIMARY; + return false; + } + + return true; +} + +void UASWaypointManager::mavftpMissionDownloadComplete(const QString& remotePath, const QByteArray& data, const QString& errorString) +{ + if (!mavftpReadActive || remotePath != MAVFTPFileFormats::missionPath()) { + return; + } + + const bool readToEdit = mavftpReadToEdit; + mavftpReadActive = false; + mavftpReadToEdit = false; + current_state = WP_IDLE; + current_count = 0; + current_wp_id = 0; + current_partner_systemid = 0; + current_partner_compid = MAV_COMP_ID_PRIMARY; + + if (!errorString.isEmpty()) { + QLOG_WARN() << "MAVFTP mission download failed, falling back to mission protocol:" << errorString; + readWaypointsViaMavlink(readToEdit); + return; + } + + QString parseError; + if (!loadMissionFromMavftpData(data, readToEdit, &parseError)) { + QLOG_WARN() << "MAVFTP mission parse failed, falling back to mission protocol:" << parseError; + readWaypointsViaMavlink(readToEdit); + return; + } + + waypointIDHandled = 65534; + emit readGlobalWPFromUAS(false); + + const QTime time = QTime::currentTime(); + emit updateStatusString(tr("done. (updated at %1)").arg(time.toString())); + QLOG_DEBUG() << "Loaded" << waypointsViewOnly.count() << "mission items from MAVFTP mission file"; +} + +void UASWaypointManager::mavftpMissionUploadComplete(const QString& remotePath, const QString& errorString) +{ + if (!mavftpWriteActive || remotePath != MAVFTPFileFormats::missionPath()) { + return; + } + + mavftpWriteActive = false; + current_state = WP_IDLE; + current_count = 0; + current_wp_id = 0; + current_partner_systemid = 0; + current_partner_compid = MAV_COMP_ID_PRIMARY; + + if (!errorString.isEmpty()) { + QLOG_WARN() << "MAVFTP mission upload failed, falling back to mission protocol:" << errorString; + writeWaypointsViaMavlink(); + return; + } + + emit updateStatusString(QStringLiteral("done.")); + readWaypoints(false); +} + +bool UASWaypointManager::loadMissionFromMavftpData(const QByteArray& data, bool readToEdit, QString* errorString) +{ + QList items; + if (!MAVFTPFileFormats::parseMissionFile(data, &items, errorString)) { + return false; + } + + qDeleteAll(waypointsViewOnly); + waypointsViewOnly.clear(); + emit waypointViewOnlyListChanged(); + + if (readToEdit) { + qDeleteAll(waypointsEditable); + waypointsEditable.clear(); + currentWaypointEditable = NULL; + emit waypointEditableListChanged(); + } + + for (int i = 0; i < items.count(); i++) { + const mavlink_mission_item_int_t item = items.at(i); + const double wp_x = item.x / static_cast(1E7); + const double wp_y = item.y / static_cast(1E7); + + Waypoint* viewWaypoint = new Waypoint(item.seq, wp_x, wp_y, item.z, item.param1, item.param2, item.param3, item.param4, + item.autocontinue, item.current, static_cast(item.frame), static_cast(item.command)); + addWaypointViewOnly(viewWaypoint); + + if (readToEdit) { + Waypoint* editableWaypoint = new Waypoint(item.seq, wp_x, wp_y, item.z, item.param1, item.param2, item.param3, item.param4, + item.autocontinue, item.current, static_cast(item.frame), static_cast(item.command)); + addWaypointEditable(editableWaypoint, false); + if (item.current == 1) { + currentWaypointEditable = editableWaypoint; + } + } + } + + return true; +} + +QByteArray UASWaypointManager::buildMavftpMissionData() const +{ + QList items; + bool noCurrent = true; + for (int i = 0; i < waypointsEditable.count(); i++) { + const Waypoint* waypoint = waypointsEditable.at(i); + bool current = waypoint->getCurrent() && noCurrent; + if (waypoint->getCurrent() && noCurrent) { + noCurrent = false; + } + if (i == waypointsEditable.count() - 1 && noCurrent) { + current = true; + } + + const mavlink_mission_item_int_t item = waypointToMissionItem(waypoint, static_cast(i), current); + items.append(item); + } + + return MAVFTPFileFormats::encodeMissionFile(items); +} + +mavlink_mission_item_int_t UASWaypointManager::waypointToMissionItem(const Waypoint* waypoint, quint16 seq, bool current) const +{ + mavlink_mission_item_int_t item; + memset(&item, 0, sizeof(item)); + item.target_system = uasid; + item.target_component = m_waypointComponentID; + item.seq = seq; + item.frame = waypoint->getFrame(); + item.command = waypoint->getAction(); + item.current = current ? 1 : 0; + item.autocontinue = waypoint->getAutoContinue(); + item.param1 = waypoint->getParam1(); + item.param2 = waypoint->getParam2(); + item.param3 = waypoint->getParam3(); + item.param4 = waypoint->getParam4(); + item.x = static_cast(waypoint->getX() * 1E7); + item.y = static_cast(waypoint->getY() * 1E7); + item.z = waypoint->getZ(); + item.mission_type = MAV_MISSION_TYPE_MISSION; + return item; +} + bool UASWaypointManager::guidedModeSupported() { return (uas->getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA); @@ -925,6 +1154,15 @@ void UASWaypointManager::goToWaypoint(Waypoint *wp) // change mavlink_mission_item_t to mavlink_mission_item_int_t void UASWaypointManager::writeWaypoints() +{ + if (tryWriteWaypointsViaMavftp()) { + return; + } + + writeWaypointsViaMavlink(); +} + +void UASWaypointManager::writeWaypointsViaMavlink() { if (current_state == WP_IDLE) { // Send clear all if count == 0 diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index 3182083edc..6a145d2f46 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -32,6 +32,7 @@ This file is part of the QGROUNDCONTROL project #ifndef UASWAYPOINTMANAGER_H #define UASWAYPOINTMANAGER_H +#include #include #include #include @@ -130,6 +131,13 @@ class UASWaypointManager : public QObject private: void convertMavlinkMissionItem(mavlink_mission_item_int_t *from, mavlink_mission_item_t *to); void handleWaypointRequest(quint8 systemId, quint8 compId, quint16 wpRequestId, MissionItemEncoding wpEncoding); ///< Handles received waypoint request messages (int and float) + void readWaypointsViaMavlink(bool readToEdit); + void writeWaypointsViaMavlink(); + bool tryReadWaypointsViaMavftp(bool readToEdit); + bool tryWriteWaypointsViaMavftp(); + bool loadMissionFromMavftpData(const QByteArray& data, bool readToEdit, QString* errorString); + QByteArray buildMavftpMissionData() const; + mavlink_mission_item_int_t waypointToMissionItem(const Waypoint* waypoint, quint16 seq, bool current) const; /** @name Message send functions */ /*@{*/ @@ -163,6 +171,8 @@ public slots: void handleGlobalPositionChanged(UASInterface* mav, double lat, double lon, double alt, quint64 time); void setDefaultRelAltitude(double alt); + void mavftpMissionDownloadComplete(const QString& remotePath, const QByteArray& data, const QString& errorString); + void mavftpMissionUploadComplete(const QString& remotePath, const QString& errorString); signals: void waypointEditableListChanged(void); ///< emits signal that the list of editable waypoints has been changed @@ -203,6 +213,9 @@ public slots: double m_defaultRelativeAlt; ///< Default relative alt in meters quint16 waypointIDHandled; + bool mavftpReadActive; + bool mavftpWriteActive; + bool mavftpReadToEdit; }; #endif // UASWAYPOINTMANAGER_H diff --git a/src/ui/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc index 6e3a704bd6..88bcf841cb 100644 --- a/src/ui/QGCParamWidget.cc +++ b/src/ui/QGCParamWidget.cc @@ -143,6 +143,7 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) : // New parameters from UAS connect(uas, SIGNAL(parameterChanged(int,int,int,int,QString,QVariant)), this, SLOT(addParameter(int,int,int,int,QString,QVariant))); + connect(uas, SIGNAL(mavftpParameterUploadComplete(QString)), this, SLOT(mavftpParameterUploadComplete(QString))); // Connect retransmission guard connect(this, SIGNAL(requestParameter(int,QString)), uas, SLOT(requestParameter(int,QString))); @@ -1271,6 +1272,30 @@ void QGCParamWidget::writeParameters() if (changedParamCount > 0) { + QMap*> typedChangedValues; + for (i = changedValues.begin(); i != changedValues.end(); ++i) { + QMap* typedComponentValues = new QMap(); + typedChangedValues.insert(i.key(), typedComponentValues); + + QMap* sourceValues = i.value(); + QMap* currentValues = parameters.value(i.key(), NULL); + QMap::iterator j; + for (j = sourceValues->begin(); j != sourceValues->end(); ++j) { + typedComponentValues->insert(j.key(), currentValues ? currentValues->value(j.key(), j.value()) : j.value()); + } + } + + const bool mavftpUploadStarted = mav && mav->uploadParametersViaMavftp(typedChangedValues); + qDeleteAll(typedChangedValues); + + if (mavftpUploadStarted) { + QPalette pal = statusLabel->palette(); + pal.setColor(backgroundRole(), QGC::colorOrange); + statusLabel->setPalette(pal); + statusLabel->setText(tr("Uploading %1 parameters via MAVFTP...").arg(changedParamCount)); + return; + } + QMessageBox msgBox; msgBox.setText(tr("There are locally changed parameters. Please transmit them first () or update them with the onboard values () before storing onboard from RAM to ROM.")); msgBox.exec(); @@ -1288,6 +1313,32 @@ void QGCParamWidget::readParameters() mav->readParametersFromStorage(); } +void QGCParamWidget::mavftpParameterUploadComplete(QString errorString) +{ + if (!errorString.isEmpty()) { + QPalette pal = statusLabel->palette(); + pal.setColor(backgroundRole(), QGC::colorRed); + statusLabel->setPalette(pal); + statusLabel->setText(tr("MAVFTP parameter upload failed: %1").arg(errorString)); + return; + } + + foreach (int component, changedValues.keys()) { + changedValues.value(component)->clear(); + } + foreach (int component, transmissionMissingWriteAckPackets.keys()) { + transmissionMissingWriteAckPackets.value(component)->clear(); + } + transmissionActive = false; + transmissionListMode = false; + + QPalette pal = statusLabel->palette(); + pal.setColor(backgroundRole(), QGC::colorGreen); + statusLabel->setPalette(pal); + statusLabel->setText(tr("Uploaded parameters via MAVFTP. Refreshing...")); + requestParameterList(); +} + /** * Clear all data in the parameter widget */ diff --git a/src/ui/QGCParamWidget.h b/src/ui/QGCParamWidget.h index abd2e79f43..53803a0c00 100644 --- a/src/ui/QGCParamWidget.h +++ b/src/ui/QGCParamWidget.h @@ -91,6 +91,8 @@ public slots: void writeParameters(); /** @brief Read the parameters from permanent storage to RAM */ void readParameters(); + /** @brief Handle completion of a MAVFTP parameter upload */ + void mavftpParameterUploadComplete(QString errorString); /** @brief Clear the parameter list */ void clear(); /** @brief Update when user changes parameters */ diff --git a/tests/mavftp/MAVFTPUnitTest.cc b/tests/mavftp/MAVFTPUnitTest.cc new file mode 100644 index 0000000000..15403deeeb --- /dev/null +++ b/tests/mavftp/MAVFTPUnitTest.cc @@ -0,0 +1,410 @@ +#include "MAVFTPFileFormats.h" +#include "MAVFTPProtocol.h" + +#include + +#include +#include + +class MAVFTPUnitTest : public QObject +{ + Q_OBJECT + +private slots: + void parsePackedParameterFileWithDefaults(); + void rejectMalformedParameterFiles(); + void encodeParameterUploadFile(); + void rejectInvalidParameterUploads(); + void encodeAndParseMissionFile(); + void rejectMalformedMissionFiles(); + void encodeAndDecodeFtpPayloads(); + void rejectInvalidFtpPayloads(); + +private: + static void appendUInt16(QByteArray* data, quint16 value); + static void writeUInt16(QByteArray* data, int offset, quint16 value); + static void appendUInt32(QByteArray* data, quint32 value); + static void appendFloat(QByteArray* data, float value); + static void appendParamRecord(QByteArray* data, int type, bool hasDefault, int commonLength, + const QByteArray& suffix, const QByteArray& value, + const QByteArray& defaultValue = QByteArray()); + static QByteArray int8Value(qint8 value); + static QByteArray int16Value(qint16 value); + static QByteArray int32Value(qint32 value); + static QByteArray floatValue(float value); + static void compareFloat(float actual, float expected); + static void compareItem(const mavlink_mission_item_int_t& actual, const mavlink_mission_item_int_t& expected); +}; + +void MAVFTPUnitTest::appendUInt16(QByteArray* data, quint16 value) +{ + data->append(static_cast(value & 0xff)); + data->append(static_cast((value >> 8) & 0xff)); +} + +void MAVFTPUnitTest::writeUInt16(QByteArray* data, int offset, quint16 value) +{ + (*data)[offset] = static_cast(value & 0xff); + (*data)[offset + 1] = static_cast((value >> 8) & 0xff); +} + +void MAVFTPUnitTest::appendUInt32(QByteArray* data, quint32 value) +{ + data->append(static_cast(value & 0xff)); + data->append(static_cast((value >> 8) & 0xff)); + data->append(static_cast((value >> 16) & 0xff)); + data->append(static_cast((value >> 24) & 0xff)); +} + +void MAVFTPUnitTest::appendFloat(QByteArray* data, float value) +{ + quint32 raw = 0; + memcpy(&raw, &value, sizeof(raw)); + appendUInt32(data, raw); +} + +void MAVFTPUnitTest::appendParamRecord(QByteArray* data, int type, bool hasDefault, int commonLength, + const QByteArray& suffix, const QByteArray& value, + const QByteArray& defaultValue) +{ + data->append(static_cast(type | (hasDefault ? 0x10 : 0))); + data->append(static_cast(commonLength | ((suffix.size() - 1) << 4))); + data->append(suffix); + data->append(value); + if (hasDefault) { + data->append(defaultValue); + } +} + +QByteArray MAVFTPUnitTest::int8Value(qint8 value) +{ + QByteArray data; + data.append(static_cast(value)); + return data; +} + +QByteArray MAVFTPUnitTest::int16Value(qint16 value) +{ + QByteArray data; + appendUInt16(&data, static_cast(value)); + return data; +} + +QByteArray MAVFTPUnitTest::int32Value(qint32 value) +{ + QByteArray data; + appendUInt32(&data, static_cast(value)); + return data; +} + +QByteArray MAVFTPUnitTest::floatValue(float value) +{ + QByteArray data; + appendFloat(&data, value); + return data; +} + +void MAVFTPUnitTest::compareFloat(float actual, float expected) +{ + QVERIFY2(std::fabs(actual - expected) < 0.0001f, qPrintable(QStringLiteral("actual=%1 expected=%2").arg(actual).arg(expected))); +} + +void MAVFTPUnitTest::compareItem(const mavlink_mission_item_int_t& actual, const mavlink_mission_item_int_t& expected) +{ + compareFloat(actual.param1, expected.param1); + compareFloat(actual.param2, expected.param2); + compareFloat(actual.param3, expected.param3); + compareFloat(actual.param4, expected.param4); + QCOMPARE(actual.x, expected.x); + QCOMPARE(actual.y, expected.y); + compareFloat(actual.z, expected.z); + QCOMPARE(actual.seq, expected.seq); + QCOMPARE(actual.command, expected.command); + QCOMPARE(actual.target_system, expected.target_system); + QCOMPARE(actual.target_component, expected.target_component); + QCOMPARE(actual.frame, expected.frame); + QCOMPARE(actual.current, expected.current); + QCOMPARE(actual.autocontinue, expected.autocontinue); + QCOMPARE(actual.mission_type, expected.mission_type); +} + +void MAVFTPUnitTest::parsePackedParameterFileWithDefaults() +{ + QByteArray data; + appendUInt16(&data, 0x671c); + appendUInt16(&data, 4); + appendUInt16(&data, 4); + appendParamRecord(&data, 1, true, 0, "ARMED", int8Value(-3), int8Value(0)); + data.append('\0'); + data.append('\0'); + appendParamRecord(&data, 2, false, 3, "ING_CHECK", int16Value(1234)); + appendParamRecord(&data, 3, false, 0, "BATT_CAPACITY", int32Value(-4500)); + appendParamRecord(&data, 4, true, 5, "FS_VOLTS", floatValue(10.5f), floatValue(10.0f)); + + QList parameters; + QString error; + QVERIFY2(MAVFTPFileFormats::parseParameterFile(data, ¶meters, &error), qPrintable(error)); + QCOMPARE(parameters.count(), 4); + + QCOMPARE(parameters.at(0).name, QStringLiteral("ARMED")); + QCOMPARE(parameters.at(0).value.toInt(), -3); + QVERIFY(parameters.at(0).hasDefault); + QCOMPARE(parameters.at(0).defaultValue.toInt(), 0); + + QCOMPARE(parameters.at(1).name, QStringLiteral("ARMING_CHECK")); + QCOMPARE(parameters.at(1).value.toInt(), 1234); + QVERIFY(!parameters.at(1).hasDefault); + + QCOMPARE(parameters.at(2).name, QStringLiteral("BATT_CAPACITY")); + QCOMPARE(parameters.at(2).value.toInt(), -4500); + + QCOMPARE(parameters.at(3).name, QStringLiteral("BATT_FS_VOLTS")); + compareFloat(parameters.at(3).value.toFloat(), 10.5f); + QVERIFY(parameters.at(3).hasDefault); + compareFloat(parameters.at(3).defaultValue.toFloat(), 10.0f); +} + +void MAVFTPUnitTest::rejectMalformedParameterFiles() +{ + QList parameters; + QString error; + QVERIFY(!MAVFTPFileFormats::parseParameterFile(QByteArray("\x1b\x67", 2), ¶meters, &error)); + QVERIFY(error.contains(QStringLiteral("too small"))); + + QByteArray partial; + appendUInt16(&partial, 0x671b); + appendUInt16(&partial, 1); + appendUInt16(&partial, 2); + QVERIFY(!MAVFTPFileFormats::parseParameterFile(partial, ¶meters, &error)); + QVERIFY(error.contains(QStringLiteral("partial"))); + + QByteArray badPrefix; + appendUInt16(&badPrefix, 0x671b); + appendUInt16(&badPrefix, 1); + appendUInt16(&badPrefix, 1); + appendParamRecord(&badPrefix, 3, false, 7, "BAD", int32Value(1)); + QVERIFY(!MAVFTPFileFormats::parseParameterFile(badPrefix, ¶meters, &error)); + QVERIFY(error.contains(QStringLiteral("prefix"))); + + QByteArray truncatedDefault; + appendUInt16(&truncatedDefault, 0x671c); + appendUInt16(&truncatedDefault, 1); + appendUInt16(&truncatedDefault, 1); + appendParamRecord(&truncatedDefault, 4, true, 0, "FLOAT_PARAM", floatValue(1.0f), QByteArray("\x00\x00", 2)); + QVERIFY(!MAVFTPFileFormats::parseParameterFile(truncatedDefault, ¶meters, &error)); + QVERIFY(error.contains(QStringLiteral("default"))); +} + +void MAVFTPUnitTest::encodeParameterUploadFile() +{ + QMap values; + values.insert(QStringLiteral("ATC_RAT_RLL_I"), QVariant(0.12)); + values.insert(QStringLiteral("ATC_RAT_RLL_P"), QVariant(0.135)); + values.insert(QStringLiteral("LOG_BITMASK"), QVariant(131071)); + values.insert(QStringLiteral("SYSID_THISMAV"), QVariant(QChar(7))); + + QByteArray encoded; + QString error; + QVERIFY2(MAVFTPFileFormats::encodeParameterUploadFile(values, &encoded, &error), qPrintable(error)); + + QByteArray expected; + appendUInt16(&expected, 0x671b); + appendUInt16(&expected, 4); + appendUInt16(&expected, 0); + appendParamRecord(&expected, 4, false, 0, "ATC_RAT_RLL_I", floatValue(0.12f)); + appendParamRecord(&expected, 4, false, 12, "P", floatValue(0.135f)); + appendParamRecord(&expected, 3, false, 0, "LOG_BITMASK", int32Value(131071)); + appendParamRecord(&expected, 1, false, 0, "SYSID_THISMAV", int8Value(7)); + writeUInt16(&expected, 4, static_cast(expected.size())); + + QCOMPARE(encoded, expected); +} + +void MAVFTPUnitTest::rejectInvalidParameterUploads() +{ + QByteArray encoded; + QString error; + + QMap badName; + badName.insert(QStringLiteral("PARAM_NAME_TOO_LONG"), QVariant(1)); + QVERIFY(!MAVFTPFileFormats::encodeParameterUploadFile(badName, &encoded, &error)); + QVERIFY(error.contains(QStringLiteral("invalid"))); + + QMap badType; + badType.insert(QStringLiteral("BIG_UNSIGNED"), QVariant(static_cast(0xffffffffU))); + QVERIFY(!MAVFTPFileFormats::encodeParameterUploadFile(badType, &encoded, &error)); + QVERIFY(error.contains(QStringLiteral("unsupported"))); +} + +void MAVFTPUnitTest::encodeAndParseMissionFile() +{ + QList items; + + mavlink_mission_item_int_t takeoff; + memset(&takeoff, 0, sizeof(takeoff)); + takeoff.param1 = 15.0f; + takeoff.param2 = 0.5f; + takeoff.param3 = 1.5f; + takeoff.param4 = 90.0f; + takeoff.x = 473977420; + takeoff.y = -1220840000; + takeoff.z = 45.5f; + takeoff.seq = 0; + takeoff.command = MAV_CMD_NAV_TAKEOFF; + takeoff.target_system = 1; + takeoff.target_component = MAV_COMP_ID_MISSIONPLANNER; + takeoff.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT; + takeoff.current = 1; + takeoff.autocontinue = 1; + takeoff.mission_type = MAV_MISSION_TYPE_MISSION; + items.append(takeoff); + + mavlink_mission_item_int_t waypoint; + memset(&waypoint, 0, sizeof(waypoint)); + waypoint.param1 = 1.0f; + waypoint.param2 = 2.0f; + waypoint.param3 = 3.0f; + waypoint.param4 = 4.0f; + waypoint.x = -353632619; + waypoint.y = 1491652370; + waypoint.z = 120.0f; + waypoint.seq = 1; + waypoint.command = MAV_CMD_NAV_WAYPOINT; + waypoint.target_system = 1; + waypoint.target_component = MAV_COMP_ID_MISSIONPLANNER; + waypoint.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT; + waypoint.current = 0; + waypoint.autocontinue = 1; + waypoint.mission_type = MAV_MISSION_TYPE_MISSION; + items.append(waypoint); + + const QByteArray data = MAVFTPFileFormats::encodeMissionFile(items); + QCOMPARE(data.size(), 10 + 2 * MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); + QCOMPARE(static_cast(data.at(0)), static_cast(0x3d)); + QCOMPARE(static_cast(data.at(1)), static_cast(0x76)); + QCOMPARE(static_cast(data.at(8)), static_cast(2)); + QCOMPARE(static_cast(data.at(9)), static_cast(0)); + + QList parsed; + QString error; + QVERIFY2(MAVFTPFileFormats::parseMissionFile(data, &parsed, &error), qPrintable(error)); + QCOMPARE(parsed.count(), 2); + compareItem(parsed.at(0), takeoff); + compareItem(parsed.at(1), waypoint); +} + +void MAVFTPUnitTest::rejectMalformedMissionFiles() +{ + QList parsed; + QString error; + QVERIFY(!MAVFTPFileFormats::parseMissionFile(QByteArray("\x3d", 1), &parsed, &error)); + QVERIFY(error.contains(QStringLiteral("too short"))); + + QByteArray badMagic; + appendUInt16(&badMagic, 0x1234); + appendUInt16(&badMagic, MAV_MISSION_TYPE_MISSION); + appendUInt16(&badMagic, 0); + appendUInt16(&badMagic, 0); + appendUInt16(&badMagic, 0); + QVERIFY(!MAVFTPFileFormats::parseMissionFile(badMagic, &parsed, &error)); + QVERIFY(error.contains(QStringLiteral("magic"))); + + QByteArray partialStart; + appendUInt16(&partialStart, 0x763d); + appendUInt16(&partialStart, MAV_MISSION_TYPE_MISSION); + appendUInt16(&partialStart, 0); + appendUInt16(&partialStart, 1); + appendUInt16(&partialStart, 0); + QVERIFY(!MAVFTPFileFormats::parseMissionFile(partialStart, &parsed, &error)); + QVERIFY(error.contains(QStringLiteral("partial"))); + + QByteArray truncated; + appendUInt16(&truncated, 0x763d); + appendUInt16(&truncated, MAV_MISSION_TYPE_MISSION); + appendUInt16(&truncated, 0); + appendUInt16(&truncated, 0); + appendUInt16(&truncated, 1); + truncated.append(QByteArray(8, '\0')); + QVERIFY(!MAVFTPFileFormats::parseMissionFile(truncated, &parsed, &error)); + QVERIFY(error.contains(QStringLiteral("expected"))); +} + +void MAVFTPUnitTest::encodeAndDecodeFtpPayloads() +{ + uint8_t payload[MAVFTPProtocol::PayloadLength]; + QString error; + QVERIFY2(MAVFTPProtocol::encodePayload(42, 7, MAVFTPProtocol::OpReadFile, + MAVFTPProtocol::MaxDataLength, 0x01020304, + QByteArray(), payload, sizeof(payload), &error), qPrintable(error)); + + QCOMPARE(static_cast(payload[0]), static_cast(42)); + QCOMPARE(static_cast(payload[1]), static_cast(0)); + QCOMPARE(static_cast(payload[2]), static_cast(7)); + QCOMPARE(static_cast(payload[3]), static_cast(MAVFTPProtocol::OpReadFile)); + QCOMPARE(static_cast(payload[4]), static_cast(MAVFTPProtocol::MaxDataLength)); + QCOMPARE(static_cast(payload[8]), static_cast(0x04)); + QCOMPARE(static_cast(payload[9]), static_cast(0x03)); + QCOMPARE(static_cast(payload[10]), static_cast(0x02)); + QCOMPARE(static_cast(payload[11]), static_cast(0x01)); + + const QByteArray writeData("abc"); + QVERIFY2(MAVFTPProtocol::encodePayload(44, 7, MAVFTPProtocol::OpWriteFile, + writeData.size(), 251, writeData, + payload, sizeof(payload), &error), qPrintable(error)); + MAVFTPProtocol::Packet packet; + QVERIFY2(MAVFTPProtocol::decodePayload(payload, sizeof(payload), &packet, &error), qPrintable(error)); + QCOMPARE(packet.opcode, static_cast(MAVFTPProtocol::OpWriteFile)); + QCOMPARE(packet.size, static_cast(3)); + QCOMPARE(packet.offset, static_cast(251)); + QCOMPARE(packet.data, writeData); + + memset(payload, 0, sizeof(payload)); + payload[0] = 45; + payload[2] = 7; + payload[3] = MAVFTPProtocol::OpAck; + payload[4] = 1; + payload[5] = MAVFTPProtocol::OpOpenFileRO; + payload[MAVFTPProtocol::HeaderLength] = 99; + QVERIFY2(MAVFTPProtocol::decodePayload(payload, sizeof(payload), &packet, &error), qPrintable(error)); + QCOMPARE(packet.sequence, static_cast(45)); + QCOMPARE(packet.opcode, static_cast(MAVFTPProtocol::OpAck)); + QCOMPARE(packet.requestOpcode, static_cast(MAVFTPProtocol::OpOpenFileRO)); + QCOMPARE(packet.data, QByteArray(1, static_cast(99))); + + memset(payload, 0, sizeof(payload)); + payload[3] = MAVFTPProtocol::OpNack; + payload[4] = 1; + payload[5] = MAVFTPProtocol::OpReadFile; + payload[MAVFTPProtocol::HeaderLength] = MAVFTPProtocol::ErrEndOfFile; + QVERIFY2(MAVFTPProtocol::decodePayload(payload, sizeof(payload), &packet, &error), qPrintable(error)); + QCOMPARE(MAVFTPProtocol::responseErrorCode(packet), static_cast(MAVFTPProtocol::ErrEndOfFile)); + QVERIFY(MAVFTPProtocol::errorString(MAVFTPProtocol::responseErrorCode(packet)).contains(QStringLiteral("end of file"))); + + QCOMPARE(MAVFTPProtocol::expectedResponseSequence(0xfffe), static_cast(0xffff)); + QCOMPARE(MAVFTPProtocol::expectedResponseSequence(0xffff), static_cast(0)); +} + +void MAVFTPUnitTest::rejectInvalidFtpPayloads() +{ + uint8_t payload[MAVFTPProtocol::PayloadLength]; + QString error; + const QByteArray tooLarge(MAVFTPProtocol::MaxDataLength + 1, 'x'); + QVERIFY(!MAVFTPProtocol::encodePayload(1, 0, MAVFTPProtocol::OpWriteFile, + tooLarge.size(), 0, tooLarge, + payload, sizeof(payload), &error)); + QVERIFY(error.contains(QStringLiteral("too large"))); + + QVERIFY(!MAVFTPProtocol::encodePayload(1, 0, MAVFTPProtocol::OpWriteFile, + 1, 0, QByteArray("ab", 2), + payload, sizeof(payload), &error)); + QVERIFY(error.contains(QStringLiteral("too large"))); + + memset(payload, 0, sizeof(payload)); + payload[4] = MAVFTPProtocol::MaxDataLength + 1; + MAVFTPProtocol::Packet packet; + QVERIFY(!MAVFTPProtocol::decodePayload(payload, sizeof(payload), &packet, &error)); + QVERIFY(error.contains(QStringLiteral("too large"))); +} + +QTEST_MAIN(MAVFTPUnitTest) +#include "MAVFTPUnitTest.moc"