From 663aa1b779b3937a38ba06d2cfcc31a594384760 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 5 May 2014 08:58:15 +0200 Subject: [PATCH 01/31] Added file manager and associated widgets / handling --- qgroundcontrol.pro | 14 ++++++-- src/uas/QGCUASFileManager.cc | 14 ++++++++ src/uas/QGCUASFileManager.h | 23 ++++++++++++++ src/uas/UAS.cc | 1 + src/uas/UAS.h | 7 ++++ src/uas/UASInterface.h | 9 ++++++ src/ui/MainWindow.cc | 2 ++ src/ui/MainWindow.h | 4 +++ src/ui/QGCUASFileView.cc | 18 +++++++++++ src/ui/QGCUASFileView.h | 26 +++++++++++++++ src/ui/QGCUASFileView.ui | 32 +++++++++++++++++++ src/ui/QGCUASFileViewMulti.cc | 74 +++++++++++++++++++++++++++++++++++++++++++ src/ui/QGCUASFileViewMulti.h | 36 +++++++++++++++++++++ src/ui/QGCUASFileViewMulti.ui | 27 ++++++++++++++++ 14 files changed, 284 insertions(+), 3 deletions(-) create mode 100644 src/uas/QGCUASFileManager.cc create mode 100644 src/uas/QGCUASFileManager.h create mode 100644 src/ui/QGCUASFileView.cc create mode 100644 src/ui/QGCUASFileView.h create mode 100644 src/ui/QGCUASFileView.ui create mode 100644 src/ui/QGCUASFileViewMulti.cc create mode 100644 src/ui/QGCUASFileViewMulti.h create mode 100644 src/ui/QGCUASFileViewMulti.ui diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 135006d..9e9072a 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -292,6 +292,7 @@ FORMS += \ src/ui/designer/QGCCommandButton.ui \ src/ui/QGCMAVLinkLogPlayer.ui \ src/ui/QGCWaypointListMulti.ui \ + src/ui/QGCUASFileViewMulti.ui \ src/ui/QGCUDPLinkConfiguration.ui \ src/ui/QGCTCPLinkConfiguration.ui \ src/ui/QGCSettingsWidget.ui \ @@ -369,7 +370,8 @@ FORMS += \ src/ui/px4_configuration/QGCPX4AirframeConfig.ui \ src/ui/px4_configuration/QGCPX4MulticopterConfig.ui \ src/ui/px4_configuration/QGCPX4SensorCalibration.ui \ - src/ui/designer/QGCXYPlot.ui + src/ui/designer/QGCXYPlot.ui \ + src/ui/QGCUASFileView.ui HEADERS += \ src/MG.h \ @@ -457,6 +459,7 @@ HEADERS += \ src/comm/MAVLinkSimulationMAV.h \ src/uas/QGCMAVLinkUASFactory.h \ src/ui/QGCWaypointListMulti.h \ + src/ui/QGCUASFileViewMulti.h \ src/ui/QGCUDPLinkConfiguration.h \ src/ui/QGCTCPLinkConfiguration.h \ src/ui/QGCSettingsWidget.h \ @@ -562,7 +565,9 @@ HEADERS += \ src/ui/designer/QGCXYPlot.h \ src/ui/menuactionhelper.h \ src/uas/UASManagerInterface.h \ - src/uas/QGCUASParamManagerInterface.h + src/uas/QGCUASParamManagerInterface.h \ + src/uas/QGCUASFileManager.h \ + src/ui/QGCUASFileView.h SOURCES += \ src/main.cc \ @@ -644,6 +649,7 @@ SOURCES += \ src/comm/MAVLinkSimulationMAV.cc \ src/uas/QGCMAVLinkUASFactory.cc \ src/ui/QGCWaypointListMulti.cc \ + src/ui/QGCUASFileViewMulti.cc \ src/ui/QGCUDPLinkConfiguration.cc \ src/ui/QGCTCPLinkConfiguration.cc \ src/ui/QGCSettingsWidget.cc \ @@ -745,4 +751,6 @@ SOURCES += \ src/ui/px4_configuration/QGCPX4MulticopterConfig.cc \ src/ui/px4_configuration/QGCPX4SensorCalibration.cc \ src/ui/designer/QGCXYPlot.cc \ - src/ui/menuactionhelper.cpp + src/ui/menuactionhelper.cpp \ + src/uas/QGCUASFileManager.cc \ + src/ui/QGCUASFileView.cc diff --git a/src/uas/QGCUASFileManager.cc b/src/uas/QGCUASFileManager.cc new file mode 100644 index 0000000..c900bfd --- /dev/null +++ b/src/uas/QGCUASFileManager.cc @@ -0,0 +1,14 @@ +#include "QGCUASFileManager.h" +#include "QGC.h" + +QGCUASFileManager::QGCUASFileManager(QObject* parent, UASInterface* uas) : + QObject(parent), + _mav(uas) +{ +} + +void QGCUASFileManager::nothingMessage() { + mavlink_message_t message; + + _mav->sendMessage(message); +} diff --git a/src/uas/QGCUASFileManager.h b/src/uas/QGCUASFileManager.h new file mode 100644 index 0000000..d1d1288 --- /dev/null +++ b/src/uas/QGCUASFileManager.h @@ -0,0 +1,23 @@ +#ifndef QGCUASFILEMANAGER_H +#define QGCUASFILEMANAGER_H + +#include +#include "UASInterface.h" + +class QGCUASFileManager : public QObject +{ + Q_OBJECT +public: + QGCUASFileManager(QObject* parent, UASInterface* uas); + +signals: + +public slots: + void nothingMessage(); + +protected: + UASInterface* _mav; + +}; + +#endif // QGCUASFILEMANAGER_H diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 018611c..43f3bc7 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -139,6 +139,7 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), airSpeed(std::numeric_limits::quiet_NaN()), groundSpeed(std::numeric_limits::quiet_NaN()), waypointManager(this), + fileManager(this, this), attitudeKnown(false), attitudeStamped(false), diff --git a/src/uas/UAS.h b/src/uas/UAS.h index ed90a1b..a7e0bc5 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -41,6 +41,7 @@ This file is part of the QGROUNDCONTROL project #include "QGCJSBSimLink.h" #include "QGCXPlaneLink.h" #include "QGCUASParamManager.h" +#include "QGCUASFileManager.h" /** @@ -369,6 +370,7 @@ public: #endif friend class UASWaypointManager; + friend class QGCUASFileManager; protected: //COMMENTS FOR TEST UNIT /// LINK ID AND STATUS @@ -471,6 +473,7 @@ protected: //COMMENTS FOR TEST UNIT double groundSpeed; ///< Groundspeed double bearingToWaypoint; ///< Bearing to next waypoint UASWaypointManager waypointManager; + QGCUASFileManager fileManager; /// ATTITUDE bool attitudeKnown; ///< True if attitude was received, false else @@ -552,6 +555,10 @@ public: return ¶mMgr; } + virtual QGCUASFileManager* getFileManager() { + return &fileManager; + } + /** @brief Get the HIL simulation */ QGCHilLink* getHILSimulation() const { return simulation; diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 6395674..64ac3d2 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -52,6 +52,8 @@ This file is part of the QGROUNDCONTROL project #endif #endif +class QGCUASFileManager; + enum BatteryType { NICD = 0, @@ -158,6 +160,13 @@ public: /** @brief Get reference to the param manager **/ virtual QGCUASParamManagerInterface* getParamManager() = 0; + virtual QGCUASFileManager* getFileManager() = 0; + + /** @brief Send a message over this link (to this or to all UAS on this link) */ + virtual void sendMessage(LinkInterface* link, mavlink_message_t message) = 0; + /** @brief Send a message over all links this UAS can be reached with (!= all links) */ + virtual void sendMessage(mavlink_message_t message) = 0; + /* COMMUNICATION FLAGS */ enum CommStatus { diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index f6b71c5..2d22446 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -71,6 +71,7 @@ This file is part of the QGROUNDCONTROL project #include "SerialSettingsDialog.h" #include "terminalconsole.h" #include "menuactionhelper.h" +#include "QGCUASFileViewMulti.h" // Add support for the MAVLink generator UI if it's been requested. #ifdef QGC_MAVGEN_ENABLED @@ -615,6 +616,7 @@ void MainWindow::buildCommonWidgets() createDockWidget(engineeringView,new QGCMAVLinkInspector(mavlink,this),tr("MAVLink Inspector"),"MAVLINK_INSPECTOR_DOCKWIDGET",VIEW_ENGINEER,Qt::RightDockWidgetArea); createDockWidget(engineeringView,new ParameterInterface(this),tr("Onboard Parameters"),"PARAMETER_INTERFACE_DOCKWIDGET",VIEW_ENGINEER,Qt::RightDockWidgetArea); + createDockWidget(engineeringView,new QGCUASFileViewMulti(this),tr("Onboard Files"),"FILE_VIEW_DOCKWIDGET",VIEW_ENGINEER,Qt::RightDockWidgetArea); createDockWidget(simView,new ParameterInterface(this),tr("Onboard Parameters"),"PARAMETER_INTERFACE_DOCKWIDGET",VIEW_SIMULATION,Qt::RightDockWidgetArea); menuActionHelper->createToolAction(tr("Status Details"), "UAS_STATUS_DETAILS_DOCKWIDGET"); diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 8e2ee96..6bebb48 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -76,6 +76,7 @@ This file is part of the QGROUNDCONTROL project #include "QGCMAVLinkLogPlayer.h" #include "QGCVehicleConfig.h" #include "MAVLinkDecoder.h" +#include "QGCUASFileViewMulti.h" class QGCMapTool; class QGCMAVLinkMessageSender; @@ -86,6 +87,7 @@ class Linecharts; class QGCDataPlot2D; class JoystickWidget; class MenuActionHelper; +class QGCUASFileViewMulti; /** * @brief Main Application Window @@ -463,6 +465,8 @@ protected: QGCMAVLinkLogPlayer* logPlayer; QMap hilDocks; + QPointer fileWidget; + // Popup widgets JoystickWidget* joystickWidget; diff --git a/src/ui/QGCUASFileView.cc b/src/ui/QGCUASFileView.cc new file mode 100644 index 0000000..4f9a4e9 --- /dev/null +++ b/src/ui/QGCUASFileView.cc @@ -0,0 +1,18 @@ +#include "QGCUASFileView.h" +#include "uas/QGCUASFileManager.h" +#include "ui_QGCUASFileView.h" + +QGCUASFileView::QGCUASFileView(QWidget *parent, QGCUASFileManager *manager) : + QWidget(parent), + _manager(manager), + ui(new Ui::QGCUASFileView) +{ + ui->setupUi(this); + + connect(ui->testButton, SIGNAL(clicked()), _manager, SLOT(nothingMessage())); +} + +QGCUASFileView::~QGCUASFileView() +{ + delete ui; +} diff --git a/src/ui/QGCUASFileView.h b/src/ui/QGCUASFileView.h new file mode 100644 index 0000000..92f179a --- /dev/null +++ b/src/ui/QGCUASFileView.h @@ -0,0 +1,26 @@ +#ifndef QGCUASFILEVIEW_H +#define QGCUASFILEVIEW_H + +#include +#include "uas/QGCUASFileManager.h" + +namespace Ui { +class QGCUASFileView; +} + +class QGCUASFileView : public QWidget +{ + Q_OBJECT + +public: + explicit QGCUASFileView(QWidget *parent, QGCUASFileManager *manager); + ~QGCUASFileView(); + +protected: + QGCUASFileManager* _manager; + +private: + Ui::QGCUASFileView *ui; +}; + +#endif // QGCUASFILEVIEW_H diff --git a/src/ui/QGCUASFileView.ui b/src/ui/QGCUASFileView.ui new file mode 100644 index 0000000..2304a78 --- /dev/null +++ b/src/ui/QGCUASFileView.ui @@ -0,0 +1,32 @@ + + + QGCUASFileView + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + 30 + 20 + 114 + 32 + + + + Test + + + + + + diff --git a/src/ui/QGCUASFileViewMulti.cc b/src/ui/QGCUASFileViewMulti.cc new file mode 100644 index 0000000..fb627ea --- /dev/null +++ b/src/ui/QGCUASFileViewMulti.cc @@ -0,0 +1,74 @@ +#include "QGCUASFileViewMulti.h" +#include "ui_QGCUASFileViewMulti.h" +#include "UASInterface.h" +#include "UASManager.h" +#include "QGCUASFileView.h" + +QGCUASFileViewMulti::QGCUASFileViewMulti(QWidget *parent) : + QWidget(parent), + ui(new Ui::QGCUASFileViewMulti) +{ + ui->setupUi(this); + setMinimumSize(600, 80); + connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(systemCreated(UASInterface*))); + connect(UASManager::instance(), SIGNAL(activeUASSet(int)), this, SLOT(systemSetActive(int))); + + if (UASManager::instance()->getActiveUAS()) { + systemCreated(UASManager::instance()->getActiveUAS()); + systemSetActive(UASManager::instance()->getActiveUAS()->getUASID()); + } + +} + +void QGCUASFileViewMulti::systemDeleted(QObject* uas) +{ + UASInterface* mav = dynamic_cast(uas); + if (mav) + { + int id = mav->getUASID(); + QGCUASFileView* list = lists.value(id, NULL); + if (list) + { + delete list; + lists.remove(id); + } + } +} + +void QGCUASFileViewMulti::systemCreated(UASInterface* uas) +{ + if (!uas) { + return; + } + + QGCUASFileView* list = new QGCUASFileView(ui->stackedWidget, uas->getFileManager()); + lists.insert(uas->getUASID(), list); + ui->stackedWidget->addWidget(list); + // Ensure widget is deleted when system is deleted + connect(uas, SIGNAL(destroyed(QObject*)), this, SLOT(systemDeleted(QObject*))); +} + +void QGCUASFileViewMulti::systemSetActive(int uas) +{ + QGCUASFileView* list = lists.value(uas, NULL); + if (list) { + ui->stackedWidget->setCurrentWidget(list); + } +} + +QGCUASFileViewMulti::~QGCUASFileViewMulti() +{ + delete ui; +} + +void QGCUASFileViewMulti::changeEvent(QEvent *e) +{ + QWidget::changeEvent(e); + switch (e->type()) { + case QEvent::LanguageChange: + ui->retranslateUi(this); + break; + default: + break; + } +} diff --git a/src/ui/QGCUASFileViewMulti.h b/src/ui/QGCUASFileViewMulti.h new file mode 100644 index 0000000..6b77613 --- /dev/null +++ b/src/ui/QGCUASFileViewMulti.h @@ -0,0 +1,36 @@ +#ifndef QGCUASFILEVIEWMULTI_H +#define QGCUASFILEVIEWMULTI_H + +#include +#include + +#include "QGCUASFileView.h" +#include "UASInterface.h" + +namespace Ui +{ +class QGCUASFileViewMulti; +} + +class QGCUASFileViewMulti : public QWidget +{ + Q_OBJECT + +public: + explicit QGCUASFileViewMulti(QWidget *parent = 0); + ~QGCUASFileViewMulti(); + +public slots: + void systemDeleted(QObject* uas); + void systemCreated(UASInterface* uas); + void systemSetActive(int uas); + +protected: + void changeEvent(QEvent *e); + QMap lists; + +private: + Ui::QGCUASFileViewMulti *ui; +}; + +#endif // QGCUASFILEVIEWMULTI_H diff --git a/src/ui/QGCUASFileViewMulti.ui b/src/ui/QGCUASFileViewMulti.ui new file mode 100644 index 0000000..169c00a --- /dev/null +++ b/src/ui/QGCUASFileViewMulti.ui @@ -0,0 +1,27 @@ + + + QGCUASFileViewMulti + + + + 0 + 0 + 400 + 300 + + + + Form + + + + 0 + + + + + + + + + From c1f0d7071bc029d4f0c378a781bd3bcb9340aabf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 5 May 2014 09:03:02 +0200 Subject: [PATCH 02/31] Add missing bits to unit test --- src/qgcunittest/MockUAS.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/qgcunittest/MockUAS.h b/src/qgcunittest/MockUAS.h index 8f6ad10..3c715c0 100644 --- a/src/qgcunittest/MockUAS.h +++ b/src/qgcunittest/MockUAS.h @@ -109,6 +109,12 @@ public: virtual bool systemCanReverse() const { Q_ASSERT(false); return false; }; virtual QString getSystemTypeName() { Q_ASSERT(false); return _bogusString; }; virtual int getAutopilotType() { Q_ASSERT(false); return 0; }; + virtual QGCUASFileManager* getFileManager() {Q_ASSERT(false); return NULL; } + + /** @brief Send a message over this link (to this or to all UAS on this link) */ + virtual void sendMessage(LinkInterface* link, mavlink_message_t message){Q_ASSERT(false);} + /** @brief Send a message over all links this UAS can be reached with (!= all links) */ + virtual void sendMessage(mavlink_message_t message) {Q_ASSERT(false);} virtual QString getAutopilotTypeName() { Q_ASSERT(false); return _bogusString; }; virtual void setAutopilotType(int apType) { Q_UNUSED(apType); Q_ASSERT(false); }; virtual QMap getComponents() { Q_ASSERT(false); return _bogusMapIntQString; }; From a7f039bf858019d0cef1337ec3132fb3d7a74203 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 5 May 2014 13:50:04 +0200 Subject: [PATCH 03/31] Operational toy examples --- src/uas/QGCMAVLinkUASFactory.cc | 1 + src/uas/QGCUASFileManager.cc | 102 +++++++++++++++++++++++++++++++++++++++- src/uas/QGCUASFileManager.h | 18 +++++++ src/ui/QGCUASFileView.cc | 22 +++++++++ src/ui/QGCUASFileView.h | 4 ++ src/ui/QGCUASFileView.ui | 62 ++++++++++++++++++------ 6 files changed, 192 insertions(+), 17 deletions(-) diff --git a/src/uas/QGCMAVLinkUASFactory.cc b/src/uas/QGCMAVLinkUASFactory.cc index 6b1b996..77860ae 100644 --- a/src/uas/QGCMAVLinkUASFactory.cc +++ b/src/uas/QGCMAVLinkUASFactory.cc @@ -30,6 +30,7 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte mav->setSystemType((int)heartbeat->type); // Connect this robot to the UAS object connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); + connect(mavlink, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), mav->getFileManager(), SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); #ifdef QGC_PROTOBUF_ENABLED connect(mavlink, SIGNAL(extendedMessageReceived(LinkInterface*, std::tr1::shared_ptr)), mav, SLOT(receiveExtendedMessage(LinkInterface*, std::tr1::shared_ptr))); #endif diff --git a/src/uas/QGCUASFileManager.cc b/src/uas/QGCUASFileManager.cc index c900bfd..6034117 100644 --- a/src/uas/QGCUASFileManager.cc +++ b/src/uas/QGCUASFileManager.cc @@ -1,14 +1,112 @@ #include "QGCUASFileManager.h" #include "QGC.h" +#include "MAVLinkProtocol.h" + +#include +#include + +static const quint32 crctab[] = +{ + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d +}; QGCUASFileManager::QGCUASFileManager(QObject* parent, UASInterface* uas) : QObject(parent), - _mav(uas) + _mav(uas), + _encdata_seq(0) +{ +} + +quint32 +QGCUASFileManager::crc32(const uint8_t *src, unsigned len, unsigned state) { + for (unsigned i = 0; i < len; i++) + state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8); + return state; } -void QGCUASFileManager::nothingMessage() { +void QGCUASFileManager::nothingMessage() +{ mavlink_message_t message; + unsigned crcstate = 0; + RequestHeader hdr; + hdr.magic = 9; + crcstate = crc32((uint8_t*)&hdr, 4, crcstate); + hdr.crc32 = crcstate; + + mavlink_msg_encapsulated_data_pack(250, 0, &message, _encdata_seq, (uint8_t*)&hdr); + _mav->sendMessage(message); + qDebug() << "Sent message!"; +} + +void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t message) +{ + switch (message.msgid) { + case MAVLINK_MSG_ID_ENCAPSULATED_DATA: + emit statusMessage("GOT ENC DATA"); + break; + } +} + +void QGCUASFileManager::listRecursively(const QString &from) +{ + // This sends out one line to the text area + emit statusMessage("/"); + emit statusMessage(" fs/"); + emit statusMessage(" microsd/"); + emit statusMessage(" log001.bin"); +} + +void QGCUASFileManager::downloadPath(const QString &from, const QString &to) +{ + + // Send path, e.g. /fs/microsd and download content + // recursively into a local directory + + char buf[255]; + unsigned len = 10; + + QByteArray data(buf, len); + QString filename = "log001.bin"; // XXX get this from onboard + + // Qt takes care of slash conversions in paths + QFile file(to + QDir::separator() + filename); + file.open(QIODevice::WriteOnly); + file.write(data); + file.close(); + + emit statusMessage(QString("Downloaded: %1 to directory %2").arg(filename).arg(to)); } diff --git a/src/uas/QGCUASFileManager.h b/src/uas/QGCUASFileManager.h index d1d1288..e046190 100644 --- a/src/uas/QGCUASFileManager.h +++ b/src/uas/QGCUASFileManager.h @@ -11,12 +11,30 @@ public: QGCUASFileManager(QObject* parent, UASInterface* uas); signals: + void statusMessage(const QString& msg); public slots: + void receiveMessage(LinkInterface* link, mavlink_message_t message); void nothingMessage(); + void listRecursively(const QString &from); + void downloadPath(const QString& from, const QString& to); protected: UASInterface* _mav; + quint16 _encdata_seq; + + struct RequestHeader + { + uint8_t magic; + uint8_t session; + uint8_t opcode; + uint8_t size; + uint32_t crc32; + uint32_t offset; + uint8_t data[]; + }; + + static quint32 crc32(const uint8_t *src, unsigned len, unsigned state); }; diff --git a/src/ui/QGCUASFileView.cc b/src/ui/QGCUASFileView.cc index 4f9a4e9..f3a73dd 100644 --- a/src/ui/QGCUASFileView.cc +++ b/src/ui/QGCUASFileView.cc @@ -2,6 +2,9 @@ #include "uas/QGCUASFileManager.h" #include "ui_QGCUASFileView.h" +#include +#include + QGCUASFileView::QGCUASFileView(QWidget *parent, QGCUASFileManager *manager) : QWidget(parent), _manager(manager), @@ -10,9 +13,28 @@ QGCUASFileView::QGCUASFileView(QWidget *parent, QGCUASFileManager *manager) : ui->setupUi(this); connect(ui->testButton, SIGNAL(clicked()), _manager, SLOT(nothingMessage())); + connect(ui->listFilesButton, SIGNAL(clicked()), this, SLOT(listFiles())); + connect(ui->downloadButton, SIGNAL(clicked()), this, SLOT(downloadFiles())); + + connect(_manager, SIGNAL(statusMessage(QString)), ui->messageArea, SLOT(appendPlainText(QString))); } QGCUASFileView::~QGCUASFileView() { delete ui; } + +void QGCUASFileView::listFiles() +{ + _manager->listRecursively(ui->pathLineEdit->text()); +} + +void QGCUASFileView::downloadFiles() +{ + QString dir = QFileDialog::getExistingDirectory(this, tr("Open Directory"), + QDir::homePath(), + QFileDialog::ShowDirsOnly + | QFileDialog::DontResolveSymlinks); + // And now download to this location + _manager->downloadPath(ui->pathLineEdit->text(), dir); +} diff --git a/src/ui/QGCUASFileView.h b/src/ui/QGCUASFileView.h index 92f179a..0bb3700 100644 --- a/src/ui/QGCUASFileView.h +++ b/src/ui/QGCUASFileView.h @@ -16,6 +16,10 @@ public: explicit QGCUASFileView(QWidget *parent, QGCUASFileManager *manager); ~QGCUASFileView(); +public slots: + void listFiles(); + void downloadFiles(); + protected: QGCUASFileManager* _manager; diff --git a/src/ui/QGCUASFileView.ui b/src/ui/QGCUASFileView.ui index 2304a78..1cbc196 100644 --- a/src/ui/QGCUASFileView.ui +++ b/src/ui/QGCUASFileView.ui @@ -6,26 +6,58 @@ 0 0 - 400 - 300 + 414 + 518 Form - - - - 30 - 20 - 114 - 32 - - - - Test - - + + + + + List Files + + + + + + + + + + + 1 + + + + + + + + Null Message + + + + + + + Download File + + + + + + + Path: + + + + + + + From c420ddfa55af2e452a79c91afdcbbb2b0df4eb9a Mon Sep 17 00:00:00 2001 From: none Date: Mon, 5 May 2014 23:20:58 -0700 Subject: [PATCH 04/31] Tinker with the NOP message so that the receiver is happy with it. --- src/uas/QGCUASFileManager.cc | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/uas/QGCUASFileManager.cc b/src/uas/QGCUASFileManager.cc index 6034117..9e59099 100644 --- a/src/uas/QGCUASFileManager.cc +++ b/src/uas/QGCUASFileManager.cc @@ -60,11 +60,12 @@ void QGCUASFileManager::nothingMessage() { mavlink_message_t message; - unsigned crcstate = 0; RequestHeader hdr; - hdr.magic = 9; - crcstate = crc32((uint8_t*)&hdr, 4, crcstate); - hdr.crc32 = crcstate; + hdr.magic = 'f'; + hdr.session = 0; + hdr.crc32 = 0; + hdr.size = 0; + hdr.crc32 = crc32((uint8_t*)&hdr, sizeof(hdr) + hdr.size, 0); mavlink_msg_encapsulated_data_pack(250, 0, &message, _encdata_seq, (uint8_t*)&hdr); From e5ea25d800750f402388c779aac75cd26d7589ab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Jun 2014 20:34:47 +0200 Subject: [PATCH 05/31] Added support to clear status areas with --- src/uas/QGCUASFileManager.h | 1 + src/ui/QGCUASFileView.cc | 1 + 2 files changed, 2 insertions(+) diff --git a/src/uas/QGCUASFileManager.h b/src/uas/QGCUASFileManager.h index e046190..55edb1b 100644 --- a/src/uas/QGCUASFileManager.h +++ b/src/uas/QGCUASFileManager.h @@ -12,6 +12,7 @@ public: signals: void statusMessage(const QString& msg); + void resetStatusMessages(); public slots: void receiveMessage(LinkInterface* link, mavlink_message_t message); diff --git a/src/ui/QGCUASFileView.cc b/src/ui/QGCUASFileView.cc index f3a73dd..3d525cc 100644 --- a/src/ui/QGCUASFileView.cc +++ b/src/ui/QGCUASFileView.cc @@ -17,6 +17,7 @@ QGCUASFileView::QGCUASFileView(QWidget *parent, QGCUASFileManager *manager) : connect(ui->downloadButton, SIGNAL(clicked()), this, SLOT(downloadFiles())); connect(_manager, SIGNAL(statusMessage(QString)), ui->messageArea, SLOT(appendPlainText(QString))); + connect(_manager, SIGNAL(resetStatusMessages()), ui->messageArea, SLOT(clear())); } QGCUASFileView::~QGCUASFileView() From 85e138a971d5766eb3cfc6287200c1a6d64703d8 Mon Sep 17 00:00:00 2001 From: none Date: Sat, 7 Jun 2014 12:28:39 -0700 Subject: [PATCH 06/31] remote file listing --- src/uas/QGCUASFileManager.cc | 119 +++++++++++++++++++++++++++++++++++++++---- src/uas/QGCUASFileManager.h | 59 +++++++++++++++++++-- 2 files changed, 166 insertions(+), 12 deletions(-) diff --git a/src/uas/QGCUASFileManager.cc b/src/uas/QGCUASFileManager.cc index 9e59099..9562aa5 100644 --- a/src/uas/QGCUASFileManager.cc +++ b/src/uas/QGCUASFileManager.cc @@ -4,6 +4,7 @@ #include #include +#include static const quint32 crctab[] = { @@ -75,20 +76,120 @@ void QGCUASFileManager::nothingMessage() void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t message) { - switch (message.msgid) { - case MAVLINK_MSG_ID_ENCAPSULATED_DATA: - emit statusMessage("GOT ENC DATA"); - break; + if (message.msgid != MAVLINK_MSG_ID_ENCAPSULATED_DATA) { + // wtf, not for us + return; + } + + mavlink_encapsulated_data_t data; + mavlink_msg_encapsulated_data_decode(&message, &data); + const RequestHeader *hdr = (const RequestHeader *)&data.data[0]; + unsigned seqnr = data.seqnr; + + switch (_current_operation) { + case kCOIdle: + // we should not be seeing anything here.. shut the other guy up + sendReset(); + break; + + case kCOList: + if (hdr->opcode == kCmdList) { + listDecode(&hdr->data[0], hdr->size); + } + break; } } void QGCUASFileManager::listRecursively(const QString &from) { - // This sends out one line to the text area - emit statusMessage("/"); - emit statusMessage(" fs/"); - emit statusMessage(" microsd/"); - emit statusMessage(" log001.bin"); + if (_current_operation != kCOIdle) { + // XXX beep and don't do anything + } + + // XXX clear the text widget + emit statusMessage("requesting list..."); + + // initialise the lister + _list_path = from; + _list_offset = 0; + _current_operation = kCOList; + + // and send the initial request + sendList(); +} + +void QGCUASFileManager::listDecode(const uint8_t *data, unsigned len) +{ + unsigned offset = 0; + unsigned files = 0; + + // parse filenames out of the buffer + while (offset < len) { + + // get the length of the name + unsigned nlen = strnlen((const char *)data + offset, len - offset); + if (nlen == 0) { + break; + } + + // put it in the view + emit statusMessage(QString((const char *)data + offset)); + + // account for the name + NUL + offset += nlen + 1; + + // account for the file + files++; + } + + // we have run out of files to list + if (files == 0) { + _current_operation = kCOIdle; + } else { + // update our state + _list_offset += files; + + // and send another request + sendList(); + } +} + +void QGCUASFileManager::sendReset() +{ + mavlink_message_t message; + + RequestHeader hdr; + hdr.magic = 'f'; + hdr.session = 0; + hdr.opcode = kCmdReset; + hdr.crc32 = 0; + hdr.offset = 0; + hdr.size = 0; + hdr.crc32 = crc32((uint8_t*)&hdr, sizeof(hdr) + hdr.size, 0); + + mavlink_msg_encapsulated_data_pack(250, 0, &message, _encdata_seq, (uint8_t*)&hdr); // XXX 250 is a magic length + + _mav->sendMessage(message); +} + +void QGCUASFileManager::sendList() +{ + mavlink_message_t message; + + RequestHeader hdr; + hdr.magic = 'f'; + hdr.session = 0; + hdr.opcode = kCmdList; + hdr.crc32 = 0; + hdr.offset = _list_offset; + strncpy((char *)&hdr.data[0], _list_path.toStdString().c_str(), 200); // XXX should have a real limit here + hdr.size = strlen((const char *)&hdr.data[0]); + + hdr.crc32 = crc32((uint8_t*)&hdr, sizeof(hdr) + hdr.size, 0); + + mavlink_msg_encapsulated_data_pack(250, 0, &message, _encdata_seq, (uint8_t*)&hdr); // XXX 250 is a magic length + + _mav->sendMessage(message); } void QGCUASFileManager::downloadPath(const QString &from, const QString &to) diff --git a/src/uas/QGCUASFileManager.h b/src/uas/QGCUASFileManager.h index 55edb1b..9b88f93 100644 --- a/src/uas/QGCUASFileManager.h +++ b/src/uas/QGCUASFileManager.h @@ -19,11 +19,9 @@ public slots: void nothingMessage(); void listRecursively(const QString &from); void downloadPath(const QString& from, const QString& to); +// void timedOut(); protected: - UASInterface* _mav; - quint16 _encdata_seq; - struct RequestHeader { uint8_t magic; @@ -35,6 +33,61 @@ protected: uint8_t data[]; }; + enum Opcode + { + kCmdNone, // ignored, always acked + kCmdTerminate, // releases sessionID, closes file + kCmdReset, // terminates all sessions + kCmdList, // list files in from + kCmdOpen, // opens for reading, returns + kCmdRead, // reads bytes from in + kCmdCreate, // creates for writing, returns + kCmdWrite, // appends bytes at in + kCmdRemove, // remove file (only if created by server?) + + kRspAck, + kRspNak + }; + + enum ErrorCode + { + kErrNone, + kErrNoRequest, + kErrNoSession, + kErrSequence, + kErrNotDir, + kErrNotFile, + kErrEOF, + kErrNotAppend, + kErrTooBig, + kErrIO, + kErrPerm + }; + + + enum OperationState + { + kCOIdle, // not doing anything + + kCOList, // waiting for a List response + }; + + OperationState _current_operation; + unsigned _retry_counter; + + UASInterface* _mav; + quint16 _encdata_seq; + + unsigned _session_id; // session ID for current session + unsigned _list_offset; // offset for the current List operation + QString _list_path; // path for the current List operation + + void sendTerminate(); + void sendReset(); + + void sendList(); + void listDecode(const uint8_t *data, unsigned len); + static quint32 crc32(const uint8_t *src, unsigned len, unsigned state); }; From 914eeb9574301f31e850583b4fd0e7fc997dd1b3 Mon Sep 17 00:00:00 2001 From: none Date: Sat, 7 Jun 2014 12:55:45 -0700 Subject: [PATCH 07/31] Add some debugging to try to work out where our messages are... --- src/uas/QGCUASFileManager.cc | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/src/uas/QGCUASFileManager.cc b/src/uas/QGCUASFileManager.cc index 9562aa5..3ae7eea 100644 --- a/src/uas/QGCUASFileManager.cc +++ b/src/uas/QGCUASFileManager.cc @@ -76,7 +76,10 @@ void QGCUASFileManager::nothingMessage() void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t message) { + emit statusMessage("msg"); + if (message.msgid != MAVLINK_MSG_ID_ENCAPSULATED_DATA) { + emit statusMessage("not encap data"); // wtf, not for us return; } @@ -89,14 +92,20 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me switch (_current_operation) { case kCOIdle: // we should not be seeing anything here.. shut the other guy up + emit statusMessage("resetting file transfer session"); sendReset(); break; case kCOList: - if (hdr->opcode == kCmdList) { + if (hdr->opcode == kRspAck) { listDecode(&hdr->data[0], hdr->size); + } else { + emit statusMessage("unexpected opcode in List mode"); } break; + + default: + emit statusMessage("message in unexpected state"); } } @@ -106,8 +115,8 @@ void QGCUASFileManager::listRecursively(const QString &from) // XXX beep and don't do anything } - // XXX clear the text widget - emit statusMessage("requesting list..."); + // clear the text widget + emit resetStatusMessages(); // initialise the lister _list_path = from; @@ -189,6 +198,8 @@ void QGCUASFileManager::sendList() mavlink_msg_encapsulated_data_pack(250, 0, &message, _encdata_seq, (uint8_t*)&hdr); // XXX 250 is a magic length + emit statusMessage("sending List request..."); + _mav->sendMessage(message); } From 05b647d48cb96e9a5c939b739976f73b3dd09487 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Jun 2014 22:58:24 +0200 Subject: [PATCH 08/31] Protect against uninitialized data --- src/ui/QGCHilXPlaneConfiguration.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/ui/QGCHilXPlaneConfiguration.cc b/src/ui/QGCHilXPlaneConfiguration.cc index dc1cd52..6ba50ed 100644 --- a/src/ui/QGCHilXPlaneConfiguration.cc +++ b/src/ui/QGCHilXPlaneConfiguration.cc @@ -50,6 +50,10 @@ void QGCHilXPlaneConfiguration::setVersion(int version) void QGCHilXPlaneConfiguration::toggleSimulation(bool connect) { + if (!link) { + return; + } + Q_UNUSED(connect); if (!link->isConnected()) { From 4d85f0b5ecd31f69c4708844e35651b7ac5331ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Jun 2014 22:58:39 +0200 Subject: [PATCH 09/31] Hook up receiving messages path --- src/uas/QGCUASFileManager.cc | 6 ++++-- src/uas/UAS.cc | 2 ++ 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/uas/QGCUASFileManager.cc b/src/uas/QGCUASFileManager.cc index 3ae7eea..f03f84f 100644 --- a/src/uas/QGCUASFileManager.cc +++ b/src/uas/QGCUASFileManager.cc @@ -76,14 +76,16 @@ void QGCUASFileManager::nothingMessage() void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t message) { - emit statusMessage("msg"); + Q_UNUSED(link); if (message.msgid != MAVLINK_MSG_ID_ENCAPSULATED_DATA) { - emit statusMessage("not encap data"); // wtf, not for us return; } + emit statusMessage("msg"); + qDebug() << "FTP GOT MESSAGE"; + mavlink_encapsulated_data_t data; mavlink_msg_encapsulated_data_decode(&message, &data); const RequestHeader *hdr = (const RequestHeader *)&data.data[0]; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 585781b..64f133c 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -176,6 +176,8 @@ UAS::UAS(MAVLinkProtocol* protocol, QThread* thread, int id) : UASInterface(), componentMulti[i] = false; } + connect(mavlink, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), &fileManager, SLOT(receiveMessage(LinkInterface*,mavlink_message_t))); + // Store a list of available actions for this UAS. // Basically everything exposed as a SLOT with no return value or arguments. From b943c494921f19fb99b7d81d16a3cc802853ab51 Mon Sep 17 00:00:00 2001 From: none Date: Sun, 8 Jun 2014 10:14:16 -0700 Subject: [PATCH 10/31] decode errors in list mode --- src/uas/QGCUASFileManager.cc | 52 +++++++++++++++++++++++++++++++++++--------- src/uas/QGCUASFileManager.h | 1 + 2 files changed, 43 insertions(+), 10 deletions(-) diff --git a/src/uas/QGCUASFileManager.cc b/src/uas/QGCUASFileManager.cc index f03f84f..ec6e576 100644 --- a/src/uas/QGCUASFileManager.cc +++ b/src/uas/QGCUASFileManager.cc @@ -83,26 +83,25 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me return; } - emit statusMessage("msg"); - qDebug() << "FTP GOT MESSAGE"; - mavlink_encapsulated_data_t data; mavlink_msg_encapsulated_data_decode(&message, &data); const RequestHeader *hdr = (const RequestHeader *)&data.data[0]; unsigned seqnr = data.seqnr; + // XXX VALIDATE MESSAGE + switch (_current_operation) { case kCOIdle: // we should not be seeing anything here.. shut the other guy up - emit statusMessage("resetting file transfer session"); + qDebug() << "FTP resetting file transfer session"; sendReset(); break; case kCOList: if (hdr->opcode == kRspAck) { listDecode(&hdr->data[0], hdr->size); - } else { - emit statusMessage("unexpected opcode in List mode"); + } else if (hdr->opcode == kRspNak) { + emit statusMessage(QString("error: ").append(errorString(hdr->data[0]))); } break; @@ -139,12 +138,17 @@ void QGCUASFileManager::listDecode(const uint8_t *data, unsigned len) // get the length of the name unsigned nlen = strnlen((const char *)data + offset, len - offset); - if (nlen == 0) { + if (nlen < 2) { break; } + QString s((const char *)data + offset + 1); + if (data[0] == 'D') { + s.append('/'); + } + // put it in the view - emit statusMessage(QString((const char *)data + offset)); + emit statusMessage(s); // account for the name + NUL offset += nlen + 1; @@ -200,8 +204,6 @@ void QGCUASFileManager::sendList() mavlink_msg_encapsulated_data_pack(250, 0, &message, _encdata_seq, (uint8_t*)&hdr); // XXX 250 is a magic length - emit statusMessage("sending List request..."); - _mav->sendMessage(message); } @@ -225,3 +227,33 @@ void QGCUASFileManager::downloadPath(const QString &from, const QString &to) emit statusMessage(QString("Downloaded: %1 to directory %2").arg(filename).arg(to)); } + +QString QGCUASFileManager::errorString(uint8_t errorCode) +{ + switch(errorCode) { + case kErrNone: + return QString("no error"); + case kErrNoRequest: + return QString("bad request"); + case kErrNoSession: + return QString("bad session"); + case kErrSequence: + return QString("bad sequence number"); + case kErrNotDir: + return QString("not a directory"); + case kErrNotFile: + return QString("not a file"); + case kErrEOF: + return QString("read beyond end of file"); + case kErrNotAppend: + return QString("write not at end of file"); + case kErrTooBig: + return QString("file too big"); + case kErrIO: + return QString("device I/O error"); + case kErrPerm: + return QString("permission denied"); + default: + return QString("bad error code"); + } +} diff --git a/src/uas/QGCUASFileManager.h b/src/uas/QGCUASFileManager.h index 9b88f93..8f8c462 100644 --- a/src/uas/QGCUASFileManager.h +++ b/src/uas/QGCUASFileManager.h @@ -89,6 +89,7 @@ protected: void listDecode(const uint8_t *data, unsigned len); static quint32 crc32(const uint8_t *src, unsigned len, unsigned state); + static QString errorString(uint8_t errorCode); }; From 1db7551f8b8d4782dabf44a8f1d373059e12bca6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Jun 2014 19:32:11 +0200 Subject: [PATCH 11/31] Prevent the system from sending a stop HIL command if HIL is not actually active --- src/uas/UAS.cc | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index bb6e3c2..03f0837 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -2867,16 +2867,19 @@ void UAS::toggleArmedState() void UAS::goAutonomous() { setMode((base_mode & ~(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED)) | (MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED), 0); + qDebug() << __FILE__ << __LINE__ << "Going autonomous"; } void UAS::goManual() { setMode((base_mode & ~(MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED)) | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, 0); + qDebug() << __FILE__ << __LINE__ << "Going manual"; } void UAS::toggleAutonomy() { setMode(base_mode ^ MAV_MODE_FLAG_AUTO_ENABLED ^ MAV_MODE_FLAG_MANUAL_INPUT_ENABLED ^ MAV_MODE_FLAG_GUIDED_ENABLED ^ MAV_MODE_FLAG_STABILIZE_ENABLED, 0); + qDebug() << __FILE__ << __LINE__ << "Toggling autonomy"; } /** @@ -3330,6 +3333,7 @@ void UAS::startHil() hilEnabled = true; sensorHil = false; setMode(base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode); + qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; // Connect HIL simulation link simulation->connectSimulation(); } @@ -3339,8 +3343,11 @@ void UAS::startHil() */ void UAS::stopHil() { - if (simulation) simulation->disconnectSimulation(); - setMode(base_mode & ~MAV_MODE_FLAG_HIL_ENABLED, custom_mode); + if (simulation && simulation->isConnected()) { + simulation->disconnectSimulation(); + setMode(base_mode & ~MAV_MODE_FLAG_HIL_ENABLED, custom_mode); + qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to disable."; + } hilEnabled = false; sensorHil = false; } From 38a6fe81d8156b2b2bb8eb32eb8731076b84cb93 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 19 Jun 2014 16:59:45 -0700 Subject: [PATCH 12/31] Fix warnings --- src/qgcunittest/MockUAS.h | 4 ++-- src/uas/QGCUASFileManager.cc | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/qgcunittest/MockUAS.h b/src/qgcunittest/MockUAS.h index 3c715c0..d91ab19 100644 --- a/src/qgcunittest/MockUAS.h +++ b/src/qgcunittest/MockUAS.h @@ -112,9 +112,9 @@ public: virtual QGCUASFileManager* getFileManager() {Q_ASSERT(false); return NULL; } /** @brief Send a message over this link (to this or to all UAS on this link) */ - virtual void sendMessage(LinkInterface* link, mavlink_message_t message){Q_ASSERT(false);} + virtual void sendMessage(LinkInterface* link, mavlink_message_t message){ Q_UNUSED(link); Q_UNUSED(message); Q_ASSERT(false); } /** @brief Send a message over all links this UAS can be reached with (!= all links) */ - virtual void sendMessage(mavlink_message_t message) {Q_ASSERT(false);} + virtual void sendMessage(mavlink_message_t message) { Q_UNUSED(link); Q_UNUSED(message); Q_ASSERT(false); } virtual QString getAutopilotTypeName() { Q_ASSERT(false); return _bogusString; }; virtual void setAutopilotType(int apType) { Q_UNUSED(apType); Q_ASSERT(false); }; virtual QMap getComponents() { Q_ASSERT(false); return _bogusMapIntQString; }; diff --git a/src/uas/QGCUASFileManager.cc b/src/uas/QGCUASFileManager.cc index ec6e576..64689ad 100644 --- a/src/uas/QGCUASFileManager.cc +++ b/src/uas/QGCUASFileManager.cc @@ -86,7 +86,6 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me mavlink_encapsulated_data_t data; mavlink_msg_encapsulated_data_decode(&message, &data); const RequestHeader *hdr = (const RequestHeader *)&data.data[0]; - unsigned seqnr = data.seqnr; // XXX VALIDATE MESSAGE @@ -209,7 +208,8 @@ void QGCUASFileManager::sendList() void QGCUASFileManager::downloadPath(const QString &from, const QString &to) { - + Q_UNUSED(from); + // Send path, e.g. /fs/microsd and download content // recursively into a local directory From 9557ba9d6582a31e4d7561a9453bceb1a1f414a0 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 19 Jun 2014 16:59:53 -0700 Subject: [PATCH 13/31] Add missing widget --- src/ui/MainWindow.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index c631202..52aa44c 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -788,6 +788,10 @@ void MainWindow::loadDockWidget(const QString& name) { createDockWidget(centerStack->currentWidget(),new ParameterInterface(this),tr("Onboard Parameters"),"PARAMETER_INTERFACE_DOCKWIDGET",currentView,Qt::RightDockWidgetArea); } + else if (name == "FILE_VIEW_DOCKWIDGET") + { + createDockWidget(centerStack->currentWidget(),new QGCUASFileViewMulti(this),tr("Onboard Files"),"FILE_VIEW_DOCKWIDGET",VIEW_ENGINEER,Qt::RightDockWidgetArea); + } else if (name == "UAS_STATUS_DETAILS_DOCKWIDGET") { createDockWidget(centerStack->currentWidget(),new UASInfoWidget(this),tr("Status Details"),"UAS_STATUS_DETAILS_DOCKWIDGET",currentView,Qt::RightDockWidgetArea); From 85d15747cda66c44c82e2dff5136c761dba0b745 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 19 Jun 2014 17:00:29 -0700 Subject: [PATCH 14/31] Start of mock FileSever for unit testing --- qgroundcontrol.pro | 3 ++ src/qgcunittest/MockMavlinkFileServer.cc | 87 ++++++++++++++++++++++++++++++++ src/qgcunittest/MockMavlinkFileServer.h | 85 +++++++++++++++++++++++++++++++ src/qgcunittest/MockMavlinkInterface.cc | 9 ++++ src/qgcunittest/MockMavlinkInterface.h | 43 ++++++++++++++++ 5 files changed, 227 insertions(+) create mode 100644 src/qgcunittest/MockMavlinkFileServer.cc create mode 100644 src/qgcunittest/MockMavlinkFileServer.h create mode 100644 src/qgcunittest/MockMavlinkInterface.cc create mode 100644 src/qgcunittest/MockMavlinkInterface.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 070402c..f0290eb 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -186,6 +186,8 @@ DebugBuild { src/qgcunittest/MockUASManager.h \ src/qgcunittest/MockUAS.h \ src/qgcunittest/MockQGCUASParamManager.h \ + src/qgcunittest/MockMavlinkInterface.h \ + src/qgcunittest/MockMavlinkFileServer.h \ src/qgcunittest/MultiSignalSpy.h \ src/qgcunittest/FlightModeConfigTest.h @@ -194,6 +196,7 @@ DebugBuild { src/qgcunittest/MockUASManager.cc \ src/qgcunittest/MockUAS.cc \ src/qgcunittest/MockQGCUASParamManager.cc \ + src/qgcunittest/MockMavlinkFileServer.cc \ src/qgcunittest/MultiSignalSpy.cc \ src/qgcunittest/FlightModeConfigTest.cc } diff --git a/src/qgcunittest/MockMavlinkFileServer.cc b/src/qgcunittest/MockMavlinkFileServer.cc new file mode 100644 index 0000000..2dc67a8 --- /dev/null +++ b/src/qgcunittest/MockMavlinkFileServer.cc @@ -0,0 +1,87 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + 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. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#include "MockMavlinkFileServer.h" + +void MockMavlinkFileServer::sendMessage(mavlink_message_t message) +{ + Q_ASSERT(message.msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA); + + mavlink_encapsulated_data_t data; + mavlink_msg_encapsulated_data_decode(&message, &data); + const RequestHeader *hdr = (const RequestHeader *)&data.data[0]; + + // FIXME: Check CRC + + switch (hdr->opcode) { + case kCmdNone: + // ignored, always acked + + RequestHeader ackHdr; + ackHdr.magic = 'f'; + ackHdr.opcode = kRspAck; + ackHdr.session = 0; + ackHdr.crc32 = 0; + ackHdr.size = 0; + // FIXME: Add CRC + //ackHdr.crc32 = crc32((uint8_t*)&hdr, sizeof(hdr) + hdr.size, 0); + + mavlink_message_t ackMessage; + mavlink_msg_encapsulated_data_pack(250, 0, &ackMessage, 0 /*_encdata_seq*/, (uint8_t*)&ackHdr); + emit messageReceived(NULL, ackMessage); + break; + + case kCmdTerminate: + // releases sessionID, closes file + case kCmdReset: + // terminates all sessions + case kCmdList: + // list files in from + case kCmdOpen: + // opens for reading, returns + case kCmdRead: + // reads bytes from in + case kCmdCreate: + // creates for writing, returns + case kCmdWrite: + // appends bytes at in + case kCmdRemove: + // remove file (only if created by server?) + default: + // nack for all NYI opcodes + + RequestHeader nakHdr; + nakHdr.opcode = kRspNak; + nakHdr.magic = 'f'; + nakHdr.session = 0; + nakHdr.crc32 = 0; + nakHdr.size = 0; + // FIXME: Add CRC + //ackHdr.crc32 = crc32((uint8_t*)&hdr, sizeof(hdr) + hdr.size, 0); + + mavlink_message_t nakMessage; + mavlink_msg_encapsulated_data_pack(250, 0, &nakMessage, 0 /*_encdata_seq*/, (uint8_t*)&nakHdr); + emit messageReceived(NULL, nakMessage); + break; + } +} diff --git a/src/qgcunittest/MockMavlinkFileServer.h b/src/qgcunittest/MockMavlinkFileServer.h new file mode 100644 index 0000000..bca48ff --- /dev/null +++ b/src/qgcunittest/MockMavlinkFileServer.h @@ -0,0 +1,85 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + 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. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#ifndef MOCKMAVLINKFILESERVER_H +#define MOCKMAVLINKFILESERVER_H + +#include "MockMavlinkInterface.h" + +class MockMavlinkFileServer : public MockMavlinkInterface +{ + Q_OBJECT + +public: + MockMavlinkFileServer(void) { }; + virtual void sendMessage(mavlink_message_t message); + +private: + // FIXME: These should be in a mavlink header somewhere shouldn't they? + + struct RequestHeader + { + uint8_t magic; + uint8_t session; + uint8_t opcode; + uint8_t size; + uint32_t crc32; + uint32_t offset; + uint8_t data[]; + }; + + enum Opcode + { + kCmdNone, // ignored, always acked + kCmdTerminate, // releases sessionID, closes file + kCmdReset, // terminates all sessions + kCmdList, // list files in from + kCmdOpen, // opens for reading, returns + kCmdRead, // reads bytes from in + kCmdCreate, // creates for writing, returns + kCmdWrite, // appends bytes at in + kCmdRemove, // remove file (only if created by server?) + + kRspAck, + kRspNak + }; + + enum ErrorCode + { + kErrNone, + kErrNoRequest, + kErrNoSession, + kErrSequence, + kErrNotDir, + kErrNotFile, + kErrEOF, + kErrNotAppend, + kErrTooBig, + kErrIO, + kErrPerm + }; + + +}; + +#endif diff --git a/src/qgcunittest/MockMavlinkInterface.cc b/src/qgcunittest/MockMavlinkInterface.cc new file mode 100644 index 0000000..d9313fa --- /dev/null +++ b/src/qgcunittest/MockMavlinkInterface.cc @@ -0,0 +1,9 @@ +// +// MockMavlinkInterface.cc +// QGroundControl +// +// Created by Donald Gagne on 6/19/14. +// Copyright (c) 2014 Donald Gagne. All rights reserved. +// + +#include "MockMavlinkInterface.h" diff --git a/src/qgcunittest/MockMavlinkInterface.h b/src/qgcunittest/MockMavlinkInterface.h new file mode 100644 index 0000000..12ef447 --- /dev/null +++ b/src/qgcunittest/MockMavlinkInterface.h @@ -0,0 +1,43 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + 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. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#include + +#include "QGCMAVLink.h" +#include "LinkInterface.h" + +#ifndef MOCKMAVLINKINTERFACE_H +#define MOCKMAVLINKINTERFACE_H + +class MockMavlinkInterface : public QObject +{ + Q_OBJECT + +public: + virtual void sendMessage(mavlink_message_t message) = 0; + +signals: + void messageReceived(LinkInterface* link, mavlink_message_t message); +}; + +#endif From a6fae6b5392c74caf58174d118df287f78b532b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Jun 2014 14:38:20 +0200 Subject: [PATCH 15/31] Fix HIL link reconnect issues due to missing threading flag usage, warn user about missing config and take him to config view if necessary --- qgroundcontrol.pro | 6 ++- src/comm/QGCXPlaneLink.cc | 114 ++++++++++++++++++++++------------------ src/uas/QGCMAVLinkUASFactory.cc | 15 ++++++ src/uas/QGXPX4UAS.cc | 37 +++++++++++++ src/uas/QGXPX4UAS.h | 21 ++++++++ src/uas/UAS.cc | 1 + src/uas/UAS.h | 1 + src/uas/UASInterface.h | 5 ++ src/ui/MainWindow.cc | 21 ++++++++ src/ui/MainWindow.h | 1 + 10 files changed, 168 insertions(+), 54 deletions(-) create mode 100644 src/uas/QGXPX4UAS.cc create mode 100644 src/uas/QGXPX4UAS.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 070402c..74868c4 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -567,7 +567,8 @@ HEADERS += \ src/uas/QGCUASFileManager.h \ src/ui/QGCUASFileView.h \ src/uas/QGCUASWorker.h \ - src/CmdLineOptParser.h + src/CmdLineOptParser.h \ + src/uas/QGXPX4UAS.h SOURCES += \ src/main.cc \ @@ -755,4 +756,5 @@ SOURCES += \ src/uas/QGCUASFileManager.cc \ src/ui/QGCUASFileView.cc \ src/uas/QGCUASWorker.cc \ - src/CmdLineOptParser.cc + src/CmdLineOptParser.cc \ + src/uas/QGXPX4UAS.cc diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index b30b521..f829b7e 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -64,9 +64,12 @@ QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress // http://blog.qt.digia.com/blog/2010/06/17/youre-doing-it-wrong/ moveToThread(this); + setTerminationEnabled(false); + this->localHost = localHost; this->localPort = localPort/*+mav->getUASID()*/; - this->connectState = false; + connectState = false; + this->name = tr("X-Plane Link (localPort:%1)").arg(localPort); setRemoteHost(remoteHost); loadSettings(); @@ -151,13 +154,27 @@ void QGCXPlaneLink::setVersion(unsigned int version) **/ void QGCXPlaneLink::run() { - if (!mav) return; - if (connectState) return; + if (!mav) { + emit statusMessage("No MAV present"); + return; + } + + if (connectState) { + emit statusMessage("Already connected"); + return; + } socket = new QUdpSocket(this); socket->moveToThread(this); connectState = socket->bind(localHost, localPort); - if (!connectState) return; + if (!connectState) { + + emit statusMessage("Binding socket failed!"); + + delete socket; + socket = NULL; + return; + } QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes())); @@ -208,8 +225,6 @@ void QGCXPlaneLink::run() } } - //qDebug() << "REQ SEND TO:" << localAddrStr << localPortStr; - ip.index = 0; strncpy(ip.str_ipad_them, localAddrStr.toAscii(), qMin((int)sizeof(ip.str_ipad_them), 16)); strncpy(ip.str_port_them, localPortStr.toAscii(), qMin((int)sizeof(ip.str_port_them), 6)); @@ -217,10 +232,36 @@ void QGCXPlaneLink::run() writeBytes((const char*)&ip, sizeof(ip)); + _should_exit = false; + while(!_should_exit) { QCoreApplication::processEvents(); QGC::SLEEP::msleep(5); } + + if (mav) + { + disconnect(mav, SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8))); + disconnect(mav, SIGNAL(hilActuatorsChanged(quint64, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(quint64,float,float,float,float,float,float,float,float))); + + disconnect(this, SIGNAL(hilGroundTruthChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilGroundTruth(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); + disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); + disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64,double,double,double,int,float,float,float,float,float,float,float,int))); + disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32))); + + // Do not toggle HIL state on the UAS - this is not the job of this link, but of the + // UAS object + } + + connectState = false; + + socket->close(); + delete socket; + socket = NULL; + + emit simulationDisconnected(); + emit simulationConnected(false); + emit finished(); } void QGCXPlaneLink::setPort(int localPort) @@ -839,50 +880,15 @@ qint64 QGCXPlaneLink::bytesAvailable() **/ bool QGCXPlaneLink::disconnectSimulation() { - if (!connectState) return true; - - connectState = false; - - if (process) disconnect(process, SIGNAL(error(QProcess::ProcessError)), - this, SLOT(processError(QProcess::ProcessError))); - if (mav) + if (connectState) { - disconnect(mav, SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8))); - disconnect(mav, SIGNAL(hilActuatorsChanged(quint64, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(quint64,float,float,float,float,float,float,float,float))); - - disconnect(this, SIGNAL(hilGroundTruthChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilGroundTruth(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); - disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); - disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64,double,double,double,int,float,float,float,float,float,float,float,int))); - disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32))); - - UAS* uas = dynamic_cast(mav); - if (uas) - { - uas->stopHil(); - } + _should_exit = true; + wait(); + } else { + emit simulationDisconnected(); + emit simulationConnected(false); } - if (process) - { - process->close(); - delete process; - process = NULL; - } - if (terraSync) - { - terraSync->close(); - delete terraSync; - terraSync = NULL; - } - if (socket) - { - socket->close(); - delete socket; - socket = NULL; - } - - emit simulationDisconnected(); - emit simulationConnected(false); return !connectState; } @@ -1017,11 +1023,15 @@ void QGCXPlaneLink::setRandomAttitude() **/ bool QGCXPlaneLink::connectSimulation() { - qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort; - // XXX Hack - storeSettings(); - - start(HighPriority); + if (connectState) { + qDebug() << "Simulation already active"; + } else { + qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort; + // XXX Hack + storeSettings(); + + start(HighPriority); + } return true; } diff --git a/src/uas/QGCMAVLinkUASFactory.cc b/src/uas/QGCMAVLinkUASFactory.cc index 4f1b7da..ccf4c06 100644 --- a/src/uas/QGCMAVLinkUASFactory.cc +++ b/src/uas/QGCMAVLinkUASFactory.cc @@ -1,6 +1,7 @@ #include "QGCMAVLinkUASFactory.h" #include "UASManager.h" #include "QGCUASWorker.h" +#include "QGXPX4UAS.h" QGCMAVLinkUASFactory::QGCMAVLinkUASFactory(QObject *parent) : QObject(parent) @@ -58,6 +59,20 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte uas = mav; } break; + case MAV_AUTOPILOT_PX4: + { + QGXPX4UAS* px4 = new QGXPX4UAS(mavlink, worker, sysid); + // Set the system type + px4->setSystemType((int)heartbeat->type); + + // Connect this robot to the UAS object + // it is IMPORTANT here to use the right object type, + // else the slot of the parent object is called (and thus the special + // packets never reach their goal) + connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), px4, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); + uas = px4; + } + break; case MAV_AUTOPILOT_SLUGS: { SlugsMAV* mav = new SlugsMAV(mavlink, worker, sysid); diff --git a/src/uas/QGXPX4UAS.cc b/src/uas/QGXPX4UAS.cc new file mode 100644 index 0000000..723c7d4 --- /dev/null +++ b/src/uas/QGXPX4UAS.cc @@ -0,0 +1,37 @@ +#include "QGXPX4UAS.h" + +QGXPX4UAS::QGXPX4UAS(MAVLinkProtocol* mavlink, QThread* thread, int id) : + UAS(mavlink, thread, id) +{ +} + +/** + * This function is called by MAVLink once a complete, uncorrupted (CRC check valid) + * mavlink packet is received. + * + * @param link Hardware link the message came from (e.g. /dev/ttyUSB0 or UDP port). + * messages can be sent back to the system via this link + * @param message MAVLink message, as received from the MAVLink protocol stack + */ +void QGXPX4UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) +{ + UAS::receiveMessage(link, message); +} + +void QGXPX4UAS::processParamValueMsgHook(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue) +{ + int compId = msg.compid; + if (paramName == "SYS_AUTOSTART") { + + bool ok; + + int val = parameters.value(compId)->value(paramName).toInt(&ok); + + if (ok && val == 0) { + emit misconfigurationDetected(this); + qDebug() << "HINTING MISCONFIGURATION"; + } + + qDebug() << "SYS_AUTOSTART FOUND WITH VAL: " << val; + } +} diff --git a/src/uas/QGXPX4UAS.h b/src/uas/QGXPX4UAS.h new file mode 100644 index 0000000..9876ee4 --- /dev/null +++ b/src/uas/QGXPX4UAS.h @@ -0,0 +1,21 @@ +#ifndef QGXPX4UAS_H +#define QGXPX4UAS_H + +#include "UAS.h" + +class QGXPX4UAS : public UAS +{ + Q_OBJECT + Q_INTERFACES(UASInterface) +public: + QGXPX4UAS(MAVLinkProtocol* mavlink, QThread* thread, int id); + +public slots: + /** @brief Receive a MAVLink message from this MAV */ + void receiveMessage(LinkInterface* link, mavlink_message_t message); + + virtual void processParamValueMsgHook(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue); + +}; + +#endif // QGXPX4UAS_H diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 03f0837..210bbee 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1047,6 +1047,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) paramVal.type = rawValue.param_type; processParamValueMsg(message, parameterName,rawValue,paramVal); + processParamValueMsgHook(message, parameterName,rawValue,paramVal); } break; diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 8800636..dcaf7cc 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -981,6 +981,7 @@ protected: quint64 getUnixReferenceTime(quint64 time); virtual void processParamValueMsg(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue); + virtual void processParamValueMsgHook(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue) {}; int componentID[256]; bool componentMulti[256]; diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index d0ea85f..cf697ea 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -439,6 +439,11 @@ signals: void poiFound(UASInterface* uas, int type, int colorIndex, QString message, float x, float y, float z); void poiConnectionFound(UASInterface* uas, int type, int colorIndex, QString message, float x1, float y1, float z1, float x2, float y2, float z2); + /** + * @brief A misconfiguration has been detected by the UAS + */ + void misconfigurationDetected(UASInterface* uas); + /** @brief A text message from the system has been received */ void textMessageReceived(int uasid, int componentid, int severity, QString text); diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 2883dad..68ce49b 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -1736,6 +1736,7 @@ void MainWindow::UASCreated(UASInterface* uas) connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(UASSpecsChanged(int))); connect(uas, SIGNAL(valueChanged(int,QString,QString,QVariant,quint64)), this, SIGNAL(valueChanged(int,QString,QString,QVariant,quint64))); + connect(uas, SIGNAL(misconfigurationDetected(UASInterface*)), this, SLOT(handleMisconfiguration(UASInterface*))); // HIL showHILConfigurationWidget(uas); @@ -1981,6 +1982,26 @@ void MainWindow::setAdvancedMode(bool isAdvancedMode) settings.setValue("ADVANCED_MODE",isAdvancedMode); } +void MainWindow::handleMisconfiguration(UASInterface* uas) { + + // Ask user if he wants to handle this now + QMessageBox msgBox(this); + msgBox.setIcon(QMessageBox::Information); + msgBox.setText(tr("Missing or Invalid Onboard Configuration")); + msgBox.setInformativeText(tr("The onboard system configuration is missing or incomplete. Do you want to resolve this now?")); + msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel); + msgBox.setDefaultButton(QMessageBox::Ok); + int val = msgBox.exec(); + + if (val == QMessageBox::Ok) { + // He wants to handle it, make sure this system is selected + UASManager::instance()->setActiveUAS(uas); + + // Flick to config view + loadHardwareConfigView(); + } +} + void MainWindow::loadEngineerView() { if (currentView != VIEW_ENGINEER) diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 887407b..5cc2545 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -224,6 +224,7 @@ public slots: /** @brief Sets advanced mode, allowing for editing of tool widget locations */ void setAdvancedMode(bool isAdvancedMode); + void handleMisconfiguration(UASInterface* uas); /** @brief Load configuration views */ void loadHardwareConfigView(); void loadSoftwareConfigView(); From f3c8e5276fe7deef6adb075aff46a3afbd2448d8 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 25 Jun 2014 17:12:41 -0700 Subject: [PATCH 16/31] Add new QGCUASFileManager unit test --- qgroundcontrol.pro | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index f0290eb..4a86efe 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -189,7 +189,8 @@ DebugBuild { src/qgcunittest/MockMavlinkInterface.h \ src/qgcunittest/MockMavlinkFileServer.h \ src/qgcunittest/MultiSignalSpy.h \ - src/qgcunittest/FlightModeConfigTest.h + src/qgcunittest/FlightModeConfigTest.h \ + src/qgcunittest/QGCUASFileManagerTest.h SOURCES += \ src/qgcunittest/UASUnitTest.cc \ @@ -198,7 +199,8 @@ DebugBuild { src/qgcunittest/MockQGCUASParamManager.cc \ src/qgcunittest/MockMavlinkFileServer.cc \ src/qgcunittest/MultiSignalSpy.cc \ - src/qgcunittest/FlightModeConfigTest.cc + src/qgcunittest/FlightModeConfigTest.cc \ + src/qgcunittest/QGCUASFileManagerTest.cc } # From 23c77aefcb04cd2115486b328b6f463845603d08 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 25 Jun 2014 17:13:02 -0700 Subject: [PATCH 17/31] Mock mavlink ftp server --- src/qgcunittest/MockMavlinkFileServer.cc | 141 ++++++++++++++++++++++--------- src/qgcunittest/MockMavlinkFileServer.h | 55 +++--------- src/qgcunittest/MockMavlinkInterface.h | 1 + 3 files changed, 113 insertions(+), 84 deletions(-) diff --git a/src/qgcunittest/MockMavlinkFileServer.cc b/src/qgcunittest/MockMavlinkFileServer.cc index 2dc67a8..b17aa89 100644 --- a/src/qgcunittest/MockMavlinkFileServer.cc +++ b/src/qgcunittest/MockMavlinkFileServer.cc @@ -25,63 +25,124 @@ void MockMavlinkFileServer::sendMessage(mavlink_message_t message) { + QGCUASFileManager::Request ackResponse; + QString path; + Q_ASSERT(message.msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA); - mavlink_encapsulated_data_t data; - mavlink_msg_encapsulated_data_decode(&message, &data); - const RequestHeader *hdr = (const RequestHeader *)&data.data[0]; + mavlink_encapsulated_data_t requestEncapsulatedData; + mavlink_msg_encapsulated_data_decode(&message, &requestEncapsulatedData); + QGCUASFileManager::Request* request = (QGCUASFileManager::Request*)&requestEncapsulatedData.data[0]; - // FIXME: Check CRC + // Validate CRC + if (request->hdr.crc32 != QGCUASFileManager::crc32(request)) { + _sendNak(QGCUASFileManager::kErrCrc); + } - switch (hdr->opcode) { - case kCmdNone: + switch (request->hdr.opcode) { + case QGCUASFileManager::kCmdTestNoAck: + // ignored, ack not sent back, for testing only + break; + + case QGCUASFileManager::kCmdReset: + // terminates all sessions + // Fall through to send back Ack + + case QGCUASFileManager::kCmdNone: // ignored, always acked + ackResponse.hdr.magic = 'f'; + ackResponse.hdr.opcode = QGCUASFileManager::kRspAck; + ackResponse.hdr.session = 0; + ackResponse.hdr.crc32 = 0; + ackResponse.hdr.size = 0; + _emitResponse(&ackResponse); + break; + + case QGCUASFileManager::kCmdList: + // We only support root path + path = (char *)&request->data[0]; + if (!path.isEmpty() && path != "/") { + _sendNak(QGCUASFileManager::kErrNotDir); + break; + } + + if (request->hdr.offset > (uint32_t)_fileList.size()) { + _sendNak(QGCUASFileManager::kErrEOF); + break; + } + + ackResponse.hdr.magic = 'f'; + ackResponse.hdr.opcode = QGCUASFileManager::kRspAck; + ackResponse.hdr.session = 0; + ackResponse.hdr.size = 0; - RequestHeader ackHdr; - ackHdr.magic = 'f'; - ackHdr.opcode = kRspAck; - ackHdr.session = 0; - ackHdr.crc32 = 0; - ackHdr.size = 0; - // FIXME: Add CRC - //ackHdr.crc32 = crc32((uint8_t*)&hdr, sizeof(hdr) + hdr.size, 0); + if (request->hdr.offset == 0) { + // Requesting first batch of file names + Q_ASSERT(_fileList.size()); + char *bufPtr = (char *)&ackResponse.data[0]; + for (int i=0; i<_fileList.size(); i++) { + const char *filename = _fileList[i].toStdString().c_str(); + size_t cchFilename = strlen(filename); + strcpy(bufPtr, filename); + ackResponse.hdr.size += cchFilename + 1; + bufPtr += cchFilename + 1; + } + + // Final double termination + *bufPtr = 0; + ackResponse.hdr.size++; + + } else { + // All filenames fit in first ack, send final null terminated ack + ackResponse.data[0] = 0; + ackResponse.hdr.size = 1; + } - mavlink_message_t ackMessage; - mavlink_msg_encapsulated_data_pack(250, 0, &ackMessage, 0 /*_encdata_seq*/, (uint8_t*)&ackHdr); - emit messageReceived(NULL, ackMessage); + _emitResponse(&ackResponse); + break; + + // Remainder of commands are NYI - case kCmdTerminate: + case QGCUASFileManager::kCmdTerminate: // releases sessionID, closes file - case kCmdReset: - // terminates all sessions - case kCmdList: - // list files in from - case kCmdOpen: + case QGCUASFileManager::kCmdOpen: // opens for reading, returns - case kCmdRead: + case QGCUASFileManager::kCmdRead: // reads bytes from in - case kCmdCreate: + case QGCUASFileManager::kCmdCreate: // creates for writing, returns - case kCmdWrite: + case QGCUASFileManager::kCmdWrite: // appends bytes at in - case kCmdRemove: + case QGCUASFileManager::kCmdRemove: // remove file (only if created by server?) default: // nack for all NYI opcodes - - RequestHeader nakHdr; - nakHdr.opcode = kRspNak; - nakHdr.magic = 'f'; - nakHdr.session = 0; - nakHdr.crc32 = 0; - nakHdr.size = 0; - // FIXME: Add CRC - //ackHdr.crc32 = crc32((uint8_t*)&hdr, sizeof(hdr) + hdr.size, 0); - - mavlink_message_t nakMessage; - mavlink_msg_encapsulated_data_pack(250, 0, &nakMessage, 0 /*_encdata_seq*/, (uint8_t*)&nakHdr); - emit messageReceived(NULL, nakMessage); + _sendNak(QGCUASFileManager::kErrUnknownCommand); break; } } + +void MockMavlinkFileServer::_sendNak(QGCUASFileManager::ErrorCode error) +{ + QGCUASFileManager::Request nakResponse; + + nakResponse.hdr.magic = 'f'; + nakResponse.hdr.opcode = QGCUASFileManager::kRspNak; + nakResponse.hdr.session = 0; + nakResponse.hdr.size = sizeof(nakResponse.data[0]); + nakResponse.data[0] = error; + + _emitResponse(&nakResponse); +} + +void MockMavlinkFileServer::_emitResponse(QGCUASFileManager::Request* request) +{ + mavlink_message_t mavlinkMessage; + + request->hdr.crc32 = QGCUASFileManager::crc32(request); + + mavlink_msg_encapsulated_data_pack(250, 0, &mavlinkMessage, 0 /*_encdata_seq*/, (uint8_t*)request); + + emit messageReceived(NULL, mavlinkMessage); +} \ No newline at end of file diff --git a/src/qgcunittest/MockMavlinkFileServer.h b/src/qgcunittest/MockMavlinkFileServer.h index bca48ff..0bccd3d 100644 --- a/src/qgcunittest/MockMavlinkFileServer.h +++ b/src/qgcunittest/MockMavlinkFileServer.h @@ -25,6 +25,10 @@ #define MOCKMAVLINKFILESERVER_H #include "MockMavlinkInterface.h" +#include "QGCUASFileManager.h" + + +#include class MockMavlinkFileServer : public MockMavlinkInterface { @@ -32,54 +36,17 @@ class MockMavlinkFileServer : public MockMavlinkInterface public: MockMavlinkFileServer(void) { }; - virtual void sendMessage(mavlink_message_t message); - -private: - // FIXME: These should be in a mavlink header somewhere shouldn't they? - struct RequestHeader - { - uint8_t magic; - uint8_t session; - uint8_t opcode; - uint8_t size; - uint32_t crc32; - uint32_t offset; - uint8_t data[]; - }; + void setFileList(QStringList& fileList) { _fileList = fileList; } - enum Opcode - { - kCmdNone, // ignored, always acked - kCmdTerminate, // releases sessionID, closes file - kCmdReset, // terminates all sessions - kCmdList, // list files in from - kCmdOpen, // opens for reading, returns - kCmdRead, // reads bytes from in - kCmdCreate, // creates for writing, returns - kCmdWrite, // appends bytes at in - kCmdRemove, // remove file (only if created by server?) - - kRspAck, - kRspNak - }; + // From MockMavlinkInterface + virtual void sendMessage(mavlink_message_t message); - enum ErrorCode - { - kErrNone, - kErrNoRequest, - kErrNoSession, - kErrSequence, - kErrNotDir, - kErrNotFile, - kErrEOF, - kErrNotAppend, - kErrTooBig, - kErrIO, - kErrPerm - }; +private: + void _sendNak(QGCUASFileManager::ErrorCode error); + void _emitResponse(QGCUASFileManager::Request* request); - + QStringList _fileList; }; #endif diff --git a/src/qgcunittest/MockMavlinkInterface.h b/src/qgcunittest/MockMavlinkInterface.h index 12ef447..5aaab3d 100644 --- a/src/qgcunittest/MockMavlinkInterface.h +++ b/src/qgcunittest/MockMavlinkInterface.h @@ -37,6 +37,7 @@ public: virtual void sendMessage(mavlink_message_t message) = 0; signals: + // link argument will always be NULL void messageReceived(LinkInterface* link, mavlink_message_t message); }; From 587bf78a3f724bc898440227bb8cf871a074714b Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 25 Jun 2014 17:13:33 -0700 Subject: [PATCH 18/31] Mock mavlink plugin support --- src/qgcunittest/MockUAS.cc | 14 +++++++++++++- src/qgcunittest/MockUAS.h | 13 ++++++++++--- 2 files changed, 23 insertions(+), 4 deletions(-) diff --git a/src/qgcunittest/MockUAS.cc b/src/qgcunittest/MockUAS.cc index ff3688e..443bb93 100644 --- a/src/qgcunittest/MockUAS.cc +++ b/src/qgcunittest/MockUAS.cc @@ -27,7 +27,8 @@ QString MockUAS::_bogusStaticString; MockUAS::MockUAS(void) : _systemType(MAV_TYPE_QUADROTOR), - _systemId(1) + _systemId(1), + _mavlinkPlugin(NULL) { } @@ -41,4 +42,15 @@ void MockUAS::setMockParametersAndSignal(MockQGCUASParamManager::ParamMap_t& map i.next(); emit parameterChanged(_systemId, 0, i.key(), i.value()); } +} + +void MockUAS::sendMessage(mavlink_message_t message) +{ + Q_UNUSED(link); + + if (!_mavlinkPlugin) { + Q_ASSERT(false); + } + + _mavlinkPlugin->sendMessage(message); } \ No newline at end of file diff --git a/src/qgcunittest/MockUAS.h b/src/qgcunittest/MockUAS.h index d91ab19..5b3a529 100644 --- a/src/qgcunittest/MockUAS.h +++ b/src/qgcunittest/MockUAS.h @@ -26,6 +26,7 @@ #include "UASInterface.h" #include "MockQGCUASParamManager.h" +#include "MockMavlinkInterface.h" #include @@ -50,6 +51,9 @@ public: virtual int getUASID(void) const { return _systemId; } virtual QGCUASParamManagerInterface* getParamManager() { return &_paramManager; }; + // sendMessage is only supported if a mavlink plugin is installed. + virtual void sendMessage(mavlink_message_t message); + public: // MockUAS methods MockUAS(void); @@ -63,12 +67,15 @@ public: /// allows you to simulate parameter input and validate parameter setting MockQGCUASParamManager* getMockQGCUASParamManager(void) { return &_paramManager; } - /// Sets the parameter map into the mock QGCUASParamManager and signals parameterChanged for + /// @brief Sets the parameter map into the mock QGCUASParamManager and signals parameterChanged for /// each param void setMockParametersAndSignal(MockQGCUASParamManager::ParamMap_t& map); void emitRemoteControlChannelRawChanged(int channel, float raw) { emit remoteControlChannelRawChanged(channel, raw); } + /// @brief Installs a mavlink plugin. Only a single mavlink plugin is supported at a time. + void setMockMavlinkPlugin(MockMavlinkInterface* mavlinkPlugin) { _mavlinkPlugin = mavlinkPlugin; }; + public: // Unimplemented UASInterface overrides virtual QString getUASName() const { Q_ASSERT(false); return _bogusString; }; @@ -113,8 +120,6 @@ public: /** @brief Send a message over this link (to this or to all UAS on this link) */ virtual void sendMessage(LinkInterface* link, mavlink_message_t message){ Q_UNUSED(link); Q_UNUSED(message); Q_ASSERT(false); } - /** @brief Send a message over all links this UAS can be reached with (!= all links) */ - virtual void sendMessage(mavlink_message_t message) { Q_UNUSED(link); Q_UNUSED(message); Q_ASSERT(false); } virtual QString getAutopilotTypeName() { Q_ASSERT(false); return _bogusString; }; virtual void setAutopilotType(int apType) { Q_UNUSED(apType); Q_ASSERT(false); }; virtual QMap getComponents() { Q_ASSERT(false); return _bogusMapIntQString; }; @@ -182,6 +187,8 @@ private: int _systemId; MockQGCUASParamManager _paramManager; + + MockMavlinkInterface* _mavlinkPlugin; ///< Mock Mavlink plugin, NULL for none // Bogus variables used for return types of NYI methods QString _bogusString; From 1a4a9f4fc654e65461cb484e0ddb216b4540212e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 25 Jun 2014 17:13:46 -0700 Subject: [PATCH 19/31] QGCUASFileManager unit test --- src/qgcunittest/QGCUASFileManagerTest.cc | 156 +++++++++++++++++++++++++++++++ src/qgcunittest/QGCUASFileManagerTest.h | 92 ++++++++++++++++++ 2 files changed, 248 insertions(+) create mode 100644 src/qgcunittest/QGCUASFileManagerTest.cc create mode 100644 src/qgcunittest/QGCUASFileManagerTest.h diff --git a/src/qgcunittest/QGCUASFileManagerTest.cc b/src/qgcunittest/QGCUASFileManagerTest.cc new file mode 100644 index 0000000..fe325de --- /dev/null +++ b/src/qgcunittest/QGCUASFileManagerTest.cc @@ -0,0 +1,156 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + 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. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#include "QGCUASFileManagerTest.h" + +/// @file +/// @brief QGCUASFileManager unit test +/// +/// @author Don Gagne + +QGCUASFileManagerUnitTest::QGCUASFileManagerUnitTest(void) : + _fileManager(NULL), + _multiSpy(NULL) +{ + +} + +// Called once before all test cases are run +void QGCUASFileManagerUnitTest::initTestCase(void) +{ + _mockUAS.setMockMavlinkPlugin(&_mockFileServer); +} + +// Called before every test case +void QGCUASFileManagerUnitTest::init(void) +{ + Q_ASSERT(_multiSpy == NULL); + + _fileManager = new QGCUASFileManager(NULL, &_mockUAS); + Q_CHECK_PTR(_fileManager); + + bool connected = connect(&_mockFileServer, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), _fileManager, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); + Q_ASSERT(connected); + + connected = connect(_fileManager, SIGNAL(statusMessage(const QString&)), this, SLOT(statusMessage(const QString&))); + Q_ASSERT(connected); + + _rgSignals[statusMessageSignalIndex] = SIGNAL(statusMessage(const QString&)); + _rgSignals[errorMessageSignalIndex] = SIGNAL(errorMessage(const QString&)); + _rgSignals[resetStatusMessagesSignalIndex] = SIGNAL(resetStatusMessages(void)); + + _multiSpy = new MultiSignalSpy(); + Q_CHECK_PTR(_multiSpy); + QCOMPARE(_multiSpy->init(_fileManager, _rgSignals, _cSignals), true); +} + +// Called after every test case +void QGCUASFileManagerUnitTest::cleanup(void) +{ + Q_ASSERT(_multiSpy); + Q_ASSERT(_fileManager); + + delete _fileManager; + delete _multiSpy; + + _fileManager = NULL; + _multiSpy = NULL; +} + +/// @brief Connected to QGCUASFileManager statusMessage signal in order to catch list command output +void QGCUASFileManagerUnitTest::statusMessage(const QString& msg) +{ + // Keep a list of all names received so we can test it for correctness + _fileListReceived += msg; +} + + +void QGCUASFileManagerUnitTest::_ackTest(void) +{ + Q_ASSERT(_fileManager); + Q_ASSERT(_multiSpy); + Q_ASSERT(_multiSpy->checkNoSignals() == true); + + // If the file manager doesn't receive an ack it will timeout and emit an error. So make sure + // we don't get any error signals. + QVERIFY(_fileManager->_sendCmdTestAck()); + QVERIFY(_multiSpy->checkNoSignals()); +} + +void QGCUASFileManagerUnitTest::_noAckTest(void) +{ + Q_ASSERT(_fileManager); + Q_ASSERT(_multiSpy); + Q_ASSERT(_multiSpy->checkNoSignals() == true); + + // This should not get the ack back and timeout. + QVERIFY(_fileManager->_sendCmdTestNoAck()); + QTest::qWait(2000); // Let the file manager timeout, magic number 2 secs must be larger than file manager ack timeout + QCOMPARE(_multiSpy->checkOnlySignalByMask(errorMessageSignalMask), true); +} + +void QGCUASFileManagerUnitTest::_resetTest(void) +{ + Q_ASSERT(_fileManager); + Q_ASSERT(_multiSpy); + Q_ASSERT(_multiSpy->checkNoSignals() == true); + + // Send a reset command + // We should not get any signals back from this + QVERIFY(_fileManager->_sendCmdReset()); + QVERIFY(_multiSpy->checkNoSignals()); +} + +void QGCUASFileManagerUnitTest::_listTest(void) +{ + Q_ASSERT(_fileManager); + Q_ASSERT(_multiSpy); + Q_ASSERT(_multiSpy->checkNoSignals() == true); + + // Send a bogus path + // We should get a single resetStatusMessages signal + // We should get a single errorMessage signal + _fileManager->listRecursively("/bogus"); + QCOMPARE(_multiSpy->checkOnlySignalByMask(errorMessageSignalMask | resetStatusMessagesSignalMask), true); + _multiSpy->clearAllSignals(); + + // Send a list command at the root of the directory tree + // We should get back a single resetStatusMessages signal + // We should not get back an errorMessage signal + // We should get back one or more statusMessage signals + // The returned list should match out inputs + + QStringList fileList; + fileList << "Ddir" << "Ffoo" << "Fbar"; + _mockFileServer.setFileList(fileList); + + QStringList fileListExpected; + fileListExpected << "dir/" << "foo" << "bar"; + + _fileListReceived.clear(); + + _fileManager->listRecursively("/"); + QCOMPARE(_multiSpy->checkSignalByMask(resetStatusMessagesSignalMask), true); // We should be told to reset status messages + QCOMPARE(_multiSpy->checkNoSignalByMask(errorMessageSignalMask), true); // We should not get an error signals + QVERIFY(_fileListReceived == fileListExpected); +} diff --git a/src/qgcunittest/QGCUASFileManagerTest.h b/src/qgcunittest/QGCUASFileManagerTest.h new file mode 100644 index 0000000..2a76fbb --- /dev/null +++ b/src/qgcunittest/QGCUASFileManagerTest.h @@ -0,0 +1,92 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + 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. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#ifndef QGCUASFILEMANAGERTEST_H +#define QGCUASFILEMANAGERTEST_H + +#include +#include + +#include "AutoTest.h" +#include "MockUAS.h" +#include "MockMavlinkFileServer.h" +#include "QGCUASFileManager.h" +#include "MultiSignalSpy.h" + +/// @file +/// @brief QGCUASFileManager unit test +/// +/// @author Don Gagne + +class QGCUASFileManagerUnitTest : public QObject +{ + Q_OBJECT + +public: + QGCUASFileManagerUnitTest(void); + +private slots: + // Test case initialization + void initTestCase(void); + void init(void); + void cleanup(void); + + // Test cases + void _ackTest(void); + void _noAckTest(void); + void _resetTest(void); + void _listTest(void); + + // Connected to QGCUASFileManager statusMessage signal + void statusMessage(const QString&); + +private: + enum { + statusMessageSignalIndex = 0, + errorMessageSignalIndex, + resetStatusMessagesSignalIndex, + + maxSignalIndex + }; + + enum { + statusMessageSignalMask = 1 << statusMessageSignalIndex, + errorMessageSignalMask = 1 << errorMessageSignalIndex, + resetStatusMessagesSignalMask = 1 << resetStatusMessagesSignalIndex, + }; + + MockUAS _mockUAS; + MockMavlinkFileServer _mockFileServer; + + QGCUASFileManager* _fileManager; + + MultiSignalSpy* _multiSpy; + static const size_t _cSignals = maxSignalIndex; + const char* _rgSignals[_cSignals]; + + QStringList _fileListReceived; +}; + +DECLARE_TEST(QGCUASFileManagerUnitTest) + +#endif From e2ce56dc05c33a3cee8e0f8cd103636597952823 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 25 Jun 2014 17:14:35 -0700 Subject: [PATCH 20/31] List command is now fully working Also changes for Ack timeouts and unit testing --- src/uas/QGCUASFileManager.cc | 269 +++++++++++++++++++++++++++++-------------- src/uas/QGCUASFileManager.h | 105 ++++++++++++----- 2 files changed, 263 insertions(+), 111 deletions(-) diff --git a/src/uas/QGCUASFileManager.cc b/src/uas/QGCUASFileManager.cc index 64689ad..5b48b30 100644 --- a/src/uas/QGCUASFileManager.cc +++ b/src/uas/QGCUASFileManager.cc @@ -1,3 +1,26 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + 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. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + #include "QGCUASFileManager.h" #include "QGC.h" #include "MAVLinkProtocol.h" @@ -44,84 +67,106 @@ static const quint32 crctab[] = QGCUASFileManager::QGCUASFileManager(QObject* parent, UASInterface* uas) : QObject(parent), + _currentOperation(kCOIdle), _mav(uas), _encdata_seq(0) { + bool connected = connect(&_ackTimer, SIGNAL(timeout()), this, SLOT(_ackTimeout())); + Q_ASSERT(connected); } -quint32 -QGCUASFileManager::crc32(const uint8_t *src, unsigned len, unsigned state) +/// @brief Calculates a 32 bit CRC for the specified request. +/// @param request Request to calculate CRC for. request->size must be set correctly. +/// @param state previous crc state +/// @return Calculated CRC +quint32 QGCUASFileManager::crc32(Request* request, unsigned state) { - for (unsigned i = 0; i < len; i++) - state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8); + uint8_t* data = (uint8_t*)request; + size_t cbData = sizeof(RequestHeader) + request->hdr.size; + + // Always calculate CRC with 0 initial CRC value + quint32 crcSave = request->hdr.crc32; + request->hdr.crc32 = 0; + + for (size_t i=0; i < cbData; i++) + state = crctab[(state ^ data[i]) & 0xff] ^ (state >> 8); + + request->hdr.crc32 = crcSave; + return state; } void QGCUASFileManager::nothingMessage() { - mavlink_message_t message; - - RequestHeader hdr; - hdr.magic = 'f'; - hdr.session = 0; - hdr.crc32 = 0; - hdr.size = 0; - hdr.crc32 = crc32((uint8_t*)&hdr, sizeof(hdr) + hdr.size, 0); - - mavlink_msg_encapsulated_data_pack(250, 0, &message, _encdata_seq, (uint8_t*)&hdr); - - _mav->sendMessage(message); - qDebug() << "Sent message!"; + // FIXME: Connect ui correctly } void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t message) { Q_UNUSED(link); - + if (message.msgid != MAVLINK_MSG_ID_ENCAPSULATED_DATA) { // wtf, not for us return; } - + mavlink_encapsulated_data_t data; mavlink_msg_encapsulated_data_decode(&message, &data); - const RequestHeader *hdr = (const RequestHeader *)&data.data[0]; - - // XXX VALIDATE MESSAGE - - switch (_current_operation) { - case kCOIdle: - // we should not be seeing anything here.. shut the other guy up - qDebug() << "FTP resetting file transfer session"; - sendReset(); - break; - - case kCOList: - if (hdr->opcode == kRspAck) { - listDecode(&hdr->data[0], hdr->size); - } else if (hdr->opcode == kRspNak) { - emit statusMessage(QString("error: ").append(errorString(hdr->data[0]))); - } - break; + Request* request = (Request*)&data.data[0]; - default: - emit statusMessage("message in unexpected state"); + qDebug() << "FTP: opcode:" << request->hdr.opcode; + + // FIXME: Check CRC + + if (request->hdr.opcode == kRspAck) { + _clearAckTimeout(); + + switch (_currentOperation) { + case kCOIdle: + // we should not be seeing anything here.. shut the other guy up + qDebug() << "FTP resetting file transfer session"; + _sendCmdReset(); + break; + + case kCOAck: + // We are expecting an ack back + _clearAckTimeout(); + _currentOperation = kCOIdle; + break; + + case kCOList: + listDecode(&request->data[0], request->hdr.size); + break; + + default: + _emitErrorMessage("Ack received in unexpected state"); + break; + } + } else if (request->hdr.opcode == kRspNak) { + _clearAckTimeout(); + _emitErrorMessage(QString("Nak received, error: ").append(errorString(request->data[0]))); + _currentOperation = kCOIdle; + } else { + // Note that we don't change our operation state. If something goes wrong beyond this, the operation + // will time out. + _emitErrorMessage(tr("Unknown opcode returned server: %1").arg(request->hdr.opcode)); } } void QGCUASFileManager::listRecursively(const QString &from) { - if (_current_operation != kCOIdle) { - // XXX beep and don't do anything + if (_currentOperation != kCOIdle) { + _emitErrorMessage(tr("Command not sent. Waiting for previous command to complete.")); + return; } // clear the text widget emit resetStatusMessages(); // initialise the lister - _list_path = from; - _list_offset = 0; - _current_operation = kCOList; + _listPath = from; + _listOffset = 0; + _currentOperation = kCOList; // and send the initial request sendList(); @@ -134,15 +179,17 @@ void QGCUASFileManager::listDecode(const uint8_t *data, unsigned len) // parse filenames out of the buffer while (offset < len) { - + const char * ptr = (const char *)data + offset; + // get the length of the name - unsigned nlen = strnlen((const char *)data + offset, len - offset); + unsigned nlen = strnlen(ptr, len - offset); if (nlen < 2) { break; } - QString s((const char *)data + offset + 1); - if (data[0] == 'D') { + // Returned names are prepended with D for directory or F for file + QString s(ptr + 1); + if (*ptr == 'D') { s.append('/'); } @@ -158,52 +205,29 @@ void QGCUASFileManager::listDecode(const uint8_t *data, unsigned len) // we have run out of files to list if (files == 0) { - _current_operation = kCOIdle; + _currentOperation = kCOIdle; } else { // update our state - _list_offset += files; + _listOffset += files; // and send another request sendList(); } } -void QGCUASFileManager::sendReset() -{ - mavlink_message_t message; - - RequestHeader hdr; - hdr.magic = 'f'; - hdr.session = 0; - hdr.opcode = kCmdReset; - hdr.crc32 = 0; - hdr.offset = 0; - hdr.size = 0; - hdr.crc32 = crc32((uint8_t*)&hdr, sizeof(hdr) + hdr.size, 0); - - mavlink_msg_encapsulated_data_pack(250, 0, &message, _encdata_seq, (uint8_t*)&hdr); // XXX 250 is a magic length - - _mav->sendMessage(message); -} - void QGCUASFileManager::sendList() { - mavlink_message_t message; - - RequestHeader hdr; - hdr.magic = 'f'; - hdr.session = 0; - hdr.opcode = kCmdList; - hdr.crc32 = 0; - hdr.offset = _list_offset; - strncpy((char *)&hdr.data[0], _list_path.toStdString().c_str(), 200); // XXX should have a real limit here - hdr.size = strlen((const char *)&hdr.data[0]); - - hdr.crc32 = crc32((uint8_t*)&hdr, sizeof(hdr) + hdr.size, 0); - - mavlink_msg_encapsulated_data_pack(250, 0, &message, _encdata_seq, (uint8_t*)&hdr); // XXX 250 is a magic length + Request request; - _mav->sendMessage(message); + request.hdr.magic = 'f'; + request.hdr.session = 0; + request.hdr.opcode = kCmdList; + request.hdr.offset = _listOffset; + + strncpy((char *)&request.data[0], _listPath.toStdString().c_str(), sizeof(request.data)); + request.hdr.size = strnlen((const char *)&request.data[0], sizeof(request.data)); + + _sendRequest(&request); } void QGCUASFileManager::downloadPath(const QString &from, const QString &to) @@ -253,7 +277,84 @@ QString QGCUASFileManager::errorString(uint8_t errorCode) return QString("device I/O error"); case kErrPerm: return QString("permission denied"); + case kErrUnknownCommand: + return QString("unknown command"); + case kErrCrc: + return QString("bad crc"); default: - return QString("bad error code"); + return QString("unknown error code"); } } + +/// @brief Sends a command which only requires an opcode and no additional data +/// @param opcode Opcode to send +/// @param newOpState State to put state machine into +/// @return TRUE: command sent, FALSE: command not sent, waiting for previous command to finish +bool QGCUASFileManager::_sendOpcodeOnlyCmd(uint8_t opcode, OperationState newOpState) +{ + if (_currentOperation != kCOIdle) { + // Can't have multiple commands in play at the same time + return false; + } + + Request request; + request.hdr.magic = 'f'; + request.hdr.session = 0; + request.hdr.opcode = opcode; + request.hdr.offset = 0; + request.hdr.size = 0; + + _currentOperation = newOpState; + _setupAckTimeout(); + + _sendRequest(&request); + + return TRUE; +} + +/// @brief Starts the ack timeout timer +void QGCUASFileManager::_setupAckTimeout(void) +{ + qDebug() << "Setting Ack"; + + Q_ASSERT(!_ackTimer.isActive()); + + _ackTimer.setSingleShot(true); + _ackTimer.start(_ackTimerTimeoutMsecs); +} + +/// @brief Clears the ack timeout timer +void QGCUASFileManager::_clearAckTimeout(void) +{ + qDebug() << "Clearing Ack"; + + Q_ASSERT(_ackTimer.isActive()); + + _ackTimer.stop(); +} + +/// @brief Called when ack timeout timer fires +void QGCUASFileManager::_ackTimeout(void) +{ + _currentOperation = kCOIdle; + _emitErrorMessage(tr("Timeout waiting for ack")); +} + +void QGCUASFileManager::_emitErrorMessage(const QString& msg) +{ + qDebug() << "QGCUASFileManager:" << msg; + emit errorMessage(msg); +} + +/// @brief Sends the specified Request out to the UAS. +void QGCUASFileManager::_sendRequest(Request* request) +{ + mavlink_message_t message; + + _setupAckTimeout(); + + request->hdr.crc32 = crc32(request); + // FIXME: Send correct system id instead of harcoded 250 + mavlink_msg_encapsulated_data_pack(250, 0, &message, _encdata_seq, (uint8_t*)request); + _mav->sendMessage(message); +} \ No newline at end of file diff --git a/src/uas/QGCUASFileManager.h b/src/uas/QGCUASFileManager.h index 8f8c462..5c41c0b 100644 --- a/src/uas/QGCUASFileManager.h +++ b/src/uas/QGCUASFileManager.h @@ -1,7 +1,31 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + 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. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + #ifndef QGCUASFILEMANAGER_H #define QGCUASFILEMANAGER_H #include + #include "UASInterface.h" class QGCUASFileManager : public QObject @@ -9,17 +33,22 @@ class QGCUASFileManager : public QObject Q_OBJECT public: QGCUASFileManager(QObject* parent, UASInterface* uas); + + /// These methods are only used for testing purposes. + bool _sendCmdTestAck(void) { return _sendOpcodeOnlyCmd(kCmdNone, kCOAck); }; + bool _sendCmdTestNoAck(void) { return _sendOpcodeOnlyCmd(kCmdTestNoAck, kCOAck); }; + bool _sendCmdReset(void) { return _sendOpcodeOnlyCmd(kCmdReset, kCOAck); }; signals: void statusMessage(const QString& msg); void resetStatusMessages(); + void errorMessage(const QString& ms); public slots: void receiveMessage(LinkInterface* link, mavlink_message_t message); void nothingMessage(); void listRecursively(const QString &from); void downloadPath(const QString& from, const QString& to); -// void timedOut(); protected: struct RequestHeader @@ -30,23 +59,32 @@ protected: uint8_t size; uint32_t crc32; uint32_t offset; - uint8_t data[]; }; + struct Request + { + struct RequestHeader hdr; + // The entire Request must fit into the data member of the mavlink_encapsulated_data_t structure. We use as many leftover bytes + // after we use up space for the RequestHeader for the data portion of the Request. + uint8_t data[sizeof(((mavlink_encapsulated_data_t*)0)->data) - sizeof(RequestHeader)]; + }; + enum Opcode { - kCmdNone, // ignored, always acked + kCmdNone, // ignored, always acked kCmdTerminate, // releases sessionID, closes file - kCmdReset, // terminates all sessions - kCmdList, // list files in from - kCmdOpen, // opens for reading, returns - kCmdRead, // reads bytes from in - kCmdCreate, // creates for writing, returns - kCmdWrite, // appends bytes at in - kCmdRemove, // remove file (only if created by server?) + kCmdReset, // terminates all sessions + kCmdList, // list files in from + kCmdOpen, // opens for reading, returns + kCmdRead, // reads bytes from in + kCmdCreate, // creates for writing, returns + kCmdWrite, // appends bytes at in + kCmdRemove, // remove file (only if created by server?) kRspAck, - kRspNak + kRspNak, + + kCmdTestNoAck, // ignored, ack not sent back, for testing only, should timeout waiting for ack }; enum ErrorCode @@ -61,36 +99,49 @@ protected: kErrNotAppend, kErrTooBig, kErrIO, - kErrPerm + kErrPerm, + kErrUnknownCommand, + kErrCrc }; enum OperationState { kCOIdle, // not doing anything - + kCOAck, // waiting for an Ack kCOList, // waiting for a List response }; - - OperationState _current_operation; - unsigned _retry_counter; - - UASInterface* _mav; - quint16 _encdata_seq; - - unsigned _session_id; // session ID for current session - unsigned _list_offset; // offset for the current List operation - QString _list_path; // path for the current List operation - - void sendTerminate(); - void sendReset(); + + +protected slots: + void _ackTimeout(void); + +protected: + bool _sendOpcodeOnlyCmd(uint8_t opcode, OperationState newOpState); + void _setupAckTimeout(void); + void _clearAckTimeout(void); + void _emitErrorMessage(const QString& msg); + void _sendRequest(Request* request); void sendList(); void listDecode(const uint8_t *data, unsigned len); - static quint32 crc32(const uint8_t *src, unsigned len, unsigned state); + static quint32 crc32(Request* request, unsigned state = 0); static QString errorString(uint8_t errorCode); + OperationState _currentOperation; ///> Current operation of state machine + QTimer _ackTimer; ///> Used to signal a timeout waiting for an ack + static const int _ackTimerTimeoutMsecs = 1000; ///> Timeout in msecs for ack timer + + UASInterface* _mav; + quint16 _encdata_seq; + + unsigned _listOffset; // offset for the current List operation + QString _listPath; // path for the current List operation + + // We give MockMavlinkFileServer friend access so that it can use the data structures and opcodes + // to build a mock mavlink file server for testing. + friend class MockMavlinkFileServer; }; #endif // QGCUASFILEMANAGER_H From 3f85733946208ce7e33c4b305d42dc45ef685374 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 25 Jun 2014 17:15:29 -0700 Subject: [PATCH 21/31] Fix image packet code These changes prevent random encapsulated data packets as being thought to be images. --- src/uas/UAS.cc | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 64f133c..79e5e95 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -149,6 +149,8 @@ UAS::UAS(MAVLinkProtocol* protocol, QThread* thread, int id) : UASInterface(), pitch(0.0), yaw(0.0), + imagePackets(0), // We must initialize to 0, otherwise extended data packets maybe incorrectly thought to be images + blockHomePositionChanges(false), receivedMode(false), @@ -1321,6 +1323,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // NO VALID TRANSACTION - ABORT // Restart statemachine imagePacketsArrived = 0; + break; } for (int i = 0; i < imagePayload; ++i) @@ -1337,6 +1340,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) if ((imagePacketsArrived >= imagePackets)) { // Restart statemachine + imagePackets = 0; + imagePacketsArrived = 0; emit imageReady(this); //qDebug() << "imageReady emitted. all packets arrived"; } From 81452e02e817a27b6a8fa21d1c90ad7ddaadb438 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 25 Jun 2014 17:15:44 -0700 Subject: [PATCH 22/31] Error messages to text box --- src/ui/QGCUASFileView.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ui/QGCUASFileView.cc b/src/ui/QGCUASFileView.cc index 3d525cc..09ccc3b 100644 --- a/src/ui/QGCUASFileView.cc +++ b/src/ui/QGCUASFileView.cc @@ -17,6 +17,7 @@ QGCUASFileView::QGCUASFileView(QWidget *parent, QGCUASFileManager *manager) : connect(ui->downloadButton, SIGNAL(clicked()), this, SLOT(downloadFiles())); connect(_manager, SIGNAL(statusMessage(QString)), ui->messageArea, SLOT(appendPlainText(QString))); + connect(_manager, SIGNAL(errorMessage(QString)), ui->messageArea, SLOT(appendPlainText(QString))); connect(_manager, SIGNAL(resetStatusMessages()), ui->messageArea, SLOT(clear())); } From a8f64c96240cf7f07ef14c1fb5b76eabcbb74a12 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 13:44:11 +0200 Subject: [PATCH 23/31] Update mavlink with latest static file fixes from upstream repo --- libs/mavlink/include/mavlink/v1.0/checksum.h | 2 +- libs/mavlink/include/mavlink/v1.0/common/common.h | 4 ++++ libs/mavlink/include/mavlink/v1.0/mavlink_helpers.h | 21 ++++++++++++++------- libs/mavlink/include/mavlink/v1.0/mavlink_types.h | 4 ++-- 4 files changed, 21 insertions(+), 10 deletions(-) diff --git a/libs/mavlink/include/mavlink/v1.0/checksum.h b/libs/mavlink/include/mavlink/v1.0/checksum.h index 948e080..7d9b6ac 100644 --- a/libs/mavlink/include/mavlink/v1.0/checksum.h +++ b/libs/mavlink/include/mavlink/v1.0/checksum.h @@ -73,7 +73,7 @@ static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length) * @param data new bytes to hash * @param crcAccum the already accumulated checksum **/ -static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length) +static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint16_t length) { const uint8_t *p = (const uint8_t *)pBuffer; while (length--) { diff --git a/libs/mavlink/include/mavlink/v1.0/common/common.h b/libs/mavlink/include/mavlink/v1.0/common/common.h index 020211d..de6e220 100644 --- a/libs/mavlink/include/mavlink/v1.0/common/common.h +++ b/libs/mavlink/include/mavlink/v1.0/common/common.h @@ -5,6 +5,10 @@ #ifndef COMMON_H #define COMMON_H +#ifndef MAVLINK_H + #error Wrong include order: common.h MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set all defines from mavlink.h manually. +#endif + #ifdef __cplusplus extern "C" { #endif diff --git a/libs/mavlink/include/mavlink/v1.0/mavlink_helpers.h b/libs/mavlink/include/mavlink/v1.0/mavlink_helpers.h index 96672f8..1639a83 100644 --- a/libs/mavlink/include/mavlink/v1.0/mavlink_helpers.h +++ b/libs/mavlink/include/mavlink/v1.0/mavlink_helpers.h @@ -73,7 +73,6 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui #endif { // This code part is the same for all messages; - uint16_t checksum; msg->magic = MAVLINK_STX; msg->len = length; msg->sysid = system_id; @@ -81,12 +80,13 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui // One sequence number per component msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; - checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN); + msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN); + crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len); #if MAVLINK_CRC_EXTRA - crc_accumulate(crc_extra, &checksum); + crc_accumulate(crc_extra, &msg->checksum); #endif - mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF); - mavlink_ck_b(msg) = (uint8_t)(checksum >> 8); + mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF); + mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8); return length + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -133,7 +133,7 @@ MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint buf[4] = mavlink_system.compid; buf[5] = msgid; status->current_tx_seq++; - checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); crc_accumulate_buffer(&checksum, packet, length); #if MAVLINK_CRC_EXTRA crc_accumulate(crc_extra, &checksum); @@ -158,6 +158,7 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m ck[0] = (uint8_t)(msg->checksum & 0xFF); ck[1] = (uint8_t)(msg->checksum >> 8); + // XXX use the right sequence here MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES); @@ -172,7 +173,13 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m */ MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg) { - memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len); + memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); + + uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); + + ck[0] = (uint8_t)(msg->checksum & 0xFF); + ck[1] = (uint8_t)(msg->checksum >> 8); + return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len; } diff --git a/libs/mavlink/include/mavlink/v1.0/mavlink_types.h b/libs/mavlink/include/mavlink/v1.0/mavlink_types.h index 4019c61..43658e6 100644 --- a/libs/mavlink/include/mavlink/v1.0/mavlink_types.h +++ b/libs/mavlink/include/mavlink/v1.0/mavlink_types.h @@ -28,6 +28,7 @@ #define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES) +#pragma pack(push, 1) typedef struct param_union { union { float param_float; @@ -62,13 +63,12 @@ typedef struct __mavlink_message { uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; } mavlink_message_t; - typedef struct __mavlink_extended_message { mavlink_message_t base_msg; int32_t extended_payload_len; ///< Length of extended payload if any uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN]; } mavlink_extended_message_t; - +#pragma pack(pop) typedef enum { MAVLINK_TYPE_CHAR = 0, From 30b9072d1764b9741de7246fd2b676a4d7a1e9d7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 13:51:11 +0200 Subject: [PATCH 24/31] Warning fix --- src/uas/QGCUASFileManager.cc | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/uas/QGCUASFileManager.cc b/src/uas/QGCUASFileManager.cc index 5b48b30..41d27d4 100644 --- a/src/uas/QGCUASFileManager.cc +++ b/src/uas/QGCUASFileManager.cc @@ -73,6 +73,8 @@ QGCUASFileManager::QGCUASFileManager(QObject* parent, UASInterface* uas) : { bool connected = connect(&_ackTimer, SIGNAL(timeout()), this, SLOT(_ackTimeout())); Q_ASSERT(connected); + + Q_UNUSED(connected); } /// @brief Calculates a 32 bit CRC for the specified request. @@ -357,4 +359,4 @@ void QGCUASFileManager::_sendRequest(Request* request) // FIXME: Send correct system id instead of harcoded 250 mavlink_msg_encapsulated_data_pack(250, 0, &message, _encdata_seq, (uint8_t*)request); _mav->sendMessage(message); -} \ No newline at end of file +} From f12c0bf32578e64767981a586b3cd20662f4186c Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 26 Jun 2014 15:13:10 -0700 Subject: [PATCH 25/31] Add support for FTP open/read commands --- src/qgcunittest/MockMavlinkFileServer.cc | 205 ++++++++++++++++++++++++------- src/qgcunittest/MockMavlinkFileServer.h | 20 ++- src/qgcunittest/QGCUASFileManagerTest.cc | 64 +++++++++- src/qgcunittest/QGCUASFileManagerTest.h | 3 + src/uas/QGCUASFileManager.cc | 160 +++++++++++++++++++----- src/uas/QGCUASFileManager.h | 21 +++- src/ui/QGCUASFileView.cc | 4 +- 7 files changed, 394 insertions(+), 83 deletions(-) diff --git a/src/qgcunittest/MockMavlinkFileServer.cc b/src/qgcunittest/MockMavlinkFileServer.cc index b17aa89..8264edb 100644 --- a/src/qgcunittest/MockMavlinkFileServer.cc +++ b/src/qgcunittest/MockMavlinkFileServer.cc @@ -23,11 +23,160 @@ #include "MockMavlinkFileServer.h" -void MockMavlinkFileServer::sendMessage(mavlink_message_t message) +const char* MockMavlinkFileServer::smallFilename = "small"; +const char* MockMavlinkFileServer::largeFilename = "large"; + +// FIXME: -2 to avoid eof on full packet +const uint8_t MockMavlinkFileServer::smallFileLength = sizeof(((QGCUASFileManager::Request*)0)->data) - 2; +const uint8_t MockMavlinkFileServer::largeFileLength = sizeof(((QGCUASFileManager::Request*)0)->data) + 1; + +const uint8_t MockMavlinkFileServer::_sessionId = 1; + + +/// @brief Handles List command requests. Only supports root folder paths. +/// File list returned is set using the setFileList method. +void MockMavlinkFileServer::_listCommand(QGCUASFileManager::Request* request) { QGCUASFileManager::Request ackResponse; QString path; + // We only support root path + path = (char *)&request->data[0]; + if (!path.isEmpty() && path != "/") { + _sendNak(QGCUASFileManager::kErrNotDir); + return; + } + + // Offset requested is past the end of the list + if (request->hdr.offset > (uint32_t)_fileList.size()) { + _sendNak(QGCUASFileManager::kErrEOF); + return; + } + + ackResponse.hdr.magic = 'f'; + ackResponse.hdr.opcode = QGCUASFileManager::kRspAck; + ackResponse.hdr.session = 0; + ackResponse.hdr.size = 0; + + if (request->hdr.offset == 0) { + // Requesting first batch of file names + Q_ASSERT(_fileList.size()); + char *bufPtr = (char *)&ackResponse.data[0]; + for (int i=0; i<_fileList.size(); i++) { + const char *filename = _fileList[i].toStdString().c_str(); + size_t cchFilename = strlen(filename); + strcpy(bufPtr, filename); + ackResponse.hdr.size += cchFilename + 1; + bufPtr += cchFilename + 1; + } + + // Final double termination + *bufPtr = 0; + ackResponse.hdr.size++; + + } else { + // All filenames fit in first ack, send final null terminated ack + ackResponse.data[0] = 0; + ackResponse.hdr.size = 1; + } + + _emitResponse(&ackResponse); +} + +/// @brief Handles Open command requests. Two filenames are supported: +/// '/small' - file fits in a single packet +/// '/large' - file requires multiple packets to be sent +/// In all cases file contents are one byte data length, followed by a single +/// byte repeating increasing sequence (0x00, 0x01, .. 0xFF) for specified +/// number of bytes. +void MockMavlinkFileServer::_openCommand(QGCUASFileManager::Request* request) +{ + QGCUASFileManager::Request response; + QString path; + + // Make sure one byte of length is enough to overflow into two packets. + Q_ASSERT((sizeof(request->data) & 0xFF) == sizeof(request->data)); + + path = (char *)&request->data[0]; + if (path == smallFilename) { + _readFileLength = smallFileLength; + qDebug() << "Reading file length" << smallFileLength; + } else if (path == largeFilename) { + _readFileLength = largeFileLength; + qDebug() << "Reading file length" << largeFileLength; + } else { + _sendNak(QGCUASFileManager::kErrNotFile); + return; + } + + response.hdr.magic = 'f'; + response.hdr.opcode = QGCUASFileManager::kRspAck; + response.hdr.session = _sessionId; + response.hdr.size = 0; + + _emitResponse(&response); +} + +/// @brief Handles Read command requests. +void MockMavlinkFileServer::_readCommand(QGCUASFileManager::Request* request) +{ + qDebug() << "Read command:" << request->hdr.offset; + + QGCUASFileManager::Request response; + + if (request->hdr.session != _sessionId) { + _sendNak(QGCUASFileManager::kErrNoSession); + return; + } + + if (request->hdr.size > sizeof(response.data)) { + _sendNak(QGCUASFileManager::kErrPerm); + return; + } + + uint8_t size = 0; + uint32_t offset = request->hdr.offset; + + // Offset check is > instead of >= to take into accoutn extra length byte at beginning of file + if (offset > _readFileLength) { + _sendNak(QGCUASFileManager::kErrIO); + return; + } + + // Write length byte if needed + if (offset == 0) { + response.data[0] = _readFileLength; + offset++; + size++; + } + + // Write file bytes. Data is a repeating sequence of 0x00, 0x01, .. 0xFF. + for (; size < sizeof(response.data) && offset <= _readFileLength; offset++, size++) { + response.data[size] = (offset - 1) & 0xFF; + } + + qDebug() << "_readCommand bytes written" << size; + + // If we didn't write any bytes it was a bad request + if (size == 0) { + _sendNak(QGCUASFileManager::kErrEOF); + return; + } + + response.hdr.magic = 'f'; + response.hdr.opcode = QGCUASFileManager::kRspAck; + response.hdr.session = _sessionId; + response.hdr.size = size; + response.hdr.offset = request->hdr.offset; + + _emitResponse(&response); +} + +/// @brief Handles messages sent to the FTP server. +void MockMavlinkFileServer::sendMessage(mavlink_message_t message) +{ + QGCUASFileManager::Request ackResponse; + Q_ASSERT(message.msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA); mavlink_encapsulated_data_t requestEncapsulatedData; @@ -59,57 +208,21 @@ void MockMavlinkFileServer::sendMessage(mavlink_message_t message) break; case QGCUASFileManager::kCmdList: - // We only support root path - path = (char *)&request->data[0]; - if (!path.isEmpty() && path != "/") { - _sendNak(QGCUASFileManager::kErrNotDir); - break; - } - - if (request->hdr.offset > (uint32_t)_fileList.size()) { - _sendNak(QGCUASFileManager::kErrEOF); - break; - } - - ackResponse.hdr.magic = 'f'; - ackResponse.hdr.opcode = QGCUASFileManager::kRspAck; - ackResponse.hdr.session = 0; - ackResponse.hdr.size = 0; - - if (request->hdr.offset == 0) { - // Requesting first batch of file names - Q_ASSERT(_fileList.size()); - char *bufPtr = (char *)&ackResponse.data[0]; - for (int i=0; i<_fileList.size(); i++) { - const char *filename = _fileList[i].toStdString().c_str(); - size_t cchFilename = strlen(filename); - strcpy(bufPtr, filename); - ackResponse.hdr.size += cchFilename + 1; - bufPtr += cchFilename + 1; - } - - // Final double termination - *bufPtr = 0; - ackResponse.hdr.size++; - - } else { - // All filenames fit in first ack, send final null terminated ack - ackResponse.data[0] = 0; - ackResponse.hdr.size = 1; - } + _listCommand(request); + break; - _emitResponse(&ackResponse); + case QGCUASFileManager::kCmdOpen: + _openCommand(request); + break; + case QGCUASFileManager::kCmdRead: + _readCommand(request); break; - + // Remainder of commands are NYI case QGCUASFileManager::kCmdTerminate: // releases sessionID, closes file - case QGCUASFileManager::kCmdOpen: - // opens for reading, returns - case QGCUASFileManager::kCmdRead: - // reads bytes from in case QGCUASFileManager::kCmdCreate: // creates for writing, returns case QGCUASFileManager::kCmdWrite: @@ -123,6 +236,7 @@ void MockMavlinkFileServer::sendMessage(mavlink_message_t message) } } +/// @brief Sends a Nak with the specified error code. void MockMavlinkFileServer::_sendNak(QGCUASFileManager::ErrorCode error) { QGCUASFileManager::Request nakResponse; @@ -136,6 +250,7 @@ void MockMavlinkFileServer::_sendNak(QGCUASFileManager::ErrorCode error) _emitResponse(&nakResponse); } +/// @brief Emits a Request through the messageReceived signal. void MockMavlinkFileServer::_emitResponse(QGCUASFileManager::Request* request) { mavlink_message_t mavlinkMessage; diff --git a/src/qgcunittest/MockMavlinkFileServer.h b/src/qgcunittest/MockMavlinkFileServer.h index 0bccd3d..6abe12f 100644 --- a/src/qgcunittest/MockMavlinkFileServer.h +++ b/src/qgcunittest/MockMavlinkFileServer.h @@ -27,6 +27,11 @@ #include "MockMavlinkInterface.h" #include "QGCUASFileManager.h" +/// @file +/// @brief Mock implementation of Mavlink FTP server. Used as mavlink plugin to MockUAS. +/// Only root directory access is supported. +/// +/// @author Don Gagne #include @@ -37,16 +42,29 @@ class MockMavlinkFileServer : public MockMavlinkInterface public: MockMavlinkFileServer(void) { }; + /// @brief Sets the list of files returned by the List command. Prepend names with F or D + /// to indicate (F)ile or (D)irectory. void setFileList(QStringList& fileList) { _fileList = fileList; } // From MockMavlinkInterface virtual void sendMessage(mavlink_message_t message); + static const char* smallFilename; + static const char* largeFilename; + static const uint8_t smallFileLength; + static const uint8_t largeFileLength; + private: void _sendNak(QGCUASFileManager::ErrorCode error); void _emitResponse(QGCUASFileManager::Request* request); + void _listCommand(QGCUASFileManager::Request* request); + void _openCommand(QGCUASFileManager::Request* request); + void _readCommand(QGCUASFileManager::Request* request); + + QStringList _fileList; ///< List of files returned by List command - QStringList _fileList; + static const uint8_t _sessionId; + uint8_t _readFileLength; ///< Length of active file being read }; #endif diff --git a/src/qgcunittest/QGCUASFileManagerTest.cc b/src/qgcunittest/QGCUASFileManagerTest.cc index fe325de..c18b116 100644 --- a/src/qgcunittest/QGCUASFileManagerTest.cc +++ b/src/qgcunittest/QGCUASFileManagerTest.cc @@ -24,7 +24,9 @@ #include "QGCUASFileManagerTest.h" /// @file -/// @brief QGCUASFileManager unit test +/// @brief QGCUASFileManager unit test. Note: All code here assumes all work between +/// the unit test, mack mavlink file server and file manager is happening on +/// the same thread. /// /// @author Don Gagne @@ -154,3 +156,63 @@ void QGCUASFileManagerUnitTest::_listTest(void) QCOMPARE(_multiSpy->checkNoSignalByMask(errorMessageSignalMask), true); // We should not get an error signals QVERIFY(_fileListReceived == fileListExpected); } + +void QGCUASFileManagerUnitTest::_validateFileContents(const QString& filePath, uint8_t length) +{ + QFile file(filePath); + QVERIFY(file.open(QIODevice::ReadOnly)); + QByteArray bytes = file.readAll(); + file.close(); + + QCOMPARE(bytes.length(), length + 1); // +1 for length byte + + // Validate length byte + QCOMPARE((uint8_t)bytes[0], length); + + // Validate file contents + for (uint8_t i=1; icheckNoSignals() == true); + + // Send a bogus path + // We should get a single resetStatusMessages signal + // We should get a single errorMessage signal + _fileManager->downloadPath("bogus", QDir::temp()); + QCOMPARE(_multiSpy->checkOnlySignalByMask(errorMessageSignalMask | resetStatusMessagesSignalMask), true); + _multiSpy->clearAllSignals(); + + // Clean previous downloads + + QString smallFilePath; + smallFilePath = QDir::temp().absoluteFilePath(MockMavlinkFileServer::smallFilename); + if (QFile::exists(smallFilePath)) { + Q_ASSERT(QFile::remove(smallFilePath)); + } + + QString largeFilePath; + largeFilePath = QDir::temp().absoluteFilePath(MockMavlinkFileServer::largeFilename); + if (QFile::exists(largeFilePath)) { + Q_ASSERT(QFile::remove(largeFilePath)); + } + + // Send a valid file to download + // We should get a single resetStatusMessages signal + // We should get a single statusMessage signal, which indicated download completion + + _fileManager->downloadPath(MockMavlinkFileServer::smallFilename, QDir::temp()); + QCOMPARE(_multiSpy->checkOnlySignalByMask(statusMessageSignalMask | resetStatusMessagesSignalMask), true); + _multiSpy->clearAllSignals(); + _validateFileContents(smallFilePath, MockMavlinkFileServer::smallFileLength); + + _fileManager->downloadPath(MockMavlinkFileServer::largeFilename, QDir::temp()); + QCOMPARE(_multiSpy->checkOnlySignalByMask(statusMessageSignalMask | resetStatusMessagesSignalMask), true); + _multiSpy->clearAllSignals(); + _validateFileContents(largeFilePath, MockMavlinkFileServer::largeFileLength); +} diff --git a/src/qgcunittest/QGCUASFileManagerTest.h b/src/qgcunittest/QGCUASFileManagerTest.h index 2a76fbb..a2c6ac3 100644 --- a/src/qgcunittest/QGCUASFileManagerTest.h +++ b/src/qgcunittest/QGCUASFileManagerTest.h @@ -56,11 +56,14 @@ private slots: void _noAckTest(void); void _resetTest(void); void _listTest(void); + void _openTest(void); // Connected to QGCUASFileManager statusMessage signal void statusMessage(const QString&); private: + void _validateFileContents(const QString& filePath, uint8_t length); + enum { statusMessageSignalIndex = 0, errorMessageSignalIndex, diff --git a/src/uas/QGCUASFileManager.cc b/src/uas/QGCUASFileManager.cc index 41d27d4..1efe9b2 100644 --- a/src/uas/QGCUASFileManager.cc +++ b/src/uas/QGCUASFileManager.cc @@ -69,7 +69,8 @@ QGCUASFileManager::QGCUASFileManager(QObject* parent, UASInterface* uas) : QObject(parent), _currentOperation(kCOIdle), _mav(uas), - _encdata_seq(0) + _encdata_seq(0), + _activeSession(0) { bool connected = connect(&_ackTimer, SIGNAL(timeout()), this, SLOT(_ackTimeout())); Q_ASSERT(connected); @@ -103,6 +104,81 @@ void QGCUASFileManager::nothingMessage() // FIXME: Connect ui correctly } +/// @brief Respond to the Ack associated with the Open command. +void QGCUASFileManager::_openResponse(Request* openAck) +{ + _currentOperation = kCORead; + _activeSession = openAck->hdr.session; + _readOffset = 0; + _readFileAccumulator.clear(); + + Request request; + request.hdr.magic = 'f'; + request.hdr.session = _activeSession; + request.hdr.opcode = kCmdRead; + request.hdr.offset = _readOffset; + request.hdr.size = sizeof(request.data); + + _sendRequest(&request); +} + +/// @brief Respond to the Ack associated with the Read command. +void QGCUASFileManager::_readResponse(Request* readAck) +{ + if (readAck->hdr.session != _activeSession) { + _currentOperation = kCOIdle; + _readFileAccumulator.clear(); + _emitErrorMessage(tr("Read: Incorrect session returned")); + return; + } + + if (readAck->hdr.offset != _readOffset) { + _currentOperation = kCOIdle; + _readFileAccumulator.clear(); + _emitErrorMessage(tr("Read: Offset returned (%1) differs from offset requested (%2)").arg(readAck->hdr.offset).arg(_readOffset)); + return; + } + + qDebug() << "Accumulator size" << readAck->hdr.size; + _readFileAccumulator.append((const char*)readAck->data, readAck->hdr.size); + + if (readAck->hdr.size == sizeof(readAck->data)) { + // Still more data to read + _currentOperation = kCORead; + + _readOffset += sizeof(readAck->data); + + Request request; + request.hdr.magic = 'f'; + request.hdr.session = _activeSession; + request.hdr.opcode = kCmdRead; + request.hdr.offset = _readOffset; + request.hdr.size = sizeof(request.data); + + _sendRequest(&request); + } else { + _currentOperation = kCOIdle; + + QString downloadFilePath = _readFileDownloadDir.absoluteFilePath(_readFileDownloadFilename); + + QFile file(downloadFilePath); + if (!file.open(QIODevice::WriteOnly | QIODevice::Truncate)) { + _emitErrorMessage(tr("Unable to open local file for writing (%1)").arg(downloadFilePath)); + return; + } + + qint64 bytesWritten = file.write((const char *)_readFileAccumulator, _readFileAccumulator.length()); + if (bytesWritten != _readFileAccumulator.length()) { + file.close(); + _emitErrorMessage(tr("Unable to write data to local file (%1)").arg(downloadFilePath)); + return; + } + file.close(); + + _emitStatusMessage(tr("Download complete '%1'").arg(downloadFilePath)); + } +} + void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t message) { Q_UNUSED(link); @@ -116,8 +192,6 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me mavlink_msg_encapsulated_data_decode(&message, &data); Request* request = (Request*)&data.data[0]; - qDebug() << "FTP: opcode:" << request->hdr.opcode; - // FIXME: Check CRC if (request->hdr.opcode == kRspAck) { @@ -132,14 +206,21 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me case kCOAck: // We are expecting an ack back - _clearAckTimeout(); _currentOperation = kCOIdle; break; case kCOList: listDecode(&request->data[0], request->hdr.size); break; + + case kCOOpen: + _openResponse(request); + break; + case kCORead: + _readResponse(request); + break; + default: _emitErrorMessage("Ack received in unexpected state"); break; @@ -196,7 +277,7 @@ void QGCUASFileManager::listDecode(const uint8_t *data, unsigned len) } // put it in the view - emit statusMessage(s); + _emitStatusMessage(s); // account for the name + NUL offset += nlen + 1; @@ -217,6 +298,12 @@ void QGCUASFileManager::listDecode(const uint8_t *data, unsigned len) } } +void QGCUASFileManager::_fillRequestWithString(Request* request, const QString& str) +{ + strncpy((char *)&request->data[0], str.toStdString().c_str(), sizeof(request->data)); + request->hdr.size = strnlen((const char *)&request->data[0], sizeof(request->data)); +} + void QGCUASFileManager::sendList() { Request request; @@ -226,32 +313,44 @@ void QGCUASFileManager::sendList() request.hdr.opcode = kCmdList; request.hdr.offset = _listOffset; - strncpy((char *)&request.data[0], _listPath.toStdString().c_str(), sizeof(request.data)); - request.hdr.size = strnlen((const char *)&request.data[0], sizeof(request.data)); + _fillRequestWithString(&request, _listPath); _sendRequest(&request); } -void QGCUASFileManager::downloadPath(const QString &from, const QString &to) +/// @brief Downloads the specified file. +/// @param from File to download from UAS, fully qualified path +/// @param downloadDir Local directory to download file to +void QGCUASFileManager::downloadPath(const QString& from, const QDir& downloadDir) { - Q_UNUSED(from); + if (from.isEmpty()) { + return; + } - // Send path, e.g. /fs/microsd and download content - // recursively into a local directory - - char buf[255]; - unsigned len = 10; - - QByteArray data(buf, len); - QString filename = "log001.bin"; // XXX get this from onboard - - // Qt takes care of slash conversions in paths - QFile file(to + QDir::separator() + filename); - file.open(QIODevice::WriteOnly); - file.write(data); - file.close(); + _readFileDownloadDir.setPath(downloadDir.absolutePath()); + + // We need to strip off the file name from the fully qualified path. We can't use the usual QDir + // routines because this path does not exist locally. + int i; + for (i=from.size()-1; i>=0; i--) { + if (from[i] == '/') { + break; + } + } + i++; // move past slash + _readFileDownloadFilename = from.right(from.size() - i); + + emit resetStatusMessages(); + + _currentOperation = kCOOpen; - emit statusMessage(QString("Downloaded: %1 to directory %2").arg(filename).arg(to)); + Request request; + request.hdr.magic = 'f'; + request.hdr.session = 0; + request.hdr.opcode = kCmdOpen; + request.hdr.offset = 0; + _fillRequestWithString(&request, from); + _sendRequest(&request); } QString QGCUASFileManager::errorString(uint8_t errorCode) @@ -307,7 +406,6 @@ bool QGCUASFileManager::_sendOpcodeOnlyCmd(uint8_t opcode, OperationState newOpS request.hdr.size = 0; _currentOperation = newOpState; - _setupAckTimeout(); _sendRequest(&request); @@ -317,8 +415,6 @@ bool QGCUASFileManager::_sendOpcodeOnlyCmd(uint8_t opcode, OperationState newOpS /// @brief Starts the ack timeout timer void QGCUASFileManager::_setupAckTimeout(void) { - qDebug() << "Setting Ack"; - Q_ASSERT(!_ackTimer.isActive()); _ackTimer.setSingleShot(true); @@ -328,8 +424,6 @@ void QGCUASFileManager::_setupAckTimeout(void) /// @brief Clears the ack timeout timer void QGCUASFileManager::_clearAckTimeout(void) { - qDebug() << "Clearing Ack"; - Q_ASSERT(_ackTimer.isActive()); _ackTimer.stop(); @@ -344,10 +438,16 @@ void QGCUASFileManager::_ackTimeout(void) void QGCUASFileManager::_emitErrorMessage(const QString& msg) { - qDebug() << "QGCUASFileManager:" << msg; + qDebug() << "QGCUASFileManager: Error" << msg; emit errorMessage(msg); } +void QGCUASFileManager::_emitStatusMessage(const QString& msg) +{ + qDebug() << "QGCUASFileManager: Status" << msg; + emit statusMessage(msg); +} + /// @brief Sends the specified Request out to the UAS. void QGCUASFileManager::_sendRequest(Request* request) { diff --git a/src/uas/QGCUASFileManager.h b/src/uas/QGCUASFileManager.h index 5c41c0b..408c74c 100644 --- a/src/uas/QGCUASFileManager.h +++ b/src/uas/QGCUASFileManager.h @@ -25,6 +25,7 @@ #define QGCUASFILEMANAGER_H #include +#include #include "UASInterface.h" @@ -48,7 +49,7 @@ public slots: void receiveMessage(LinkInterface* link, mavlink_message_t message); void nothingMessage(); void listRecursively(const QString &from); - void downloadPath(const QString& from, const QString& to); + void downloadPath(const QString& from, const QDir& downloadDir); protected: struct RequestHeader @@ -109,7 +110,9 @@ protected: { kCOIdle, // not doing anything kCOAck, // waiting for an Ack - kCOList, // waiting for a List response + kCOList, // waiting for List response + kCOOpen, // waiting for Open response + kCORead, // waiting for Read response }; @@ -121,7 +124,11 @@ protected: void _setupAckTimeout(void); void _clearAckTimeout(void); void _emitErrorMessage(const QString& msg); + void _emitStatusMessage(const QString& msg); void _sendRequest(Request* request); + void _fillRequestWithString(Request* request, const QString& str); + void _openResponse(Request* openAck); + void _readResponse(Request* readAck); void sendList(); void listDecode(const uint8_t *data, unsigned len); @@ -136,8 +143,14 @@ protected: UASInterface* _mav; quint16 _encdata_seq; - unsigned _listOffset; // offset for the current List operation - QString _listPath; // path for the current List operation + unsigned _listOffset; ///> offset for the current List operation + QString _listPath; ///> path for the current List operation + + uint8_t _activeSession; ///> currently active session, 0 for none + uint32_t _readOffset; ///> current read offset + QByteArray _readFileAccumulator; ///> Holds file being downloaded + QDir _readFileDownloadDir; ///> Directory to download file to + QString _readFileDownloadFilename; ///> Filename (no path) for download file // We give MockMavlinkFileServer friend access so that it can use the data structures and opcodes // to build a mock mavlink file server for testing. diff --git a/src/ui/QGCUASFileView.cc b/src/ui/QGCUASFileView.cc index 09ccc3b..f932348 100644 --- a/src/ui/QGCUASFileView.cc +++ b/src/ui/QGCUASFileView.cc @@ -33,10 +33,10 @@ void QGCUASFileView::listFiles() void QGCUASFileView::downloadFiles() { - QString dir = QFileDialog::getExistingDirectory(this, tr("Open Directory"), + QString dir = QFileDialog::getExistingDirectory(this, tr("Download Directory"), QDir::homePath(), QFileDialog::ShowDirsOnly | QFileDialog::DontResolveSymlinks); // And now download to this location - _manager->downloadPath(ui->pathLineEdit->text(), dir); + _manager->downloadPath(ui->pathLineEdit->text(), QDir(dir)); } From e2e246fe5da6e2736b9d35e386fc85371c7fceb6 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 26 Jun 2014 19:18:09 -0700 Subject: [PATCH 26/31] Support for Open/Read/Terminate FTP commands MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Also modified protocol to better support eof’s on list and read commands. --- src/qgcunittest/MockMavlinkFileServer.cc | 159 ++++++++++++++++++----------- src/qgcunittest/MockMavlinkFileServer.h | 18 +++- src/qgcunittest/QGCUASFileManagerTest.cc | 64 ++++++------ src/uas/QGCUASFileManager.cc | 168 +++++++++++++++++++------------ src/uas/QGCUASFileManager.h | 57 ++++++----- src/ui/QGCUASFileView.cc | 2 +- 6 files changed, 284 insertions(+), 184 deletions(-) diff --git a/src/qgcunittest/MockMavlinkFileServer.cc b/src/qgcunittest/MockMavlinkFileServer.cc index 8264edb..ef56b6a 100644 --- a/src/qgcunittest/MockMavlinkFileServer.cc +++ b/src/qgcunittest/MockMavlinkFileServer.cc @@ -23,20 +23,31 @@ #include "MockMavlinkFileServer.h" -const char* MockMavlinkFileServer::smallFilename = "small"; -const char* MockMavlinkFileServer::largeFilename = "large"; - -// FIXME: -2 to avoid eof on full packet -const uint8_t MockMavlinkFileServer::smallFileLength = sizeof(((QGCUASFileManager::Request*)0)->data) - 2; -const uint8_t MockMavlinkFileServer::largeFileLength = sizeof(((QGCUASFileManager::Request*)0)->data) + 1; +const MockMavlinkFileServer::FileTestCase MockMavlinkFileServer::rgFileTestCases[MockMavlinkFileServer::cFileTestCases] = { + // File fits one Read Ack packet, partially filling data + { "partial.qgc", sizeof(((QGCUASFileManager::Request*)0)->data) - 1 }, + // File fits one Read Ack packet, exactly filling all data + { "exact.qgc", sizeof(((QGCUASFileManager::Request*)0)->data) }, + // File is larger than a single Read Ack packets, requires multiple Reads + { "multi.qgc", sizeof(((QGCUASFileManager::Request*)0)->data) + 1 }, +}; +// We only support a single fixed session const uint8_t MockMavlinkFileServer::_sessionId = 1; +MockMavlinkFileServer::MockMavlinkFileServer(void) +{ + +} + + /// @brief Handles List command requests. Only supports root folder paths. /// File list returned is set using the setFileList method. void MockMavlinkFileServer::_listCommand(QGCUASFileManager::Request* request) { + // FIXME: Does not support directories that span multiple packets + QGCUASFileManager::Request ackResponse; QString path; @@ -56,7 +67,9 @@ void MockMavlinkFileServer::_listCommand(QGCUASFileManager::Request* request) ackResponse.hdr.magic = 'f'; ackResponse.hdr.opcode = QGCUASFileManager::kRspAck; ackResponse.hdr.session = 0; + ackResponse.hdr.offset = request->hdr.offset; ackResponse.hdr.size = 0; + ackResponse.hdr.errCode = QGCUASFileManager::kErrNone; if (request->hdr.offset == 0) { // Requesting first batch of file names @@ -69,42 +82,35 @@ void MockMavlinkFileServer::_listCommand(QGCUASFileManager::Request* request) ackResponse.hdr.size += cchFilename + 1; bufPtr += cchFilename + 1; } - - // Final double termination - *bufPtr = 0; - ackResponse.hdr.size++; - } else { - // All filenames fit in first ack, send final null terminated ack - ackResponse.data[0] = 0; - ackResponse.hdr.size = 1; + // FIXME: Does not support directories that span multiple packets + _sendNak(QGCUASFileManager::kErrEOF); } _emitResponse(&ackResponse); } -/// @brief Handles Open command requests. Two filenames are supported: -/// '/small' - file fits in a single packet -/// '/large' - file requires multiple packets to be sent -/// In all cases file contents are one byte data length, followed by a single -/// byte repeating increasing sequence (0x00, 0x01, .. 0xFF) for specified -/// number of bytes. +/// @brief Handles Open command requests. void MockMavlinkFileServer::_openCommand(QGCUASFileManager::Request* request) { QGCUASFileManager::Request response; QString path; - // Make sure one byte of length is enough to overflow into two packets. - Q_ASSERT((sizeof(request->data) & 0xFF) == sizeof(request->data)); + size_t cchPath = strnlen((char *)request->data, sizeof(request->data)); + Q_ASSERT(cchPath != sizeof(request->data)); + path = (char *)request->data; + + // Check path against one of our known test cases - path = (char *)&request->data[0]; - if (path == smallFilename) { - _readFileLength = smallFileLength; - qDebug() << "Reading file length" << smallFileLength; - } else if (path == largeFilename) { - _readFileLength = largeFileLength; - qDebug() << "Reading file length" << largeFileLength; - } else { + bool found = false; + for (size_t i=0; ihdr.offset; - QGCUASFileManager::Request response; if (request->hdr.session != _sessionId) { @@ -129,53 +134,70 @@ void MockMavlinkFileServer::_readCommand(QGCUASFileManager::Request* request) return; } - if (request->hdr.size > sizeof(response.data)) { - _sendNak(QGCUASFileManager::kErrPerm); - return; - } - - uint8_t size = 0; - uint32_t offset = request->hdr.offset; + uint32_t readOffset = request->hdr.offset; // offset into file for reading + uint8_t cDataBytes = 0; // current number of data bytes used - // Offset check is > instead of >= to take into accoutn extra length byte at beginning of file - if (offset > _readFileLength) { - _sendNak(QGCUASFileManager::kErrIO); + if (readOffset > _readFileLength) { + _sendNak(QGCUASFileManager::kErrEOF); return; } // Write length byte if needed - if (offset == 0) { + if (readOffset == 0) { response.data[0] = _readFileLength; - offset++; - size++; + readOffset++; + cDataBytes++; } // Write file bytes. Data is a repeating sequence of 0x00, 0x01, .. 0xFF. - for (; size < sizeof(response.data) && offset <= _readFileLength; offset++, size++) { - response.data[size] = (offset - 1) & 0xFF; + for (; cDataBytes < sizeof(response.data) && readOffset < _readFileLength; readOffset++, cDataBytes++) { + // Subtract one from readOffset to take into account length byte and start file data a 0x00 + response.data[cDataBytes] = (readOffset - 1) & 0xFF; } - qDebug() << "_readCommand bytes written" << size; - - // If we didn't write any bytes it was a bad request - if (size == 0) { - _sendNak(QGCUASFileManager::kErrEOF); - return; - } + // We should always have written something, otherwise there is something wrong with the code above + Q_ASSERT(cDataBytes); response.hdr.magic = 'f'; response.hdr.opcode = QGCUASFileManager::kRspAck; response.hdr.session = _sessionId; - response.hdr.size = size; + response.hdr.size = cDataBytes; response.hdr.offset = request->hdr.offset; + if (readOffset >= _readFileLength) { + // Something wrong with the reading code, should not have gone past last byte + Q_ASSERT(readOffset == _readFileLength); + + // We have read all the bytes in the file + response.hdr.errCode = QGCUASFileManager::kErrNone; + + } else { + // There are still more bytes left to read in the file + response.hdr.errCode = QGCUASFileManager::kErrMore; + } + _emitResponse(&response); } +/// @brief Handles Terminate commands +void MockMavlinkFileServer::_terminateCommand(QGCUASFileManager::Request* request) +{ + if (request->hdr.session != _sessionId) { + _sendNak(QGCUASFileManager::kErrNoSession); + return; + } + + _sendAck(); + + // Let our test harness know that we got a terminate command. This is used to validate the a Terminate is correctly + // sent after an Open. + emit terminateCommandReceived(); +} + /// @brief Handles messages sent to the FTP server. void MockMavlinkFileServer::sendMessage(mavlink_message_t message) { - QGCUASFileManager::Request ackResponse; + QGCUASFileManager::Request ackResponse; Q_ASSERT(message.msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA); @@ -204,6 +226,7 @@ void MockMavlinkFileServer::sendMessage(mavlink_message_t message) ackResponse.hdr.session = 0; ackResponse.hdr.crc32 = 0; ackResponse.hdr.size = 0; + ackResponse.hdr.errCode = QGCUASFileManager::kErrNone; _emitResponse(&ackResponse); break; @@ -219,10 +242,12 @@ void MockMavlinkFileServer::sendMessage(mavlink_message_t message) _readCommand(request); break; + case QGCUASFileManager::kCmdTerminate: + _terminateCommand(request); + break; + // Remainder of commands are NYI - case QGCUASFileManager::kCmdTerminate: - // releases sessionID, closes file case QGCUASFileManager::kCmdCreate: // creates for writing, returns case QGCUASFileManager::kCmdWrite: @@ -236,6 +261,20 @@ void MockMavlinkFileServer::sendMessage(mavlink_message_t message) } } +/// @brief Sends an Ack +void MockMavlinkFileServer::_sendAck(void) +{ + QGCUASFileManager::Request ackResponse; + + ackResponse.hdr.magic = 'f'; + ackResponse.hdr.opcode = QGCUASFileManager::kRspAck; + ackResponse.hdr.session = 0; + ackResponse.hdr.size = 0; + ackResponse.hdr.errCode = QGCUASFileManager::kErrNone; + + _emitResponse(&ackResponse); +} + /// @brief Sends a Nak with the specified error code. void MockMavlinkFileServer::_sendNak(QGCUASFileManager::ErrorCode error) { @@ -244,8 +283,8 @@ void MockMavlinkFileServer::_sendNak(QGCUASFileManager::ErrorCode error) nakResponse.hdr.magic = 'f'; nakResponse.hdr.opcode = QGCUASFileManager::kRspNak; nakResponse.hdr.session = 0; - nakResponse.hdr.size = sizeof(nakResponse.data[0]); - nakResponse.data[0] = error; + nakResponse.hdr.size = 0; + nakResponse.hdr.errCode = error; _emitResponse(&nakResponse); } diff --git a/src/qgcunittest/MockMavlinkFileServer.h b/src/qgcunittest/MockMavlinkFileServer.h index 6abe12f..7c581c7 100644 --- a/src/qgcunittest/MockMavlinkFileServer.h +++ b/src/qgcunittest/MockMavlinkFileServer.h @@ -40,7 +40,7 @@ class MockMavlinkFileServer : public MockMavlinkInterface Q_OBJECT public: - MockMavlinkFileServer(void) { }; + MockMavlinkFileServer(void); /// @brief Sets the list of files returned by the List command. Prepend names with F or D /// to indicate (F)ile or (D)irectory. @@ -49,17 +49,25 @@ public: // From MockMavlinkInterface virtual void sendMessage(mavlink_message_t message); - static const char* smallFilename; - static const char* largeFilename; - static const uint8_t smallFileLength; - static const uint8_t largeFileLength; + struct FileTestCase { + const char* filename; + uint8_t length; + }; + + static const size_t cFileTestCases = 3; + static const FileTestCase rgFileTestCases[cFileTestCases]; + +signals: + void terminateCommandReceived(void); private: + void _sendAck(void); void _sendNak(QGCUASFileManager::ErrorCode error); void _emitResponse(QGCUASFileManager::Request* request); void _listCommand(QGCUASFileManager::Request* request); void _openCommand(QGCUASFileManager::Request* request); void _readCommand(QGCUASFileManager::Request* request); + void _terminateCommand(QGCUASFileManager::Request* request); QStringList _fileList; ///< List of files returned by List command diff --git a/src/qgcunittest/QGCUASFileManagerTest.cc b/src/qgcunittest/QGCUASFileManagerTest.cc index c18b116..7da1dbd 100644 --- a/src/qgcunittest/QGCUASFileManagerTest.cc +++ b/src/qgcunittest/QGCUASFileManagerTest.cc @@ -132,7 +132,7 @@ void QGCUASFileManagerUnitTest::_listTest(void) // Send a bogus path // We should get a single resetStatusMessages signal // We should get a single errorMessage signal - _fileManager->listRecursively("/bogus"); + _fileManager->listDirectory("/bogus"); QCOMPARE(_multiSpy->checkOnlySignalByMask(errorMessageSignalMask | resetStatusMessagesSignalMask), true); _multiSpy->clearAllSignals(); @@ -151,7 +151,7 @@ void QGCUASFileManagerUnitTest::_listTest(void) _fileListReceived.clear(); - _fileManager->listRecursively("/"); + _fileManager->listDirectory("/"); QCOMPARE(_multiSpy->checkSignalByMask(resetStatusMessagesSignalMask), true); // We should be told to reset status messages QCOMPARE(_multiSpy->checkNoSignalByMask(errorMessageSignalMask), true); // We should not get an error signals QVERIFY(_fileListReceived == fileListExpected); @@ -160,16 +160,20 @@ void QGCUASFileManagerUnitTest::_listTest(void) void QGCUASFileManagerUnitTest::_validateFileContents(const QString& filePath, uint8_t length) { QFile file(filePath); + + // Make sure file size is correct + QCOMPARE(file.size(), (qint64)length); + + // Read data QVERIFY(file.open(QIODevice::ReadOnly)); QByteArray bytes = file.readAll(); file.close(); - QCOMPARE(bytes.length(), length + 1); // +1 for length byte - // Validate length byte QCOMPARE((uint8_t)bytes[0], length); - // Validate file contents + // Validate file contents: + // Repeating 0x00, 0x01 .. 0xFF until file is full for (uint8_t i=1; iclearAllSignals(); // Clean previous downloads - - QString smallFilePath; - smallFilePath = QDir::temp().absoluteFilePath(MockMavlinkFileServer::smallFilename); - if (QFile::exists(smallFilePath)) { - Q_ASSERT(QFile::remove(smallFilePath)); - } - - QString largeFilePath; - largeFilePath = QDir::temp().absoluteFilePath(MockMavlinkFileServer::largeFilename); - if (QFile::exists(largeFilePath)) { - Q_ASSERT(QFile::remove(largeFilePath)); + for (size_t i=0; idownloadPath(MockMavlinkFileServer::smallFilename, QDir::temp()); - QCOMPARE(_multiSpy->checkOnlySignalByMask(statusMessageSignalMask | resetStatusMessagesSignalMask), true); - _multiSpy->clearAllSignals(); - _validateFileContents(smallFilePath, MockMavlinkFileServer::smallFileLength); - _fileManager->downloadPath(MockMavlinkFileServer::largeFilename, QDir::temp()); - QCOMPARE(_multiSpy->checkOnlySignalByMask(statusMessageSignalMask | resetStatusMessagesSignalMask), true); - _multiSpy->clearAllSignals(); - _validateFileContents(largeFilePath, MockMavlinkFileServer::largeFileLength); + // Run through the set of file test cases + + // We setup a spy on the terminate command signal so that we can determine that a Terminate command was + // correctly sent after the Open/Read commands complete. + QSignalSpy terminateSpy(&_mockFileServer, SIGNAL(terminateCommandReceived())); + + for (size_t i=0; idownloadPath(MockMavlinkFileServer::rgFileTestCases[i].filename, QDir::temp()); + + // We should get a single resetStatusMessages signal + // We should get a single statusMessage signal, which indicated download completion + QCOMPARE(_multiSpy->checkOnlySignalByMask(statusMessageSignalMask | resetStatusMessagesSignalMask), true); + _multiSpy->clearAllSignals(); + + // We should get a single Terminate command + QCOMPARE(terminateSpy.count(), 1); + terminateSpy.clear(); + + QString filePath = QDir::temp().absoluteFilePath(MockMavlinkFileServer::rgFileTestCases[i].filename); + _validateFileContents(filePath, MockMavlinkFileServer::rgFileTestCases[i].length); + } } diff --git a/src/uas/QGCUASFileManager.cc b/src/uas/QGCUASFileManager.cc index 1efe9b2..41412c0 100644 --- a/src/uas/QGCUASFileManager.cc +++ b/src/uas/QGCUASFileManager.cc @@ -104,13 +104,14 @@ void QGCUASFileManager::nothingMessage() // FIXME: Connect ui correctly } -/// @brief Respond to the Ack associated with the Open command. -void QGCUASFileManager::_openResponse(Request* openAck) +/// @brief Respond to the Ack associated with the Open command with the next Read command. +void QGCUASFileManager::_openAckResponse(Request* openAck) { _currentOperation = kCORead; _activeSession = openAck->hdr.session; - _readOffset = 0; - _readFileAccumulator.clear(); + + _readOffset = 0; // Start reading at beginning of file + _readFileAccumulator.clear(); // Start with an empty file Request request; request.hdr.magic = 'f'; @@ -123,7 +124,7 @@ void QGCUASFileManager::_openResponse(Request* openAck) } /// @brief Respond to the Ack associated with the Read command. -void QGCUASFileManager::_readResponse(Request* readAck) +void QGCUASFileManager::_readAckResponse(Request* readAck) { if (readAck->hdr.session != _activeSession) { _currentOperation = kCOIdle; @@ -139,24 +140,27 @@ void QGCUASFileManager::_readResponse(Request* readAck) return; } - qDebug() << "Accumulator size" << readAck->hdr.size; _readFileAccumulator.append((const char*)readAck->data, readAck->hdr.size); - if (readAck->hdr.size == sizeof(readAck->data)) { - // Still more data to read + if (readAck->hdr.errCode == kErrMore) { + // Still more data to read, send next read request + _currentOperation = kCORead; - _readOffset += sizeof(readAck->data); + _readOffset += readAck->hdr.size; Request request; request.hdr.magic = 'f'; request.hdr.session = _activeSession; request.hdr.opcode = kCmdRead; request.hdr.offset = _readOffset; - request.hdr.size = sizeof(request.data); _sendRequest(&request); } else { + // We're at the end of the file, we can write it out now + + Q_ASSERT(readAck->hdr.errCode == kErrNone); + _currentOperation = kCOIdle; QString downloadFilePath = _readFileDownloadDir.absoluteFilePath(_readFileDownloadFilename); @@ -176,6 +180,70 @@ void QGCUASFileManager::_readResponse(Request* readAck) file.close(); _emitStatusMessage(tr("Download complete '%1'").arg(downloadFilePath)); + + // Close the open session + _sendTerminateCommand(); + } +} + +/// @brief Respond to the Ack associated with the List command. +void QGCUASFileManager::_listAckResponse(Request* listAck) +{ + if (listAck->hdr.offset != _listOffset) { + _currentOperation = kCOIdle; + _emitErrorMessage(tr("List: Offset returned (%1) differs from offset requested (%2)").arg(listAck->hdr.offset).arg(_listOffset)); + return; + } + + uint8_t offset = 0; + uint8_t cListEntries = 0; + uint8_t cBytes = listAck->hdr.size; + + // parse filenames out of the buffer + while (offset < cBytes) { + const char * ptr = ((const char *)listAck->data) + offset; + + // get the length of the name + uint8_t cBytesLeft = cBytes - offset; + size_t nlen = strnlen(ptr, cBytesLeft); + if (nlen < 2) { + _currentOperation = kCOIdle; + _emitErrorMessage(tr("Incorrectly formed list entry: '%1'").arg(ptr)); + return; + } else if (nlen == cBytesLeft) { + _currentOperation = kCOIdle; + _emitErrorMessage(tr("Missing NULL termination in list entry")); + return; + } + + // Returned names are prepended with D for directory or F for file + QString s(ptr + 1); + if (*ptr == 'D') { + s.append('/'); + } else if (*ptr != 'F') { + _currentOperation = kCOIdle; + _emitErrorMessage(tr("Unknown prefix list entry: '%1'").arg(ptr)); + return; + } + + // put it in the view + _emitStatusMessage(s); + + // account for the name + NUL + offset += nlen + 1; + + cListEntries++; + } + + if (listAck->hdr.errCode == kErrMore) { + // Still more data to read, send next list request + _currentOperation = kCOList; + _listOffset += cListEntries; + _sendListCommand(); + } else { + // We've gotten the last list entries we can go back to idle + Q_ASSERT(listAck->hdr.errCode == kErrNone); + _currentOperation = kCOIdle; } } @@ -200,7 +268,6 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me switch (_currentOperation) { case kCOIdle: // we should not be seeing anything here.. shut the other guy up - qDebug() << "FTP resetting file transfer session"; _sendCmdReset(); break; @@ -210,15 +277,15 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me break; case kCOList: - listDecode(&request->data[0], request->hdr.size); + _listAckResponse(request); break; case kCOOpen: - _openResponse(request); + _openAckResponse(request); break; case kCORead: - _readResponse(request); + _readAckResponse(request); break; default: @@ -227,7 +294,7 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me } } else if (request->hdr.opcode == kRspNak) { _clearAckTimeout(); - _emitErrorMessage(QString("Nak received, error: ").append(errorString(request->data[0]))); + _emitErrorMessage(tr("Nak received, error: %1").arg(request->hdr.errCode)); _currentOperation = kCOIdle; } else { // Note that we don't change our operation state. If something goes wrong beyond this, the operation @@ -236,7 +303,7 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me } } -void QGCUASFileManager::listRecursively(const QString &from) +void QGCUASFileManager::listDirectory(const QString& dirPath) { if (_currentOperation != kCOIdle) { _emitErrorMessage(tr("Command not sent. Waiting for previous command to complete.")); @@ -247,55 +314,12 @@ void QGCUASFileManager::listRecursively(const QString &from) emit resetStatusMessages(); // initialise the lister - _listPath = from; + _listPath = dirPath; _listOffset = 0; _currentOperation = kCOList; // and send the initial request - sendList(); -} - -void QGCUASFileManager::listDecode(const uint8_t *data, unsigned len) -{ - unsigned offset = 0; - unsigned files = 0; - - // parse filenames out of the buffer - while (offset < len) { - const char * ptr = (const char *)data + offset; - - // get the length of the name - unsigned nlen = strnlen(ptr, len - offset); - if (nlen < 2) { - break; - } - - // Returned names are prepended with D for directory or F for file - QString s(ptr + 1); - if (*ptr == 'D') { - s.append('/'); - } - - // put it in the view - _emitStatusMessage(s); - - // account for the name + NUL - offset += nlen + 1; - - // account for the file - files++; - } - - // we have run out of files to list - if (files == 0) { - _currentOperation = kCOIdle; - } else { - // update our state - _listOffset += files; - - // and send another request - sendList(); - } + _sendListCommand(); } void QGCUASFileManager::_fillRequestWithString(Request* request, const QString& str) @@ -304,7 +328,7 @@ void QGCUASFileManager::_fillRequestWithString(Request* request, const QString& request->hdr.size = strnlen((const char *)&request->data[0], sizeof(request->data)); } -void QGCUASFileManager::sendList() +void QGCUASFileManager::_sendListCommand(void) { Request request; @@ -432,8 +456,26 @@ void QGCUASFileManager::_clearAckTimeout(void) /// @brief Called when ack timeout timer fires void QGCUASFileManager::_ackTimeout(void) { - _currentOperation = kCOIdle; _emitErrorMessage(tr("Timeout waiting for ack")); + + switch (_currentOperation) { + case kCORead: + _currentOperation = kCOAck; + _sendTerminateCommand(); + break; + default: + _currentOperation = kCOIdle; + break; + } +} + +void QGCUASFileManager::_sendTerminateCommand(void) +{ + Request request; + request.hdr.magic = 'f'; + request.hdr.session = _activeSession; + request.hdr.opcode = kCmdTerminate; + _sendRequest(&request); } void QGCUASFileManager::_emitErrorMessage(const QString& msg) diff --git a/src/uas/QGCUASFileManager.h b/src/uas/QGCUASFileManager.h index 408c74c..48fb5be 100644 --- a/src/uas/QGCUASFileManager.h +++ b/src/uas/QGCUASFileManager.h @@ -48,18 +48,19 @@ signals: public slots: void receiveMessage(LinkInterface* link, mavlink_message_t message); void nothingMessage(); - void listRecursively(const QString &from); + void listDirectory(const QString& dirPath); void downloadPath(const QString& from, const QDir& downloadDir); protected: struct RequestHeader { - uint8_t magic; - uint8_t session; - uint8_t opcode; - uint8_t size; - uint32_t crc32; - uint32_t offset; + uint8_t magic; ///> Magic byte 'f' to idenitfy FTP protocol + uint8_t session; ///> Session id for read and write commands + uint8_t opcode; ///> Command opcode + uint8_t size; ///> Size of data + uint32_t crc32; ///> CRC for entire Request structure, with crc32 set to 0 + uint32_t offset; ///> Offsets for List and Read commands + uint8_t errCode; ///> Error code from Ack and Naks (ignored for commands) }; struct Request @@ -72,25 +73,29 @@ protected: enum Opcode { - kCmdNone, // ignored, always acked - kCmdTerminate, // releases sessionID, closes file - kCmdReset, // terminates all sessions - kCmdList, // list files in from - kCmdOpen, // opens for reading, returns - kCmdRead, // reads bytes from in - kCmdCreate, // creates for writing, returns - kCmdWrite, // appends bytes at in - kCmdRemove, // remove file (only if created by server?) - - kRspAck, - kRspNak, + // Commands + kCmdNone, ///> ignored, always acked + kCmdTerminate, ///> releases sessionID, closes file + kCmdReset, ///> terminates all sessions + kCmdList, ///> list files in from + kCmdOpen, ///> opens for reading, returns + kCmdRead, ///> reads bytes from in + kCmdCreate, ///> creates for writing, returns + kCmdWrite, ///> appends bytes at in + kCmdRemove, ///> remove file (only if created by server?) + + // Responses + kRspAck, ///> positive acknowledgement of previous command + kRspNak, ///> negative acknowledgement of previous command - kCmdTestNoAck, // ignored, ack not sent back, for testing only, should timeout waiting for ack + // Used for testing only, not part of protocol + kCmdTestNoAck, // ignored, ack not sent back, should timeout waiting for ack }; enum ErrorCode { kErrNone, + kErrMore, kErrNoRequest, kErrNoSession, kErrSequence, @@ -127,12 +132,12 @@ protected: void _emitStatusMessage(const QString& msg); void _sendRequest(Request* request); void _fillRequestWithString(Request* request, const QString& str); - void _openResponse(Request* openAck); - void _readResponse(Request* readAck); - - void sendList(); - void listDecode(const uint8_t *data, unsigned len); - + void _openAckResponse(Request* openAck); + void _readAckResponse(Request* readAck); + void _listAckResponse(Request* listAck); + void _sendListCommand(void); + void _sendTerminateCommand(void); + static quint32 crc32(Request* request, unsigned state = 0); static QString errorString(uint8_t errorCode); diff --git a/src/ui/QGCUASFileView.cc b/src/ui/QGCUASFileView.cc index f932348..55412e1 100644 --- a/src/ui/QGCUASFileView.cc +++ b/src/ui/QGCUASFileView.cc @@ -28,7 +28,7 @@ QGCUASFileView::~QGCUASFileView() void QGCUASFileView::listFiles() { - _manager->listRecursively(ui->pathLineEdit->text()); + _manager->listDirectory(ui->pathLineEdit->text()); } void QGCUASFileView::downloadFiles() From d2c3e365a9569e096be7c236dfdfbfeb7e242916 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Fri, 27 Jun 2014 20:15:42 -0700 Subject: [PATCH 27/31] Switch to kErrEOF style read loop handling --- src/qgcunittest/MockMavlinkFileServer.cc | 28 ++------ src/uas/QGCUASFileManager.cc | 118 +++++++++++++++++++------------ src/uas/QGCUASFileManager.h | 3 +- 3 files changed, 78 insertions(+), 71 deletions(-) diff --git a/src/qgcunittest/MockMavlinkFileServer.cc b/src/qgcunittest/MockMavlinkFileServer.cc index ef56b6a..4e8d66f 100644 --- a/src/qgcunittest/MockMavlinkFileServer.cc +++ b/src/qgcunittest/MockMavlinkFileServer.cc @@ -69,7 +69,6 @@ void MockMavlinkFileServer::_listCommand(QGCUASFileManager::Request* request) ackResponse.hdr.session = 0; ackResponse.hdr.offset = request->hdr.offset; ackResponse.hdr.size = 0; - ackResponse.hdr.errCode = QGCUASFileManager::kErrNone; if (request->hdr.offset == 0) { // Requesting first batch of file names @@ -82,12 +81,12 @@ void MockMavlinkFileServer::_listCommand(QGCUASFileManager::Request* request) ackResponse.hdr.size += cchFilename + 1; bufPtr += cchFilename + 1; } + + _emitResponse(&ackResponse); } else { // FIXME: Does not support directories that span multiple packets _sendNak(QGCUASFileManager::kErrEOF); } - - _emitResponse(&ackResponse); } /// @brief Handles Open command requests. @@ -119,7 +118,6 @@ void MockMavlinkFileServer::_openCommand(QGCUASFileManager::Request* request) response.hdr.opcode = QGCUASFileManager::kRspAck; response.hdr.session = _sessionId; response.hdr.size = 0; - response.hdr.errCode = QGCUASFileManager::kErrNone; _emitResponse(&response); } @@ -137,7 +135,7 @@ void MockMavlinkFileServer::_readCommand(QGCUASFileManager::Request* request) uint32_t readOffset = request->hdr.offset; // offset into file for reading uint8_t cDataBytes = 0; // current number of data bytes used - if (readOffset > _readFileLength) { + if (readOffset >= _readFileLength) { _sendNak(QGCUASFileManager::kErrEOF); return; } @@ -159,22 +157,10 @@ void MockMavlinkFileServer::_readCommand(QGCUASFileManager::Request* request) Q_ASSERT(cDataBytes); response.hdr.magic = 'f'; - response.hdr.opcode = QGCUASFileManager::kRspAck; response.hdr.session = _sessionId; response.hdr.size = cDataBytes; response.hdr.offset = request->hdr.offset; - - if (readOffset >= _readFileLength) { - // Something wrong with the reading code, should not have gone past last byte - Q_ASSERT(readOffset == _readFileLength); - - // We have read all the bytes in the file - response.hdr.errCode = QGCUASFileManager::kErrNone; - - } else { - // There are still more bytes left to read in the file - response.hdr.errCode = QGCUASFileManager::kErrMore; - } + response.hdr.opcode = QGCUASFileManager::kRspAck; _emitResponse(&response); } @@ -226,7 +212,6 @@ void MockMavlinkFileServer::sendMessage(mavlink_message_t message) ackResponse.hdr.session = 0; ackResponse.hdr.crc32 = 0; ackResponse.hdr.size = 0; - ackResponse.hdr.errCode = QGCUASFileManager::kErrNone; _emitResponse(&ackResponse); break; @@ -270,7 +255,6 @@ void MockMavlinkFileServer::_sendAck(void) ackResponse.hdr.opcode = QGCUASFileManager::kRspAck; ackResponse.hdr.session = 0; ackResponse.hdr.size = 0; - ackResponse.hdr.errCode = QGCUASFileManager::kErrNone; _emitResponse(&ackResponse); } @@ -283,8 +267,8 @@ void MockMavlinkFileServer::_sendNak(QGCUASFileManager::ErrorCode error) nakResponse.hdr.magic = 'f'; nakResponse.hdr.opcode = QGCUASFileManager::kRspNak; nakResponse.hdr.session = 0; - nakResponse.hdr.size = 0; - nakResponse.hdr.errCode = error; + nakResponse.hdr.size = 1; + nakResponse.data[0] = error; _emitResponse(&nakResponse); } diff --git a/src/uas/QGCUASFileManager.cc b/src/uas/QGCUASFileManager.cc index 41412c0..8eaee04 100644 --- a/src/uas/QGCUASFileManager.cc +++ b/src/uas/QGCUASFileManager.cc @@ -65,6 +65,7 @@ static const quint32 crctab[] = 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d }; + QGCUASFileManager::QGCUASFileManager(QObject* parent, UASInterface* uas) : QObject(parent), _currentOperation(kCOIdle), @@ -74,8 +75,7 @@ QGCUASFileManager::QGCUASFileManager(QObject* parent, UASInterface* uas) : { bool connected = connect(&_ackTimer, SIGNAL(timeout()), this, SLOT(_ackTimeout())); Q_ASSERT(connected); - - Q_UNUSED(connected); + Q_UNUSED(connected); // Silence retail unused variable error } /// @brief Calculates a 32 bit CRC for the specified request. @@ -123,6 +123,34 @@ void QGCUASFileManager::_openAckResponse(Request* openAck) _sendRequest(&request); } +/// @brief Closes out a read session by writing the file and doing cleanup. +/// @param success true: successful download completion, false: error during download +void QGCUASFileManager::_closeReadSession(bool success) +{ + if (success) { + QString downloadFilePath = _readFileDownloadDir.absoluteFilePath(_readFileDownloadFilename); + + QFile file(downloadFilePath); + if (!file.open(QIODevice::WriteOnly | QIODevice::Truncate)) { + _emitErrorMessage(tr("Unable to open local file for writing (%1)").arg(downloadFilePath)); + return; + } + + qint64 bytesWritten = file.write((const char *)_readFileAccumulator, _readFileAccumulator.length()); + if (bytesWritten != _readFileAccumulator.length()) { + file.close(); + _emitErrorMessage(tr("Unable to write data to local file (%1)").arg(downloadFilePath)); + return; + } + file.close(); + + _emitStatusMessage(tr("Download complete '%1'").arg(downloadFilePath)); + } + + // Close the open session + _sendTerminateCommand(); +} + /// @brief Respond to the Ack associated with the Read command. void QGCUASFileManager::_readAckResponse(Request* readAck) { @@ -142,8 +170,8 @@ void QGCUASFileManager::_readAckResponse(Request* readAck) _readFileAccumulator.append((const char*)readAck->data, readAck->hdr.size); - if (readAck->hdr.errCode == kErrMore) { - // Still more data to read, send next read request + if (readAck->hdr.size == sizeof(readAck->data)) { + // Possibly still more data to read, send next read request _currentOperation = kCORead; @@ -157,32 +185,9 @@ void QGCUASFileManager::_readAckResponse(Request* readAck) _sendRequest(&request); } else { - // We're at the end of the file, we can write it out now - - Q_ASSERT(readAck->hdr.errCode == kErrNone); - + // We only receieved a partial buffer back. These means we are at EOF _currentOperation = kCOIdle; - - QString downloadFilePath = _readFileDownloadDir.absoluteFilePath(_readFileDownloadFilename); - - QFile file(downloadFilePath); - if (!file.open(QIODevice::WriteOnly | QIODevice::Truncate)) { - _emitErrorMessage(tr("Unable to open local file for writing (%1)").arg(downloadFilePath)); - return; - } - - qint64 bytesWritten = file.write((const char *)_readFileAccumulator, _readFileAccumulator.length()); - if (bytesWritten != _readFileAccumulator.length()) { - file.close(); - _emitErrorMessage(tr("Unable to write data to local file (%1)").arg(downloadFilePath)); - return; - } - file.close(); - - _emitStatusMessage(tr("Download complete '%1'").arg(downloadFilePath)); - - // Close the open session - _sendTerminateCommand(); + _closeReadSession(true /* success */); } } @@ -216,34 +221,33 @@ void QGCUASFileManager::_listAckResponse(Request* listAck) return; } - // Returned names are prepended with D for directory or F for file + // Returned names are prepended with D for directory, F for file, U for unknown + QString s(ptr + 1); if (*ptr == 'D') { s.append('/'); - } else if (*ptr != 'F') { - _currentOperation = kCOIdle; - _emitErrorMessage(tr("Unknown prefix list entry: '%1'").arg(ptr)); - return; } - // put it in the view - _emitStatusMessage(s); - + if (*ptr == 'F' || *ptr == 'D') { + // put it in the view + _emitStatusMessage(s); + } + // account for the name + NUL offset += nlen + 1; cListEntries++; } - if (listAck->hdr.errCode == kErrMore) { - // Still more data to read, send next list request + if (listAck->hdr.size == 0) { + // Directory is empty, we're done + Q_ASSERT(listAck->hdr.opcode == kRspAck); + _currentOperation = kCOIdle; + } else { + // Possibly more entries to come, need to keep trying till we get EOF _currentOperation = kCOList; _listOffset += cListEntries; _sendListCommand(); - } else { - // We've gotten the last list entries we can go back to idle - Q_ASSERT(listAck->hdr.errCode == kErrNone); - _currentOperation = kCOIdle; } } @@ -256,6 +260,8 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me return; } + _clearAckTimeout(); + mavlink_encapsulated_data_t data; mavlink_msg_encapsulated_data_decode(&message, &data); Request* request = (Request*)&data.data[0]; @@ -263,7 +269,6 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me // FIXME: Check CRC if (request->hdr.opcode == kRspAck) { - _clearAckTimeout(); switch (_currentOperation) { case kCOIdle: @@ -293,13 +298,32 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me break; } } else if (request->hdr.opcode == kRspNak) { - _clearAckTimeout(); - _emitErrorMessage(tr("Nak received, error: %1").arg(request->hdr.errCode)); + Q_ASSERT(request->hdr.size == 1); // Should only have one byte of error code + + OperationState previousOperation = _currentOperation; + uint8_t errorCode = request->data[0]; + _currentOperation = kCOIdle; + + if (previousOperation == kCOList && errorCode == kErrEOF) { + // This is not an error, just the end of the read loop + return; + } else if (previousOperation == kCORead && errorCode == kErrEOF) { + // This is not an error, just the end of the read loop + _closeReadSession(true /* success */); + return; + } else { + // Generic Nak handling + if (previousOperation == kCORead) { + // Nak error during read loop, download failed + _closeReadSession(false /* failure */); + } + _emitErrorMessage(tr("Nak received, error: %1").arg(errorString(request->data[0]))); + } } else { // Note that we don't change our operation state. If something goes wrong beyond this, the operation // will time out. - _emitErrorMessage(tr("Unknown opcode returned server: %1").arg(request->hdr.opcode)); + _emitErrorMessage(tr("Unknown opcode returned from server: %1").arg(request->hdr.opcode)); } } diff --git a/src/uas/QGCUASFileManager.h b/src/uas/QGCUASFileManager.h index 48fb5be..77cf33f 100644 --- a/src/uas/QGCUASFileManager.h +++ b/src/uas/QGCUASFileManager.h @@ -60,7 +60,6 @@ protected: uint8_t size; ///> Size of data uint32_t crc32; ///> CRC for entire Request structure, with crc32 set to 0 uint32_t offset; ///> Offsets for List and Read commands - uint8_t errCode; ///> Error code from Ack and Naks (ignored for commands) }; struct Request @@ -95,7 +94,6 @@ protected: enum ErrorCode { kErrNone, - kErrMore, kErrNoRequest, kErrNoSession, kErrSequence, @@ -137,6 +135,7 @@ protected: void _listAckResponse(Request* listAck); void _sendListCommand(void); void _sendTerminateCommand(void); + void _closeReadSession(bool success); static quint32 crc32(Request* request, unsigned state = 0); static QString errorString(uint8_t errorCode); From 94aaeb561f9fa61d4c9a85ecd474a7ed10fb25e1 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Fri, 27 Jun 2014 20:42:19 -0700 Subject: [PATCH 28/31] Fix warning from Linux/Windows build --- src/qgcunittest/MockUAS.cc | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/qgcunittest/MockUAS.cc b/src/qgcunittest/MockUAS.cc index 443bb93..82dedff 100644 --- a/src/qgcunittest/MockUAS.cc +++ b/src/qgcunittest/MockUAS.cc @@ -46,8 +46,6 @@ void MockUAS::setMockParametersAndSignal(MockQGCUASParamManager::ParamMap_t& map void MockUAS::sendMessage(mavlink_message_t message) { - Q_UNUSED(link); - if (!_mavlinkPlugin) { Q_ASSERT(false); } From 6d2bdd849a38275e3a8a51800cda080108c61cb7 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 28 Jun 2014 10:52:47 -0700 Subject: [PATCH 29/31] Workaround odd Windows std::string problem --- src/qgcunittest/MockMavlinkFileServer.cc | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/qgcunittest/MockMavlinkFileServer.cc b/src/qgcunittest/MockMavlinkFileServer.cc index 4e8d66f..68ad158 100644 --- a/src/qgcunittest/MockMavlinkFileServer.cc +++ b/src/qgcunittest/MockMavlinkFileServer.cc @@ -69,15 +69,17 @@ void MockMavlinkFileServer::_listCommand(QGCUASFileManager::Request* request) ackResponse.hdr.session = 0; ackResponse.hdr.offset = request->hdr.offset; ackResponse.hdr.size = 0; + + qDebug() << _fileList; if (request->hdr.offset == 0) { // Requesting first batch of file names Q_ASSERT(_fileList.size()); char *bufPtr = (char *)&ackResponse.data[0]; for (int i=0; i<_fileList.size(); i++) { - const char *filename = _fileList[i].toStdString().c_str(); - size_t cchFilename = strlen(filename); - strcpy(bufPtr, filename); + strcpy(bufPtr, _fileList[i].toStdString().c_str()); + size_t cchFilename = strlen(bufPtr); + Q_ASSERT(cchFilename); ackResponse.hdr.size += cchFilename + 1; bufPtr += cchFilename + 1; } From 70c5ce7315c1f653791c3b9733866de79ff610c1 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 28 Jun 2014 10:56:25 -0700 Subject: [PATCH 30/31] Remove missed qDebug --- src/qgcunittest/MockMavlinkFileServer.cc | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/qgcunittest/MockMavlinkFileServer.cc b/src/qgcunittest/MockMavlinkFileServer.cc index 68ad158..6577611 100644 --- a/src/qgcunittest/MockMavlinkFileServer.cc +++ b/src/qgcunittest/MockMavlinkFileServer.cc @@ -70,8 +70,6 @@ void MockMavlinkFileServer::_listCommand(QGCUASFileManager::Request* request) ackResponse.hdr.offset = request->hdr.offset; ackResponse.hdr.size = 0; - qDebug() << _fileList; - if (request->hdr.offset == 0) { // Requesting first batch of file names Q_ASSERT(_fileList.size()); From 84cd97cab89f39d78345554f1e402fc271c5998e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 5 Jul 2014 19:39:58 -0700 Subject: [PATCH 31/31] Changed FTP to tree view Also added selection from tree view for downloading --- src/uas/QGCUASFileManager.cc | 17 +--- src/uas/QGCUASFileManager.h | 4 +- src/ui/QGCUASFileView.cc | 199 ++++++++++++++++++++++++++++++++++++++----- src/ui/QGCUASFileView.h | 52 ++++++++--- src/ui/QGCUASFileView.ui | 30 ++----- 5 files changed, 234 insertions(+), 68 deletions(-) diff --git a/src/uas/QGCUASFileManager.cc b/src/uas/QGCUASFileManager.cc index 8eaee04..41b726d 100644 --- a/src/uas/QGCUASFileManager.cc +++ b/src/uas/QGCUASFileManager.cc @@ -99,11 +99,6 @@ quint32 QGCUASFileManager::crc32(Request* request, unsigned state) return state; } -void QGCUASFileManager::nothingMessage() -{ - // FIXME: Connect ui correctly -} - /// @brief Respond to the Ack associated with the Open command with the next Read command. void QGCUASFileManager::_openAckResponse(Request* openAck) { @@ -220,17 +215,11 @@ void QGCUASFileManager::_listAckResponse(Request* listAck) _emitErrorMessage(tr("Missing NULL termination in list entry")); return; } - - // Returned names are prepended with D for directory, F for file, U for unknown - QString s(ptr + 1); - if (*ptr == 'D') { - s.append('/'); - } - + // Returned names are prepended with D for directory, F for file, U for unknown if (*ptr == 'F' || *ptr == 'D') { // put it in the view - _emitStatusMessage(s); + _emitStatusMessage(ptr); } // account for the name + NUL @@ -243,6 +232,7 @@ void QGCUASFileManager::_listAckResponse(Request* listAck) // Directory is empty, we're done Q_ASSERT(listAck->hdr.opcode == kRspAck); _currentOperation = kCOIdle; + emit listComplete(); } else { // Possibly more entries to come, need to keep trying till we get EOF _currentOperation = kCOList; @@ -307,6 +297,7 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me if (previousOperation == kCOList && errorCode == kErrEOF) { // This is not an error, just the end of the read loop + emit listComplete(); return; } else if (previousOperation == kCORead && errorCode == kErrEOF) { // This is not an error, just the end of the read loop diff --git a/src/uas/QGCUASFileManager.h b/src/uas/QGCUASFileManager.h index 77cf33f..5a3bc84 100644 --- a/src/uas/QGCUASFileManager.h +++ b/src/uas/QGCUASFileManager.h @@ -43,11 +43,11 @@ public: signals: void statusMessage(const QString& msg); void resetStatusMessages(); - void errorMessage(const QString& ms); + void errorMessage(const QString& msg); + void listComplete(void); public slots: void receiveMessage(LinkInterface* link, mavlink_message_t message); - void nothingMessage(); void listDirectory(const QString& dirPath); void downloadPath(const QString& from, const QDir& downloadDir); diff --git a/src/ui/QGCUASFileView.cc b/src/ui/QGCUASFileView.cc index 55412e1..a9ac957 100644 --- a/src/ui/QGCUASFileView.cc +++ b/src/ui/QGCUASFileView.cc @@ -1,42 +1,199 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + 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. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + #include "QGCUASFileView.h" #include "uas/QGCUASFileManager.h" -#include "ui_QGCUASFileView.h" #include #include +#include QGCUASFileView::QGCUASFileView(QWidget *parent, QGCUASFileManager *manager) : QWidget(parent), - _manager(manager), - ui(new Ui::QGCUASFileView) + _manager(manager) { - ui->setupUi(this); + _ui.setupUi(this); - connect(ui->testButton, SIGNAL(clicked()), _manager, SLOT(nothingMessage())); - connect(ui->listFilesButton, SIGNAL(clicked()), this, SLOT(listFiles())); - connect(ui->downloadButton, SIGNAL(clicked()), this, SLOT(downloadFiles())); + bool success = connect(_ui.listFilesButton, SIGNAL(clicked()), this, SLOT(_refreshTree())); + Q_ASSERT(success); + success = connect(_ui.downloadButton, SIGNAL(clicked()), this, SLOT(_downloadFiles())); + Q_ASSERT(success); + success = connect(_ui.treeWidget, SIGNAL(currentItemChanged(QTreeWidgetItem*, QTreeWidgetItem*)), this, SLOT(_currentItemChanged(QTreeWidgetItem*, QTreeWidgetItem*))); + Q_ASSERT(success); + Q_UNUSED(success); +} - connect(_manager, SIGNAL(statusMessage(QString)), ui->messageArea, SLOT(appendPlainText(QString))); - connect(_manager, SIGNAL(errorMessage(QString)), ui->messageArea, SLOT(appendPlainText(QString))); - connect(_manager, SIGNAL(resetStatusMessages()), ui->messageArea, SLOT(clear())); +void QGCUASFileView::_downloadFiles(void) +{ + QString dir = QFileDialog::getExistingDirectory(this, tr("Download Directory"), + QDir::homePath(), + QFileDialog::ShowDirsOnly + | QFileDialog::DontResolveSymlinks); + // And now download to this location + QString path; + QTreeWidgetItem* item = _ui.treeWidget->currentItem(); + if (item && item->type() == _typeFile) { + do { + path.prepend("/" + item->text(0)); + item = item->parent(); + } while (item); + qDebug() << "Download: " << path; + + bool success = connect(_manager, SIGNAL(statusMessage(QString)), this, SLOT(_downloadStatusMessage(QString))); + Q_ASSERT(success); + success = connect(_manager, SIGNAL(errorMessage(QString)), this, SLOT(_downloadStatusMessage(QString))); + Q_ASSERT(success); + Q_UNUSED(success); + _manager->downloadPath(path, QDir(dir)); + } } -QGCUASFileView::~QGCUASFileView() +void QGCUASFileView::_refreshTree(void) { - delete ui; + QTreeWidgetItem* item; + + for (int i=_ui.treeWidget->invisibleRootItem()->childCount(); i>=0; i--) { + item = _ui.treeWidget->takeTopLevelItem(i); + delete item; + } + + _walkIndexStack.clear(); + _walkItemStack.clear(); + _walkIndexStack.append(0); + _walkItemStack.append(_ui.treeWidget->invisibleRootItem()); + + bool success = connect(_manager, SIGNAL(statusMessage(QString)), this, SLOT(_treeStatusMessage(QString))); + Q_ASSERT(success); + success = connect(_manager, SIGNAL(errorMessage(QString)), this, SLOT(_treeErrorMessage(QString))); + Q_ASSERT(success); + success = connect(_manager, SIGNAL(listComplete(void)), this, SLOT(_listComplete(void))); + Q_ASSERT(success); + Q_UNUSED(success); + + qDebug() << "List: /"; + _manager->listDirectory("/"); } -void QGCUASFileView::listFiles() +void QGCUASFileView::_treeStatusMessage(const QString& msg) { - _manager->listDirectory(ui->pathLineEdit->text()); + int type; + if (msg.startsWith("F")) { + type = _typeFile; + } else if (msg.startsWith("D")) { + type = _typeDir; + if (msg == "D." || msg == "D..") { + return; + } + } else { + Q_ASSERT(false); + } + + QTreeWidgetItem* item; + if (_walkItemStack.count() == 0) { + item = new QTreeWidgetItem(_ui.treeWidget, type); + } else { + item = new QTreeWidgetItem(_walkItemStack.last(), type); + } + Q_CHECK_PTR(item); + + item->setText(0, msg.right(msg.size() - 1)); } -void QGCUASFileView::downloadFiles() +void QGCUASFileView::_treeErrorMessage(const QString& msg) { - QString dir = QFileDialog::getExistingDirectory(this, tr("Download Directory"), - QDir::homePath(), - QFileDialog::ShowDirsOnly - | QFileDialog::DontResolveSymlinks); - // And now download to this location - _manager->downloadPath(ui->pathLineEdit->text(), QDir(dir)); + QTreeWidgetItem* item; + if (_walkItemStack.count() == 0) { + item = new QTreeWidgetItem(_ui.treeWidget, _typeError); + } else { + item = new QTreeWidgetItem(_walkItemStack.last(), _typeError); + } + Q_CHECK_PTR(item); + + item->setText(0, tr("Error: ") + msg); +} + +void QGCUASFileView::_listComplete(void) +{ + // Walk the current items, traversing down into directories + +Again: + int walkIndex = _walkIndexStack.last(); + QTreeWidgetItem* parentItem = _walkItemStack.last(); + QTreeWidgetItem* childItem = parentItem->child(walkIndex); + + // Loop until we hit a directory + while (childItem && childItem->type() != _typeDir) { + // Move to next index at current level + _walkIndexStack.last() = ++walkIndex; + childItem = parentItem->child(walkIndex); + } + + if (childItem) { + // Process this item + QString text = childItem->text(0); + + // Move to the next item for processing at this level + _walkIndexStack.last() = ++walkIndex; + + // Push this new directory on the stack + _walkItemStack.append(childItem); + _walkIndexStack.append(0); + + // Ask for the directory list + QString dir; + for (int i=1; i<_walkItemStack.count(); i++) { + QTreeWidgetItem* item = _walkItemStack[i]; + dir.append("/" + item->text(0)); + } + qDebug() << "List:" << dir; + _manager->listDirectory(dir); + } else { + // We have run out of items at the this level, pop the stack and keep going at that level + _walkIndexStack.removeLast(); + _walkItemStack.removeLast(); + if (_walkIndexStack.count() != 0) { + goto Again; + } else { + disconnect(_manager, SIGNAL(statusMessage(QString)), this, SLOT(_treeStatusMessage(QString))); + disconnect(_manager, SIGNAL(errorMessage(QString)), this, SLOT(_treeErrorMessage(QString))); + disconnect(_manager, SIGNAL(listComplete(void)), this, SLOT(_listComplete(void))); + } + } +} + +void QGCUASFileView::_downloadStatusMessage(const QString& msg) +{ + disconnect(_manager, SIGNAL(statusMessage(QString)), this, SLOT(_downloadStatusMessage(QString))); + disconnect(_manager, SIGNAL(errorMessage(QString)), this, SLOT(_downloadStatusMessage(QString))); + + QMessageBox msgBox; + msgBox.setWindowModality(Qt::ApplicationModal); + msgBox.setText(msg); + msgBox.exec(); +} + +void QGCUASFileView::_currentItemChanged(QTreeWidgetItem* current, QTreeWidgetItem* previous) +{ + Q_UNUSED(previous); + _ui.downloadButton->setEnabled(current->type() == _typeFile); } diff --git a/src/ui/QGCUASFileView.h b/src/ui/QGCUASFileView.h index 0bb3700..0b33e5b 100644 --- a/src/ui/QGCUASFileView.h +++ b/src/ui/QGCUASFileView.h @@ -1,12 +1,34 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + 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. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + #ifndef QGCUASFILEVIEW_H #define QGCUASFILEVIEW_H #include -#include "uas/QGCUASFileManager.h" +#include -namespace Ui { -class QGCUASFileView; -} +#include "uas/QGCUASFileManager.h" +#include "ui_QGCUASFileView.h" class QGCUASFileView : public QWidget { @@ -14,17 +36,27 @@ class QGCUASFileView : public QWidget public: explicit QGCUASFileView(QWidget *parent, QGCUASFileManager *manager); - ~QGCUASFileView(); - -public slots: - void listFiles(); - void downloadFiles(); protected: QGCUASFileManager* _manager; + +private slots: + void _refreshTree(void); + void _downloadFiles(void); + void _treeStatusMessage(const QString& msg); + void _treeErrorMessage(const QString& msg); + void _listComplete(void); + void _downloadStatusMessage(const QString& msg); + void _currentItemChanged(QTreeWidgetItem* current, QTreeWidgetItem* previous); private: - Ui::QGCUASFileView *ui; + static const int _typeFile = QTreeWidgetItem::UserType + 1; + static const int _typeDir = QTreeWidgetItem::UserType + 2; + static const int _typeError = QTreeWidgetItem::UserType + 3; + + QList _walkIndexStack; + QList _walkItemStack; + Ui::QGCUASFileView _ui; }; #endif // QGCUASFILEVIEW_H diff --git a/src/ui/QGCUASFileView.ui b/src/ui/QGCUASFileView.ui index 1cbc196..1bb3552 100644 --- a/src/ui/QGCUASFileView.ui +++ b/src/ui/QGCUASFileView.ui @@ -14,7 +14,7 @@ Form - + List Files @@ -22,10 +22,10 @@ - - - + + true + 1 @@ -33,30 +33,16 @@ - - - - Null Message - - - - + - - Download File + + false - - - - - Path: + Download File - - -