AirMapTelemetry: rate-limit telemetry to 5Hz

parent ef729dee
......@@ -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();
}
//-----------------------------------------------------------------------------
......
......@@ -15,6 +15,7 @@
#include <QGCMAVLink.h>
#include <QObject>
#include <QElapsedTimer>
/// 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;
};
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment