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); }