Skip to content

Q
qgroundcontrol
Commit ba1bd6dd authored by Don Gagne's avatar Don Gagne
Browse files
Options

Move connection lost handling to Vehicle

parent 0e3df28c
Hide whitespace changes
Inline Side-by-side
Showing with 68 additions and 94 deletions
src/Vehicle/Vehicle.cc
View file @ ba1bd6dd
......@@ -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
View file @ ba1bd6dd
......@@ -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
View file @ ba1bd6dd
......@@ -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
View file @ ba1bd6dd
......@@ -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
View file @ ba1bd6dd
......@@ -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
View file @ ba1bd6dd
......@@ -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()
}
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment