New Vehicle::sendMavCommand - Queued MAV_CMD with retry

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, MAV_CMD command, MAV_RESULT 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, MAV_CMD command, MAV_RESULT 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/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, MAV_CMD command, MAV_RESULT 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, MAV_CMD command, MAV_RESULT 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/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, MAV_CMD command, MAV_RESULT 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/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;