Merge pull request #4004 from mavlink/revert-3967-QGCMav2.0

Revert "Mavlink 2.0 support"

QGCExternalLibs.pri

@@ -17,7 +17,7 @@ WindowsBuild {
 # a single compiled codebase this hardwiring of dialect can go away. But until then
 # this "workaround" is needed.
 
-MAVLINKPATH_REL = libs/mavlink/include/mavlink/v2.0
+MAVLINKPATH_REL = libs/mavlink/include/mavlink/v1.0
 MAVLINKPATH = $$BASEDIR/$$MAVLINKPATH_REL
 MAVLINK_CONF = ardupilotmega
 DEFINES += MAVLINK_NO_DATA

src/QmlControls/QGroundControlQmlGlobal.cc

@@ -33,8 +33,6 @@ SettingsFact* QGroundControlQmlGlobal::_speedUnitsFact =
 FactMetaData* QGroundControlQmlGlobal::_speedUnitsMetaData =                        NULL;
 SettingsFact* QGroundControlQmlGlobal::_batteryPercentRemainingAnnounceFact =       NULL;
 FactMetaData* QGroundControlQmlGlobal::_batteryPercentRemainingAnnounceMetaData =   NULL;
-SettingsFact* QGroundControlQmlGlobal::_mavlinkVersionFact =                        NULL;
-FactMetaData* QGroundControlQmlGlobal::_mavlinkVersionMetaData =                    NULL;
 
 const char* QGroundControlQmlGlobal::_virtualTabletJoystickKey  = "VirtualTabletJoystick";
 const char* QGroundControlQmlGlobal::_baseFontPointSizeKey      = "BaseDeviceFontPointSize";
@@ -199,6 +197,12 @@ void QGroundControlQmlGlobal::setIsMultiplexingEnabled(bool enable)
     emit isMultiplexingEnabledChanged(enable);
 }
 
+void QGroundControlQmlGlobal::setIsVersionCheckEnabled(bool enable)
+{
+    qgcApp()->toolbox()->mavlinkProtocol()->enableVersionCheck(enable);
+    emit isVersionCheckEnabledChanged(enable);
+}
+
 void QGroundControlQmlGlobal::setMavlinkSystemID(int id)
 {
     qgcApp()->toolbox()->mavlinkProtocol()->setSystemId(id);
@@ -361,25 +365,6 @@ Fact* QGroundControlQmlGlobal::batteryPercentRemainingAnnounce(void)
     return _batteryPercentRemainingAnnounceFact;
 }
 
-Fact* QGroundControlQmlGlobal::mavlinkVersion(void)
-{
-    if (!_mavlinkVersionFact) {
-        QStringList     enumStrings;
-        QVariantList    enumValues;
-
-        _mavlinkVersionFact = new SettingsFact(QString(), "MavlinkVersion", FactMetaData::valueTypeUint32, MavlinkVersion2IfVehicle2);
-        _mavlinkVersionMetaData = new FactMetaData(FactMetaData::valueTypeUint32);
-
-        enumStrings << "Always use version 1" << "Default to 1, switch to 2 if Vehicle sends version 2" << "Always use version 2";
-        enumValues << QVariant::fromValue((uint32_t)MavlinkVersionAlways1) << QVariant::fromValue((uint32_t)MavlinkVersion2IfVehicle2) << QVariant::fromValue((uint32_t)MavlinkVersionAlways2);
-
-        _mavlinkVersionMetaData->setEnumInfo(enumStrings, enumValues);
-        _mavlinkVersionFact->setMetaData(_mavlinkVersionMetaData);
-    }
-
-    return _mavlinkVersionFact;
-}
-
 bool QGroundControlQmlGlobal::linesIntersect(QPointF line1A, QPointF line1B, QPointF line2A, QPointF line2B)
 {
     QPointF intersectPoint;

src/QmlControls/QGroundControlQmlGlobal.h

@@ -60,13 +60,6 @@ public:
         SpeedUnitsKnots,
     };
 
-    enum MavlinkVersionSend {
-        MavlinkVersionAlways1 = 0,
-        MavlinkVersion2IfVehicle2,
-        MavlinkVersionAlways2,
-    };
-
-
     Q_ENUMS(DistanceUnits)
     Q_ENUMS(AreaUnits)
     Q_ENUMS(SpeedUnits)
@@ -95,8 +88,8 @@ public:
 
     // MavLink Protocol
     Q_PROPERTY(bool     isMultiplexingEnabled   READ isMultiplexingEnabled      WRITE setIsMultiplexingEnabled      NOTIFY isMultiplexingEnabledChanged)
+    Q_PROPERTY(bool     isVersionCheckEnabled   READ isVersionCheckEnabled      WRITE setIsVersionCheckEnabled      NOTIFY isVersionCheckEnabledChanged)
     Q_PROPERTY(int      mavlinkSystemID         READ mavlinkSystemID            WRITE setMavlinkSystemID            NOTIFY mavlinkSystemIDChanged)
-    Q_PROPERTY(Fact*    mavlinkVersion                  READ mavlinkVersion                     CONSTANT)
 
     Q_PROPERTY(Fact*    offlineEditingFirmwareType      READ offlineEditingFirmwareType         CONSTANT)
     Q_PROPERTY(Fact*    offlineEditingVehicleType       READ offlineEditingVehicleType          CONSTANT)
@@ -187,6 +180,7 @@ public:
     qreal   baseFontPointSize       () { return _baseFontPointSize; }
 
     bool    isMultiplexingEnabled   () { return _toolbox->mavlinkProtocol()->multiplexingEnabled(); }
+    bool    isVersionCheckEnabled   () { return _toolbox->mavlinkProtocol()->versionCheckEnabled(); }
     int     mavlinkSystemID         () { return _toolbox->mavlinkProtocol()->getSystemId(); }
 
     QGeoCoordinate lastKnownHomePosition() { return qgcApp()->lastKnownHomePosition(); }
@@ -199,7 +193,6 @@ public:
     static Fact* areaUnits                      (void);
     static Fact* speedUnits                     (void);
     static Fact* batteryPercentRemainingAnnounce(void);
-    static Fact* mavlinkVersion                 (void);
 
     //-- TODO: Make this into an actual preference.
     bool    isAdvancedMode          () { return false; }
@@ -212,6 +205,7 @@ public:
     void    setBaseFontPointSize        (qreal size);
 
     void    setIsMultiplexingEnabled    (bool enable);
+    void    setIsVersionCheckEnabled    (bool enable);
     void    setMavlinkSystemID          (int  id);
 
     QString parameterFileExtension(void) const  { return QGCApplication::parameterFileExtension; }
@@ -229,6 +223,7 @@ signals:
     void virtualTabletJoystickChanged   (bool enabled);
     void baseFontPointSizeChanged       (qreal size);
     void isMultiplexingEnabledChanged   (bool enabled);
+    void isVersionCheckEnabledChanged   (bool enabled);
     void mavlinkSystemIDChanged         (int id);
     void flightMapPositionChanged       (QGeoCoordinate flightMapPosition);
     void flightMapZoomChanged           (double flightMapZoom);
@@ -263,8 +258,6 @@ private:
     static FactMetaData*    _speedUnitsMetaData;
     static SettingsFact*    _batteryPercentRemainingAnnounceFact;
     static FactMetaData*    _batteryPercentRemainingAnnounceMetaData;
-    static SettingsFact*    _mavlinkVersionFact;
-    static FactMetaData*    _mavlinkVersionMetaData;
 
     static const char*  _virtualTabletJoystickKey;
     static const char*  _baseFontPointSizeKey;

src/Vehicle/Vehicle.cc

@@ -828,7 +828,14 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message)
     // Give the plugin a chance to adjust
     _firmwarePlugin->adjustOutgoingMavlinkMessage(this, &message);
 
-    _mavlink->sendMessage(link, message);
+    static const uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS;
+    mavlink_finalize_message_chan(&message, _mavlink->getSystemId(), _mavlink->getComponentId(), link->getMavlinkChannel(), message.len, message.len, messageKeys[message.msgid]);
+
+    // Write message into buffer, prepending start sign
+    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+    int len = mavlink_msg_to_send_buffer(buffer, &message);
+
+    link->writeBytesSafe((const char*)buffer, len);
     _messagesSent++;
     emit messagesSentChanged();
 }

src/comm/MAVLinkProtocol.cc

@@ -35,7 +35,6 @@
 #include "QGCApplication.h"
 #include "QGCLoggingCategory.h"
 #include "MultiVehicleManager.h"
-#include "QGroundControlQmlGlobal.h"
 
 Q_DECLARE_METATYPE(mavlink_message_t)
 
@@ -253,7 +252,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
                 {
                     mavlink_message_t msg;
                     mavlink_msg_ping_pack(getSystemId(), getComponentId(), &msg, ping.time_usec, ping.seq, message.sysid, message.compid);
-                    sendMessage(link, msg);
+                    _sendMessage(msg);
                 }
             }
 
@@ -394,7 +393,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
 
                     // Only forward this message to the other links,
                     // not the link the message was received on
-                    if (currLink && currLink != link) sendMessage(currLink, message);
+                    if (currLink && currLink != link) _sendMessage(currLink, message, message.sysid, message.compid);
                 }
             }
         }
@@ -426,27 +425,51 @@ int MAVLinkProtocol::getComponentId()
     return 0;
 }
 
-void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message)
+/**
+ * @param message message to send
+ */
+void MAVLinkProtocol::_sendMessage(mavlink_message_t message)
 {
-    mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(link->getMavlinkChannel());
-    switch (QGroundControlQmlGlobal::mavlinkVersion()->rawValue().toInt()) {
-    case QGroundControlQmlGlobal::MavlinkVersion2IfVehicle2:
-        if (mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
-            mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
-            break;
-        }
-        // Fallthrough to set version 2
-    case QGroundControlQmlGlobal::MavlinkVersionAlways2:
-        mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
-        break;
-    default:
-    case QGroundControlQmlGlobal::MavlinkVersionAlways1:
-        mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
-        break;
+    for (int i=0; i<_linkMgr->links()->count(); i++) {
+        LinkInterface* link = _linkMgr->links()->value<LinkInterface*>(i);
+        _sendMessage(link, message);
+    }
+}
+
+/**
+ * @param link the link to send the message over
+ * @param message message to send
+ */
+void MAVLinkProtocol::_sendMessage(LinkInterface* link, mavlink_message_t message)
+{
+    // Create buffer
+    static uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+    // Rewriting header to ensure correct link ID is set
+    static uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS;
+    mavlink_finalize_message_chan(&message, this->getSystemId(), this->getComponentId(), link->getMavlinkChannel(), message.len, message.len, messageKeys[message.msgid]);
+    // Write message into buffer, prepending start sign
+    int len = mavlink_msg_to_send_buffer(buffer, &message);
+    // If link is connected
+    if (link->isConnected())
+    {
+        // Send the portion of the buffer now occupied by the message
+        link->writeBytesSafe((const char*)buffer, len);
     }
+}
 
+/**
+ * @param link the link to send the message over
+ * @param message message to send
+ * @param systemid id of the system the message is originating from
+ * @param componentid id of the component the message is originating from
+ */
+void MAVLinkProtocol::_sendMessage(LinkInterface* link, mavlink_message_t message, quint8 systemid, quint8 componentid)
+{
     // Create buffer
     static uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+    // Rewriting header to ensure correct link ID is set
+    static uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS;
+    mavlink_finalize_message_chan(&message, systemid, componentid, link->getMavlinkChannel(), message.len, message.len, messageKeys[message.msgid]);
     // Write message into buffer, prepending start sign
     int len = mavlink_msg_to_send_buffer(buffer, &message);
     // If link is connected

src/comm/MAVLinkProtocol.h

@@ -129,8 +129,6 @@ public:
     /// Suspend/Restart logging during replay.
     void suspendLogForReplay(bool suspend);
 
-    void sendMessage(LinkInterface* link, mavlink_message_t message);
-
     // Override from QGCTool
     virtual void setToolbox(QGCToolbox *toolbox);
 
@@ -256,6 +254,10 @@ private slots:
     void _vehicleCountChanged(int count);
     
 private:
+    void _sendMessage(mavlink_message_t message);
+    void _sendMessage(LinkInterface* link, mavlink_message_t message);
+    void _sendMessage(LinkInterface* link, mavlink_message_t message, quint8 systemid, quint8 componentid);
+
 #ifndef __mobile__
     bool _closeLogFile(void);
     void _startLogging(void);

src/ui/MAVLinkDecoder.cc

@@ -44,6 +44,7 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) :
     #ifdef MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE
     messageFilter.insert(MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, false);
     #endif
+    messageFilter.insert(MAVLINK_MSG_ID_EXTENDED_MESSAGE, false);
     messageFilter.insert(MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, false);
 
     textMessageFilter.insert(MAVLINK_MSG_ID_DEBUG, false);

src/ui/preferences/MavlinkSettings.qml

@@ -15,7 +15,6 @@ import QtQuick.Dialogs          1.1
 
 import QGroundControl                       1.0
 import QGroundControl.FactSystem            1.0
-import QGroundControl.FactControls          1.0
 import QGroundControl.Controls              1.0
 import QGroundControl.ScreenTools           1.0
 import QGroundControl.MultiVehicleManager   1.0
@@ -78,21 +77,13 @@ Rectangle {
                     QGroundControl.isMultiplexingEnabled = checked
                 }
             }
-
-            // Mavlink version
-            Row {
-                spacing: ScreenTools.defaultFontPixelWidth
-
-                QGCLabel {
-                    text: qsTr("Mavlink Version:")
-                    anchors.baseline: mavlinkVersionCombo.baseline
-                }
-
-                FactComboBox {
-                    id:         mavlinkVersionCombo
-                    width:      ScreenTools.defaultFontPixelWidth * 45
-                    indexModel: false
-                    fact:       QGroundControl.mavlinkVersion
+            //-----------------------------------------------------------------
+            //-- Mavlink Version Check
+            QGCCheckBox {
+                text:       qsTr("Only accept MAVs with same protocol version")
+                checked:    QGroundControl.isVersionCheckEnabled
+                onClicked: {
+                    QGroundControl.isVersionCheckEnabled = checked
                 }
             }
         }