Merge pull request #4295 from DonLakeFlyer/MavCommandRetryWork

Mav command retry work

qgroundcontrol.pro

@@ -381,6 +381,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory  { APMFirmwarePlugin {
         src/qgcunittest/TCPLinkTest.h \
         src/qgcunittest/TCPLoopBackServer.h \
         src/qgcunittest/UnitTest.h \
+        src/Vehicle/SendMavCommandTest.h \
 
     SOURCES += \
         src/AnalyzeView/LogDownloadTest.cc \
@@ -409,6 +410,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory  { APMFirmwarePlugin {
         src/qgcunittest/TCPLoopBackServer.cc \
         src/qgcunittest/UnitTest.cc \
         src/qgcunittest/UnitTestList.cc \
+        src/Vehicle/SendMavCommandTest.cc \
 } } } } } }
 
 # Main QGC Headers and Source files

src/AutoPilotPlugins/APM/APMCompassCal.cc

@@ -145,7 +145,10 @@ CalWorkerThread::calibrate_return CalWorkerThread::calibrate(void)
                     sensorId = 6.0f;
                 }
                 if (sensorId != 0.0f) {
-                    _vehicle->doCommandLong(_vehicle->defaultComponentId(), MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS, sensorId, -sphere_x[cur_mag], -sphere_y[cur_mag], -sphere_z[cur_mag]);
+                    _vehicle->sendMavCommand(_vehicle->defaultComponentId(),
+                                             MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS,
+                                             true, /* showErrors */
+                                             sensorId, -sphere_x[cur_mag], -sphere_y[cur_mag], -sphere_z[cur_mag]);
                 }
             }
         }

src/AutoPilotPlugins/Common/ESP8266ComponentController.cc

@@ -38,7 +38,7 @@ ESP8266ComponentController::ESP8266ComponentController()
     _baudRates.append("460800");
     _baudRates.append("921600");
     connect(&_timer, &QTimer::timeout, this, &ESP8266ComponentController::_processTimeout);
-    connect(_vehicle, &Vehicle::commandLongAck, this, &ESP8266ComponentController::_commandAck);
+    connect(_vehicle, &Vehicle::mavCommandResult, this, &ESP8266ComponentController::_mavCommandResult);
     Fact* ssid = getParameterFact(MAV_COMP_ID_UDP_BRIDGE, "WIFI_SSID4");
     connect(ssid, &Fact::valueChanged, this, &ESP8266ComponentController::_ssidChanged);
     Fact* paswd = getParameterFact(MAV_COMP_ID_UDP_BRIDGE, "WIFI_PASSWORD4");
@@ -381,21 +381,23 @@ ESP8266ComponentController::_processTimeout()
 
 //-----------------------------------------------------------------------------
 void
-ESP8266ComponentController::_commandAck(uint8_t compID, uint16_t command, uint8_t result)
+ESP8266ComponentController::_mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle)
 {
-    if(compID == MAV_COMP_ID_UDP_BRIDGE) {
-        if(result != MAV_RESULT_ACCEPTED) {
+    Q_UNUSED(vehicleId);
+    Q_UNUSED(noReponseFromVehicle);
+
+    if (component == MAV_COMP_ID_UDP_BRIDGE) {
+        if (result != MAV_RESULT_ACCEPTED) {
             qWarning() << "ESP8266ComponentController command" << command << "rejected.";
             return;
         }
-        if((_waitType == WAIT_FOR_REBOOT  && command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN) ||
-           (_waitType == WAIT_FOR_RESTORE && command == MAV_CMD_PREFLIGHT_STORAGE))
-        {
+        if ((_waitType == WAIT_FOR_REBOOT  && command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN) ||
+                (_waitType == WAIT_FOR_RESTORE && command == MAV_CMD_PREFLIGHT_STORAGE)) {
             _timer.stop();
             _waitType = WAIT_FOR_NOTHING;
             emit busyChanged();
             qCDebug(ESP8266ComponentControllerLog) << "_commandAck for" << command;
-            if(command == MAV_CMD_PREFLIGHT_STORAGE) {
+            if (command == MAV_CMD_PREFLIGHT_STORAGE) {
                 _vehicle->parameterManager()->refreshAllParameters(MAV_COMP_ID_UDP_BRIDGE);
             }
         }

src/AutoPilotPlugins/Common/ESP8266ComponentController.h

@@ -83,7 +83,7 @@ signals:
 
 private slots:
     void        _processTimeout     ();
-    void        _commandAck         (uint8_t compID, uint16_t command, uint8_t result);
+    void        _mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle);
     void        _ssidChanged        (QVariant value);
     void        _passwordChanged    (QVariant value);
     void        _baudChanged        (QVariant value);

src/AutoPilotPlugins/PX4/AirframeComponentController.cc

@@ -117,7 +117,7 @@ void AirframeComponentController::_waitParamWriteSignal(QVariant value)
 
 void AirframeComponentController::_rebootAfterStackUnwind(void)
 {    
-    _uas->executeCommand(MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0);
+    _vehicle->sendMavCommand(_vehicle->defaultComponentId(), MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, true /* showError */, 1.0f);
     qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
     for (unsigned i = 0; i < 2000; i++) {
         QGC::SLEEP::usleep(500);

src/FactSystem/ParameterManager.cc

@@ -1066,7 +1066,7 @@ void ParameterManager::_initialRequestTimeout(void)
     } else {
         if (!_vehicle->genericFirmware()) {
             // Generic vehicles (like BeBop) may not have any parameters, so don't annoy the user
-            QString errorMsg = tr("Vehicle %1 did not respond to request for parameters"
+            QString errorMsg = tr("Vehicle %1 did not respond to request for parameters. "
                                   "This will cause QGroundControl to be unable to display its full user interface.").arg(_vehicle->id());
             qCDebug(ParameterManagerLog) << errorMsg;
             qgcApp()->showMessage(errorMsg);

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc

@@ -125,29 +125,11 @@ void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitu
         return;
     }
 
-    mavlink_message_t msg;
-    mavlink_command_long_t cmd;
-
-    cmd.command = (uint16_t)MAV_CMD_NAV_TAKEOFF;
-    cmd.confirmation = 0;
-    cmd.param1 = 0.0f;
-    cmd.param2 = 0.0f;
-    cmd.param3 = 0.0f;
-    cmd.param4 = 0.0f;
-    cmd.param5 = 0.0f;
-    cmd.param6 = 0.0f;
-    cmd.param7 = vehicle->altitudeAMSL()->rawValue().toFloat() +  altitudeRel; // AMSL meters
-    cmd.target_system = vehicle->id();
-    cmd.target_component = vehicle->defaultComponentId();
-
-    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
-    mavlink_msg_command_long_encode_chan(mavlink->getSystemId(),
-                                         mavlink->getComponentId(),
-                                         vehicle->priorityLink()->mavlinkChannel(),
-                                         &msg,
-                                         &cmd);
-
-    vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
+    vehicle->sendMavCommand(vehicle->defaultComponentId(),
+                            MAV_CMD_NAV_TAKEOFF,
+                            true, // show error
+                            0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                            vehicle->altitudeAMSL()->rawValue().toFloat() +  altitudeRel); // AMSL meters
 }
 
 void ArduCopterFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -283,30 +283,16 @@ QObject* PX4FirmwarePlugin::loadParameterMetaData(const QString& metaDataFile)
 
 void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
 {
-    // then tell it to loiter at the current position
-    mavlink_message_t msg;
-    mavlink_command_long_t cmd;
-
-    cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION;
-    cmd.confirmation = 0;
-    cmd.param1 = -1.0f;
-    cmd.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
-    cmd.param3 = 0.0f;
-    cmd.param4 = NAN;
-    cmd.param5 = NAN;
-    cmd.param6 = NAN;
-    cmd.param7 = NAN;
-    cmd.target_system = vehicle->id();
-    cmd.target_component = vehicle->defaultComponentId();
-
-    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
-    mavlink_msg_command_long_encode_chan(mavlink->getSystemId(),
-                                         mavlink->getComponentId(),
-                                         vehicle->priorityLink()->mavlinkChannel(),
-                                         &msg,
-                                         &cmd);
-
-    vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
+    vehicle->sendMavCommand(vehicle->defaultComponentId(),
+                            MAV_CMD_DO_REPOSITION,
+                            true,   // show error if failed
+                            -1.0f,
+                            MAV_DO_REPOSITION_FLAGS_CHANGE_MODE,
+                            0.0f,
+                            NAN,
+                            NAN,
+                            NAN,
+                            NAN);
 }
 
 void PX4FirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
@@ -321,131 +307,82 @@ void PX4FirmwarePlugin::guidedModeLand(Vehicle* vehicle)
 
 void PX4FirmwarePlugin::guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude)
 {
-    //-- If not in "guided" mode, make it so.
-    if(!isGuidedMode(vehicle))
+    if (!isGuidedMode(vehicle)) {
         setGuidedMode(vehicle, true);
-    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
-    mavlink_message_t msg;
-    mavlink_command_long_t cmd;
-    cmd.command = (uint16_t)MAV_CMD_SET_GUIDED_SUBMODE_CIRCLE;
-    cmd.confirmation = 0;
-    cmd.param1 = radius;
-    cmd.param2 = velocity;
-    cmd.param3 = altitude;
-    cmd.param4 = NAN;
-    cmd.param5 = centerCoord.isValid() ? centerCoord.latitude()  : NAN;
-    cmd.param6 = centerCoord.isValid() ? centerCoord.longitude() : NAN;
-    cmd.param7 = centerCoord.isValid() ? centerCoord.altitude()  : NAN;
-    cmd.target_system = vehicle->id();
-    cmd.target_component = vehicle->defaultComponentId();
-    mavlink_msg_command_long_encode_chan(mavlink->getSystemId(),
-                                         mavlink->getComponentId(),
-                                         vehicle->priorityLink()->mavlinkChannel(),
-                                         &msg,
-                                         &cmd);
-    vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
+    }
+
+    vehicle->sendMavCommand(vehicle->defaultComponentId(),
+                            MAV_CMD_SET_GUIDED_SUBMODE_CIRCLE,
+                            true,   // show error if fails
+                            radius,
+                            velocity,
+                            altitude,
+                            NAN,
+                            centerCoord.isValid() ? centerCoord.latitude()  : NAN,
+                            centerCoord.isValid() ? centerCoord.longitude() : NAN,
+                            centerCoord.isValid() ? centerCoord.altitude()  : NAN);
 }
 
 void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
 {
     Q_UNUSED(altitudeRel);
     if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) {
-        qgcApp()->showMessage(QStringLiteral("Unable to takeoff, vehicle position not known."));
+        qgcApp()->showMessage(tr("Unable to takeoff, vehicle position not known."));
         return;
     }
 
-    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
-
-    // Set destination altitude
-    mavlink_message_t msg;
-    mavlink_command_long_t cmd;
-
-    cmd.command = (uint16_t)MAV_CMD_NAV_TAKEOFF;
-    cmd.confirmation = 0;
-    cmd.param1 = -1.0f;
-    cmd.param2 = 0.0f;
-    cmd.param3 = 0.0f;
-    cmd.param4 = NAN;
-    cmd.param5 = NAN;
-    cmd.param6 = NAN;
-    cmd.param7 = vehicle->altitudeAMSL()->rawValue().toDouble() + altitudeRel;
-    cmd.target_system = vehicle->id();
-    cmd.target_component = vehicle->defaultComponentId();
-
-    mavlink_msg_command_long_encode_chan(mavlink->getSystemId(),
-                                         mavlink->getComponentId(),
-                                         vehicle->priorityLink()->mavlinkChannel(),
-                                         &msg,
-                                         &cmd);
-    vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
+    vehicle->sendMavCommand(vehicle->defaultComponentId(),
+                            MAV_CMD_NAV_TAKEOFF,
+                            true,   // show error is fails
+                            -1.0f,
+                            0.0f,
+                            0.0f,
+                            NAN,
+                            NAN,
+                            NAN,
+                            vehicle->altitudeAMSL()->rawValue().toDouble() + altitudeRel);
 }
 
 void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
 {
     if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) {
-        qgcApp()->showMessage(QStringLiteral("Unable to go to location, vehicle position not known."));
+        qgcApp()->showMessage(tr("Unable to go to location, vehicle position not known."));
         return;
     }
 
-    mavlink_message_t msg;
-    mavlink_command_long_t cmd;
-
-    cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION;
-    cmd.confirmation = 0;
-    cmd.param1 = -1.0f;
-    cmd.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
-    cmd.param3 = 0.0f;
-    cmd.param4 = NAN;
-    cmd.param5 = gotoCoord.latitude();
-    cmd.param6 = gotoCoord.longitude();
-    cmd.param7 = vehicle->altitudeAMSL()->rawValue().toDouble();
-    cmd.target_system = vehicle->id();
-    cmd.target_component = vehicle->defaultComponentId();
-
-    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
-    mavlink_msg_command_long_encode_chan(mavlink->getSystemId(),
-                                         mavlink->getComponentId(),
-                                         vehicle->priorityLink()->mavlinkChannel(),
-                                         &msg,
-                                         &cmd);
-
-    vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
+    vehicle->sendMavCommand(vehicle->defaultComponentId(),
+                            MAV_CMD_DO_REPOSITION,
+                            true,   // show error is fails
+                            -1.0f,
+                            MAV_DO_REPOSITION_FLAGS_CHANGE_MODE,
+                            0.0f,
+                            NAN,
+                            gotoCoord.latitude(),
+                            gotoCoord.longitude(),
+                            vehicle->altitudeAMSL()->rawValue().toFloat());
 }
 
 void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel)
 {
     if (!vehicle->homePositionAvailable()) {
-        qgcApp()->showMessage(QStringLiteral("Unable to change altitude, home position unknown."));
+        qgcApp()->showMessage(tr("Unable to change altitude, home position unknown."));
         return;
     }
     if (qIsNaN(vehicle->homePosition().altitude())) {
-        qgcApp()->showMessage(QStringLiteral("Unable to change altitude, home position altitude unknown."));
+        qgcApp()->showMessage(tr("Unable to change altitude, home position altitude unknown."));
         return;
     }
 
-    mavlink_message_t msg;
-    mavlink_command_long_t cmd;
-
-    cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION;
-    cmd.confirmation = 0;
-    cmd.param1 = -1.0f;
-    cmd.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
-    cmd.param3 = 0.0f;
-    cmd.param4 = NAN;
-    cmd.param5 = NAN;
-    cmd.param6 = NAN;
-    cmd.param7 = vehicle->homePosition().altitude() + altitudeRel;
-    cmd.target_system = vehicle->id();
-    cmd.target_component = vehicle->defaultComponentId();
-
-    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
-    mavlink_msg_command_long_encode_chan(mavlink->getSystemId(),
-                                         mavlink->getComponentId(),
-                                         vehicle->priorityLink()->mavlinkChannel(),
-                                         &msg,
-                                         &cmd);
-
-    vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
+    vehicle->sendMavCommand(vehicle->defaultComponentId(),
+                            MAV_CMD_DO_REPOSITION,
+                            true,   // show error is fails
+                            -1.0f,
+                            MAV_DO_REPOSITION_FLAGS_CHANGE_MODE,
+                            0.0f,
+                            NAN,
+                            NAN,
+                            NAN,
+                            vehicle->homePosition().altitude() + altitudeRel);
 }
 
 void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)

src/Vehicle/MAVLinkLogManager.cc

@@ -19,8 +19,6 @@
 #include <QFile>
 #include <QFileInfo>
 
-#define kTimeOutMilliseconds 1000
-
 QGC_LOGGING_CATEGORY(MAVLinkLogManagerLog, "MAVLinkLogManagerLog")
 
 static const char* kEmailAddressKey         = "MAVLinkLogEmail";
@@ -301,7 +299,6 @@ MAVLinkLogManager::MAVLinkLogManager(QGCApplication* app)
     , _loggingDisabled(false)
     , _logProcessor(NULL)
     , _deleteAfterUpload(false)
-    , _loggingCmdTryCount(0)
 {
     //-- Get saved settings
     QSettings settings;
@@ -347,7 +344,6 @@ MAVLinkLogManager::setToolbox(QGCToolbox* toolbox)
     qmlRegisterUncreatableType<MAVLinkLogManager>("QGroundControl.MAVLinkLogManager", 1, 0, "MAVLinkLogManager", "Reference only");
     if(!_loggingDisabled) {
         connect(toolbox->multiVehicleManager(), &MultiVehicleManager::activeVehicleChanged, this, &MAVLinkLogManager::_activeVehicleChanged);
-        connect(&_ackTimer, &QTimer::timeout, this, &MAVLinkLogManager::_processCmdAck);
     }
 }
 
@@ -540,8 +536,6 @@ MAVLinkLogManager::startLogging()
         if(_createNewLog()) {
             _vehicle->startMavlinkLog();
             _logRunning = true;
-            _loggingCmdTryCount = 0;
-            _ackTimer.start(kTimeOutMilliseconds);
             emit logRunningChanged();
         }
     }
@@ -570,11 +564,6 @@ MAVLinkLogManager::stopLogging()
         delete _logProcessor;
         _logProcessor = NULL;
         _logRunning = false;
-        if(_vehicle) {
-            //-- Setup a timer to make sure vehicle received the command
-            _loggingCmdTryCount = 0;
-            _ackTimer.start(kTimeOutMilliseconds);
-        }
         emit logRunningChanged();
     }
 }
@@ -742,9 +731,9 @@ MAVLinkLogManager::_activeVehicleChanged(Vehicle* vehicle)
     //   For now, we only handle one log download at a time.
     // Disconnect the previous one (if any)
     if(_vehicle) {
-        disconnect(_vehicle, &Vehicle::armedChanged,   this, &MAVLinkLogManager::_armedChanged);
-        disconnect(_vehicle, &Vehicle::mavlinkLogData, this, &MAVLinkLogManager::_mavlinkLogData);
-        disconnect(_vehicle, &Vehicle::commandLongAck, this, &MAVLinkLogManager::_commandLongAck);
+        disconnect(_vehicle, &Vehicle::armedChanged,        this, &MAVLinkLogManager::_armedChanged);
+        disconnect(_vehicle, &Vehicle::mavlinkLogData,      this, &MAVLinkLogManager::_mavlinkLogData);
+        disconnect(_vehicle, &Vehicle::mavCommandResult,    this, &MAVLinkLogManager::_mavCommandResult);
         _vehicle = NULL;
         //-- Stop logging (if that's the case)
         stopLogging();
@@ -753,51 +742,17 @@ MAVLinkLogManager::_activeVehicleChanged(Vehicle* vehicle)
     // Connect new system
     if(vehicle) {
         _vehicle = vehicle;
-        connect(_vehicle, &Vehicle::armedChanged,   this, &MAVLinkLogManager::_armedChanged);
-        connect(_vehicle, &Vehicle::mavlinkLogData, this, &MAVLinkLogManager::_mavlinkLogData);
-        connect(_vehicle, &Vehicle::commandLongAck, this, &MAVLinkLogManager::_commandLongAck);
+        connect(_vehicle, &Vehicle::armedChanged,       this, &MAVLinkLogManager::_armedChanged);
+        connect(_vehicle, &Vehicle::mavlinkLogData,     this, &MAVLinkLogManager::_mavlinkLogData);
+        connect(_vehicle, &Vehicle::mavCommandResult,   this, &MAVLinkLogManager::_mavCommandResult);
         emit canStartLogChanged();
     }
 }
 
 //-----------------------------------------------------------------------------
 void
-MAVLinkLogManager::_processCmdAck()
-{
-    if(_loggingCmdTryCount++ > 3) {
-        _ackTimer.stop();
-        //-- Give up
-        if(_logRunning) {
-            qCWarning(MAVLinkLogManagerLog) << "Start MAVLink log command had no response.";
-            _discardLog();
-        } else {
-            qCWarning(MAVLinkLogManagerLog) << "Stop MAVLink log command had no response.";
-        }
-    } else {
-        if(_vehicle) {
-            if(_logRunning) {
-                _vehicle->startMavlinkLog();
-                qCWarning(MAVLinkLogManagerLog) << "Start MAVLink log command sent again.";
-            } else {
-                _vehicle->stopMavlinkLog();
-                qCWarning(MAVLinkLogManagerLog) << "Stop MAVLink log command sent again.";
-            }
-            _ackTimer.start(kTimeOutMilliseconds);
-        } else {
-            //-- Vehicle went away on us
-            _ackTimer.stop();
-        }
-    }
-}
-
-//-----------------------------------------------------------------------------
-void
 MAVLinkLogManager::_mavlinkLogData(Vehicle* /*vehicle*/, uint8_t /*target_system*/, uint8_t /*target_component*/, uint16_t sequence, uint8_t first_message, QByteArray data, bool /*acked*/)
 {
-    //-- Disable timer if we got a message before an ACK for the start command
-    if(_logRunning) {
-        _ackTimer.stop();
-    }
     if(_logProcessor && _logProcessor->valid()) {
         if(!_logProcessor->processStreamData(sequence, first_message, data)) {
             qCCritical(MAVLinkLogManagerLog) << "Error writing MAVLink log file:" << _logProcessor->fileName();
@@ -814,12 +769,15 @@ MAVLinkLogManager::_mavlinkLogData(Vehicle* /*vehicle*/, uint8_t /*target_system
 
 //-----------------------------------------------------------------------------
 void
-MAVLinkLogManager::_commandLongAck(uint8_t /*compID*/, uint16_t command, uint8_t result)
+MAVLinkLogManager::_mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle)
 {
+    Q_UNUSED(vehicleId);
+    Q_UNUSED(component);
+    Q_UNUSED(noReponseFromVehicle)
+
     if(command == MAV_CMD_LOGGING_START || command == MAV_CMD_LOGGING_STOP) {
-        _ackTimer.stop();
         //-- Did it fail?
-        if(result) {
+        if(result != MAV_RESULT_ACCEPTED) {
             if(command == MAV_CMD_LOGGING_STOP) {
                 //-- Not that it could happen but...
                 qCWarning(MAVLinkLogManagerLog) << "Stop MAVLink log command failed.";

src/Vehicle/MAVLinkLogManager.h

@@ -172,8 +172,7 @@ private slots:
     void _activeVehicleChanged      (Vehicle* vehicle);
     void _mavlinkLogData            (Vehicle* vehicle, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t first_message, QByteArray data, bool acked);
     void _armedChanged              (bool armed);
-    void _commandLongAck            (uint8_t compID, uint16_t command, uint8_t result);
-    void _processCmdAck             ();
+    void _mavCommandResult          (int vehicleId, int component, int command, int result, bool noReponseFromVehicle);
 
 private:
     bool _sendLog                   (const QString& logFile);
@@ -200,8 +199,6 @@ private:
     bool                    _loggingDisabled;
     MAVLinkLogProcessor*    _logProcessor;
     bool                    _deleteAfterUpload;
-    int                     _loggingCmdTryCount;
-    QTimer                  _ackTimer;
 };
 
 #endif

src/Vehicle/SendMavCommandTest.cc

@@ -0,0 +1,151 @@
+/****************************************************************************
+ *
+ *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+
+#include "SendMavCommandTest.h"
+#include "MultiVehicleManager.h"
+#include "QGCApplication.h"
+
+void SendMavCommandTest::_noFailure(void)
+{
+    _connectMockLink();
+
+    MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
+    Vehicle* vehicle = vehicleMgr->activeVehicle();
+    QVERIFY(vehicle);
+
+    vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_1, true /* showError */);
+
+    QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool)));
+    QCOMPARE(spyResult.wait(10000), true);
+    QList<QVariant> arguments = spyResult.takeFirst();
+    QCOMPARE(arguments.count(), 5);
+    QCOMPARE(arguments.at(0).toInt(), vehicle->id());
+    QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_1);
+    QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_ACCEPTED);
+    QCOMPARE(arguments.at(4).toBool(), false);
+}
+
+void SendMavCommandTest::_failureShowError(void)
+{
+    // Will pop error about request failure
+    setExpectedMessageBox(QMessageBox::Ok);
+
+    _connectMockLink();
+
+    MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
+    Vehicle* vehicle = vehicleMgr->activeVehicle();
+    QVERIFY(vehicle);
+
+    vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_2, true /* showError */);
+
+    QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool)));
+    QCOMPARE(spyResult.wait(10000), true);
+    QList<QVariant> arguments = spyResult.takeFirst();
+    QCOMPARE(arguments.count(), 5);
+    QCOMPARE(arguments.at(0).toInt(), vehicle->id());
+    QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_2);
+    QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED);
+    QCOMPARE(arguments.at(4).toBool(), false);
+
+    // User should have been notified
+    checkExpectedMessageBox();
+}
+
+void SendMavCommandTest::_failureNoShowError(void)
+{
+    _connectMockLink();
+
+    MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
+    Vehicle* vehicle = vehicleMgr->activeVehicle();
+    QVERIFY(vehicle);
+
+    vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_2, false /* showError */);
+
+    QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool)));
+    QCOMPARE(spyResult.wait(10000), true);
+    QList<QVariant> arguments = spyResult.takeFirst();
+    QCOMPARE(arguments.count(), 5);
+    QCOMPARE(arguments.at(0).toInt(), vehicle->id());
+    QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_2);
+    QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED);
+    QCOMPARE(arguments.at(4).toBool(), false);
+}
+
+void SendMavCommandTest::_noFailureAfterRetry(void)
+{
+    _connectMockLink();
+
+    MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
+    Vehicle* vehicle = vehicleMgr->activeVehicle();
+    QVERIFY(vehicle);
+
+    vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_3, true /* showError */);
+
+    QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool)));
+    QCOMPARE(spyResult.wait(10000), true);
+    QList<QVariant> arguments = spyResult.takeFirst();
+    QCOMPARE(arguments.count(), 5);
+    QCOMPARE(arguments.at(0).toInt(), vehicle->id());
+    QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_3);
+    QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_ACCEPTED);
+    QCOMPARE(arguments.at(4).toBool(), false);
+}
+
+void SendMavCommandTest::_failureAfterRetry(void)
+{
+    // Will pop error about request failure
+    setExpectedMessageBox(QMessageBox::Ok);
+
+    _connectMockLink();
+
+    MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
+    Vehicle* vehicle = vehicleMgr->activeVehicle();
+    QVERIFY(vehicle);
+
+    vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_4, true /* showError */);
+
+    QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool)));
+    QCOMPARE(spyResult.wait(10000), true);
+    QList<QVariant> arguments = spyResult.takeFirst();
+    QCOMPARE(arguments.count(), 5);
+    QCOMPARE(arguments.at(0).toInt(), vehicle->id());
+    QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_4);
+    QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED);
+    QCOMPARE(arguments.at(4).toBool(), false);
+
+    // User should have been notified
+    checkExpectedMessageBox();
+}
+
+void SendMavCommandTest::_failureAfterNoReponse(void)
+{
+    // Will pop error about request failure
+    setExpectedMessageBox(QMessageBox::Ok);
+
+    _connectMockLink();
+
+    MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
+    Vehicle* vehicle = vehicleMgr->activeVehicle();
+    QVERIFY(vehicle);
+
+    vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_5, true /* showError */);
+
+    QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool)));
+    QCOMPARE(spyResult.wait(10000), true);
+    QList<QVariant> arguments = spyResult.takeFirst();
+    QCOMPARE(arguments.count(), 5);
+    QCOMPARE(arguments.at(0).toInt(), vehicle->id());
+    QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_5);
+    QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED);
+    QCOMPARE(arguments.at(4).toBool(), true);
+
+    // User should have been notified
+    checkExpectedMessageBox();
+}

src/Vehicle/SendMavCommandTest.h

@@ -0,0 +1,31 @@
+/****************************************************************************
+ *
+ *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+
+#ifndef SendMavCommandTest_H
+#define SendMavCommandTest_H
+
+#include "UnitTest.h"
+
+class SendMavCommandTest : public UnitTest
+{
+    Q_OBJECT
+    
+private slots:
+    void _noFailure(void);
+    void _failureShowError(void);
+    void _failureNoShowError(void);
+    void _noFailureAfterRetry(void);
+    void _failureAfterRetry(void);
+    void _failureAfterNoReponse(void);
+
+private:
+};
+
+#endif

src/Vehicle/Vehicle.cc

@@ -174,12 +174,18 @@ Vehicle::Vehicle(LinkInterface*             link,
     _prearmErrorTimer.setInterval(_prearmErrorTimeoutMSecs);
     _prearmErrorTimer.setSingleShot(true);
 
-    // Connection Lost time
-    _connectionLostTimer.setInterval(Vehicle::_connectionLostTimeoutMSecs);
+    // Connection Lost timer
+    _connectionLostTimer.setInterval(_connectionLostTimeoutMSecs);
     _connectionLostTimer.setSingleShot(false);
     _connectionLostTimer.start();
     connect(&_connectionLostTimer, &QTimer::timeout, this, &Vehicle::_connectionLostTimeout);
 
+    // Send MAV_CMD ack timer
+    _mavCommandAckTimer.setSingleShot(true);
+    _mavCommandAckTimer.setInterval(_mavCommandAckTimeoutMSecs);
+    _mavCommandAckTimer.setSingleShot(false);
+    connect(&_mavCommandAckTimer, &QTimer::timeout, this, &Vehicle::_sendMavCommandAgain);
+
     _mav = uas();
 
     // Listen for system messages
@@ -212,23 +218,11 @@ Vehicle::Vehicle(LinkInterface*             link,
     _rallyPointManager = _firmwarePlugin->newRallyPointManager(this);
     connect(_rallyPointManager, &RallyPointManager::error, this, &Vehicle::_rallyPointManagerError);
 
-    // Ask the vehicle for firmware version info. This must be MAV_COMP_ID_ALL since we don't know default component id yet.
-
-    mavlink_message_t       versionMsg;
-    mavlink_command_long_t  versionCmd;
-
-    versionCmd.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES;
-    versionCmd.confirmation = 0;
-    versionCmd.param1 = 1; // Request firmware version
-    versionCmd.param2 = versionCmd.param3 = versionCmd.param4 = versionCmd.param5 = versionCmd.param6 = versionCmd.param7 = 0;
-    versionCmd.target_system = id();
-    versionCmd.target_component = MAV_COMP_ID_ALL;
-    mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(),
-                                         _mavlink->getComponentId(),
-                                         priorityLink()->mavlinkChannel(),
-                                         &versionMsg,
-                                         &versionCmd);
-    sendMessageMultiple(versionMsg);
+    // Ask the vehicle for firmware version info.
+    sendMavCommand(MAV_COMP_ID_ALL,                         // Don't know default component id yet.
+                    MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
+                   false,                                   // No error shown if fails
+                    1);                                     // Request firmware version
 
     _firmwarePlugin->initializeVehicle(this);
 
@@ -564,40 +558,42 @@ void Vehicle::_handleHilActuatorControls(mavlink_message_t &message)
 
 void Vehicle::_handleCommandAck(mavlink_message_t& message)
 {
+    bool showError = true;
+
     mavlink_command_ack_t ack;
     mavlink_msg_command_ack_decode(&message, &ack);
 
-    emit commandLongAck(message.compid, ack.command, ack.result);
+    if (_mavCommandQueue.count() && ack.command == _mavCommandQueue[0].command) {
+        _mavCommandAckTimer.stop();
+        showError = _mavCommandQueue[0].showError;
+        _mavCommandQueue.removeFirst();
+    }
+
+    emit mavCommandResult(_id, message.compid, ack.command, ack.result, false /* noResponsefromVehicle */);
+
+    if (showError) {
+        QString commandName = qgcApp()->toolbox()->missionCommandTree()->friendlyName((MAV_CMD)ack.command);
 
-    // Disregard failures for these (handled above)
-    switch (ack.command) {
-        case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
-        case MAV_CMD_LOGGING_START:
-        case MAV_CMD_LOGGING_STOP:
-            return;
+        switch (ack.result) {
+        case MAV_RESULT_TEMPORARILY_REJECTED:
+            qgcApp()->showMessage(tr("%1 command temporarily rejected").arg(commandName));
+            break;
+        case MAV_RESULT_DENIED:
+            qgcApp()->showMessage(tr("%1 command denied").arg(commandName));
+            break;
+        case MAV_RESULT_UNSUPPORTED:
+            qgcApp()->showMessage(tr("%1 command not supported").arg(commandName));
+            break;
+        case MAV_RESULT_FAILED:
+            qgcApp()->showMessage(tr("%1 command failed").arg(commandName));
+            break;
         default:
+            // Do nothing
             break;
+        }
     }
 
-    QString commandName = qgcApp()->toolbox()->missionCommandTree()->friendlyName((MAV_CMD)ack.command);
-
-    switch (ack.result) {
-    case MAV_RESULT_TEMPORARILY_REJECTED:
-        qgcApp()->showMessage(tr("%1 command temporarily rejected").arg(commandName));
-        break;
-    case MAV_RESULT_DENIED:
-        qgcApp()->showMessage(tr("%1 command denied").arg(commandName));
-        break;
-    case MAV_RESULT_UNSUPPORTED:
-        qgcApp()->showMessage(tr("%1 command not supported").arg(commandName));
-        break;
-    case MAV_RESULT_FAILED:
-        qgcApp()->showMessage(tr("%1 command failed").arg(commandName));
-        break;
-    default:
-        // Do nothing
-        break;
-    }
+    _sendNextQueuedMavCommand();
 }
 
 void Vehicle::_handleExtendedSysState(mavlink_message_t& message)
@@ -1303,29 +1299,10 @@ QGeoCoordinate Vehicle::homePosition(void)
 void Vehicle::setArmed(bool armed)
 {
     // We specifically use COMMAND_LONG:MAV_CMD_COMPONENT_ARM_DISARM since it is supported by more flight stacks.
-
-    mavlink_message_t msg;
-    mavlink_command_long_t cmd;
-
-    cmd.command = (uint16_t)MAV_CMD_COMPONENT_ARM_DISARM;
-    cmd.confirmation = 0;
-    cmd.param1 = armed ? 1.0f : 0.0f;
-    cmd.param2 = 0.0f;
-    cmd.param3 = 0.0f;
-    cmd.param4 = 0.0f;
-    cmd.param5 = 0.0f;
-    cmd.param6 = 0.0f;
-    cmd.param7 = 0.0f;
-    cmd.target_system = id();
-    cmd.target_component = defaultComponentId();
-
-    mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(),
-                                         _mavlink->getComponentId(),
-                                         priorityLink()->mavlinkChannel(),
-                                         &msg,
-                                         &cmd);
-
-    sendMessageOnLink(priorityLink(), msg);
+    sendMavCommand(defaultComponentId(),
+                   MAV_CMD_COMPONENT_ARM_DISARM,
+                   true,    // show error if fails
+                   armed ? 1.0f : 0.0f);
 }
 
 bool Vehicle::flightModeSetAvailable(void)
@@ -1812,27 +1789,11 @@ void Vehicle::setGuidedMode(bool guidedMode)
 
 void Vehicle::emergencyStop(void)
 {
-    mavlink_message_t msg;
-    mavlink_command_long_t cmd;
-
-    cmd.command = (uint16_t)MAV_CMD_COMPONENT_ARM_DISARM;
-    cmd.confirmation = 0;
-    cmd.param1 = 0.0f;
-    cmd.param2 = 21196.0f;  // Magic number for emergency stop
-    cmd.param3 = 0.0f;
-    cmd.param4 = 0.0f;
-    cmd.param5 = 0.0f;
-    cmd.param6 = 0.0f;
-    cmd.param7 = 0.0f;
-    cmd.target_system = id();
-    cmd.target_component = defaultComponentId();
-    mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(),
-                                         _mavlink->getComponentId(),
-                                         priorityLink()->mavlinkChannel(),
-                                         &msg,
-                                         &cmd);
-
-    sendMessageOnLink(priorityLink(), msg);
+    sendMavCommand(defaultComponentId(),
+                   MAV_CMD_COMPONENT_ARM_DISARM,
+                   true,        // show error if fails
+                   0.0f,
+                   21196.0f);  // Magic number for emergency stop
 }
 
 void Vehicle::setCurrentMissionSequence(int seq)
@@ -1851,22 +1812,59 @@ void Vehicle::setCurrentMissionSequence(int seq)
     sendMessageOnLink(priorityLink(), msg);
 }
 
-void Vehicle::doCommandLong(int component, MAV_CMD command, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
+void Vehicle::sendMavCommand(int component, MAV_CMD command, bool showError, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
 {
+    MavCommandQueueEntry_t entry;
+
+    entry.component = component;
+    entry.command = command;
+    entry.showError = showError;
+    entry.rgParam[0] = param1;
+    entry.rgParam[1] = param2;
+    entry.rgParam[2] = param3;
+    entry.rgParam[3] = param4;
+    entry.rgParam[4] = param5;
+    entry.rgParam[5] = param6;
+    entry.rgParam[6] = param7;
+
+    _mavCommandQueue.append(entry);
+
+    if (_mavCommandQueue.count() == 1) {
+        _mavCommandRetryCount = 0;
+        _sendMavCommandAgain();
+    }
+}
+
+void Vehicle::_sendMavCommandAgain(void)
+{
+    MavCommandQueueEntry_t& queuedCommand = _mavCommandQueue[0];
+
+    if (_mavCommandRetryCount++ > _mavCommandMaxRetryCount) {
+        emit mavCommandResult(_id, queuedCommand.component, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);
+        if (queuedCommand.showError) {
+            qgcApp()->showMessage(tr("Vehicle did not respond to command: %1").arg(qgcApp()->toolbox()->missionCommandTree()->friendlyName(queuedCommand.command)));
+        }
+        _mavCommandQueue.removeFirst();
+        _sendNextQueuedMavCommand();
+        return;
+    }
+
+    _mavCommandAckTimer.start();
+
     mavlink_message_t       msg;
     mavlink_command_long_t  cmd;
 
-    cmd.command = command;
+    cmd.command = queuedCommand.command;
     cmd.confirmation = 0;
-    cmd.param1 = param1;
-    cmd.param2 = param2;
-    cmd.param3 = param3;
-    cmd.param4 = param4;
-    cmd.param5 = param5;
-    cmd.param6 = param6;
-    cmd.param7 = param7;
-    cmd.target_system = id();
-    cmd.target_component = component;
+    cmd.param1 = queuedCommand.rgParam[0];
+    cmd.param2 = queuedCommand.rgParam[1];
+    cmd.param3 = queuedCommand.rgParam[2];
+    cmd.param4 = queuedCommand.rgParam[3];
+    cmd.param5 = queuedCommand.rgParam[4];
+    cmd.param6 = queuedCommand.rgParam[5];
+    cmd.param7 = queuedCommand.rgParam[6];
+    cmd.target_system = _id;
+    cmd.target_component = queuedCommand.component;
     mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(),
                                          _mavlink->getComponentId(),
                                          priorityLink()->mavlinkChannel(),
@@ -1876,6 +1874,15 @@ void Vehicle::doCommandLong(int component, MAV_CMD command, float param1, float
     sendMessageOnLink(priorityLink(), msg);
 }
 
+void Vehicle::_sendNextQueuedMavCommand(void)
+{
+    if (_mavCommandQueue.count()) {
+        _mavCommandRetryCount = 0;
+        _sendMavCommandAgain();
+    }
+}
+
+
 void Vehicle::setPrearmError(const QString& prearmError)
 {
     _prearmError = prearmError;
@@ -1921,7 +1928,7 @@ QString Vehicle::firmwareVersionTypeString(void) const
 
 void Vehicle::rebootVehicle()
 {
-    doCommandLong(defaultComponentId(), MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+    sendMavCommand(defaultComponentId(), MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, true, 1.0f);
 }
 
 int Vehicle::defaultComponentId(void)
@@ -1941,7 +1948,7 @@ void Vehicle::setSoloFirmware(bool soloFirmware)
     // Temporarily removed, waiting for new command implementation
 void Vehicle::motorTest(int motor, int percent, int timeoutSecs)
 {
-    doCommandLong(defaultComponentId(), MAV_CMD_DO_MOTOR_TEST, motor, MOTOR_TEST_THROTTLE_PERCENT, percent, timeoutSecs);
+    doCommandLongUnverified(defaultComponentId(), MAV_CMD_DO_MOTOR_TEST, motor, MOTOR_TEST_THROTTLE_PERCENT, percent, timeoutSecs);
 }
 #endif
 
@@ -2044,12 +2051,12 @@ VehicleGPSFactGroup::VehicleGPSFactGroup(QObject* parent)
 
 void Vehicle::startMavlinkLog()
 {
-    doCommandLong(defaultComponentId(), MAV_CMD_LOGGING_START);
+    sendMavCommand(defaultComponentId(), MAV_CMD_LOGGING_START, false /* showError */);
 }
 
 void Vehicle::stopMavlinkLog()
 {
-    doCommandLong(defaultComponentId(), MAV_CMD_LOGGING_STOP);
+    sendMavCommand(defaultComponentId(), MAV_CMD_LOGGING_STOP, false /* showError */);
 }
 
 void Vehicle::_ackMavlinkLogData(uint16_t sequence)

src/Vehicle/Vehicle.h

@@ -553,7 +553,13 @@ public:
     static const int cMaxRcChannels = 18;
 
     bool containsLink(LinkInterface* link) { return _links.contains(link); }
-    void doCommandLong(int component, MAV_CMD command, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f);
+
+    /// Sends the specified MAV_CMD to the vehicle. If no Ack is received command will be retried. If a sendMavCommand is already in progress
+    /// the command will be queued and sent when the previous command completes.
+    ///     @param component Component to send to
+    ///     @param command MAV_CMD to send
+    ///     @param showError true: Display error to user if command failed, false:  no error shown
+    void sendMavCommand(int component, MAV_CMD command, bool showError, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f);
 
     int firmwareMajorVersion(void) const { return _firmwareMajorVersion; }
     int firmwareMinorVersion(void) const { return _firmwareMinorVersion; }
@@ -609,7 +615,6 @@ signals:
     void flyingChanged(bool flying);
     void guidedModeChanged(bool guidedMode);
     void prearmErrorChanged(const QString& prearmError);
-    void commandLongAck(uint8_t compID, uint16_t command, uint8_t result);
     void soloFirmwareChanged(bool soloFirmware);
     void unhealthySensorsChanged(void);
 
@@ -653,6 +658,14 @@ signals:
     // Mavlink Log Download
     void mavlinkLogData (Vehicle* vehicle, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t first_message, QByteArray data, bool acked);
 
+    /// Signalled in response to usage of sendMavCommand
+    ///     @param vehicleId Vehicle which command was sent to
+    ///     @param component Component which command was sent to
+    ///     @param command MAV_CMD Command which was sent
+    ///     @param result MAV_RESULT returned in ack
+    ///     @param noResponseFromVehicle true: vehicle did not respond to command, false: vehicle responsed, MAV_RESULT in result
+    void mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle);
+
 private slots:
     void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
     void _linkInactiveOrDeleted(LinkInterface* link);
@@ -682,6 +695,7 @@ private slots:
     void _prearmErrorTimeout(void);
     void _newMissionItemsAvailable(void);
     void _newGeoFenceAvailable(void);
+    void _sendMavCommandAgain(void);
 
 private:
     bool _containsLink(LinkInterface* link);
@@ -713,6 +727,7 @@ private:
     void _handleMavlinkLoggingData(mavlink_message_t& message);
     void _handleMavlinkLoggingDataAcked(mavlink_message_t& message);
     void _ackMavlinkLogData(uint16_t sequence);
+    void _sendNextQueuedMavCommand(void);
 
 private:
     int     _id;                    ///< Mavlink system id
@@ -765,6 +780,19 @@ private:
     uint32_t        _onboardControlSensorsHealth;
     uint32_t        _onboardControlSensorsUnhealthy;
 
+    typedef struct {
+        int     component;
+        MAV_CMD command;
+        float   rgParam[7];
+        bool    showError;
+    } MavCommandQueueEntry_t;
+
+    QList<MavCommandQueueEntry_t>   _mavCommandQueue;
+    QTimer                          _mavCommandAckTimer;
+    int                             _mavCommandRetryCount;
+    static const int                _mavCommandMaxRetryCount = 3;
+    static const int                _mavCommandAckTimeoutMSecs = 1000;
+
     QString             _prearmError;
     QTimer              _prearmErrorTimer;
     static const int    _prearmErrorTimeoutMSecs = 35 * 1000;   ///< Take away prearm error after 35 seconds

src/ViewWidgets/CustomCommandWidgetController.cc

@@ -21,10 +21,10 @@
 const char* CustomCommandWidgetController::_settingsKey = "CustomCommand.QmlFile";
 
 CustomCommandWidgetController::CustomCommandWidgetController(void) :
-	_uas(NULL)
+    _vehicle(NULL)
 {
     if(qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()) {
-        _uas = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->uas();
+        _vehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
     }
     QSettings settings;
     _customQmlFile = settings.value(_settingsKey).toString();
@@ -33,15 +33,21 @@ CustomCommandWidgetController::CustomCommandWidgetController(void) :
 
 void CustomCommandWidgetController::sendCommand(int commandId, QVariant componentId, QVariant confirm, QVariant param1, QVariant param2, QVariant param3, QVariant param4, QVariant param5, QVariant param6, QVariant param7)
 {
-    if(_uas) {
-        _uas->executeCommand((MAV_CMD)commandId, confirm.toInt(), param1.toFloat(), param2.toFloat(), param3.toFloat(), param4.toFloat(), param5.toFloat(), param6.toFloat(), param7.toFloat(), componentId.toInt());
+    Q_UNUSED(confirm);
+
+    if(_vehicle) {
+        _vehicle->sendMavCommand(componentId.toInt(),
+                                 (MAV_CMD)commandId,
+                                 true,  // show error if fails
+                                 param1.toFloat(), param2.toFloat(), param3.toFloat(), param4.toFloat(), param5.toFloat(), param6.toFloat(), param7.toFloat());
     }
 }
 
 void CustomCommandWidgetController::_activeVehicleChanged(Vehicle* activeVehicle)
 {
-    if(activeVehicle)
-        _uas = activeVehicle->uas();
+    if (activeVehicle) {
+        _vehicle = activeVehicle;
+    }
 }
 
 void CustomCommandWidgetController::selectQmlFile(void)

src/ViewWidgets/CustomCommandWidgetController.h

@@ -13,7 +13,6 @@
 
 #include <QObject>
 
-#include "UASInterface.h"
 #include "AutoPilotPlugin.h"
 #include "FactPanelController.h"
 
@@ -37,7 +36,7 @@ private slots:
     void _activeVehicleChanged  (Vehicle* activeVehicle);
 
 private:
-	UASInterface*       _uas;
+    Vehicle*            _vehicle;
     QString             _customQmlFile;
     static const char*  _settingsKey;
 };

src/comm/MockLink.cc

@@ -807,6 +807,40 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
         commandResult = MAV_RESULT_ACCEPTED;
         _respondWithAutopilotVersion();
         break;
+    case MAV_CMD_USER_1:
+        // Test command which always returns MAV_RESULT_ACCEPTED
+        commandResult = MAV_RESULT_ACCEPTED;
+        break;
+    case MAV_CMD_USER_2:
+        // Test command which always returns MAV_RESULT_FAILED
+        commandResult = MAV_RESULT_FAILED;
+        break;
+    case MAV_CMD_USER_3:
+        // Test command which returns MAV_RESULT_ACCEPTED on second attempt
+        static bool firstCmdUser3 = true;
+        if (firstCmdUser3) {
+           firstCmdUser3 = false;
+           return;
+        } else {
+            firstCmdUser3 = true;
+            commandResult = MAV_RESULT_ACCEPTED;
+        }
+        break;
+    case MAV_CMD_USER_4:
+        // Test command which returns MAV_RESULT_FAILED on second attempt
+        static bool firstCmdUser4 = true;
+        if (firstCmdUser4) {
+           firstCmdUser4 = false;
+           return;
+        } else {
+            firstCmdUser4 = true;
+            commandResult = MAV_RESULT_FAILED;
+        }
+        break;
+    case MAV_CMD_USER_5:
+        // No response
+        return;
+        break;
     }
 
     mavlink_message_t commandAck;

src/qgcunittest/UnitTestList.cc

@@ -31,6 +31,7 @@
 #include "ParameterManagerTest.h"
 #include "MissionCommandTreeTest.h"
 #include "LogDownloadTest.h"
+#include "SendMavCommandTest.h"
 
 UT_REGISTER_TEST(FactSystemTestGeneric)
 UT_REGISTER_TEST(FactSystemTestPX4)
@@ -50,6 +51,7 @@ UT_REGISTER_TEST(TCPLinkTest)
 UT_REGISTER_TEST(ParameterManagerTest)
 UT_REGISTER_TEST(MissionCommandTreeTest)
 UT_REGISTER_TEST(LogDownloadTest)
+UT_REGISTER_TEST(SendMavCommandTest)
 
 // List of unit test which are currently disabled.
 // If disabling a new test, include reason in comment.

src/uas/UAS.cc

@@ -1233,33 +1233,6 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName,
     emit parameterUpdate(uasId, compId, paramName, rawValue.param_count, rawValue.param_index, rawValue.param_type, paramValue);
 }
 
-void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component)
-{
-    if (!_vehicle) {
-        return;
-    }
-
-    mavlink_message_t msg;
-    mavlink_command_long_t cmd;
-    cmd.command = (uint16_t)command;
-    cmd.confirmation = confirmation;
-    cmd.param1 = param1;
-    cmd.param2 = param2;
-    cmd.param3 = param3;
-    cmd.param4 = param4;
-    cmd.param5 = param5;
-    cmd.param6 = param6;
-    cmd.param7 = param7;
-    cmd.target_system = uasId;
-    cmd.target_component = component;
-    mavlink_msg_command_long_encode_chan(mavlink->getSystemId(),
-                                         mavlink->getComponentId(),
-                                         _vehicle->priorityLink()->mavlinkChannel(),
-                                         &msg,
-                                         &cmd);
-    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
-}
-
 /**
 * Set the manual control commands.
 * This can only be done if the system has manual inputs enabled and is armed.

src/uas/UAS.h

@@ -472,9 +472,6 @@ public:
     void requestImage();
 
 public slots:
-    /** @brief Executes a command with 7 params */
-    void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component);
-
     /** @brief Order the robot to pair its receiver **/
     void pairRX(int rxType, int rxSubType);
 

src/uas/UASInterface.h

@@ -141,10 +141,6 @@ public:
     virtual void stopBusConfig(void) = 0;
 
 public slots:
-
-    /** @brief Executes a command **/
-    virtual void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component) = 0;
-
     /** @brief Order the robot to pair its receiver **/
     virtual void pairRX(int rxType, int rxSubType) = 0;