diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 1991995eaf436af9d14e187aa32c02ea457359a5..1010aed33998536958f37e287e9767099f1a857f 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -42,6 +42,7 @@ QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog") const char* Vehicle::_settingsGroup = "Vehicle%1"; // %1 replaced with mavlink system id const char* Vehicle::_joystickModeSettingsKey = "JoystickMode"; const char* Vehicle::_joystickEnabledSettingsKey = "JoystickEnabled"; +const char* Vehicle::_communicationInactivityKey = "CommunicationInactivity"; Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType) : _id(vehicleId) @@ -92,6 +93,7 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType, , _base_mode(0) , _custom_mode(0) , _nextSendMessageMultipleIndex(0) + , _communicationInactivityTimeoutMSecs(_communicationInactivityTimeoutMSecsDefault) { _addLink(link); @@ -162,6 +164,10 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType, _mapTrajectoryTimer.setInterval(_mapTrajectoryMsecsBetweenPoints); connect(&_mapTrajectoryTimer, &QTimer::timeout, this, &Vehicle::_addNewMapTrajectoryPoint); + + _communicationInactivityTimer.setInterval(_communicationInactivityTimeoutMSecs); + connect(&_communicationInactivityTimer, &QTimer::timeout, this, &Vehicle::_communicationInactivityTimedOut); + _communicationInactivityTimer.start(); } Vehicle::~Vehicle() @@ -178,6 +184,8 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes if (message.sysid != _id && message.sysid != 0) { return; } + + _communicationInactivityTimer.start(); if (!_containsLink(link)) { _addLink(link); @@ -848,6 +856,7 @@ void Vehicle::_loadSettings(void) } _joystickEnabled = settings.value(_joystickEnabledSettingsKey, false).toBool(); + _communicationInactivityTimeoutMSecs = settings.value(_communicationInactivityKey, _communicationInactivityTimeoutMSecsDefault).toInt(); } void Vehicle::_saveSettings(void) @@ -858,6 +867,7 @@ void Vehicle::_saveSettings(void) settings.setValue(_joystickModeSettingsKey, _joystickMode); settings.setValue(_joystickEnabledSettingsKey, _joystickEnabled); + settings.setValue(_communicationInactivityKey, _communicationInactivityTimeoutMSecs); } int Vehicle::joystickMode(void) @@ -1113,3 +1123,13 @@ void Vehicle::_parametersReady(bool parametersReady) _missionManager->requestMissionItems(); } } + +void Vehicle::_communicationInactivityTimedOut(void) +{ + // Vechile is no longer communicating with us, disconnect all links + + LinkManager* linkMgr = LinkManager::instance(); + for (int i=0; i<_links.count(); i++) { + linkMgr->disconnectLink(_links[i].data()); + } +} diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 1a3a6ee72faa6a0ad763165ea63b1dff75e0c145..6fbf286ae36c5a44d4690e2042d0ade48d27db1d 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -305,6 +305,7 @@ private slots: void _sendMessageMultipleNext(void); void _addNewMapTrajectoryPoint(void); void _parametersReady(bool parametersReady); + void _communicationInactivityTimedOut(void); void _handleTextMessage (int newCount); /** @brief Attitude from main autopilot / system state */ @@ -434,10 +435,15 @@ private: QGeoCoordinate _mapTrajectoryLastCoordinate; bool _mapTrajectoryHaveFirstCoordinate; static const int _mapTrajectoryMsecsBetweenPoints = 1000; + + QTimer _communicationInactivityTimer; + int _communicationInactivityTimeoutMSecs; + static const int _communicationInactivityTimeoutMSecsDefault = 30 * 1000; // Settings keys static const char* _settingsGroup; static const char* _joystickModeSettingsKey; static const char* _joystickEnabledSettingsKey; + static const char* _communicationInactivityKey; }; #endif