From ba1bd6dd042b3b5232b49303b1b8b3b2f0b8173d Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Fri, 15 Jan 2016 14:17:33 -0800 Subject: [PATCH] Move connection lost handling to Vehicle --- src/Vehicle/Vehicle.cc | 66 ++++++++++++++++++++++++---------- src/Vehicle/Vehicle.h | 22 +++++++++--- src/uas/UAS.cc | 49 ------------------------- src/uas/UAS.h | 8 ----- src/uas/UASInterface.h | 9 ----- src/ui/toolbar/MainToolBar.qml | 8 ++--- 6 files changed, 68 insertions(+), 94 deletions(-) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 06cc6d828..7b4a075b7 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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); +} diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index f3c598d71..64d2334aa 100644 --- a/src/Vehicle/Vehicle.h +++ b/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; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 1c3d6eb99..a428d7bf9 100644 --- a/src/uas/UAS.cc +++ b/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; diff --git a/src/uas/UAS.h b/src/uas/UAS.h index fcbae7379..5b63ac530 100644 --- a/src/uas/UAS.h +++ b/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); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index dce3e5b02..533fdb1fe 100644 --- a/src/uas/UASInterface.h +++ b/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") diff --git a/src/ui/toolbar/MainToolBar.qml b/src/ui/toolbar/MainToolBar.qml index 80090803c..1215c8130 100644 --- a/src/ui/toolbar/MainToolBar.qml +++ b/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() } } -- 2.22.0