LinkInterface and Vehicle uses mutexes to synchronize writing bytes to the link instead of queued signals

src/AnalyzeView/LogDownloadController.cc

@@ -478,7 +478,7 @@ LogDownloadController::_requestLogData(uint16_t id, uint32_t offset, uint32_t co
                     &msg,
                     qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->id(), qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->defaultComponentId(),
                     id, offset, count);
-        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
+        _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg);
     }
 }
 
@@ -508,7 +508,7 @@ LogDownloadController::_requestLogList(uint32_t start, uint32_t end)
                     _vehicle->defaultComponentId(),
                     start,
                     end);
-        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
+        _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg);
         //-- Wait 5 seconds before bitching about not getting anything
         _timer.start(5000);
     }
@@ -674,7 +674,7 @@ LogDownloadController::eraseAll(void)
                     _vehicle->priorityLink()->mavlinkChannel(),
                     &msg,
                     qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->id(), qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->defaultComponentId());
-        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
+        _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg);
         refresh();
     }
 }

src/AnalyzeView/MavlinkConsoleController.cc

@@ -129,7 +129,7 @@ MavlinkConsoleController::_sendSerialData(QByteArray data, bool close)
                     0,
                     chunk.size(),
                     reinterpret_cast<uint8_t*>(chunk.data()));
-        _vehicle->sendMessageOnLink(priority_link, msg);
+        _vehicle->sendMessageOnLinkThreadSafe(priority_link, msg);
         data.remove(0, chunk.size());
     }
 }

src/AutoPilotPlugins/APM/APMSensorsComponentController.cc

@@ -704,7 +704,7 @@ void APMSensorsComponentController::nextClicked(void)
                                       0,    // target_system
                                       0);   // target_component
 
-    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
+    _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg);
 
     if (_calTypeInProgress == CalTypeCompassMot) {
         _stopCalibration(StopCalibrationSuccess);

src/Camera/QGCCameraControl.cc

@@ -1183,7 +1183,7 @@ QGCCameraControl::_requestAllParameters()
         &msg,
         static_cast<uint8_t>(_vehicle->id()),
         static_cast<uint8_t>(compID()));
-    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
+    _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg);
     qCDebug(CameraControlVerboseLog) << "Request all parameters";
 }
 

src/Camera/QGCCameraIO.cc

@@ -191,7 +191,7 @@ QGCCameraParamIO::_sendParameter()
         _vehicle->priorityLink()->mavlinkChannel(),
         &msg,
         &p);
-    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
+    _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg);
     _paramWriteTimer.start();
 }
 
@@ -364,6 +364,6 @@ QGCCameraParamIO::paramRequest(bool reset)
         static_cast<uint8_t>(_control->compID()),
         param_id,
         -1);
-    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
+    _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg);
     _paramRequestTimer.start();
 }

src/FactSystem/ParameterManager.cc

@@ -526,7 +526,7 @@ void ParameterManager::refreshAllParameters(uint8_t componentId)
                                              &msg,
                                              _vehicle->id(),
                                              componentId);
-    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
+    _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg);
 
     QString what = (componentId == MAV_COMP_ID_ALL) ? "MAV_COMP_ID_ALL" : QString::number(componentId);
     qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Request to refresh all parameters for component ID:" << what;
@@ -831,7 +831,7 @@ void ParameterManager::_readParameterRaw(int componentId, const QString& paramNa
                                              componentId,                    // Target component id
                                              fixedParamName,                 // Named parameter being requested
                                              paramIndex);                    // Parameter index being requested, -1 for named
-    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
+    _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg);
 }
 
 void ParameterManager::_writeParameterRaw(int componentId, const QString& paramName, const QVariant& value)
@@ -890,7 +890,7 @@ void ParameterManager::_writeParameterRaw(int componentId, const QString& paramN
                                       _vehicle->priorityLink()->mavlinkChannel(),
                                       &msg,
                                       &p);
-    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
+    _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg);
 }
 
 void ParameterManager::_writeLocalParamCache(int vehicleId, int componentId)
@@ -997,7 +997,7 @@ void ParameterManager::_tryCacheHashLoad(int vehicleId, int componentId, QVarian
                                           _vehicle->priorityLink()->mavlinkChannel(),
                                           &msg,
                                           &p);
-        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
+        _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg);
 
         // Give the user some feedback things loaded properly
         QVariantAnimation *ani = new QVariantAnimation(this);

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -945,7 +945,7 @@ void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
                 &msg,
                 &cmd);
 
-    vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
+    vehicle->sendMessageOnLinkThreadSafe(vehicle->priorityLink(), msg);
 }
 
 void APMFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
@@ -1139,5 +1139,5 @@ void APMFirmwarePlugin::_sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMoti
                                           vehicle->priorityLink()->mavlinkChannel(),
                                           &message,
                                           &globalPositionInt);
-    vehicle->sendMessageOnLink(vehicle->priorityLink(), message);
+    vehicle->sendMessageOnLinkThreadSafe(vehicle->priorityLink(), message);
 }

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -947,5 +947,5 @@ void FirmwarePlugin::sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionRe
                                           vehicle->priorityLink()->mavlinkChannel(),
                                           &message,
                                           &follow_target);
-    vehicle->sendMessageOnLink(vehicle->priorityLink(), message);
+    vehicle->sendMessageOnLinkThreadSafe(vehicle->priorityLink(), message);
 }

src/FlightDisplay/VirtualJoystick.qml

@@ -28,7 +28,7 @@ Item {
         repeat:     true
         onTriggered: {
             if (activeVehicle) {
-                activeVehicle.virtualTabletJoystickValue(rightStick.xAxis, -rightStick.yAxis, leftStick.xAxis, leftStick.yAxis)
+                activeVehicle.virtualTabletJoystickValue(rightStick.xAxis, rightStick.yAxis, leftStick.xAxis, leftStick.yAxis)
             }
         }
     }

src/GPS/RTCM/RTCMMavlink.cc

@@ -72,6 +72,6 @@ void RTCMMavlink::sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg)
                                               vehicle->priorityLink()->mavlinkChannel(),
                                               &message,
                                               &msg);
-        vehicle->sendMessageOnLink(vehicle->priorityLink(), message);
+        vehicle->sendMessageOnLinkThreadSafe(vehicle->priorityLink(), message);
     }
 }

src/Joystick/Joystick.cc

@@ -117,8 +117,7 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC
 
 Joystick::~Joystick()
 {
-    // Crash out of the thread if it is still running
-    terminate();
+    _exitThread = true;
     wait();
     delete[] _rgAxisValues;
     delete[] _rgCalibration;
@@ -636,9 +635,9 @@ void Joystick::_handleAxis()
                 // Exponential (0% to -50% range like most RC radios)
                 // _exponential is set by a slider in joystickConfigAdvanced.qml
                 // Calculate new RPY with exponential applied
-                roll =      -_exponential*powf(roll, 3) + (1+_exponential)*roll;
-                pitch =     -_exponential*powf(pitch,3) + (1+_exponential)*pitch;
-                yaw =       -_exponential*powf(yaw,  3) + (1+_exponential)*yaw;
+                roll =  -_exponential*powf(roll, 3) + (1+_exponential)*roll;
+                pitch = -_exponential*powf(pitch,3) + (1+_exponential)*pitch;
+                yaw =   -_exponential*powf(yaw,  3) + (1+_exponential)*yaw;
             }
 
             // Adjust throttle to 0:1 range
@@ -661,7 +660,8 @@ void Joystick::_handleAxis()
                 }
             }
             uint16_t shortButtons = static_cast<uint16_t>(buttonPressedBits & 0xFFFF);
-            emit manualControl(roll, -pitch, yaw, throttle, shortButtons, _activeVehicle->joystickMode());
+            _activeVehicle->sendJoystickDataThreadSafe(roll, pitch, yaw, throttle, shortButtons);
+            emit axisValues(roll, -pitch, yaw, throttle); // Used by joystick cal screen
             if(_activeVehicle && _axisCount > 4 && _gimbalEnabled) {
                 //-- TODO: There is nothing consuming this as there are no messages to handle gimbal
                 //   the way MANUAL_CONTROL handles the other channels.
@@ -676,8 +676,6 @@ void Joystick::startPolling(Vehicle* vehicle)
     if (vehicle) {
         // If a vehicle is connected, disconnect it
         if (_activeVehicle) {
-            UAS* uas = _activeVehicle->uas();
-            disconnect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint);
             disconnect(this, &Joystick::setArmed,           _activeVehicle, &Vehicle::setArmed);
             disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight);
             disconnect(this, &Joystick::setFlightMode,      _activeVehicle, &Vehicle::setFlightMode);
@@ -700,8 +698,6 @@ void Joystick::startPolling(Vehicle* vehicle)
         // Only connect the new vehicle if it wants joystick data
         if (vehicle->joystickEnabled()) {
             _pollingStartedForCalibration = false;
-            UAS* uas = _activeVehicle->uas();
-            connect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint);
             connect(this, &Joystick::setArmed,           _activeVehicle, &Vehicle::setArmed);
             connect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight);
             connect(this, &Joystick::setFlightMode,      _activeVehicle, &Vehicle::setFlightMode);
@@ -710,8 +706,6 @@ void Joystick::startPolling(Vehicle* vehicle)
             connect(this, &Joystick::centerGimbal,       _activeVehicle, &Vehicle::centerGimbal);
             connect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue);
             connect(this, &Joystick::emergencyStop,      _activeVehicle, &Vehicle::emergencyStop);
-            // FIXME: ****
-            //connect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction);
         }
     }
     if (!isRunning()) {
@@ -724,10 +718,6 @@ void Joystick::stopPolling(void)
 {
     if (isRunning()) {
         if (_activeVehicle && _activeVehicle->joystickEnabled()) {
-            UAS* uas = _activeVehicle->uas();
-            // Neutral attitude controls
-            // emit manualControl(0, 0, 0, 0.5, 0, _activeVehicle->joystickMode());
-            disconnect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint);
             disconnect(this, &Joystick::setArmed,           _activeVehicle, &Vehicle::setArmed);
             disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight);
             disconnect(this, &Joystick::setFlightMode,      _activeVehicle, &Vehicle::setFlightMode);
@@ -736,8 +726,6 @@ void Joystick::stopPolling(void)
             disconnect(this, &Joystick::centerGimbal,       _activeVehicle, &Vehicle::centerGimbal);
             disconnect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue);
         }
-        // FIXME: ****
-        //disconnect(this, &Joystick::buttonActionTriggered,  uas, &UAS::triggerAction);
         _exitThread = true;
     }
 }

src/Joystick/Joystick.h

@@ -193,19 +193,9 @@ signals:
     void accumulatorChanged         (bool accumulator);
     void enabledChanged             (bool enabled);
     void circleCorrectionChanged    (bool circleCorrection);
-
-    /// Signal containing new joystick information
-    ///     @param roll:            Range is -1:1, negative meaning roll left, positive meaning roll right
-    ///     @param pitch:           Range i -1:1, negative meaning pitch down, positive meaning pitch up
-    ///     @param yaw:             Range is -1:1, negative meaning yaw left, positive meaning yaw right
-    ///     @param throttle:        Range is 0:1, 0 meaning no throttle, 1 meaning full throttle
-    ///     @param buttons:         Button bitmap
-    ///     @param joystickMmode:   Current joystick mode
-    void manualControl              (float roll, float pitch, float yaw, float throttle, quint16 buttons, int joystickMmode);
+    void axisValues                 (float roll, float pitch, float yaw, float throttle);
     void manualControlGimbal        (float gimbalPitch, float gimbalYaw);
 
-    void buttonActionTriggered      (int action);
-
     void gimbalEnabledChanged       ();
     void axisFrequencyChanged       ();
     void buttonFrequencyChanged     ();

src/MissionManager/MissionManager.cc

@@ -69,7 +69,7 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC
                                          &messageOut,
                                          &missionItem);
 
-    _vehicle->sendMessageOnLink(_dedicatedLink, messageOut);
+    _vehicle->sendMessageOnLinkThreadSafe(_dedicatedLink, messageOut);
     _startAckTimeout(AckGuidedItem);
     emit inProgressChanged(true);
 }

src/MissionManager/PlanManager.cc

@@ -119,7 +119,7 @@ void PlanManager::_writeMissionCount(void)
                                         _writeMissionItems.count(),
                                         _planType);
 
-    _vehicle->sendMessageOnLink(_dedicatedLink, message);
+    _vehicle->sendMessageOnLinkThreadSafe(_dedicatedLink, message);
     _startAckTimeout(AckMissionRequest);
 }
 
@@ -161,7 +161,7 @@ void PlanManager::_requestList(void)
                                                MAV_COMP_ID_AUTOPILOT1,
                                                _planType);
 
-    _vehicle->sendMessageOnLink(_dedicatedLink, message);
+    _vehicle->sendMessageOnLinkThreadSafe(_dedicatedLink, message);
     _startAckTimeout(AckMissionCount);
 }
 
@@ -301,7 +301,7 @@ void PlanManager::_readTransactionComplete(void)
                                       MAV_MISSION_ACCEPTED,
                                       _planType);
     
-    _vehicle->sendMessageOnLink(_dedicatedLink, message);
+    _vehicle->sendMessageOnLinkThreadSafe(_dedicatedLink, message);
 
     _finishTransaction(true);
 }
@@ -369,7 +369,7 @@ void PlanManager::_requestNextMissionItem(void)
                 _planType);
     }
     
-    _vehicle->sendMessageOnLink(_dedicatedLink, message);
+    _vehicle->sendMessageOnLinkThreadSafe(_dedicatedLink, message);
     _startAckTimeout(AckMissionItem);
 }
 
@@ -586,7 +586,7 @@ void PlanManager::_handleMissionRequest(const mavlink_message_t& message, bool m
                                            _planType);
     }
     
-    _vehicle->sendMessageOnLink(_dedicatedLink, messageOut);
+    _vehicle->sendMessageOnLinkThreadSafe(_dedicatedLink, messageOut);
     _startAckTimeout(AckMissionRequest);
 }
 
@@ -928,7 +928,7 @@ void PlanManager::_removeAllWorker(void)
                                             _vehicle->id(),
                                             MAV_COMP_ID_AUTOPILOT1,
                                             _planType);
-    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
+    _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), message);
     _startAckTimeout(AckMissionClearAll);
 }
 

src/Vehicle/MultiVehicleManager.cc

@@ -387,8 +387,7 @@ void MultiVehicleManager::_sendGCSHeartbeat(void)
 
             uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
             int len = mavlink_msg_to_send_buffer(buffer, &message);
-
-            link->writeBytesSafe((const char*)buffer, len);
+            link->writeBytesThreadSafe((const char*)buffer, len);
         }
     }
 }

src/Vehicle/TerrainProtocolHandler.cc

@@ -146,7 +146,7 @@ void TerrainProtocolHandler::_sendTerrainData(const QGeoCoordinate& swCorner, ui
                         _currentTerrainRequest.grid_spacing,
                         gridBit,
                         terrainData);
-            _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
+            _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg);
         }
     }
 }

src/Vehicle/Vehicle.h

@@ -561,8 +561,6 @@ public:
     Q_PROPERTY(QString              formatedMessages        READ formatedMessages                                       NOTIFY formatedMessagesChanged)
     Q_PROPERTY(QString              formatedMessage         READ formatedMessage                                        NOTIFY formatedMessageChanged)
     Q_PROPERTY(QString              latestError             READ latestError                                            NOTIFY latestErrorChanged)
-    Q_PROPERTY(int                  joystickMode            READ joystickMode           WRITE setJoystickMode           NOTIFY joystickModeChanged)
-    Q_PROPERTY(QStringList          joystickModes           READ joystickModes                                          CONSTANT)
     Q_PROPERTY(bool                 joystickEnabled         READ joystickEnabled        WRITE setJoystickEnabled        NOTIFY joystickEnabledChanged)
     Q_PROPERTY(bool                 active                  READ active                 WRITE setActive                 NOTIFY activeChanged)
     Q_PROPERTY(int                  flowImageIndex          READ flowImageIndex                                         NOTIFY flowImageIndexChanged)
@@ -809,25 +807,11 @@ public:
     QGeoCoordinate coordinate() { return _coordinate; }
     QGeoCoordinate armedPosition    () { return _armedPosition; }
 
-    typedef enum {
-        JoystickModeRC,         ///< Joystick emulates an RC Transmitter
-        JoystickModeAttitude,
-        JoystickModePosition,
-        JoystickModeForce,
-        JoystickModeVelocity,
-        JoystickModeMax
-    } JoystickMode_t;
-
     void updateFlightDistance(double distance);
 
-    int joystickMode();
-    void setJoystickMode(int mode);
-
-    /// List of joystick mode names
-    QStringList joystickModes();
-
-    bool joystickEnabled();
-    void setJoystickEnabled(bool enabled);
+    bool joystickEnabled            ();
+    void setJoystickEnabled         (bool enabled);
+    void sendJoystickDataThreadSafe (float roll, float pitch, float yaw, float thrust, quint16 buttons);
 
     // Is vehicle active with respect to current active vehicle in QGC
     bool active();
@@ -845,7 +829,7 @@ public:
 
     /// Sends a message to the specified link
     /// @return true: message sent, false: Link no longer connected
-    bool sendMessageOnLink(LinkInterface* link, mavlink_message_t message);
+    bool sendMessageOnLinkThreadSafe(LinkInterface* link, mavlink_message_t message);
 
     /// Sends the specified messages multiple times to the vehicle in order to attempt to
     /// guarantee that it makes it to the vehicle.
@@ -1145,7 +1129,6 @@ public slots:
 signals:
     void allLinksInactive               (Vehicle* vehicle);
     void coordinateChanged              (QGeoCoordinate coordinate);
-    void joystickModeChanged            (int mode);
     void joystickEnabledChanged         (bool enabled);
     void activeChanged                  (bool active);
     void mavlinkMessageReceived         (const mavlink_message_t& message);
@@ -1180,14 +1163,9 @@ signals:
     void linksPropertiesChanged         ();
     void textMessageReceived            (int uasid, int componentid, int severity, QString text);
     void checkListStateChanged          ();
-
     void messagesReceivedChanged        ();
     void messagesSentChanged            ();
     void messagesLostChanged            ();
-
-    /// Used internally to move sendMessage call to main thread
-    void _sendMessageOnLinkOnThread(LinkInterface* link, mavlink_message_t message);
-
     void messageTypeChanged             ();
     void newMessageCountChanged         ();
     void messageCountChanged            ();
@@ -1258,7 +1236,6 @@ signals:
 private slots:
     void _mavlinkMessageReceived        (LinkInterface* link, mavlink_message_t message);
     void _linkInactiveOrDeleted         (LinkInterface* link);
-    void _sendMessageOnLink             (LinkInterface* link, mavlink_message_t message);
     void _sendMessageMultipleNext       ();
     void _parametersReady               (bool parametersReady);
     void _remoteControlRSSIChanged      (uint8_t rssi);
@@ -1388,7 +1365,6 @@ private:
 
     QList<LinkInterface*> _links;
 
-    JoystickMode_t  _joystickMode;
     bool            _joystickEnabled;
 
     UAS* _uas;
@@ -1436,6 +1412,7 @@ private:
     bool            _highLatencyLink;
     bool            _receivingAttitudeQuaternion;
     CheckList       _checkListState = CheckListNotSetup;
+    QMutex          _sendMessageOnLinkMutex;
 
     QGCCameraManager* _cameras;
 
@@ -1648,7 +1625,5 @@ private:
 
     // Settings keys
     static const char* _settingsGroup;
-    static const char* _joystickModeSettingsKey;
     static const char* _joystickEnabledSettingsKey;
-
 };

src/VehicleSetup/JoystickConfigAdvanced.qml

@@ -128,22 +128,6 @@ Item {
             }
         }
         //-----------------------------------------------------------------
-        //-- Mode
-        QGCLabel {
-            Layout.alignment:   Qt.AlignVCenter
-            text:               qsTr("Joystick mode:")
-            visible:            advancedSettings.checked
-        }
-        QGCComboBox {
-            enabled:            advancedSettings.checked
-            currentIndex:       activeVehicle.joystickMode
-            width:              ScreenTools.defaultFontPixelWidth * 20
-            model:              activeVehicle.joystickModes
-            onActivated:        activeVehicle.joystickMode = index
-            Layout.alignment:   Qt.AlignVCenter
-            visible:            advancedSettings.checked
-        }
-        //-----------------------------------------------------------------
         //-- Axis Message Frequency
         QGCLabel {
             text:               qsTr("Axis frequency (Hz):")

src/VehicleSetup/JoystickConfigGeneral.qml

@@ -200,7 +200,7 @@ Item {
 
                     Connections {
                         target:             _activeJoystick
-                        onManualControl: {
+                        onAxisValues: {
                             rollAxis.axisValue      = roll  * 32768.0
                             pitchAxis.axisValue     = pitch * 32768.0
                             yawAxis.axisValue       = yaw   * 32768.0

src/comm/LinkInterface.cc

@@ -65,7 +65,6 @@ LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config, bool isPX4F
     memset(_outDataWriteAmounts,0, sizeof(_outDataWriteAmounts));
     memset(_outDataWriteTimes,  0, sizeof(_outDataWriteTimes));
 
-    QObject::connect(this, &LinkInterface::_invokeWriteBytes, this, &LinkInterface::_writeBytes);
     qRegisterMetaType<LinkInterface*>("LinkInterface*");
 }
 
@@ -210,3 +209,11 @@ void LinkInterface::stopMavlinkMessagesTimer() {
 
     _mavlinkMessagesTimers.clear();
 }
+
+void LinkInterface::writeBytesThreadSafe(const char *bytes, int length)
+{
+    QByteArray byteArray(bytes, length);
+    _writeBytesMutex.lock();
+    _writeBytes(byteArray);
+    _writeBytesMutex.unlock();
+}

src/comm/LinkInterface.h

@@ -136,33 +136,11 @@ public:
     bool connect(void);
     bool disconnect(void);
 
-public slots:
+    void writeBytesThreadSafe(const char *bytes, int length);
 
-    /**
-     * @brief This method allows to write bytes to the interface.
-     *
-     * If the underlying communication is packet oriented,
-     * one write command equals a datagram. In case of serial
-     * communication arbitrary byte lengths can be written. The method ensures
-     * thread safety regardless of the underlying LinkInterface implementation.
-     *
-     * @param bytes:  The pointer to the byte array containing the data
-     * @param length: The length of the data array
-     **/
-    void writeBytesSafe(const char *bytes, int length)
-    {
-        emit _invokeWriteBytes(QByteArray(bytes, length));
-    }
-
-private slots:
-    virtual void _writeBytes(const QByteArray) = 0;
-
-    void _activeChanged(bool active, int vehicle_id);
-    
 signals:
     void autoconnectChanged(bool autoconnect);
     void activeChanged(LinkInterface* link, bool active, int vehicle_id);
-    void _invokeWriteBytes(QByteArray);
     void highLatencyChanged(bool highLatency);
 
     /// Signalled when a link suddenly goes away due to it being removed by for example pulling the cable to the connection.
@@ -231,6 +209,9 @@ protected:
     SharedLinkConfigurationPointer _config;
     bool _highLatency;
 
+private slots:
+    void _activeChanged(bool active, int vehicle_id);
+
 private:
     /**
      * @brief logDataRateToBuffer Stores transmission times/amounts for statistics
@@ -288,6 +269,8 @@ private:
      */
     void stopMavlinkMessagesTimer();
 
+    virtual void _writeBytes(const QByteArray) = 0; // Not thread safe, only writeBytesThreadSafe is thread safe
+
     bool _mavlinkChannelSet;    ///< true: _mavlinkChannel has been set
     uint8_t _mavlinkChannel;    ///< mavlink channel to use for this link, as used by mavlink_parse_char
     
@@ -307,7 +290,8 @@ private:
     quint64 _outDataWriteAmounts[_dataRateBufferSize]; // In bytes
     qint64  _outDataWriteTimes[_dataRateBufferSize]; // in ms
     
-    mutable QMutex _dataRateMutex; // Mutex for accessing the data rate member variables
+    mutable QMutex _dataRateMutex;
+    mutable QMutex _writeBytesMutex;
 
     bool _enableRateCollection;
     bool _decodedFirstMavlinkPacket;    ///< true: link has correctly decoded it's first mavlink packet

src/comm/LinkManager.cc

@@ -969,7 +969,7 @@ void LinkManager::_activeLinkCheck(void)
     if (!found && link) {
         // See if we can get an NSH prompt on this link
         bool foundNSHPrompt = false;
-        link->writeBytesSafe("\r", 1);
+        link->writeBytesThreadSafe("\r", 1);
         QSignalSpy spy(link, SIGNAL(bytesReceived(LinkInterface*, QByteArray)));
         if (spy.wait(100)) {
             QList<QVariant> arguments = spy.takeFirst();

src/comm/MAVLinkProtocol.cc

@@ -277,7 +277,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
                 if (forwardingLink) {
                     uint8_t buf[MAVLINK_MAX_PACKET_LEN];
                     int len = mavlink_msg_to_send_buffer(buf, &_message);
-                    forwardingLink->writeBytesSafe((const char*)buf, len);
+                    forwardingLink->writeBytesThreadSafe((const char*)buf, len);
                 }
             }
 

src/comm/MockLink.cc

@@ -89,8 +89,9 @@ MockLink::MockLink(SharedLinkConfigurationPointer& config)
     _highLatency = mockConfig->highLatency();
     _failureMode = mockConfig->failureMode();
 
-    union px4_custom_mode   px4_cm;
+    QObject::connect(this, &MockLink::writeBytesQueuedSignal, this, &MockLink::_writeBytesQueued, Qt::QueuedConnection);
 
+    union px4_custom_mode   px4_cm;
     px4_cm.data = 0;
     px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
     _mavCustomMode = px4_cm.data;
@@ -411,6 +412,12 @@ void MockLink::respondWithMavlinkMessage(const mavlink_message_t& msg)
 
 /// @brief Called when QGC wants to write bytes to the MAV
 void MockLink::_writeBytes(const QByteArray bytes)
+{
+    // This prevents the responses to mavlink messages from being sent until the _writeBytes returns.
+    emit writeBytesQueuedSignal(bytes);
+}
+
+void MockLink::_writeBytesQueued(const QByteArray bytes)
 {
     if (_inNSH) {
         _handleIncomingNSHBytes(bytes.constData(), bytes.count());
@@ -422,6 +429,7 @@ void MockLink::_writeBytes(const QByteArray bytes)
 
         _handleIncomingMavlinkBytes((uint8_t *)bytes.constData(), bytes.count());
     }
+
 }
 
 /// @brief Handle incoming bytes which are meant to be interpreted by the NuttX shell

src/comm/MockLink.h

@@ -160,8 +160,12 @@ public:
     static MockLink* startAPMArduSubMockLink     (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
     static MockLink* startAPMArduRoverMockLink   (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
 
+signals:
+    void writeBytesQueuedSignal(const QByteArray bytes);
+
 private slots:
-    virtual void _writeBytes(const QByteArray bytes);
+    void _writeBytes(const QByteArray bytes) final;
+    void _writeBytesQueued(const QByteArray bytes);
 
 private slots:
     void _run1HzTasks(void);

src/qgcunittest/TCPLinkTest.cc

@@ -125,7 +125,7 @@ void TCPLinkTest::_connectSucceed_test(void)
     const char* bytesWrittenSignal = SIGNAL(bytesWritten(qint64));
     MultiSignalSpy bytesWrittenSpy;
     QCOMPARE(bytesWrittenSpy.init(_link->getSocket(), &bytesWrittenSignal, 1), true);
-    _link->writeBytesSafe(bytesOut.data(), bytesOut.size());
+    _link->writeBytesThreadSafe(bytesOut.data(), bytesOut.size());
     _multiSpy->clearAllSignals();
     
     // We emit this signal such that it will be queued and run on the TCPLink thread. This in turn

src/uas/FileManager.cc

@@ -891,5 +891,5 @@ void FileManager::_sendRequestNoAck(Request* request)
                                                  0,                  // Target component
                                                  (uint8_t*)request); // Payload
     
-    _vehicle->sendMessageOnLink(link, message);
+    _vehicle->sendMessageOnLinkThreadSafe(link, message);
 }

src/uas/UAS.cc

@@ -424,7 +424,7 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
                                        accelCal,                         // accel cal
                                        airspeedCal,                      // PX4: airspeed cal, ArduPilot: compass mot
                                        escCal);                          // esc cal
-    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
+    _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg);
 }
 
 void UAS::stopCalibration(void)
@@ -732,7 +732,7 @@ void UAS::requestImage()
                                                           &msg,
                                                           MAVLINK_DATA_STREAM_IMG_JPEG,
                                                           0, 0, 0, 0, 0, 50);
-        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
+        _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg);
     }
 }
 
@@ -811,196 +811,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);
 }
 
-/**
-* Set the manual control commands.
-* This can only be done if the system has manual inputs enabled and is armed.
-*/
-void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode)
-{
-    if (!_vehicle || !_vehicle->priorityLink()) {
-        return;
-    }
-    mavlink_message_t message;
-    if (joystickMode == Vehicle::JoystickModeAttitude) {
-        // send an external attitude setpoint command (rate control disabled)
-        float attitudeQuaternion[4];
-        mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion);
-        uint8_t typeMask = 0x7; // disable rate control
-        mavlink_msg_set_attitude_target_pack_chan(
-            mavlink->getSystemId(),
-            mavlink->getComponentId(),
-            _vehicle->priorityLink()->mavlinkChannel(),
-            &message,
-            QGC::groundTimeUsecs(),
-            this->uasId,
-            0,
-            typeMask,
-            attitudeQuaternion,
-            0,
-            0,
-            0,
-            thrust);
-    } else if (joystickMode == Vehicle::JoystickModePosition) {
-        // Send the the local position setpoint (local pos sp external message)
-        static float px = 0;
-        static float py = 0;
-        static float pz = 0;
-        //XXX: find decent scaling
-        px -= pitch;
-        py += roll;
-        pz -= 2.0f*(thrust-0.5);
-        uint16_t typeMask = (1<<11)|(7<<6)|(7<<3); // select only POSITION control
-        mavlink_msg_set_position_target_local_ned_pack_chan(
-            mavlink->getSystemId(),
-            mavlink->getComponentId(),
-            _vehicle->priorityLink()->mavlinkChannel(),
-            &message,
-            QGC::groundTimeUsecs(),
-            this->uasId,
-            0,
-            MAV_FRAME_LOCAL_NED,
-            typeMask,
-            px,
-            py,
-            pz,
-            0,
-            0,
-            0,
-            0,
-            0,
-            0,
-            yaw,
-            0);
-    } else if (joystickMode == Vehicle::JoystickModeForce) {
-        // Send the the force setpoint (local pos sp external message)
-        float dcm[3][3];
-        mavlink_euler_to_dcm(roll, pitch, yaw, dcm);
-        const float fx = -dcm[0][2] * thrust;
-        const float fy = -dcm[1][2] * thrust;
-        const float fz = -dcm[2][2] * thrust;
-        uint16_t typeMask = (3<<10)|(7<<3)|(7<<0)|(1<<9); // select only FORCE control (disable everything else)
-        mavlink_msg_set_position_target_local_ned_pack_chan(
-            mavlink->getSystemId(),
-            mavlink->getComponentId(),
-            _vehicle->priorityLink()->mavlinkChannel(),
-            &message,
-            QGC::groundTimeUsecs(),
-            this->uasId,
-            0,
-            MAV_FRAME_LOCAL_NED,
-            typeMask,
-            0,
-            0,
-            0,
-            0,
-            0,
-            0,
-            fx,
-            fy,
-            fz,
-            0,
-            0);
-    } else if (joystickMode == Vehicle::JoystickModeVelocity) {
-        // Send the the local velocity setpoint (local pos sp external message)
-        static float vx = 0;
-        static float vy = 0;
-        static float vz = 0;
-        static float yawrate = 0;
-        //XXX: find decent scaling
-        vx -= pitch;
-        vy += roll;
-        vz -= 2.0f*(thrust-0.5);
-        yawrate += yaw; //XXX: not sure what scale to apply here
-        uint16_t typeMask = (1<<10)|(7<<6)|(7<<0); // select only VELOCITY control
-        mavlink_msg_set_position_target_local_ned_pack_chan(
-            mavlink->getSystemId(),
-            mavlink->getComponentId(),
-            _vehicle->priorityLink()->mavlinkChannel(),
-            &message,
-            QGC::groundTimeUsecs(),
-            this->uasId,
-            0,
-            MAV_FRAME_LOCAL_NED,
-            typeMask,
-            0,
-            0,
-            0,
-            vx,
-            vy,
-            vz,
-            0,
-            0,
-            0,
-            0,
-            yawrate);
-    } else if (joystickMode == Vehicle::JoystickModeRC) {
-        // Store scaling values for all 3 axes
-        static const float axesScaling = 1.0 * 1000.0;
-        // Calculate the new commands for roll, pitch, yaw, and thrust
-        const float newRollCommand = roll * axesScaling;
-        // negate pitch value because pitch is negative for pitching forward but mavlink message argument is positive for forward
-        const float newPitchCommand  = -pitch * axesScaling;
-        const float newYawCommand    = yaw * axesScaling;
-        const float newThrustCommand = thrust * axesScaling;
-        // Send the MANUAL_COMMAND message
-        mavlink_msg_manual_control_pack_chan(
-            static_cast<uint8_t>(mavlink->getSystemId()),
-            static_cast<uint8_t>(mavlink->getComponentId()),
-            _vehicle->priorityLink()->mavlinkChannel(),
-            &message,
-            static_cast<uint8_t>(this->uasId),
-            static_cast<int16_t>(newPitchCommand),
-            static_cast<int16_t>(newRollCommand),
-            static_cast<int16_t>(newThrustCommand),
-            static_cast<int16_t>(newYawCommand),
-            buttons);
-    }
-    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
-}
-
-#ifndef __mobile__
-void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw)
-{
-    if (!_vehicle) {
-        return;
-    }
-    const uint8_t base_mode = _vehicle->baseMode();
-
-   // If system has manual inputs enabled and is armed
-    if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
-    {
-        mavlink_message_t message;
-        float q[4];
-        mavlink_euler_to_quaternion(roll, pitch, yaw, q);
-
-        float yawrate = 0.0f;
-
-        // Do not control rates and throttle
-        quint8 mask = (1 << 0) | (1 << 1) | (1 << 2); // ignore rates
-        mask |= (1 << 6); // ignore throttle
-        mavlink_msg_set_attitude_target_pack_chan(mavlink->getSystemId(),
-                                                  mavlink->getComponentId(),
-                                                  _vehicle->priorityLink()->mavlinkChannel(),
-                                                  &message,
-                                                  QGC::groundTimeMilliseconds(), this->uasId, _vehicle->defaultComponentId(),
-                                                  mask, q, 0, 0, 0, 0);
-        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
-        quint16 position_mask = (1 << 3) | (1 << 4) | (1 << 5) |
-            (1 << 6) | (1 << 7) | (1 << 8);
-        mavlink_msg_set_position_target_local_ned_pack_chan(mavlink->getSystemId(), mavlink->getComponentId(),
-                                                            _vehicle->priorityLink()->mavlinkChannel(),
-                                                            &message, QGC::groundTimeMilliseconds(), this->uasId, _vehicle->defaultComponentId(),
-                                                            MAV_FRAME_LOCAL_NED, position_mask, x, y, z, 0, 0, 0, 0, 0, 0, yaw, yawrate);
-        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
-        qDebug() << __FILE__ << __LINE__ << ": SENT 6DOF CONTROL MESSAGES: x" << x << " y: " << y << " z: " << z << " roll: " << roll << " pitch: " << pitch << " yaw: " << yaw;
-    }
-    else
-    {
-        qDebug() << "3DMOUSE/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send 3DMouse commands first";
-    }
-}
-#endif
-
 /**
 * Order the robot to start receiver pairing
 */

src/uas/UAS.h

@@ -193,14 +193,6 @@ public slots:
     /** @brief Order the robot to pair its receiver **/
     void pairRX(int rxType, int rxSubType);
 
-    /** @brief Set the values for the manual control of the vehicle */
-    void setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode);
-
-    /** @brief Set the values for the 6dof manual control of the vehicle */
-#ifndef __mobile__
-    void setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw);
-#endif
-
     /** @brief Receive a message from one of the communication links. */
     virtual void receiveMessage(mavlink_message_t message);