diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 3b5206dbc6d42fcaae45c1f60a03398d409de749..13740bd0196bd1a9bda9633a0068b065b01a66d4 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -48,7 +48,8 @@ UASWaypointManager::UASWaypointManager(UAS* _uas) current_partner_systemid(0), current_partner_compid(0), currentWaypointEditable(), - protocol_timer(this) + protocol_timer(this), + _updateWPlist_timer(this) { _offlineEditingModeMessage = tr("You are in offline editing mode. Make sure to save your mission to a file before connecting to a system - you will need to load the file into the system, the offline list will be cleared on connect."); @@ -67,6 +68,9 @@ UASWaypointManager::UASWaypointManager(UAS* _uas) // We signal to ourselves here so that tiemrs are started and stopped on correct thread connect(this, SIGNAL(_startProtocolTimer(void)), this, SLOT(_startProtocolTimerOnThisThread(void))); connect(this, SIGNAL(_stopProtocolTimer(void)), this, SLOT(_stopProtocolTimerOnThisThread(void))); + _updateWPlist_timer.setInterval(1500); + _updateWPlist_timer.setSingleShot(true); + connect(&_updateWPlist_timer, SIGNAL(timeout()), this, SLOT(_updateWPonTimer())); } UASWaypointManager::~UASWaypointManager() @@ -167,8 +171,20 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui } else { - qDebug("Rejecting waypoint count message, check mismatch: current_state: %d == %d, system id %d == %d, comp id %d == %d", current_state, WP_GETLIST, current_partner_systemid, systemId, current_partner_compid, compId); - } + if (current_state != WP_GETLIST_GETWPS && systemId == current_partner_systemid) + { + qDebug("Requesting new waypoints. Propably changed onboard."); + if (!_updateWPlist_timer.isActive()) + { + current_state = WP_IDLE; + _updateWPlist_timer.start(); + } + } + else + { + qDebug("Rejecting waypoint count message, check mismatch: current_state: %d == %d, system id %d == %d, comp id %d == %d", current_state, WP_GETLIST, current_partner_systemid, systemId, current_partner_compid, compId); + } + } } void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_mission_item_t *wp) @@ -1197,3 +1213,13 @@ void UASWaypointManager::_stopProtocolTimerOnThisThread(void) { protocol_timer.stop(); } + + +void UASWaypointManager::_updateWPonTimer() +{ + while (current_state != WP_IDLE) + { + QGC::SLEEP::msleep(100); + } + readWaypoints(true); +} diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index 39abc59fee2744a723593773c749faa6ad0968f1..a9555e7beb322046f66a42370de2ce2cffcbd10c 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -168,6 +168,7 @@ signals: private slots: void _startProtocolTimerOnThisThread(void); ///< Starts the protocol timer void _stopProtocolTimerOnThisThread(void); ///< Starts the protocol timer + void _updateWPonTimer(void); ///< Starts requesting WP on timer timeout private: UAS* uas; ///< Reference to the corresponding UAS @@ -184,6 +185,7 @@ private: QPointer currentWaypointEditable; ///< The currently used waypoint QList waypoint_buffer; ///< buffer for waypoints during communication QTimer protocol_timer; ///< Timer to catch timeouts + QTimer _updateWPlist_timer; ///< update WP list if modified by another instance onboard bool standalone; ///< If standalone is set, do not write to UAS int uasid; ///< The ID of the current UAS. Retrieved via `uas->getUASID();`, stored as an `int` to match its return type.