diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index 7eb94b5ecd3b3b60a19c4571c4cc7ef5b1c415da..ab680c38e600df4ba61351df64a85e96faa4fb4f 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -285,6 +285,14 @@ void MultiVehicleManager::_setActiveVehiclePhase2(void) { qCDebug(MultiVehicleManagerLog) << "_setActiveVehiclePhase2 _vehicleBeingSetActive" << _vehicleBeingSetActive; + //-- Keep track of current vehicle's coordinates + if(_activeVehicle) { + disconnect(_activeVehicle, &Vehicle::coordinateChanged, this, &MultiVehicleManager::_coordinateChanged); + } + if(_vehicleBeingSetActive) { + connect(_vehicleBeingSetActive, &Vehicle::coordinateChanged, this, &MultiVehicleManager::_coordinateChanged); + } + // Now we signal the new active vehicle _activeVehicle = _vehicleBeingSetActive; emit activeVehicleChanged(_activeVehicle); @@ -302,6 +310,12 @@ void MultiVehicleManager::_setActiveVehiclePhase2(void) } } +void MultiVehicleManager::_coordinateChanged(QGeoCoordinate coordinate) +{ + _lastKnownLocation = coordinate; + emit lastKnownLocationChanged(); +} + void MultiVehicleManager::_vehicleParametersReadyChanged(bool parametersReady) { auto* paramMgr = qobject_cast(sender()); diff --git a/src/Vehicle/MultiVehicleManager.h b/src/Vehicle/MultiVehicleManager.h index d2f0d29991ceaf48b7d8333ff75ea3142087f6ea..f1372168013a579b15198c1273583f21c48a2a2d 100644 --- a/src/Vehicle/MultiVehicleManager.h +++ b/src/Vehicle/MultiVehicleManager.h @@ -47,6 +47,9 @@ public: /// A disconnected vehicle used for offline editing. It will match the vehicle type specified in Settings. Q_PROPERTY(Vehicle* offlineEditingVehicle READ offlineEditingVehicle CONSTANT) + //-- The current vehicle's last known location + Q_PROPERTY(QGeoCoordinate lastKnownLocation READ lastKnownLocation NOTIFY lastKnownLocationChanged) + // Methods Q_INVOKABLE Vehicle* getVehicleById(int vehicleId); @@ -78,24 +81,27 @@ public: // Override from QGCTool virtual void setToolbox(QGCToolbox *toolbox); + QGeoCoordinate lastKnownLocation () { return _lastKnownLocation; } + signals: - void vehicleAdded(Vehicle* vehicle); - void vehicleRemoved(Vehicle* vehicle); - void activeVehicleAvailableChanged(bool activeVehicleAvailable); + void vehicleAdded (Vehicle* vehicle); + void vehicleRemoved (Vehicle* vehicle); + void activeVehicleAvailableChanged (bool activeVehicleAvailable); void parameterReadyVehicleAvailableChanged(bool parameterReadyVehicleAvailable); - void activeVehicleChanged(Vehicle* activeVehicle); - void gcsHeartBeatEnabledChanged(bool gcsHeartBeatEnabled); - - void _deleteVehiclePhase2Signal(void); + void activeVehicleChanged (Vehicle* activeVehicle); + void gcsHeartBeatEnabledChanged (bool gcsHeartBeatEnabled); + void lastKnownLocationChanged (); + void _deleteVehiclePhase2Signal (void); private slots: - void _deleteVehiclePhase1(Vehicle* vehicle); - void _deleteVehiclePhase2(void); - void _setActiveVehiclePhase2(void); - void _vehicleParametersReadyChanged(bool parametersReady); - void _sendGCSHeartbeat(void); - void _vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType); - void _requestProtocolVersion(unsigned version); + void _deleteVehiclePhase1 (Vehicle* vehicle); + void _deleteVehiclePhase2 (void); + void _setActiveVehiclePhase2 (void); + void _vehicleParametersReadyChanged (bool parametersReady); + void _sendGCSHeartbeat (void); + void _vehicleHeartbeatInfo (LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType); + void _requestProtocolVersion (unsigned version); + void _coordinateChanged (QGeoCoordinate coordinate); private: bool _vehicleExists(int vehicleId); @@ -115,6 +121,7 @@ private: FirmwarePluginManager* _firmwarePluginManager; JoystickManager* _joystickManager; MAVLinkProtocol* _mavlinkProtocol; + QGeoCoordinate _lastKnownLocation; QTimer _gcsHeartbeatTimer; ///< Timer to emit heartbeats bool _gcsHeartbeatEnabled; ///< Enabled/disable heartbeat emission