diff --git a/src/Vehicle/ADSBVehicle.cc b/src/Vehicle/ADSBVehicle.cc index 2aca47e5a5d5b3aa1e7fa8de80b7768bd70097ca..6fcda9ef0eedb04f0223b2553232959710279ed9 100644 --- a/src/Vehicle/ADSBVehicle.cc +++ b/src/Vehicle/ADSBVehicle.cc @@ -41,6 +41,7 @@ void ADSBVehicle::update(const QGeoCoordinate& location, float heading) emit coordinateChanged(_coordinate); emit altitudeChanged(_altitude); emit headingChanged(_heading); + _lastUpdateTimer.restart(); } void ADSBVehicle::update(mavlink_adsb_vehicle_t& adsbVehicle) @@ -84,4 +85,10 @@ void ADSBVehicle::update(mavlink_adsb_vehicle_t& adsbVehicle) _heading = newHeading; emit headingChanged(_heading); } + _lastUpdateTimer.restart(); +} + +bool ADSBVehicle::expired() +{ + return _lastUpdateTimer.hasExpired(expirationTimeoutMs); } diff --git a/src/Vehicle/ADSBVehicle.h b/src/Vehicle/ADSBVehicle.h index 5590c9c682ca3ffae87ede2f6e51b6f7bf776bd8..f0124f089bd69cefe68cbfd662fe894099a3e9eb 100644 --- a/src/Vehicle/ADSBVehicle.h +++ b/src/Vehicle/ADSBVehicle.h @@ -11,6 +11,7 @@ #include #include +#include #include "QGCMAVLink.h" @@ -39,6 +40,10 @@ public: void update(mavlink_adsb_vehicle_t& adsbVehicle); void update(const QGeoCoordinate& location, float heading); + + /// check if the vehicle is expired and should be removed + bool expired(); + signals: void coordinateChanged(QGeoCoordinate coordinate); void callsignChanged(QString callsign); @@ -46,9 +51,14 @@ signals: void headingChanged(double heading); private: + static constexpr qint64 expirationTimeoutMs = 5000; ///< timeout with no update in ms after which the vehicle is removed. + ///< AirMap sends updates for each vehicle every second. + uint32_t _icaoAddress; QString _callsign; QGeoCoordinate _coordinate; double _altitude; double _heading; + + QElapsedTimer _lastUpdateTimer; }; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 99a946534239294f8081c34176d7318b29bcf581..dd8e3bdf02007cf1c0ff1a01285d66cf75a21165 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -251,6 +251,10 @@ Vehicle::Vehicle(LinkInterface* link, _cameras = _firmwarePlugin->createCameraManager(this); emit dynamicCamerasChanged(); + connect(&_adsbTimer, &QTimer::timeout, this, &Vehicle::_adsbTimerTimeout); + _adsbTimer.setSingleShot(false); + _adsbTimer.start(1000); + _airMapController = new AirMapController(this); connect(qgcApp()->toolbox()->airMapManager(), &AirMapManager::trafficUpdate, this, &Vehicle::_trafficUpdate); } @@ -2928,7 +2932,6 @@ void Vehicle::_trafficUpdate(QString traffic_id, QString vehicle_id, QGeoCoordin Q_UNUSED(vehicle_id); // qDebug() << "traffic update:" << traffic_id << vehicle_id << heading << location; // TODO: filter based on minimum altitude? - // TODO: remove a vehicle after a timeout? if (_trafficVehicleMap.contains(traffic_id)) { _trafficVehicleMap[traffic_id]->update(location, heading); } else { @@ -2938,6 +2941,20 @@ void Vehicle::_trafficUpdate(QString traffic_id, QString vehicle_id, QGeoCoordin } } +void Vehicle::_adsbTimerTimeout() +{ + // TODO: take into account _adsbICAOMap as well? Needs to be tested, especially the timeout + + for (auto it = _trafficVehicleMap.begin(); it != _trafficVehicleMap.end();) { + if (it.value()->expired()) { + _adsbVehicles.removeOne(it.value()); + delete it.value(); + it = _trafficVehicleMap.erase(it); + } else { + ++it; + } + } +} //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 448a0c422ae7f72de5eb63498f94ed1f8417601f..acbce800b4b4209a8a3b107c2b12ffa703fb75f9 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -848,6 +848,7 @@ private slots: void _vehicleParamLoaded(bool ready); void _trafficUpdate(QString traffic_id, QString vehicle_id, QGeoCoordinate location, float heading); + void _adsbTimerTimeout(); private: bool _containsLink(LinkInterface* link); @@ -1028,6 +1029,7 @@ private: QmlObjectListModel _adsbVehicles; QMap _adsbICAOMap; QMap _trafficVehicleMap; + QTimer _adsbTimer; // Toolbox references FirmwarePluginManager* _firmwarePluginManager;