diff --git a/QGCExternalLibs.pri b/QGCExternalLibs.pri index 6524583b72c3ce825b1cc8a0b865bb4d86d2df60..05a8b4a1626580e64e961803ecd5e88552b7ed43 100644 --- a/QGCExternalLibs.pri +++ b/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/v1.0 +MAVLINKPATH_REL = libs/mavlink/include/mavlink/v2.0 MAVLINKPATH = $$BASEDIR/$$MAVLINKPATH_REL MAVLINK_CONF = ardupilotmega DEFINES += MAVLINK_NO_DATA diff --git a/src/QmlControls/QGroundControlQmlGlobal.cc b/src/QmlControls/QGroundControlQmlGlobal.cc index 0aca011312dce80e73bffeb7b1e1b71cf28eedd5..068ef70e89fa466e1782534e41ee707ef72b5ea4 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.cc +++ b/src/QmlControls/QGroundControlQmlGlobal.cc @@ -33,6 +33,8 @@ 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"; @@ -195,12 +197,6 @@ 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); @@ -363,6 +359,25 @@ 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; diff --git a/src/QmlControls/QGroundControlQmlGlobal.h b/src/QmlControls/QGroundControlQmlGlobal.h index dc7eb2b713794ed14eaa4c794017fb34387059e5..4a1e19702f2b4ad0e44a1d73192842f714eff88a 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.h +++ b/src/QmlControls/QGroundControlQmlGlobal.h @@ -60,6 +60,13 @@ public: SpeedUnitsKnots, }; + enum MavlinkVersionSend { + MavlinkVersionAlways1 = 0, + MavlinkVersion2IfVehicle2, + MavlinkVersionAlways2, + }; + + Q_ENUMS(DistanceUnits) Q_ENUMS(AreaUnits) Q_ENUMS(SpeedUnits) @@ -87,8 +94,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) @@ -178,7 +185,6 @@ 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(); } @@ -191,6 +197,7 @@ 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; } @@ -203,7 +210,6 @@ public: void setBaseFontPointSize (qreal size); void setIsMultiplexingEnabled (bool enable); - void setIsVersionCheckEnabled (bool enable); void setMavlinkSystemID (int id); QString parameterFileExtension(void) const { return QGCApplication::parameterFileExtension; } @@ -221,7 +227,6 @@ 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); @@ -255,6 +260,8 @@ private: static FactMetaData* _speedUnitsMetaData; static SettingsFact* _batteryPercentRemainingAnnounceFact; static FactMetaData* _batteryPercentRemainingAnnounceMetaData; + static SettingsFact* _mavlinkVersionFact; + static FactMetaData* _mavlinkVersionMetaData; static const char* _virtualTabletJoystickKey; static const char* _baseFontPointSizeKey; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 09fe5f613678f888c7eeab895749ed21034bd250..1ba010b135e9d34cf47e66d2e0de08b9c3841e92 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -821,14 +821,7 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message) // Give the plugin a chance to adjust _firmwarePlugin->adjustOutgoingMavlinkMessage(this, &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); + _mavlink->sendMessage(link, message, _mavlink->getSystemId(), _mavlink->getComponentId()); _messagesSent++; emit messagesSentChanged(); } diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 3002e18252618ecc18cf0fa92b220dcf2986396d..53346a55fc19d997c8f6048394aa1ad4a3487fc0 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -35,6 +35,7 @@ #include "QGCApplication.h" #include "QGCLoggingCategory.h" #include "MultiVehicleManager.h" +#include "QGroundControlQmlGlobal.h" Q_DECLARE_METATYPE(mavlink_message_t) @@ -252,7 +253,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(msg); + sendMessage(link, msg, getSystemId(), getComponentId()); } } @@ -393,7 +394,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, message.sysid, message.compid); + if (currLink && currLink != link) sendMessage(currLink, message, message.sysid, message.compid); } } } @@ -425,51 +426,35 @@ int MAVLinkProtocol::getComponentId() return 0; } -/** - * @param message message to send - */ -void MAVLinkProtocol::_sendMessage(mavlink_message_t message) -{ - for (int i=0; i<_linkMgr->links()->count(); i++) { - LinkInterface* link = _linkMgr->links()->value(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) +void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message, quint8 systemid, quint8 componentid) { + 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; + } + // 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]); + // Question: What is min_length + mavlink_finalize_message_chan(&message, systemid, componentid, link->getMavlinkChannel(), message.len, message.len, mavlink_get_crc_extra(&message)); // Write message into buffer, prepending start sign int len = mavlink_msg_to_send_buffer(buffer, &message); // If link is connected diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index 928fed1e542096317bafa7fc0d0e9d78325a48eb..9a60c0e8d35bc7c12f77a28d00a19bbc62e5495d 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -129,6 +129,8 @@ public: /// Suspend/Restart logging during replay. void suspendLogForReplay(bool suspend); + void sendMessage(LinkInterface* link, mavlink_message_t message, quint8 systemid, quint8 componentid); + // Override from QGCTool virtual void setToolbox(QGCToolbox *toolbox); @@ -254,10 +256,6 @@ 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); diff --git a/src/ui/MAVLinkDecoder.cc b/src/ui/MAVLinkDecoder.cc index b30c49b6ec836c9ef0a82a981817d82e67b903ab..b4938338d61bf730277fcd40c18b9f450d9b890a 100644 --- a/src/ui/MAVLinkDecoder.cc +++ b/src/ui/MAVLinkDecoder.cc @@ -44,7 +44,6 @@ 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); diff --git a/src/ui/preferences/MavlinkSettings.qml b/src/ui/preferences/MavlinkSettings.qml index 349b9d3855f3436c4f7ae23472c0688da18fc411..4f204235ff7e64af25baee0c2589444ff4f44e0a 100644 --- a/src/ui/preferences/MavlinkSettings.qml +++ b/src/ui/preferences/MavlinkSettings.qml @@ -15,6 +15,7 @@ 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 @@ -77,13 +78,21 @@ Rectangle { QGroundControl.isMultiplexingEnabled = checked } } - //----------------------------------------------------------------- - //-- Mavlink Version Check - QGCCheckBox { - text: qsTr("Only accept MAVs with same protocol version") - checked: QGroundControl.isVersionCheckEnabled - onClicked: { - QGroundControl.isVersionCheckEnabled = 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 } } }