Move connection lost handling to Vehicle

src/Vehicle/Vehicle.cc

@@ -34,6 +34,7 @@
 #include "ParameterLoader.h"
 #include "QGCApplication.h"
 #include "QGCImageProvider.h"
+#include "GAudioOutput.h"
 
 QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog")
 
@@ -88,7 +89,6 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _batteryVoltage(-1.0f)
     , _batteryPercent(0.0)
     , _batteryConsumed(-1.0)
-    , _currentHeartbeatTimeout(0)
     , _satelliteCount(-1)
     , _satRawHDOP(1e10f)
     , _satRawVDOP(1e10f)
@@ -97,6 +97,8 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _updateCount(0)
     , _rcRSSI(0)
     , _rcRSSIstore(100.0)
+    , _connectionLost(false)
+    , _connectionLostEnabled(true)
     , _missionManager(NULL)
     , _missionManagerInitialRequestComplete(false)
     , _parameterLoader(NULL)
@@ -138,7 +140,12 @@ Vehicle::Vehicle(LinkInterface*             link,
     connect(_refreshTimer, &QTimer::timeout, this, &Vehicle::_checkUpdate);
     _refreshTimer->setInterval(UPDATE_TIMER);
     _refreshTimer->start(UPDATE_TIMER);
-    emit heartbeatTimeoutChanged();
+
+    // Connection Lost time
+    _connectionLostTimer.setInterval(Vehicle::_connectionLostTimeoutMSecs);
+    _connectionLostTimer.setSingleShot(false);
+    _connectionLostTimer.start();
+    connect(&_connectionLostTimer, &QTimer::timeout, this, &Vehicle::_connectionLostTimeout);
 
     _mav = uas();
     // Reset satellite data (no GPS)
@@ -151,9 +158,6 @@ Vehicle::Vehicle(LinkInterface*             link,
     emit satRawCOGChanged();
     emit satelliteCountChanged();
 
-    // Reset connection lost (if any)
-    _currentHeartbeatTimeout = 0;
-    emit heartbeatTimeoutChanged();
     // Listen for system messages
     connect(qgcApp()->toolbox()->uasMessageHandler(), &UASMessageHandler::textMessageCountChanged,  this, &Vehicle::_handleTextMessage);
     connect(qgcApp()->toolbox()->uasMessageHandler(), &UASMessageHandler::textMessageReceived,      this, &Vehicle::_handletextMessageReceived);
@@ -166,7 +170,6 @@ Vehicle::Vehicle(LinkInterface*             link,
     connect(_mav, &UASInterface::altitudeChanged, this, &Vehicle::_updateAltitude);
     connect(_mav, &UASInterface::navigationControllerErrorsChanged,this, &Vehicle::_updateNavigationControllerErrors);
     connect(_mav, &UASInterface::NavigationControllerDataChanged,   this, &Vehicle::_updateNavigationControllerData);
-    connect(_mav, &UASInterface::heartbeatTimeout,                  this, &Vehicle::_heartbeatTimeout);
     connect(_mav, &UASInterface::batteryChanged,                    this, &Vehicle::_updateBatteryRemaining);
     connect(_mav, &UASInterface::batteryConsumedChanged,            this, &Vehicle::_updateBatteryConsumedChanged);
     connect(_mav, &UASInterface::localizationChanged,               this, &Vehicle::_setSatLoc);
@@ -291,6 +294,8 @@ void Vehicle::_handleHomePosition(mavlink_message_t& message)
 
 void Vehicle::_handleHeartbeat(mavlink_message_t& message)
 {
+    _connectionActive();
+
     mavlink_heartbeat_t heartbeat;
 
     mavlink_msg_heartbeat_decode(&message, &heartbeat);
@@ -786,19 +791,6 @@ void Vehicle::_updateState(UASInterface*, QString name, QString)
     }
 }
 
-void Vehicle::_heartbeatTimeout(bool timeout, unsigned int ms)
-{
-    unsigned int elapsed = ms;
-    if (!timeout)
-    {
-        elapsed = 0;
-    }
-    if(elapsed != _currentHeartbeatTimeout) {
-        _currentHeartbeatTimeout = elapsed;
-        emit heartbeatTimeoutChanged();
-    }
-}
-
 void Vehicle::_setSatelliteCount(double val, QString)
 {
     // I'm assuming that a negative value or over 99 means there is no GPS
@@ -1263,3 +1255,39 @@ void Vehicle::virtualTabletJoystickValue(double roll, double pitch, double yaw,
         _uas->setExternalControlSetpoint(roll, pitch, yaw, thrust, 0, JoystickModeRC);
     }
 }
+
+void Vehicle::setConnectionLostEnabled(bool connectionLostEnabled)
+{
+    if (_connectionLostEnabled != connectionLostEnabled) {
+        _connectionLostEnabled = connectionLostEnabled;
+        emit connectionLostEnabledChanged(_connectionLostEnabled);
+    }
+}
+
+void Vehicle::_connectionLostTimeout(void)
+{
+    if (_connectionLostEnabled && !_connectionLost) {
+        _connectionLost = true;
+        emit connectionLostChanged(true);
+
+        _say(QString("connection lost to vehicle %1").arg(id()), GAudioOutput::AUDIO_SEVERITY_NOTICE);
+    }
+}
+
+void Vehicle::_connectionActive(void)
+{
+    _connectionLostTimer.start();
+
+    if (_connectionLost) {
+        _connectionLost = false;
+        emit connectionLostChanged(false);
+
+        _say(QString("connection regained to vehicle %1").arg(id()), GAudioOutput::AUDIO_SEVERITY_NOTICE);
+    }
+}
+
+void Vehicle::_say(const QString& text, int severity)
+{
+    if (!qgcApp()->runningUnitTests())
+        qgcApp()->toolbox()->audioOutput()->say(text.toLower(), severity);
+}

src/Vehicle/Vehicle.h

@@ -98,7 +98,6 @@ public:
     Q_PROPERTY(double               satRawCOG               READ satRawCOG                              NOTIFY satRawCOGChanged)
     Q_PROPERTY(QString              currentState            READ currentState                           NOTIFY currentStateChanged)
     Q_PROPERTY(int                  satelliteLock           READ satelliteLock                          NOTIFY satelliteLockChanged)
-    Q_PROPERTY(unsigned int         heartbeatTimeout        READ heartbeatTimeout                       NOTIFY heartbeatTimeoutChanged)
     Q_PROPERTY(QmlObjectListModel*  missionItems            READ missionItemsModel                      CONSTANT)
     Q_PROPERTY(bool                 messageTypeNone         READ messageTypeNone                        NOTIFY messageTypeChanged)
     Q_PROPERTY(bool                 messageTypeNormal       READ messageTypeNormal                      NOTIFY messageTypeChanged)
@@ -118,6 +117,8 @@ public:
     Q_PROPERTY(bool                 px4Firmware             READ px4Firmware                            CONSTANT)
     Q_PROPERTY(bool                 apmFirmware             READ apmFirmware                            CONSTANT)
     Q_PROPERTY(bool                 genericFirmware         READ genericFirmware                        CONSTANT)
+    Q_PROPERTY(bool                 connectionLost          READ connectionLost                         NOTIFY connectionLostChanged)
+    Q_PROPERTY(bool                 connectionLostEnabled   READ connectionLostEnabled  WRITE setConnectionLostEnabled NOTIFY connectionLostEnabledChanged)
 
     /// Returns the number of buttons which are reserved for firmware use in the MANUAL_CONTROL mavlink
     /// message. For example PX4 Flight Stack reserves the first 8 buttons to simulate rc switches.
@@ -268,11 +269,14 @@ public:
     double          batteryConsumed     () { return _batteryConsumed; }
     QString         currentState        () { return _currentState; }
     int             satelliteLock       () { return _satelliteLock; }
-    unsigned int    heartbeatTimeout    () { return _currentHeartbeatTimeout; }
     int             rcRSSI              () { return _rcRSSI; }
     bool            px4Firmware         () { return _firmwareType == MAV_AUTOPILOT_PX4; }
     bool            apmFirmware         () { return _firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA; }
     bool            genericFirmware     () { return !px4Firmware() && !apmFirmware(); }
+    bool            connectionLost      () const { return _connectionLost; }
+    bool            connectionLostEnabled() const { return _connectionLostEnabled; }
+
+    void setConnectionLostEnabled(bool connectionLostEnabled);
 
     ParameterLoader* getParameterLoader(void);
 
@@ -296,6 +300,8 @@ signals:
     void flightModeChanged(const QString& flightMode);
     void hilModeChanged(bool hilMode);
     void missingParametersChanged(bool missingParameters);
+    void connectionLostChanged(bool connectionLost);
+    void connectionLostEnabledChanged(bool connectionLostEnabled);
 
     /// Used internally to move sendMessage call to main thread
     void _sendMessageOnThread(mavlink_message_t message);
@@ -320,7 +326,6 @@ signals:
     void batteryVoltageChanged  ();
     void batteryPercentChanged  ();
     void batteryConsumedChanged ();
-    void heartbeatTimeoutChanged();
     void currentConfigChanged   ();
     void satelliteCountChanged  ();
     void satRawHDOPChanged      ();
@@ -368,7 +373,6 @@ private slots:
     void _updateBatteryRemaining            (UASInterface*, double voltage, double, double percent, int);
     void _updateBatteryConsumedChanged      (UASInterface*, double current_consumed);
     void _updateState                       (UASInterface* system, QString name, QString description);
-    void _heartbeatTimeout                  (bool timeout, unsigned int ms);
     void _setSatelliteCount                 (double val, QString name);
     void _setSatRawHDOP                     (double val);
     void _setSatRawVDOP                     (double val);
@@ -376,6 +380,7 @@ private slots:
     void _setSatLoc                         (UASInterface* uas, int fix);
     /** @brief A new camera image has arrived */
     void _imageReady                        (UASInterface* uas);
+    void _connectionLostTimeout(void);
 
 private:
     bool _containsLink(LinkInterface* link);
@@ -390,6 +395,8 @@ private:
     void _missionManagerError(int errorCode, const QString& errorMsg);
     void _mapTrajectoryStart(void);
     void _mapTrajectoryStop(void);
+    void _connectionActive(void);
+    void _say(const QString& text, int severity);
 
     void    _addChange                      (int id);
     float   _oneDecimal                     (float value);
@@ -444,7 +451,6 @@ private:
     double          _batteryPercent;
     double          _batteryConsumed;
     QString         _currentState;
-    unsigned int    _currentHeartbeatTimeout;
     int             _satelliteCount;
     double          _satRawHDOP;
     double          _satRawVDOP;
@@ -455,6 +461,12 @@ private:
     int             _rcRSSI;
     double          _rcRSSIstore;
 
+    // Lost connection handling
+    bool                _connectionLost;
+    bool                _connectionLostEnabled;
+    static const int    _connectionLostTimeoutMSecs = 3500;  // Signal connection lost after 3.5 seconds of missed heartbeat
+    QTimer              _connectionLostTimer;
+
     MissionManager*     _missionManager;
     bool                _missionManagerInitialRequestComplete;
 

src/uas/UAS.cc

@@ -86,7 +86,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi
     manualYawAngle(0),
     manualThrust(0),
 
-    positionLock(false),
     isGlobalPositionKnown(false),
 
     latitude(0.0),
@@ -189,8 +188,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi
 #endif
 
     color = UASInterface::getNextColor();
-    connect(&statusTimeout, &QTimer::timeout, this, &UAS::updateState);
-    statusTimeout.start(500);
 }
 
 /**
@@ -201,46 +198,6 @@ int UAS::getUASID() const
     return uasId;
 }
 
-/**
-* Update the heartbeat.
-*/
-void UAS::updateState()
-{
-    // Check if heartbeat timed out
-    quint64 heartbeatInterval = QGC::groundTimeUsecs() - lastHeartbeat;
-    if (!connectionLost && (heartbeatInterval > timeoutIntervalHeartbeat))
-    {
-        connectionLost = true;
-        receivedMode = false;
-        QString audiostring = QString("Link lost to system %1").arg(this->getUASID());
-        _say(audiostring.toLower(), GAudioOutput::AUDIO_SEVERITY_ALERT);
-    }
-
-    // Update connection loss time on each iteration
-    if (connectionLost && (heartbeatInterval > timeoutIntervalHeartbeat))
-    {
-        connectionLossTime = heartbeatInterval;
-        emit heartbeatTimeout(true, heartbeatInterval/1000);
-    }
-
-    // Connection gained
-    if (connectionLost && (heartbeatInterval < timeoutIntervalHeartbeat))
-    {
-        QString audiostring = QString("Link regained to system %1").arg(this->getUASID());
-        _say(audiostring.toLower(), GAudioOutput::AUDIO_SEVERITY_NOTICE);
-        connectionLost = false;
-        connectionLossTime = 0;
-        emit heartbeatTimeout(false, 0);
-    }
-
-    // Position lock is set by the MAVLink message handler
-    // if no position lock is available, indicate an error
-    if (positionLock)
-    {
-        positionLock = false;
-    }
-}
-
 void UAS::receiveMessage(mavlink_message_t message)
 {
     if (!components.contains(message.compid))
@@ -325,8 +282,6 @@ void UAS::receiveMessage(mavlink_message_t message)
             {
                 break;
             }
-            lastHeartbeat = QGC::groundTimeUsecs();
-            emit heartbeat(this);
             mavlink_heartbeat_t state;
             mavlink_msg_heartbeat_decode(&message, &state);
 
@@ -637,8 +592,6 @@ void UAS::receiveMessage(mavlink_message_t message)
 
                 // Emit
                 emit velocityChanged_NED(this, speedX, speedY, speedZ, time);
-
-                positionLock = true;
             }
         }
             break;
@@ -678,7 +631,6 @@ void UAS::receiveMessage(mavlink_message_t message)
             setGroundSpeed(qSqrt(speedX*speedX+speedY*speedY));
             emit speedChanged(this, groundSpeed, airSpeed, time);
 
-            positionLock = true;
             isGlobalPositionKnown = true;
         }
             break;
@@ -699,7 +651,6 @@ void UAS::receiveMessage(mavlink_message_t message)
 
             if (pos.fix_type > 2)
             {
-                positionLock = true;
                 isGlobalPositionKnown = true;
 
                 latitude_gps  = pos.lat/(double)1E7;

src/uas/UAS.h

@@ -384,8 +384,6 @@ protected: //COMMENTS FOR TEST UNIT
     MAVLinkProtocol* mavlink;     ///< Reference to the MAVLink instance
     float receiveDropRate;        ///< Percentage of packets that were dropped on the MAV's receiving link (from GCS and other MAVs)
     float sendDropRate;           ///< Percentage of packets that were not received from the MAV by the GCS
-    quint64 lastHeartbeat;        ///< Time of the last heartbeat message
-    QTimer statusTimeout;       ///< Timer for various status timeouts
 
     /// BASIC UAS TYPE, NAME AND STATE
     uint8_t base_mode;                 ///< The current mode of the MAV
@@ -423,7 +421,6 @@ protected: //COMMENTS FOR TEST UNIT
     double manualThrust;        ///< Thrust set by human pilot (radians)
 
     /// POSITION
-    bool positionLock;          ///< Status if position information is available or not
     bool isGlobalPositionKnown; ///< If the global position has been received for this MAV
 
     double localX;
@@ -591,9 +588,6 @@ public slots:
     /** @brief Receive a message from one of the communication links. */
     virtual void receiveMessage(mavlink_message_t message);
 
-    /** @brief Update the system state */
-    void updateState();
-
     void startCalibration(StartCalibrationType calType);
     void stopCalibration(void);
 
@@ -607,8 +601,6 @@ public slots:
     void unsetRCToParameterMap();
 signals:
     void loadChanged(UASInterface* uas, double load);
-    /** @brief Propagate a heartbeat received from the system */
-    //void heartbeat(UASInterface* uas); // Defined in UASInterface already
     void imageStarted(quint64 timestamp);
     /** @brief A new camera image has arrived */
     void imageReady(UASInterface* uas);

src/uas/UASInterface.h

@@ -256,7 +256,6 @@ signals:
     void batteryConsumedChanged(UASInterface* uas, double current_consumed);
     void statusChanged(UASInterface* uas, QString status);
     void thrustChanged(UASInterface*, double thrust);
-    void heartbeat(UASInterface* uas);
     void attitudeChanged(UASInterface*, double roll, double pitch, double yaw, quint64 usec);
     void attitudeChanged(UASInterface*, int component, double roll, double pitch, double yaw, quint64 usec);
     void attitudeRotationRatesChanged(int uas, double rollrate, double pitchrate, double yawrate, quint64 usec);
@@ -313,8 +312,6 @@ signals:
     void localizationChanged(UASInterface* uas, int fix);
 
     // ERROR AND STATUS SIGNALS
-    /** @brief Heartbeat timed out or was regained */
-    void heartbeatTimeout(bool timeout, unsigned int ms);
     /** @brief Name of system changed */
     void nameChanged(QString newName);
     /** @brief Core specifications have changed */
@@ -329,12 +326,6 @@ signals:
 
     /** @brief Command Ack */
     void commandAck (UASInterface* uas, uint8_t compID, uint16_t command, uint8_t result);
-
-protected:
-
-    // TIMEOUT CONSTANTS
-    static const unsigned int timeoutIntervalHeartbeat = 3500 * 1000; ///< Heartbeat timeout is 3.5 seconds
-
 };
 
 Q_DECLARE_INTERFACE(UASInterface, "org.qgroundcontrol/1.0")

src/ui/toolbar/MainToolBar.qml

@@ -584,10 +584,10 @@ Rectangle {
         anchors.right:          parent.right
         anchors.verticalCenter: parent.verticalCenter
 
-        property bool vehicleInactive: activeVehicle ? activeVehicle.heartbeatTimeout != 0 : false
+        property bool vehicleConnectionLost: activeVehicle ? activeVehicle.connectionLost : false
 
         Loader {
-            source:                 activeVehicle && !parent.vehicleInactive ? "MainToolBarIndicators.qml" : ""
+            source:                 activeVehicle && !parent.vehicleConnectionLost ? "MainToolBarIndicators.qml" : ""
             anchors.left:           parent.left
             anchors.verticalCenter: parent.verticalCenter
         }
@@ -600,7 +600,7 @@ Rectangle {
             color:                  colorRed
             anchors.left:           parent.left
             anchors.verticalCenter: parent.verticalCenter
-            visible:                parent.vehicleInactive
+            visible:                parent.vehicleConnectionLost
 
         }
 
@@ -609,7 +609,7 @@ Rectangle {
             anchors.right:          parent.right
             anchors.verticalCenter: parent.verticalCenter
             text:                   "Disconnect"
-            visible:                parent.vehicleInactive
+            visible:                parent.vehicleConnectionLost
             onClicked:              activeVehicle.disconnectInactiveVehicle()
         }
     }