private bool Filter(IPacketV2 <IPayload> packetV2, int dirIndex, out string txt)
{
txt = JsonConvert.SerializeObject(packetV2, Formatting.None, new StringEnumConverter());
if (_sysId.Count != 0 && !_sysId.Contains(packetV2.SystemId))
{
return(false);
}
if (_dirIndex.Count != 0 && !_dirIndex.Contains(dirIndex))
{
return(false);
}
if (_msgId.Count != 0 && !_msgId.Contains(packetV2.MessageId))
{
return(false);
}
if (_nameFilter != null && !_nameFilter.IsMatch(packetV2.Name))
{
return(false);
}
if (_textFilter != null && !_textFilter.IsMatch(txt))
{
return(false);
}
return(true);
}
private void OnPacket(IPacketV2 <IPayload> packet)
{
Interlocked.Increment(ref _packetCount);
try
{
_rw.EnterWriteLock();
_lastPackets.Add(packet);
if (_lastPackets.Count >= MaxHistorySize)
{
_lastPackets.RemoveAt(0);
}
var exist = _items.FirstOrDefault(_ => packet.MessageId == _.Msg);
if (exist == null)
{
_items.Add(new DisplayRow {
Msg = packet.MessageId, Message = packet.Name
});
}
else
{
exist.Count++;
}
}
finally
{
_rw.ExitWriteLock();
}
}
public Task Send(IPacketV2 <IPayload> packet, CancellationToken cancel)
{
Interlocked.Increment(ref _txPackets);
var buffer = new byte[packet.GetMaxByteSize()];
var size = packet.Serialize(buffer, 0);
return(DataStream.Send(buffer, size, cancel));
}
private bool FilterVehicle(IPacketV2 <IPayload> packetV2)
{
if (_identity.TargetSystemId != 0 && _identity.TargetSystemId != packetV2.SystemId)
{
return(false);
}
if (_identity.TargetComponentId != 0 && _identity.TargetComponentId != packetV2.ComponenId)
{
return(false);
}
return(true);
}
public static bool FilterVehicle(IPacketV2 <IPayload> packetV2, MavlinkClientIdentity identity)
{
if (identity.TargetSystemId != 0 && identity.TargetSystemId != packetV2.SystemId)
{
return(false);
}
if (identity.TargetComponentId != 0 && identity.TargetComponentId != packetV2.ComponenId)
{
return(false);
}
return(true);
}
public static bool FilterVehicle(IPacketV2 <IPayload> packetV2, byte targetSystemId, byte targetComponentId)
{
if (targetSystemId != 0 && targetSystemId != packetV2.SystemId)
{
return(false);
}
if (targetComponentId != 0 && targetComponentId != packetV2.ComponenId)
{
return(false);
}
return(true);
}
public Task Send(IPacketV2 <IPayload> packet, CancellationToken cancel)
{
if (_disposed != 0)
{
return(Task.CompletedTask);
}
Interlocked.Increment(ref _txPackets);
var buffer = new byte[packet.GetMaxByteSize()];
var size = packet.Serialize(buffer, 0);
_sendPacketSubject.OnNext(packet);
return(DataStream.Send(buffer, size, cancel));
}
public async Task <TAnswerPacket> SendAndWaitAnswer <TAnswerPacket>(IPacketV2 <IPayload> packet,
CancellationToken cancel, Func <TAnswerPacket, bool> filter = null, int timeoutMs = 1000)
where TAnswerPacket : IPacketV2 <IPayload>, new()
{
var p = new TAnswerPacket();
Logger.Trace($"{LogSend} call {p.Name}");
using var linkedCancel = CancellationTokenSource.CreateLinkedTokenSource(cancel, DisposeCancel);
linkedCancel.CancelAfter(timeoutMs);
var tcs = new TaskCompletionSource <TAnswerPacket>();
using var c1 = linkedCancel.Token.Register(() => tcs.TrySetCanceled());
filter ??= (_ => true);
using var subscribe = FilterFirstAsync(filter).Subscribe(_ => tcs.TrySetResult(_));
await _connection.Send(packet, linkedCancel.Token).ConfigureAwait(false);
var result = await tcs.Task.ConfigureAwait(false);
Logger.Trace($"{LogRecv} ok {packet.Name}<=={p.Name}");
return(result);
}
private void OnPacket(MavlinkV2Connection receiveFrom, IPacketV2 <IPayload> packetV2, int dirIndex)
{
string txt;
if (Filter(packetV2, dirIndex, out txt))
{
if (!_silentMode)
{
Console.WriteLine(txt);
}
_out?.WriteLine(txt);
}
foreach (var mavlinkV2Connection in _connections)
{
// skip source link
if (mavlinkV2Connection == receiveFrom)
{
continue;
}
mavlinkV2Connection.Send(packetV2, _cancel.Token);
}
}
/// <summary>
/// Subscribe to connection packet pipe fore waiting answer packet and then send request
/// </summary>
/// <typeparam name="TAnswerPacket"></typeparam>
/// <typeparam name="TAnswerPayload"></typeparam>
/// <param name="src"></param>
/// <param name="packet"></param>
/// <param name="targetSystem"></param>
/// <param name="targetComponent"></param>
/// <param name="cancel"></param>
/// <param name="filter"></param>
/// <returns></returns>
public static async Task <TAnswerPacket> SendAndWaitAnswer <TAnswerPacket, TAnswerPayload>(this IMavlinkV2Connection src, IPacketV2 <IPayload> packet, int targetSystem, int targetComponent, CancellationToken cancel, Func <TAnswerPacket, bool> filter = null)
where TAnswerPacket : IPacketV2 <TAnswerPayload>, new() where TAnswerPayload : IPayload
{
var p = new TAnswerPacket();
var eve = new AsyncAutoResetEvent(false);
IDisposable subscribe = null;
filter = filter ?? (_ => true);
var result = default(TAnswerPacket);
try
{
subscribe = src.Where(_ => _.ComponenId == targetComponent && _.SystemId == targetSystem && _.MessageId == p.MessageId)
.Cast <TAnswerPacket>()
.FirstAsync(filter)
.Subscribe(_ =>
{
result = _;
eve.Set();
});
await src.Send(packet, cancel).ConfigureAwait(false);
await eve.WaitAsync(cancel);
}
finally
{
subscribe?.Dispose();
}
return(result);
}
/// <summary>
/// Subscribe to connection packet pipe fore waiting answer packet and then send request
/// </summary>
/// <typeparam name="TAnswerPacket"></typeparam>
/// <typeparam name="TAnswerPayload"></typeparam>
/// <param name="src"></param>
/// <param name="packet"></param>
/// <param name="targetSystem"></param>
/// <param name="targetComponent"></param>
/// <param name="cancel"></param>
/// <param name="filter"></param>
/// <returns></returns>
public static async Task <TAnswerPacket> SendAndWaitAnswer <TAnswerPacket, TAnswerPayload>(this IMavlinkV2Connection src, IPacketV2 <IPayload> packet, int targetSystem, int targetComponent, CancellationToken cancel, Func <TAnswerPacket, bool> filter = null)
where TAnswerPacket : IPacketV2 <TAnswerPayload>, new() where TAnswerPayload : IPayload
{
var p = new TAnswerPacket();
var tcs = new TaskCompletionSource <TAnswerPacket>();
using var c1 = cancel.Register(() => tcs.TrySetCanceled());
filter = filter ?? (_ => true);
using var subscribe = src.Where(_ => _.ComponenId == targetComponent && _.SystemId == targetSystem && _.MessageId == p.MessageId)
.Cast <TAnswerPacket>()
.FirstAsync(filter)
.Subscribe(_ => tcs.TrySetResult(_));
await src.Send(packet, cancel).ConfigureAwait(false);
return(await tcs.Task.ConfigureAwait(false));
}
public void OnNext(IPacketV2 <IPayload> value)
{
_encoder.OnNext(value);
}
