diff --git a/src/QGCConfig.h b/src/QGCConfig.h index 9de60c3bf8678fab133fce7f9c7922257e6bd10d..4c32051dbdcccbb4ee69967a88d54f1d653d3dff 100644 --- a/src/QGCConfig.h +++ b/src/QGCConfig.h @@ -6,8 +6,6 @@ /** @brief Polling interval in ms */ #define SERIAL_POLL_INTERVAL 4 -/** @brief Heartbeat emission rate, in Hertz (times per second) */ -#define MAVLINK_HEARTBEAT_DEFAULT_RATE 1 #define WITH_TEXT_TO_SPEECH 1 // If you need to make an incompatible changes to stored settings, bump this version number diff --git a/src/QmlControls/QGroundControlQmlGlobal.cc b/src/QmlControls/QGroundControlQmlGlobal.cc index b73ec2220f3414def698229f1e1a5e61f8542c61..f932e5c4417d439558a83c8e01e1993589bf0492 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.cc +++ b/src/QmlControls/QGroundControlQmlGlobal.cc @@ -178,12 +178,6 @@ void QGroundControlQmlGlobal::setIsSaveLogPromptNotArmed(bool prompt) emit isSaveLogPromptNotArmedChanged(prompt); } -void QGroundControlQmlGlobal::setIsHeartBeatEnabled(bool enable) -{ - qgcApp()->toolbox()->mavlinkProtocol()->enableHeartbeats(enable); - emit isHeartBeatEnabledChanged(enable); -} - void QGroundControlQmlGlobal::setIsMultiplexingEnabled(bool enable) { qgcApp()->toolbox()->mavlinkProtocol()->enableMultiplexing(enable); diff --git a/src/QmlControls/QGroundControlQmlGlobal.h b/src/QmlControls/QGroundControlQmlGlobal.h index 95e3d01448511553b7e5187668be4205ffeb1648..82c7e46274c8821a0f38d4d97ba61b0a787cb59d 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.h +++ b/src/QmlControls/QGroundControlQmlGlobal.h @@ -69,7 +69,6 @@ public: Q_PROPERTY(bool virtualTabletJoystick READ virtualTabletJoystick WRITE setVirtualTabletJoystick NOTIFY virtualTabletJoystickChanged) // MavLink Protocol - Q_PROPERTY(bool isHeartBeatEnabled READ isHeartBeatEnabled WRITE setIsHeartBeatEnabled NOTIFY isHeartBeatEnabledChanged) 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) @@ -110,7 +109,6 @@ public: bool isSaveLogPromptNotArmed () { return _app->promptFlightDataSaveNotArmed(); } bool virtualTabletJoystick () { return _virtualTabletJoystick; } - bool isHeartBeatEnabled () { return _toolbox->mavlinkProtocol()->heartbeatsEnabled(); } bool isMultiplexingEnabled () { return _toolbox->mavlinkProtocol()->multiplexingEnabled(); } bool isVersionCheckEnabled () { return _toolbox->mavlinkProtocol()->versionCheckEnabled(); } int mavlinkSystemID () { return _toolbox->mavlinkProtocol()->getSystemId(); } @@ -128,7 +126,6 @@ public: void setIsSaveLogPromptNotArmed (bool prompt); void setVirtualTabletJoystick (bool enabled); - void setIsHeartBeatEnabled (bool enable); void setIsMultiplexingEnabled (bool enable); void setIsVersionCheckEnabled (bool enable); void setMavlinkSystemID (int id); @@ -142,7 +139,6 @@ signals: void isSaveLogPromptChanged (bool prompt); void isSaveLogPromptNotArmedChanged (bool prompt); void virtualTabletJoystickChanged (bool enabled); - void isHeartBeatEnabledChanged (bool enabled); void isMultiplexingEnabledChanged (bool enabled); void isVersionCheckEnabledChanged (bool enabled); void mavlinkSystemIDChanged (int id); diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index a83db20f8903dd29a48de24ad446bc80edc0f06e..35bb23b977e2666750e21c6a9503c2fbfa2371da 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -32,6 +32,8 @@ QGC_LOGGING_CATEGORY(MultiVehicleManagerLog, "MultiVehicleManagerLog") +const char* MultiVehicleManager::_gcsHeartbeatEnabledKey = "gcsHeartbeatEnabled"; + MultiVehicleManager::MultiVehicleManager(QGCApplication* app) : QGCTool(app) , _activeVehicleAvailable(false) @@ -41,8 +43,18 @@ MultiVehicleManager::MultiVehicleManager(QGCApplication* app) , _autopilotPluginManager(NULL) , _joystickManager(NULL) , _mavlinkProtocol(NULL) + , _gcsHeartbeatEnabled(true) { + QSettings settings; + + _gcsHeartbeatEnabled = settings.value(_gcsHeartbeatEnabledKey, true).toBool(); + _gcsHeartbeatTimer.setInterval(_gcsHeartbeatRateMSecs); + _gcsHeartbeatTimer.setSingleShot(false); + connect(&_gcsHeartbeatTimer, &QTimer::timeout, this, &MultiVehicleManager::_sendGCSHeartbeat); + if (_gcsHeartbeatEnabled) { + _gcsHeartbeatTimer.start(); + } } void MultiVehicleManager::setToolbox(QGCToolbox *toolbox) @@ -238,3 +250,38 @@ Vehicle* MultiVehicleManager::getVehicleById(int vehicleId) return NULL; } + +void MultiVehicleManager::setGcsHeartbeatEnabled(bool gcsHeartBeatEnabled) +{ + if (gcsHeartBeatEnabled != _gcsHeartbeatEnabled) { + _gcsHeartbeatEnabled = gcsHeartBeatEnabled; + emit gcsHeartBeatEnabledChanged(gcsHeartBeatEnabled); + + QSettings settings; + settings.setValue(_gcsHeartbeatEnabledKey, gcsHeartBeatEnabled); + + if (gcsHeartBeatEnabled) { + _gcsHeartbeatTimer.start(); + } else { + _gcsHeartbeatTimer.stop(); + } + } +} + +void MultiVehicleManager::_sendGCSHeartbeat(void) +{ + for (int i=0; i< _vehicles.count(); i++) { + Vehicle* vehicle = qobject_cast(_vehicles[i]); + + mavlink_message_t message; + mavlink_msg_heartbeat_pack(_mavlinkProtocol->getSystemId(), + _mavlinkProtocol->getComponentId(), + &message, + MAV_TYPE_GCS, // MAV_TYPE + MAV_AUTOPILOT_INVALID, // MAV_AUTOPILOT + MAV_MODE_MANUAL_ARMED, // MAV_MODE + 0, // custom mode + MAV_STATE_ACTIVE); // MAV_STATE + vehicle->sendMessage(message); + } +} diff --git a/src/Vehicle/MultiVehicleManager.h b/src/Vehicle/MultiVehicleManager.h index f5101df9daa70830057301412e439beb4bbfe243..6942ce5a19b33555aa5332d073c72f08c5f9c75a 100644 --- a/src/Vehicle/MultiVehicleManager.h +++ b/src/Vehicle/MultiVehicleManager.h @@ -51,10 +51,11 @@ public: Q_INVOKABLE void saveSetting (const QString &key, const QString& value); Q_INVOKABLE QString loadSetting (const QString &key, const QString& defaultValue); - Q_PROPERTY(bool activeVehicleAvailable READ activeVehicleAvailable NOTIFY activeVehicleAvailableChanged) - Q_PROPERTY(bool parameterReadyVehicleAvailable READ parameterReadyVehicleAvailable NOTIFY parameterReadyVehicleAvailableChanged) - Q_PROPERTY(Vehicle* activeVehicle READ activeVehicle WRITE setActiveVehicle NOTIFY activeVehicleChanged) - Q_PROPERTY(QmlObjectListModel* vehicles READ vehicles CONSTANT) + Q_PROPERTY(bool activeVehicleAvailable READ activeVehicleAvailable NOTIFY activeVehicleAvailableChanged) + Q_PROPERTY(bool parameterReadyVehicleAvailable READ parameterReadyVehicleAvailable NOTIFY parameterReadyVehicleAvailableChanged) + Q_PROPERTY(Vehicle* activeVehicle READ activeVehicle WRITE setActiveVehicle NOTIFY activeVehicleChanged) + Q_PROPERTY(QmlObjectListModel* vehicles READ vehicles CONSTANT) + Q_PROPERTY(bool gcsHeartBeatEnabled READ gcsHeartbeatEnabled WRITE setGcsHeartbeatEnabled NOTIFY gcsHeartBeatEnabledChanged) // Methods @@ -73,6 +74,9 @@ public: QmlObjectListModel* vehicles(void) { return &_vehicles; } + bool gcsHeartbeatEnabled(void) const { return _gcsHeartbeatEnabled; } + void setGcsHeartbeatEnabled(bool gcsHeartBeatEnabled); + // Override from QGCTool virtual void setToolbox(QGCToolbox *toolbox); @@ -82,6 +86,7 @@ signals: void activeVehicleAvailableChanged(bool activeVehicleAvailable); void parameterReadyVehicleAvailableChanged(bool parameterReadyVehicleAvailable); void activeVehicleChanged(Vehicle* activeVehicle); + void gcsHeartBeatEnabledChanged(bool gcsHeartBeatEnabled); void _deleteVehiclePhase2Signal(void); @@ -91,6 +96,7 @@ private slots: void _setActiveVehiclePhase2(void); void _autopilotParametersReadyChanged(bool parametersReady); void _linkActive(LinkInterface* link, int vehicleId, int vehicleFirmwareType, int vehicleType); + void _sendGCSHeartbeat(void); private: bool _vehicleExists(int vehicleId); @@ -111,6 +117,10 @@ private: JoystickManager* _joystickManager; MAVLinkProtocol* _mavlinkProtocol; + QTimer _gcsHeartbeatTimer; ///< Timer to emit heartbeats + bool _gcsHeartbeatEnabled; ///< Enabled/disable heartbeat emission + static const int _gcsHeartbeatRateMSecs = 1000; ///< Heartbeat rate + static const char* _gcsHeartbeatEnabledKey; }; #endif diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 18f8602013dcb63e05ee44bd4aa6590870f44841..453ab1b43e9ca96d70f79dd48a69d5e8b756e69a 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -60,8 +60,6 @@ MAVLinkProtocol::MAVLinkProtocol(QGCApplication* app) , _logPromptForSave(false) , _tempLogFile(QString("%2.%3").arg(_tempLogFileTemplate).arg(_logFileExtension)) #endif - , _heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE) - , _heartbeatsEnabled(true) , _linkMgr(NULL) , _multiVehicleManager(NULL) { @@ -101,10 +99,6 @@ void MAVLinkProtocol::setToolbox(QGCToolbox *toolbox) } } - // Start heartbeat timer, emitting a heartbeat at the configured rate - connect(&_heartbeatTimer, &QTimer::timeout, this, &MAVLinkProtocol::sendHeartbeat); - _heartbeatTimer.start(1000/_heartbeatRate); - connect(this, &MAVLinkProtocol::protocolStatusMessage, _app, &QGCApplication::criticalMessageBoxOnMainThread); #ifndef __mobile__ connect(this, &MAVLinkProtocol::saveTempFlightDataLog, _app, &QGCApplication::saveTempFlightDataLogOnMainThread); @@ -120,7 +114,6 @@ void MAVLinkProtocol::loadSettings() // Load defaults from settings QSettings settings; settings.beginGroup("QGC_MAVLINK_PROTOCOL"); - enableHeartbeats(settings.value("HEARTBEATS_ENABLED", _heartbeatsEnabled).toBool()); enableVersionCheck(settings.value("VERSION_CHECK_ENABLED", m_enable_version_check).toBool()); enableMultiplexing(settings.value("MULTIPLEXING_ENABLED", m_multiplexingEnabled).toBool()); @@ -150,7 +143,6 @@ void MAVLinkProtocol::storeSettings() // Store settings QSettings settings; settings.beginGroup("QGC_MAVLINK_PROTOCOL"); - settings.setValue("HEARTBEATS_ENABLED", _heartbeatsEnabled); settings.setValue("VERSION_CHECK_ENABLED", m_enable_version_check); settings.setValue("MULTIPLEXING_ENABLED", m_multiplexingEnabled); settings.setValue("GCS_SYSTEM_ID", systemId); @@ -473,37 +465,6 @@ void MAVLinkProtocol::_sendMessage(LinkInterface* link, mavlink_message_t messag } } -/** - * The heartbeat is sent out of order and does not reset the - * periodic heartbeat emission. It will be just sent in addition. - * @return mavlink_message_t heartbeat message sent on serial link - */ -void MAVLinkProtocol::sendHeartbeat() -{ - if (_heartbeatsEnabled) - { - mavlink_message_t beat; - mavlink_msg_heartbeat_pack(getSystemId(), getComponentId(),&beat, MAV_TYPE_GCS, MAV_AUTOPILOT_INVALID, MAV_MODE_MANUAL_ARMED, 0, MAV_STATE_ACTIVE); - _sendMessage(beat); - } - if (m_authEnabled) - { - mavlink_message_t msg; - mavlink_auth_key_t auth; - memset(&auth, 0, sizeof(auth)); - memcpy(auth.key, m_authKey.toStdString().c_str(), qMin(m_authKey.length(), MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN)); - mavlink_msg_auth_key_encode(getSystemId(), getComponentId(), &msg, &auth); - _sendMessage(msg); - } -} - -/** @param enabled true to enable heartbeats emission at _heartbeatRate, false to disable */ -void MAVLinkProtocol::enableHeartbeats(bool enabled) -{ - _heartbeatsEnabled = enabled; - emit heartbeatChanged(enabled); -} - void MAVLinkProtocol::enableMultiplexing(bool enabled) { bool changed = false; @@ -569,23 +530,6 @@ void MAVLinkProtocol::enableVersionCheck(bool enabled) emit versionCheckChanged(enabled); } -/** - * The default rate is 1 Hertz. - * - * @param rate heartbeat rate in hertz (times per second) - */ -void MAVLinkProtocol::setHeartbeatRate(int rate) -{ - _heartbeatRate = rate; - _heartbeatTimer.setInterval(1000/_heartbeatRate); -} - -/** @return heartbeat rate in Hertz */ -int MAVLinkProtocol::getHeartbeatRate() -{ - return _heartbeatRate; -} - void MAVLinkProtocol::_vehicleCountChanged(int count) { #ifndef __mobile__ diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index c3de7bc1c4b7d235c629e4a65187c1f064ed6b33..a0c092b9ec0e88737f8cf0abdeb46b92dee756fd 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -72,12 +72,6 @@ public: int getSystemId(); /** @brief Get the component id of this application */ int getComponentId(); - /** @brief The auto heartbeat emission rate in Hertz */ - int getHeartbeatRate(); - /** @brief Get heartbeat state */ - bool heartbeatsEnabled() const { - return _heartbeatsEnabled; - } /** @brief Get protocol version check state */ bool versionCheckEnabled() const { @@ -155,14 +149,9 @@ public slots: /** @brief Receive bytes from a communication interface */ void receiveBytes(LinkInterface* link, QByteArray b); - /** @brief Set the rate at which heartbeats are emitted */ - void setHeartbeatRate(int rate); /** @brief Set the system id of this application */ void setSystemId(int id); - /** @brief Enable / disable the heartbeat emission */ - void enableHeartbeats(bool enabled); - /** @brief Enabled/disable packet multiplexing */ void enableMultiplexing(bool enabled); @@ -192,9 +181,6 @@ public slots: m_authKey = key; } - /** @brief Send an extra heartbeat to all connected units */ - void sendHeartbeat(); - /** @brief Load protocol settings */ void loadSettings(); /** @brief Store protocol settings */ @@ -234,8 +220,6 @@ signals: /** @brief Message received and directly copied via signal */ void messageReceived(LinkInterface* link, mavlink_message_t message); - /** @brief Emitted if heartbeat emission mode is changed */ - void heartbeatChanged(bool heartbeats); /** @brief Emitted if multiplexing is started / stopped */ void multiplexingChanged(bool enabled); /** @brief Emitted if authentication support is enabled / disabled */ @@ -300,10 +284,6 @@ private: static const char* _logFileExtension; ///< Extension for log files #endif - QTimer _heartbeatTimer; ///< Timer to emit heartbeats - int _heartbeatRate; ///< Heartbeat rate, controls the timer interval - bool _heartbeatsEnabled; ///< Enabled/disable heartbeat emission - LinkManager* _linkMgr; MultiVehicleManager* _multiVehicleManager; }; diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 073383094650912cec78c861f72608c9fca88eca..10f7eff579352bb58c32d6ae7894e502e0d9c5c5 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -399,10 +399,7 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes) void MockLink::_handleHeartBeat(const mavlink_message_t& msg) { Q_UNUSED(msg); -#if 0 - mavlink_heartbeat_t heartbeat; - mavlink_msg_heartbeat_decode(&msg, &heartbeat); -#endif + qCDebug(MockLinkLog) << "Heartbeat"; } void MockLink::_handleSetMode(const mavlink_message_t& msg) diff --git a/src/ui/MAVLinkSettingsWidget.cc b/src/ui/MAVLinkSettingsWidget.cc index 08f612717e263c23520416e346cabb97e0bd3731..9f7833ed60a634853001655bbd62b6b325e1c110 100644 --- a/src/ui/MAVLinkSettingsWidget.cc +++ b/src/ui/MAVLinkSettingsWidget.cc @@ -53,7 +53,6 @@ MAVLinkSettingsWidget::MAVLinkSettingsWidget(MAVLinkProtocol* protocol, QWidget m_ui->droneOSLineEdit->setText(protocol->getAuthKey()); // Initialize state - m_ui->heartbeatCheckBox->setChecked(protocol->heartbeatsEnabled()); m_ui->versionCheckBox->setChecked(protocol->versionCheckEnabled()); m_ui->multiplexingCheckBox->setChecked(protocol->multiplexingEnabled()); m_ui->systemIdSpinBox->setValue(protocol->getSystemId()); @@ -65,11 +64,6 @@ MAVLinkSettingsWidget::MAVLinkSettingsWidget(MAVLinkProtocol* protocol, QWidget m_ui->actionGuardCheckBox->setChecked(protocol->actionGuardEnabled()); m_ui->actionRetransmissionSpinBox->setValue(protocol->getActionRetransmissionTimeout()); - // Connect actions - // Heartbeat - connect(protocol, &MAVLinkProtocol::heartbeatChanged, m_ui->heartbeatCheckBox, &QCheckBox::setChecked); - connect(m_ui->heartbeatCheckBox, &QCheckBox::toggled, protocol, &MAVLinkProtocol::enableHeartbeats); - // Version check connect(protocol, &MAVLinkProtocol::versionCheckChanged, m_ui->versionCheckBox, &QCheckBox::setChecked); connect(m_ui->versionCheckBox, &QCheckBox::toggled, protocol, &MAVLinkProtocol::enableVersionCheck); diff --git a/src/ui/MAVLinkSettingsWidget.ui b/src/ui/MAVLinkSettingsWidget.ui index deb8e4f835ab844c20e5e04bba9c7d229bc6e67c..bec7b1ea136ac30da06aa56f5f3067333e997238 100644 --- a/src/ui/MAVLinkSettingsWidget.ui +++ b/src/ui/MAVLinkSettingsWidget.ui @@ -14,21 +14,57 @@ Form - - - - Qt::Horizontal + + + + Enable retransmission of actions / commands - - - 8 - 0 - + + + + + + Forward MAVLink packets of all links to the host below - + - - + + + + Only accept MAVs with same protocol version + + + + + + + Read request retransmission timeout + + + + + + + Enable retransmission of parameter read/write requests + + + + + + + Enable Multiplexing: Forward packets to all other links + + + + + + + Enter a comma-separated list of allowed packets + + + + + Qt::Horizontal @@ -40,13 +76,13 @@ - - + + - Time in milliseconds after which a not acknowledged write request is sent again. + Time in milliseconds after which a not acknowledged read request is sent again. - Time in milliseconds after which a not acknowledged write request is sent again. + Time in milliseconds after which a not acknowledged read request is sent again. ms @@ -60,32 +96,22 @@ 50 - - - - - - Enter a comma-separated list of allowed packets + + 50 - - - - ms - - - 20 - - - 1000 + + + + Enter your authentication token - - 10 + + 32 - + Qt::Horizontal @@ -99,12 +125,12 @@ - + - Time in milliseconds after which a not acknowledged read request is sent again. + Time in milliseconds after which a not acknowledged write request is sent again. - Time in milliseconds after which a not acknowledged read request is sent again. + Time in milliseconds after which a not acknowledged write request is sent again. ms @@ -118,12 +144,65 @@ 50 - - 50 + + + + + + Action request retransmission timeout - + + + + The system ID is the number the MAV associates with this computer + + + The system ID is the number the MAV associates with this computer + + + Groundstation MAVLink System ID + + + + + + + MAVLINK_VERSION: + + + + + + + Set the groundstation number + + + Set the groundstation number + + + 1 + + + 255 + + + + + + + Qt::Horizontal + + + + 8 + 0 + + + + + @@ -139,7 +218,7 @@ - + true @@ -156,56 +235,15 @@ - - - - Qt::Horizontal - - - - 8 - 0 - - - - - - - - Write request retransmission timeout - - - - - - - Read request retransmission timeout - - - - - - - Emit heartbeat - - - - - - Enable Multiplexing: Forward packets to all other links - - - - - + - Action request retransmission timeout + Filter multiplexed packets: Only forward selected IDs - - + + Qt::Horizontal @@ -217,51 +255,7 @@ - - - - Set the groundstation number - - - Set the groundstation number - - - 1 - - - 255 - - - - - - - Forward MAVLink packets of all links to the host below - - - - - - - Enable retransmission of actions / commands - - - - - - - Filter multiplexed packets: Only forward selected IDs - - - - - - - Only accept MAVs with same protocol version - - - - + Qt::Horizontal @@ -277,42 +271,41 @@ - - + + - Enter your authentication token - - - 32 + Write request retransmission timeout - - - - Enable retransmission of parameter read/write requests + + + + ms - - - - - - MAVLINK_VERSION: + + 20 + + + 1000 + + + 10 - - - - The system ID is the number the MAV associates with this computer - - - The system ID is the number the MAV associates with this computer + + + + Qt::Horizontal - - Groundstation MAVLink System ID + + + 8 + 0 + - + diff --git a/src/ui/preferences/MavlinkSettings.qml b/src/ui/preferences/MavlinkSettings.qml index 6852f4eb819e0f92a4fa26ecc50c7891cff830e2..ea2541f27612b718766ba377cfc7f24e4555ed19 100644 --- a/src/ui/preferences/MavlinkSettings.qml +++ b/src/ui/preferences/MavlinkSettings.qml @@ -89,9 +89,9 @@ Rectangle { //-- Mavlink Heartbeats QGCCheckBox { text: "Emit heartbeat" - checked: QGroundControl.isHeartBeatEnabled + checked: multiVehicleManager.gcsHeartBeatEnabled onClicked: { - QGroundControl.isHeartBeatEnabled = checked + multiVehicleManager.gcsHeartBeatEnabled = checked } } //-----------------------------------------------------------------