From 7bb6dc153b5e7a37f8ead87c8e9ce88f0c859910 Mon Sep 17 00:00:00 2001 From: lm Date: Sat, 19 Mar 2011 05:00:59 +0100 Subject: [PATCH] Added support for Drone OS forwarding, MAVLink auth --- src/Waypoint.h | 2 +- src/comm/MAVLinkProtocol.cc | 29 +++++++++++++ src/comm/MAVLinkProtocol.h | 18 +++++++- src/comm/UDPLink.cc | 74 +++++++++++++++++++++++++-------- src/comm/UDPLink.h | 8 ++-- src/ui/MAVLinkSettingsWidget.cc | 71 +++++++++++++++++++++++++++++++ src/ui/MAVLinkSettingsWidget.h | 6 +++ src/ui/MAVLinkSettingsWidget.ui | 62 ++++++++++++++++++++++++++- src/ui/WaypointView.cc | 11 +++-- 9 files changed, 253 insertions(+), 28 deletions(-) diff --git a/src/Waypoint.h b/src/Waypoint.h index 0990edde2..66ff69be3 100644 --- a/src/Waypoint.h +++ b/src/Waypoint.h @@ -44,7 +44,7 @@ class Waypoint : public QObject public: Waypoint(quint16 id = 0, double x = 0.0, double y = 0.0, double z = 0.0, double param1 = 0.0, double param2 = 0.0, double param3 = 0.0, double param4 = 0.0, - bool autocontinue = true, bool current = false, MAV_FRAME frame=MAV_FRAME_GLOBAL, MAV_CMD action=MAV_CMD_NAV_WAYPOINT); + bool autocontinue = true, bool current = false, MAV_FRAME frame=MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD action=MAV_CMD_NAV_WAYPOINT); ~Waypoint(); quint16 getId() const { return id; } diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 5d088cc7a..822e1da31 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -41,6 +41,8 @@ MAVLinkProtocol::MAVLinkProtocol() : heartbeatTimer(new QTimer(this)), heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE), m_heartbeatsEnabled(false), + m_multiplexingEnabled(false), + m_authEnabled(false), m_loggingEnabled(false), m_logfile(NULL), m_enable_version_check(true), @@ -52,6 +54,7 @@ MAVLinkProtocol::MAVLinkProtocol() : versionMismatchIgnore(false), systemId(QGC::defaultSystemId) { + m_authKey = "xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx"; loadSettings(); //start(QThread::LowPriority); // Start heartbeat timer, emitting a heartbeat at the configured rate @@ -101,6 +104,10 @@ void MAVLinkProtocol::loadSettings() systemId = temp; } + // Set auth key + m_authKey = settings.value("GCS_AUTH_KEY", m_authKey).toString(); + enableAuth(settings.value("GCS_AUTH_ENABLED", m_authEnabled).toBool()); + // Parameter interface settings bool ok; temp = settings.value("PARAMETER_RETRANSMISSION_TIMEOUT", m_paramRetransmissionTimeout).toInt(&ok); @@ -121,6 +128,8 @@ void MAVLinkProtocol::storeSettings() settings.setValue("VERSION_CHECK_ENABLED", m_enable_version_check); settings.setValue("MULTIPLEXING_ENABLED", m_multiplexingEnabled); settings.setValue("GCS_SYSTEM_ID", systemId); + settings.setValue("GCS_AUTH_KEY", m_authKey); + settings.setValue("GCS_AUTH_ENABLED", m_authEnabled); if (m_logfile) { // Logfile exists, store the name @@ -417,6 +426,15 @@ void MAVLinkProtocol::sendHeartbeat() mavlink_msg_heartbeat_pack(getSystemId(), getComponentId(),&beat, OCU, MAV_AUTOPILOT_GENERIC); sendMessage(beat); } + if (m_authEnabled) + { + mavlink_message_t msg; + mavlink_auth_key_t auth; + if (m_authKey.length() != MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN) m_authKey.resize(MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN); + strcpy(auth.key, m_authKey.toStdString().c_str()); + mavlink_msg_auth_key_encode(getSystemId(), getComponentId(), &msg, &auth); + sendMessage(msg); + } } /** @param enabled true to enable heartbeats emission at heartbeatRate, false to disable */ @@ -435,6 +453,17 @@ void MAVLinkProtocol::enableMultiplexing(bool enabled) if (changed) emit multiplexingChanged(m_multiplexingEnabled); } +void MAVLinkProtocol::enableAuth(bool enable) +{ + bool changed = false; + m_authEnabled = enable; + if (m_authEnabled != enable) + { + changed = true; + } + if (changed) emit authChanged(m_authEnabled); +} + void MAVLinkProtocol::enableParamGuard(bool enabled) { if (enabled != m_paramGuardEnabled) diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index f23cbf35b..e73c39354 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -73,8 +73,12 @@ public: bool versionCheckEnabled() const { return m_enable_version_check; } /** @brief Get the multiplexing state */ bool multiplexingEnabled() const { return m_multiplexingEnabled; } + /** @brief Get the authentication state */ + bool getAuthEnabled() { return m_authEnabled; } /** @brief Get the protocol version */ int getVersion() { return MAVLINK_VERSION; } + /** @brief Get the auth key */ + QString getAuthKey() { return m_authKey; } /** @brief Get the name of the packet log file */ QString getLogfileName(); /** @brief Get state of parameter retransmission */ @@ -130,6 +134,12 @@ public slots: /** @brief Enable / disable version check */ void enableVersionCheck(bool enabled); + /** @brief Enable / disable authentication */ + void enableAuth(bool enable); + + /** @brief Set authentication token */ + void setAuthKey(QString key) { m_authKey = key;} + /** @brief Send an extra heartbeat to all connected units */ void sendHeartbeat(); @@ -142,8 +152,10 @@ protected: QTimer* heartbeatTimer; ///< Timer to emit heartbeats int heartbeatRate; ///< Heartbeat rate, controls the timer interval bool m_heartbeatsEnabled; ///< Enabled/disable heartbeat emission - bool m_loggingEnabled; ///< Enable/disable packet logging bool m_multiplexingEnabled; ///< Enable/disable packet multiplexing + bool m_authEnabled; ///< Enable authentication token broadcast + QString m_authKey; ///< Authentication key + bool m_loggingEnabled; ///< Enable/disable packet logging QFile* m_logfile; ///< Logfile bool m_enable_version_check; ///< Enable checking of version match of MAV and QGC int m_paramRetransmissionTimeout; ///< Timeout for parameter retransmission @@ -169,6 +181,10 @@ signals: void loggingChanged(bool enabled); /** @brief Emitted if multiplexing is started / stopped */ void multiplexingChanged(bool enabled); + /** @brief Emitted if authentication support is enabled / disabled */ + void authKeyChanged(QString key); + /** @brief Authentication changed */ + void authChanged(bool enabled); /** @brief Emitted if version check is enabled / disabled */ void versionCheckChanged(bool enabled); /** @brief Emitted if a message from the protocol should reach the user */ diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc index 496067f88..f54d1cbb4 100644 --- a/src/comm/UDPLink.cc +++ b/src/comm/UDPLink.cc @@ -36,6 +36,7 @@ This file is part of the QGROUNDCONTROL project #include "UDPLink.h" #include "LinkManager.h" #include "QGC.h" +#include //#include UDPLink::UDPLink(QHostAddress host, quint16 port) @@ -43,10 +44,6 @@ UDPLink::UDPLink(QHostAddress host, quint16 port) this->host = host; this->port = port; this->connectState = false; - this->hosts = new QList(); - //this->ports = new QMap(); - this->ports = new QList(); - // Set unique ID and add link to the list of links this->id = getNextLinkId(); this->name = tr("UDP Link (port:%1)").arg(14550); @@ -74,8 +71,6 @@ void UDPLink::run() void UDPLink::setAddress(QString address) { Q_UNUSED(address); - // FIXME TODO Implement address - //socket->setLocalAddress(QHostAddress(address)); } void UDPLink::setPort(int port) @@ -90,19 +85,60 @@ void UDPLink::setPort(int port) */ void UDPLink::addHost(const QString& host) { + qDebug() << "UDP:" << "ADDING HOST:" << host; if (host.contains(":")) { + qDebug() << "HOST: " << host.split(":").first(); + QHostInfo info = QHostInfo::fromName(host.split(":").first()); // Add host - hosts->append(QHostAddress(host.split(":").first())); + QList hostAddresses = info.addresses(); + QHostAddress address; + for (int i = 0; i < hostAddresses.size(); i++) + { + // Exclude loopback IPv4 and all IPv6 addresses + if (!hostAddresses.at(i).toString().contains(":")) + { + address = hostAddresses.at(i); + } + } + hosts.append(address); + qDebug() << "Address:" << address.toString(); // Set port according to user input - ports->append(host.split(":").last().toInt()); + ports.append(host.split(":").last().toInt()); } else { + QHostInfo info = QHostInfo::fromName(host); // Add host - hosts->append(QHostAddress(host)); + hosts.append(info.addresses().first()); // Set port according to default (this port) - ports->append(port); + ports.append(port); + } +} + +void UDPLink::removeHost(const QString& hostname) +{ + QString host = hostname; + if (host.contains(":")) host = host.split(":").first(); + host = host.trimmed(); + QHostInfo info = QHostInfo::fromName(host); + QHostAddress address; + QList hostAddresses = info.addresses(); + for (int i = 0; i < hostAddresses.size(); i++) + { + // Exclude loopback IPv4 and all IPv6 addresses + if (!hostAddresses.at(i).toString().contains(":")) + { + address = hostAddresses.at(i); + } + } + for (int i = 0; i < hosts.count(); ++i) + { + if (hosts.at(i) == address) + { + hosts.removeAt(i); + ports.removeAt(i); + } } } @@ -113,11 +149,13 @@ void UDPLink::writeBytes(const char* data, qint64 size) //QList::iterator h; // for (h = hosts->begin(); h != hosts->end(); ++h) - for (int h = 0; h < hosts->size(); h++) + + for (int h = 0; h < hosts.size(); h++) { - QHostAddress currentHost = hosts->at(h); - quint16 currentPort = ports->at(h); + QHostAddress currentHost = hosts.at(h); + quint16 currentPort = ports.at(h); + qDebug() << "WRITING TO" << currentHost.toIPv4Address() << currentPort; for (int i=0; icontains(sender)) + if (!hosts.contains(sender)) { - hosts->append(sender); - ports->append(senderPort); + hosts.append(sender); + ports.append(senderPort); // ports->insert(sender, senderPort); } else { - int index = hosts->indexOf(sender); - ports->replace(index, senderPort); + int index = hosts.indexOf(sender); + ports.replace(index, senderPort); } } diff --git a/src/comm/UDPLink.h b/src/comm/UDPLink.h index cf5bcd573..e3205c8d0 100644 --- a/src/comm/UDPLink.h +++ b/src/comm/UDPLink.h @@ -63,6 +63,7 @@ public: int getParityType(); int getDataBitsType(); int getStopBitsType(); + QList getHosts() { return hosts; } /* Extensive statistics for scientific purposes */ qint64 getNominalDataRate(); @@ -86,6 +87,8 @@ public slots: void setPort(int port); /** @brief Add a new host to broadcast messages to */ void addHost(const QString& host); + /** @brief Remove a host from broadcasting messages to */ + void removeHost(const QString& host); // void readPendingDatagrams(); void readBytes(); @@ -106,9 +109,8 @@ protected: int id; QUdpSocket* socket; bool connectState; - QList* hosts; - // QMap* ports; - QList* ports; + QList hosts; + QList ports; quint64 bitsSentTotal; quint64 bitsSentCurrent; diff --git a/src/ui/MAVLinkSettingsWidget.cc b/src/ui/MAVLinkSettingsWidget.cc index ae2fdceb5..5559f3878 100644 --- a/src/ui/MAVLinkSettingsWidget.cc +++ b/src/ui/MAVLinkSettingsWidget.cc @@ -33,7 +33,10 @@ This file is part of the QGROUNDCONTROL project #include #include "MAVLinkSettingsWidget.h" +#include "LinkManager.h" +#include "UDPLink.h" #include "ui_MAVLinkSettingsWidget.h" +#include MAVLinkSettingsWidget::MAVLinkSettingsWidget(MAVLinkProtocol* protocol, QWidget *parent) : QWidget(parent), @@ -58,6 +61,12 @@ MAVLinkSettingsWidget::MAVLinkSettingsWidget(MAVLinkProtocol* protocol, QWidget m_ui->actionGuardCheckBox->setChecked(protocol->actionGuardEnabled()); m_ui->actionRetransmissionSpinBox->setValue(protocol->getActionRetransmissionTimeout()); + // AUTH + m_ui->droneOSCheckBox->setChecked(protocol->getAuthEnabled()); + QSettings settings; + m_ui->droneOSComboBox->setCurrentIndex(m_ui->droneOSComboBox->findText(settings.value("DRONEOS_HOST", "droneos.com:14555").toString())); + m_ui->droneOSLineEdit->setText(protocol->getAuthKey()); + // Connect actions // Heartbeat connect(protocol, SIGNAL(heartbeatChanged(bool)), m_ui->heartbeatCheckBox, SLOT(setChecked(bool))); @@ -88,7 +97,16 @@ MAVLinkSettingsWidget::MAVLinkSettingsWidget(MAVLinkProtocol* protocol, QWidget connect(m_ui->actionGuardCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableActionGuard(bool))); connect(protocol, SIGNAL(actionRetransmissionTimeoutChanged(int)), m_ui->actionRetransmissionSpinBox, SLOT(setValue(int))); connect(m_ui->actionRetransmissionSpinBox, SIGNAL(valueChanged(int)), protocol, SLOT(setActionRetransmissionTimeout(int))); + // MAVLink AUTH + connect(protocol, SIGNAL(authChanged(bool)), m_ui->droneOSCheckBox, SLOT(setChecked(bool))); + connect(m_ui->droneOSCheckBox, SIGNAL(toggled(bool)), this, SLOT(enableDroneOS(bool))); + connect(protocol, SIGNAL(authKeyChanged(QString)), m_ui->droneOSLineEdit, SLOT(setText(QString))); + connect(m_ui->droneOSLineEdit, SIGNAL(textChanged(QString)), this, SLOT(setDroneOSKey(QString))); + // Drone OS + connect(m_ui->droneOSComboBox, SIGNAL(currentIndexChanged(QString)), this, SLOT(setDroneOSHost(QString))); + // FIXME Manually trigger this action here, this brings control code to UI = BAD! + setDroneOSHost(m_ui->droneOSComboBox->currentText()); // Update values m_ui->versionLabel->setText(tr("MAVLINK_VERSION: %1").arg(protocol->getVersion())); @@ -166,6 +184,59 @@ void MAVLinkSettingsWidget::chooseLogfileName() } } +void MAVLinkSettingsWidget::enableDroneOS(bool enable) +{ + // Get current selected host and port + QString hostString = m_ui->droneOSComboBox->currentText(); + QString host = hostString.split(":").first(); + + // Delete from all lists first + UDPLink* firstUdp = NULL; + QList links = LinkManager::instance()->getLinksForProtocol(protocol); + foreach (LinkInterface* link, links) + { + UDPLink* udp = dynamic_cast(link); + if (udp) + { + if (!firstUdp) firstUdp = udp; + // Remove current hosts + for (int i = 0; i < m_ui->droneOSComboBox->count(); ++i) + { + QString oldHostString = m_ui->droneOSComboBox->itemText(i); + oldHostString = hostString.split(":").first(); + udp->removeHost(oldHostString); + } + } + } + + // Re-add if enabled + if (enable) + { + if (firstUdp) + { + firstUdp->addHost(hostString); + } + // Set key + protocol->setAuthKey(m_ui->droneOSLineEdit->text().trimmed()); + QSettings settings; + settings.setValue("DRONEOS_HOST", m_ui->droneOSComboBox->currentText()); + settings.sync(); + } + protocol->enableAuth(enable); +} + +void MAVLinkSettingsWidget::setDroneOSKey(QString key) +{ + Q_UNUSED(key); + enableDroneOS(m_ui->droneOSCheckBox->isChecked()); +} + +void MAVLinkSettingsWidget::setDroneOSHost(QString host) +{ + Q_UNUSED(host); + enableDroneOS(m_ui->droneOSCheckBox->isChecked()); +} + MAVLinkSettingsWidget::~MAVLinkSettingsWidget() { delete m_ui; diff --git a/src/ui/MAVLinkSettingsWidget.h b/src/ui/MAVLinkSettingsWidget.h index c1d99eb72..0fb14502b 100644 --- a/src/ui/MAVLinkSettingsWidget.h +++ b/src/ui/MAVLinkSettingsWidget.h @@ -20,6 +20,12 @@ public slots: void updateLogfileName(const QString& fileName); /** @brief Start the file select dialog for the log file */ void chooseLogfileName(); + /** @brief Enable DroneOS forwarding */ + void enableDroneOS(bool enable); + + void setDroneOSKey(QString key); + + void setDroneOSHost(QString host); protected: MAVLinkProtocol* protocol; diff --git a/src/ui/MAVLinkSettingsWidget.ui b/src/ui/MAVLinkSettingsWidget.ui index c3e023b78..33ad6b9b7 100644 --- a/src/ui/MAVLinkSettingsWidget.ui +++ b/src/ui/MAVLinkSettingsWidget.ui @@ -7,7 +7,7 @@ 0 0 431 - 387 + 530 @@ -288,6 +288,66 @@ + + + + + 0 + 0 + + + + 0 + + + Qt::Horizontal + + + + + + + Forward MAVLink packets of all links to http://droneos.com + + + + + + + Qt::Horizontal + + + + 8 + 0 + + + + + + + + Enter your DroneOS API Token/Key + + + 32 + + + + + + + + droneos.com:14555 + + + + + localhost:14555 + + + + diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointView.cc index 00f5480c0..c4218bd1e 100644 --- a/src/ui/WaypointView.cc +++ b/src/ui/WaypointView.cc @@ -50,8 +50,9 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) : m_ui->comboBox_action->addItem(tr("Other"), MAV_CMD_ENUM_END); // add frames - m_ui->comboBox_frame->addItem("Global",MAV_FRAME_GLOBAL); - m_ui->comboBox_frame->addItem("Local",MAV_FRAME_LOCAL); + m_ui->comboBox_frame->addItem("Abs. Alt/Global",MAV_FRAME_GLOBAL); + m_ui->comboBox_frame->addItem("Rel. Alt/Global", MAV_FRAME_GLOBAL_RELATIVE_ALT); + m_ui->comboBox_frame->addItem("Local/Abs. Alt.",MAV_FRAME_LOCAL); m_ui->comboBox_frame->addItem("Mission",MAV_FRAME_MISSION); // Initialize view correctly @@ -308,6 +309,7 @@ void WaypointView::updateFrameView(int frame) switch(frame) { case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: m_ui->posNSpinBox->hide(); m_ui->posESpinBox->hide(); m_ui->posDSpinBox->hide(); @@ -388,7 +390,7 @@ void WaypointView::updateValues() } switch(frame) { - case(MAV_FRAME_LOCAL): + case MAV_FRAME_LOCAL: { if (m_ui->posNSpinBox->value() != wp->getX()) { @@ -404,7 +406,8 @@ void WaypointView::updateValues() } } break; - case(MAV_FRAME_GLOBAL): + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: { if (m_ui->latSpinBox->value() != wp->getX()) { -- 2.22.0