zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Mavlink 2.0 support

QGC4.4
Don Gagne 9 years ago
parent
f434cda097
commit
fa7d4906db
8 changed files with 75 additions and 69 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      QGCExternalLibs.pri
  2. 27
      src/QmlControls/QGroundControlQmlGlobal.cc
  3. 15
      src/QmlControls/QGroundControlQmlGlobal.h
  4. 9
      src/Vehicle/Vehicle.cc
  5. 61
      src/comm/MAVLinkProtocol.cc
  6. 6
      src/comm/MAVLinkProtocol.h
  7. 1
      src/ui/MAVLinkDecoder.cc
  8. 23
      src/ui/preferences/MavlinkSettings.qml

2
QGCExternalLibs.pri
Unescape View File

@ -17,7 +17,7 @@ WindowsBuild { @@ -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

27
src/QmlControls/QGroundControlQmlGlobal.cc
Unescape View File

@ -33,6 +33,8 @@ SettingsFact* QGroundControlQmlGlobal::_speedUnitsFact = @@ -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) @@ -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) @@ -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;

15
src/QmlControls/QGroundControlQmlGlobal.h
Unescape View File

@ -60,6 +60,13 @@ public: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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;

9
src/Vehicle/Vehicle.cc
Unescape View File

@ -821,14 +821,7 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message) @@ -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();
}

61
src/comm/MAVLinkProtocol.cc
Unescape View File

@ -35,6 +35,7 @@ @@ -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) @@ -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) @@ -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);
}
}
}
@ -426,50 +427,34 @@ int MAVLinkProtocol::getComponentId() @@ -426,50 +427,34 @@ int MAVLinkProtocol::getComponentId()
}
/**
* @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<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)
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

6
src/comm/MAVLinkProtocol.h
Unescape View File

@ -129,6 +129,8 @@ public: @@ -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: @@ -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);

1
src/ui/MAVLinkDecoder.cc
Unescape View File

@ -44,7 +44,6 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) : @@ -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);

23
src/ui/preferences/MavlinkSettings.qml
Unescape View File

@ -15,6 +15,7 @@ import QtQuick.Dialogs 1.1 @@ -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 { @@ -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
}
}
}

Write Preview
Loading…
Cancel
Save