Skip to content

Q
qgroundcontrol
Commit fa7d4906 authored by Don Gagne's avatar Don Gagne
Browse files
Options

Mavlink 2.0 support

parent f434cda0
Hide whitespace changes
Inline Side-by-side
Showing with 75 additions and 69 deletions
QGCExternalLibs.pri
View file @ fa7d4906
......@@ -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
......
src/QmlControls/QGroundControlQmlGlobal.cc
View file @ fa7d4906
......@@ -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;
......
src/QmlControls/QGroundControlQmlGlobal.h
View file @ fa7d4906
......@@ -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;
......
src/Vehicle/Vehicle.cc
View file @ fa7d4906
......@@ -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();
}
......
src/comm/MAVLinkProtocol.cc
View file @ fa7d4906
......@@ -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<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
......
src/comm/MAVLinkProtocol.h
View file @ fa7d4906
......@@ -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);
......
src/ui/MAVLinkDecoder.cc
View file @ fa7d4906
......@@ -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);
......
src/ui/preferences/MavlinkSettings.qml
View file @ fa7d4906
......@@ -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
}
}
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment