Merge pull request #2634 from DonLakeFlyer/GCSHeartbeat

Move GCS heartbeat to MultiVehicleManager

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

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);

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);

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<Vehicle*>(_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);
+    }
+}

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

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__

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;
 };

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)

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);

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
                 }
             }
             //-----------------------------------------------------------------