public HeartbeatClient(IMavlinkV2Connection connection, MavlinkClientIdentity config, int heartBeatTimeoutMs = 2000)
{
_heartBeatTimeoutMs = heartBeatTimeoutMs;
connection
.FilterVehicle(config)
.Select(_ => _.Sequence)
.Subscribe(_ =>
{
Interlocked.Exchange(ref _lastPacketId, _);
Interlocked.Increment(ref _packetCounter);
}, _disposeCancel.Token);
connection
.FilterVehicle(config)
.Where(_ => _.MessageId == HeartbeatPacket.PacketMessageId)
.Cast <HeartbeatPacket>()
.Select(_ => _.Payload)
.Subscribe(_heartBeat);
_disposeCancel.Token.Register(() => _heartBeat.Dispose());
connection
.FilterVehicle(config)
.Select(_ => 1)
.Buffer(TimeSpan.FromSeconds(1))
.Select(_ => _.Sum()).Subscribe(_packetRate, _disposeCancel.Token);
_disposeCancel.Token.Register(() => _packetRate.Dispose());
Observable.Timer(TimeSpan.FromSeconds(1), TimeSpan.FromSeconds(1)).Subscribe(CheckConnection, _disposeCancel.Token);
RawHeartbeat.Subscribe(_ =>
{
if (_disposeCancel.IsCancellationRequested)
{
return;
}
_lastHeartbeat = DateTime.Now;
_link.Upgrade();
CalculateLinqQuality();
}, _disposeCancel.Token);
_disposeCancel.Token.Register(() => _link.Dispose());
}
