private void HandleParams()
{
_paramsSubscribe = _connection
.Where(FilterVehicle)
.Where(_ => _.MessageId == ParamValuePacket.PacketMessageId)
.Cast <ParamValuePacket>().Subscribe(UpdateParam);
}
/// <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);
}
private void HandleRadioStatus()
{
_connection
.Where(_ => _.MessageId == RadioStatusPacket.PacketMessageId)
.Cast <RadioStatusPacket>()
.Select(_ => _.Payload)
.Subscribe(_radioStatus, _disposeCancel.Token);
_disposeCancel.Token.Register(() => _globalPositionInt.Dispose());
}
protected void Subscribe <TPacket, TPayload>(Action <TPacket> callback)
where TPacket : IPacketV2 <TPayload>, new() where TPayload : IPayload
{
var id = new TPacket().MessageId;
_connection
.Where(_ => _.MessageId == id)
.Cast <TPacket>()
.ObserveOn(TaskPoolScheduler.Default)
.Subscribe(callback).DisposeItWith(Disposable);
}
public CommandLongServer(IMavlinkV2Connection connection, IPacketSequenceCalculator seq, MavlinkServerIdentity identity)
{
_connection = connection;
_seq = seq;
_identity = identity;
connection
.Where(_ => _.MessageId == CommandLongPacket.PacketMessageId)
.Cast <CommandLongPacket>()
.Where(_ => _.Payload.TargetComponent == identity.ComponentId && _.Payload.TargetSystem == identity.SystemId)
.ObserveOn(TaskPoolScheduler.Default)
.Subscribe(OnRequest, _disposeCancel.Token);
}
public V2ExtensionServer(IMavlinkV2Connection connection, IPacketSequenceCalculator seq, MavlinkServerIdentity identity)
{
_connection = connection;
_seq = seq;
_identity = identity;
connection
.Where(_ => _.MessageId == V2ExtensionPacket.PacketMessageId)
.Cast <V2ExtensionPacket>().Where(_ =>
(_.Payload.TargetSystem == 0 || _.Payload.TargetSystem == _identity.SystemId) &&
(_.Payload.TargetComponent == 0 || _.Payload.TargetComponent == _identity.ComponenId))
.Subscribe(_onData, _disposeCancel.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 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 RawTelemetry(IMavlinkV2Connection connection, RawTelemetryConfig config)
{
_config = config;
_inputPackets = connection.Where(FilterVehicle);
HandleStatistic();
HandleHeartbeat(config);
HandleSystemStatus();
HandleGps();
HandleHighresImu();
HandleVfrHud();
HandleAttitude();
HandleBatteryStatus();
HandleAltitude();
HandleExtendedSysState();
HandleHome();
}
public static IObservable <IPacketV2 <IPayload> > FilterVehicle(this IMavlinkV2Connection src, MavlinkClientIdentity identity)
{
return(src.Where(_ => FilterVehicle(_, identity)));
}
public async Task <CommandAckPayload> CommandInt(MavCmd command, MavFrame frame, bool current, bool autocontinue, float param1, float param2,
float param3, float param4, int x, int y, float z, int attemptCount, CancellationToken cancel)
{
var packet = new CommandIntPacket()
{
ComponenId = _identity.ComponentId,
SystemId = _identity.SystemId,
Sequence = _seq.GetNextSequenceNumber(),
Payload =
{
Command = command,
TargetComponent = _identity.TargetComponentId,
TargetSystem = _identity.TargetSystemId,
Frame = frame,
Param1 = param1,
Param2 = param2,
Param3 = param3,
Param4 = param4,
Current = (byte)(current ? 1:0),
Autocontinue = (byte)(autocontinue ? 1:0),
X = x,
Y = y,
Z = z,
}
};
byte currentAttept = 0;
CommandAckPacket result = null;
while (currentAttept < attemptCount)
{
++currentAttept;
using (var timeoutCancel = new CancellationTokenSource(_config.CommandTimeoutMs))
using (var linkedCancel = CancellationTokenSource.CreateLinkedTokenSource(cancel, timeoutCancel.Token))
{
IDisposable subscribe = null;
try
{
var eve = new AsyncAutoResetEvent(false);
subscribe = _connection.Where(FilterVehicle).Where(_ => _.MessageId == CommandAckPacket.PacketMessageId)
.Cast <CommandAckPacket>()
.FirstAsync(_ => _.Payload.Command == command)
// 21.04.2019 comment this filter, because work in progress https://mavlink.io/en/messages/common.html#COMMAND_ACK
// .FirstAsync(_ => _.Payload.TargetComponent == _config.ComponentId &&
// _.Payload.TargetSystem == _config.SystemId)
.Subscribe(_ =>
{
result = _;
eve.Set();
});
await _connection.Send(packet, linkedCancel.Token).ConfigureAwait(false);
await eve.WaitAsync(linkedCancel.Token);
break;
}
catch (TaskCanceledException)
{
if (!timeoutCancel.IsCancellationRequested)
{
throw;
}
}
finally
{
subscribe?.Dispose();
}
}
}
if (result == null)
{
throw new TimeoutException(string.Format("Timeout to execute command '{0:G}' with '{1}' attempts (timeout {1} times by {2:g} )", command, currentAttept, TimeSpan.FromMilliseconds(_config.CommandTimeoutMs)));
}
return(result.Payload);
}
public async Task <int> MissionRequestCount(int attemptCount, CancellationToken cancel)
{
_logger.Trace($"[MISSION]=> Begin request items count with {attemptCount} attempts");
var packet = new MissionRequestListPacket()
{
ComponenId = _identity.ComponentId,
SystemId = _identity.SystemId,
Sequence = _seq.GetNextSequenceNumber(),
Payload =
{
TargetComponent = _identity.TargetComponentId,
TargetSystem = _identity.TargetSystemId,
}
};
byte currentAttept = 0;
MissionCountPacket result = null;
while (currentAttept < attemptCount)
{
++currentAttept;
using (var timeoutCancel = new CancellationTokenSource(_config.CommandTimeoutMs))
using (var linkedCancel = CancellationTokenSource.CreateLinkedTokenSource(cancel, timeoutCancel.Token))
{
IDisposable subscribe = null;
try
{
var eve = new AsyncAutoResetEvent(false);
subscribe = _mavlink.Where(FilterVehicle).Where(_ => _.MessageId == MissionCountPacket.PacketMessageId)
.Cast <MissionCountPacket>()
.FirstAsync()
.Subscribe(_ =>
{
result = _;
eve.Set();
});
await _mavlink.Send(packet, linkedCancel.Token).ConfigureAwait(false);
await eve.WaitAsync(linkedCancel.Token);
break;
}
catch (TaskCanceledException e)
{
_logger.Warn(e, $"[MISSION]=> Request {currentAttept} of {attemptCount} items count error:{e.Message}");
if (!timeoutCancel.IsCancellationRequested)
{
throw;
}
}
finally
{
subscribe?.Dispose();
}
}
}
if (result == null)
{
throw new TimeoutException(string.Format("Timeout to request mission items with '{0}' attempts (timeout {0} times by {1:g} )", currentAttept, TimeSpan.FromMilliseconds(_config.CommandTimeoutMs)));
}
_logger.Info($"[MISSION]<== Mission item count: {result.Payload.Count} items");
return(result.Payload.Count);
}
