diff --git a/src/Airmap/AirMapTelemetry.cc b/src/Airmap/AirMapTelemetry.cc index 8550d323b61f9e7e78592453e8030bd54ca189b6..02b455b1c676bea567fd6c0b7044328e49b465e1 100644 --- a/src/Airmap/AirMapTelemetry.cc +++ b/src/Airmap/AirMapTelemetry.cc @@ -67,6 +67,12 @@ AirMapTelemetry::_handleGlobalPositionInt(const mavlink_message_t& message) if (!isTelemetryStreaming()) { return; } + // rate-limit updates to 5 Hz + if (!_timerLastSent.hasExpired(200)) { + return; + } + _timerLastSent.restart(); + mavlink_global_position_int_t globalPosition; mavlink_msg_global_position_int_decode(&message, &globalPosition); Telemetry::Position position{ @@ -123,6 +129,7 @@ AirMapTelemetry::startTelemetryStream(const QString& flightID) QString::fromStdString(result.error().message()), description); } }); + _timerLastSent.start(); } //----------------------------------------------------------------------------- diff --git a/src/Airmap/AirMapTelemetry.h b/src/Airmap/AirMapTelemetry.h index 8b67ae03962a65e4fc905488df3a1591cf8caf90..707a1dc4ac5465d9d33101eac0ebbd5321b52cd1 100644 --- a/src/Airmap/AirMapTelemetry.h +++ b/src/Airmap/AirMapTelemetry.h @@ -15,6 +15,7 @@ #include #include +#include /// Class to send telemetry data to AirMap class AirMapTelemetry : public QObject, public LifetimeChecker @@ -51,5 +52,6 @@ private: std::string _key; ///< key for AES encryption (16 bytes) QString _flightID; float _lastHdop = 1.f; + QElapsedTimer _timerLastSent; };