public Vehicle(IMavlinkV2Connection connection, VehicleConfig config) { if (connection == null) { throw new ArgumentNullException(nameof(connection)); } if (config == null) { throw new ArgumentNullException(nameof(config)); } _mavlinkConnection = connection; _rtt = new RawTelemetry(_mavlinkConnection, new RawTelemetryConfig { ComponentId = config.ComponentId, HeartbeatTimeoutMs = config.HeartbeatTimeoutMs, SystemId = config.SystemId }); _params = new VehicleParameterProtocol(_mavlinkConnection, new VehicleParameterProtocolConfig { ComponentId = config.ComponentId, SystemId = config.SystemId, ReadWriteTimeoutMs = config.ReadParamTimeoutMs, TimeoutToReadAllParamsMs = config.TimeoutToReadAllParamsMs }); _vehicleCommands = new VehicleCommandProtocol(_mavlinkConnection, new CommandProtocolConfig { ComponentId = config.ComponentId, CommandTimeoutMs = config.CommandTimeoutMs, SystemId = config.SystemId }); _mission = new VehicleMissionProtocol(_mavlinkConnection, new VehicleMissionProtocolConfig { ComponentId = config.ComponentId, SystemId = config.SystemId }); _offboard = new OffboardMode(_mavlinkConnection, new OffboardModeConfig()); }
public DebugServer(IMavlinkV2Connection connection, IPacketSequenceCalculator seq, MavlinkServerIdentity identity) { _connection = connection; _seq = seq; _identity = identity; _bootTime = DateTime.Now; }
public StatusTextServer(IMavlinkV2Connection connection, IPacketSequenceCalculator seq, MavlinkServerIdentity identity, StatusTextLoggerConfig config) { if (connection == null) { throw new ArgumentNullException(nameof(connection)); } if (seq == null) { throw new ArgumentNullException(nameof(seq)); } if (config == null) { throw new ArgumentNullException(nameof(config)); } _connection = connection; _seq = seq; _identity = identity; _config = config; _logger.Debug($"Create status logger for [sys:{identity.SystemId}, com:{identity.ComponenId}] with send rate:{config.MaxSendRateHz} Hz, buffer size: {config.MaxQueueSize}"); Observable.Timer(TimeSpan.FromSeconds(1.0 / _config.MaxSendRateHz), TimeSpan.FromSeconds(1.0 / _config.MaxSendRateHz)).Subscribe(TrySend, _disposeCancel.Token); }
protected MavlinkMicroserviceClient(IMavlinkV2Connection connection, MavlinkClientIdentity identity, IPacketSequenceCalculator seq, string ifcLogName) { _connection = connection ?? throw new ArgumentNullException(nameof(connection)); _identity = identity ?? throw new ArgumentNullException(nameof(identity)); _seq = seq ?? throw new ArgumentNullException(nameof(seq)); _ifcLogName = ifcLogName; }
public MissionClient(IMavlinkV2Connection mavlink, IPacketSequenceCalculator seq, MavlinkClientIdentity identity, MissionClientConfig config) { _mavlink = mavlink ?? throw new ArgumentNullException(nameof(mavlink)); _seq = seq ?? throw new ArgumentNullException(nameof(seq)); _identity = identity ?? throw new ArgumentNullException(nameof(identity)); _config = config; }
public MavlinkParameterClient(IMavlinkV2Connection connection, MavlinkClientIdentity identity, VehicleParameterProtocolConfig config) { _connection = connection ?? throw new ArgumentNullException(nameof(connection)); _identity = identity; _config = config ?? throw new ArgumentNullException(nameof(config)); HandleParams(); Converter = new MavParamArdupilotValueConverter(); }
public MissionClient(IMavlinkV2Connection mavlink, MavlinkClientIdentity identity, IPacketSequenceCalculator seq, MissionClientConfig config) : base(mavlink, identity, seq, "MISSION") { _config = config; _missionCurrent = new RxValue <ushort>().DisposeItWith(Disposable); _missionReached = new RxValue <ushort>().DisposeItWith(Disposable); mavlink.FilterVehicle(identity).Filter <MissionCurrentPacket>().Select(_ => _.Payload.Seq).Subscribe(_missionCurrent) .DisposeItWith(Disposable); mavlink.FilterVehicle(identity).Filter <MissionItemReachedPacket>().Select(_ => _.Payload.Seq).Subscribe(_missionReached) .DisposeItWith(Disposable); }
public VehiclePx4(IMavlinkV2Connection conn, MavlinkClientIdentity identity, MavlinkClientConfig config) : base(conn, identity, config) { _identity = identity; Connection .Where(FilterVehicle) .Where(_ => _.MessageId == HeartbeatPacket.PacketMessageId) .Cast <HeartbeatPacket>() .Select(_ => new Px4VehicleMode(_.Payload)) .Subscribe(_px4Mode); }
public VehiclePx4(IMavlinkV2Connection conn, VehicleConfig config) : base(conn, config) { _config = config; Connection .Where(FilterVehicle) .Where(_ => _.MessageId == HeartbeatPacket.PacketMessageId) .Cast <HeartbeatPacket>() .Select(_ => new Px4VehicleMode(_.Payload)) .Subscribe(_px4Mode); }
public MavlinkClient(IMavlinkV2Connection connection, MavlinkClientIdentity identity, MavlinkClientConfig config, IPacketSequenceCalculator sequence = null, bool disposeConnection = true) { if (config == null) { throw new ArgumentNullException(nameof(config)); } _seq = sequence ?? new PacketSequenceCalculator(); Identity = identity; _mavlinkConnection = connection ?? throw new ArgumentNullException(nameof(connection)); _rtt = new MavlinkTelemetry(_mavlinkConnection, identity, _seq); Disposable.Add(_rtt); _params = new MavlinkParameterClient(_mavlinkConnection, identity, _seq, new VehicleParameterProtocolConfig { ReadWriteTimeoutMs = config.ReadParamTimeoutMs, TimeoutToReadAllParamsMs = config.TimeoutToReadAllParamsMs }); Disposable.Add(_params); _mavlinkCommands = new MavlinkCommandClient(_mavlinkConnection, identity, _seq, new CommandProtocolConfig { CommandTimeoutMs = config.CommandTimeoutMs }); Disposable.Add(_mavlinkCommands); _mission = new MissionClient(_mavlinkConnection, identity, _seq, new MissionClientConfig { CommandTimeoutMs = config.CommandTimeoutMs }); Disposable.Add(_mission); _mavlinkOffboard = new MavlinkOffboardMode(_mavlinkConnection, identity, _seq); Disposable.Add(_mavlinkOffboard); _mode = new MavlinkCommon(_mavlinkConnection, identity, _seq); Disposable.Add(_mode); _debugs = new DebugClient(_mavlinkConnection, identity, _seq); Disposable.Add(_debugs); _heartbeat = new HeartbeatClient(_mavlinkConnection, identity, _seq); Disposable.Add(_heartbeat); _logging = new LoggingClient(_mavlinkConnection, identity, _seq); Disposable.Add(_logging); _v2Ext = new V2ExtensionClient(_mavlinkConnection, _seq, identity); Disposable.Add(_v2Ext); _rtk = new DgpsClient(_mavlinkConnection, identity, _seq); Disposable.Add(_rtt); if (disposeConnection) { Disposable.Add(_mavlinkConnection); } }
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); }
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 MavlinkClient(IMavlinkV2Connection connection, MavlinkClientIdentity identity, MavlinkClientConfig config, IPacketSequenceCalculator _sequence = null) { if (connection == null) { throw new ArgumentNullException(nameof(connection)); } if (config == null) { throw new ArgumentNullException(nameof(config)); } _seq = _sequence ?? new PacketSequenceCalculator(); Identity = identity; _mavlinkConnection = connection; _rtt = new MavlinkTelemetry(_mavlinkConnection, identity); _disposeCancel.Token.Register(() => _rtt.Dispose()); _params = new MavlinkParameterClient(_mavlinkConnection, identity, new VehicleParameterProtocolConfig { ReadWriteTimeoutMs = config.ReadParamTimeoutMs, TimeoutToReadAllParamsMs = config.TimeoutToReadAllParamsMs }); _disposeCancel.Token.Register(() => _params.Dispose()); _mavlinkCommands = new MavlinkCommandClient(_mavlinkConnection, identity, _seq, new CommandProtocolConfig { CommandTimeoutMs = config.CommandTimeoutMs }); _disposeCancel.Token.Register(() => _mavlinkCommands.Dispose()); _mission = new MissionClient(_mavlinkConnection, _seq, identity, new MissionClientConfig { CommandTimeoutMs = config.CommandTimeoutMs }); _disposeCancel.Token.Register(() => _mission.Dispose()); _mavlinkOffboard = new MavlinkOffboardMode(_mavlinkConnection, identity); _disposeCancel.Token.Register(() => _mavlinkOffboard.Dispose()); _mode = new MavlinkCommon(_mavlinkConnection, identity); _disposeCancel.Token.Register(() => _mode.Dispose()); _debugs = new DebugClient(_mavlinkConnection, identity); _disposeCancel.Token.Register(() => _debugs.Dispose()); _heartbeat = new HeartbeatClient(_mavlinkConnection, identity); _disposeCancel.Token.Register(() => _heartbeat.Dispose()); _logging = new LoggingClient(_mavlinkConnection, identity); _disposeCancel.Token.Register(() => _logging.Dispose()); _v2Ext = new V2ExtensionClient(_mavlinkConnection, _seq, identity); _disposeCancel.Token.Register(() => _v2Ext.Dispose()); _rtk = new DgpsClient(_mavlinkConnection, _seq, identity); _disposeCancel.Token.Register(() => _rtt.Dispose()); }
public VehicleCommandProtocol(IMavlinkV2Connection connection, CommandProtocolConfig config) { if (connection == null) { throw new ArgumentNullException(nameof(connection)); } if (config == null) { throw new ArgumentNullException(nameof(config)); } _connection = connection; _config = config; }
public VehicleMissionProtocol(IMavlinkV2Connection mavlink, VehicleMissionProtocolConfig config) { if (mavlink == null) { throw new ArgumentNullException(nameof(mavlink)); } if (config == null) { throw new ArgumentNullException(nameof(config)); } _mavlink = mavlink; _config = config; }
public LoggingServer(IMavlinkV2Connection connection, IPacketSequenceCalculator seq, MavlinkServerIdentity identity) { if (connection == null) { throw new ArgumentNullException(nameof(connection)); } if (seq == null) { throw new ArgumentNullException(nameof(seq)); } _connection = connection; _seq = seq; _identity = identity; }
public MavlinkServerBase(IMavlinkV2Connection connection, MavlinkServerIdentity identity, IPacketSequenceCalculator sequenceCalculator = null) { _seq = sequenceCalculator ?? new PacketSequenceCalculator(); Heartbeat = new MavlinkHeartbeatServer(connection, _seq, identity, new MavlinkHeartbeatServerConfig { HeartbeatRateMs = 1000 }); StatusText = new StatusTextServer(connection, _seq, identity, new StatusTextLoggerConfig { MaxQueueSize = 100, MaxSendRateHz = 10 }); CommandLong = new CommandLongServer(connection, _seq, identity); Debug = new DebugServer(connection, _seq, identity); Logging = new LoggingServer(connection, _seq, identity); V2Extension = new V2ExtensionServer(connection, _seq, identity); }
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 MavlinkTelemetry(IMavlinkV2Connection connection, MavlinkClientIdentity config) { _config = config; _connection = connection; _inputPackets = connection.FilterVehicle(config); HandleSystemStatus(); HandleGps(); HandleHighresImu(); HandleVfrHud(); HandleAttitude(); HandleBatteryStatus(); HandleAltitude(); HandleExtendedSysState(); HandleHome(); HandleGlobalPositionInt(); HandleRadioStatus(); }
public MavlinkCommandClient(IMavlinkV2Connection connection, MavlinkClientIdentity identity, IPacketSequenceCalculator seq, CommandProtocolConfig config) { if (connection == null) { throw new ArgumentNullException(nameof(connection)); } if (seq == null) { throw new ArgumentNullException(nameof(seq)); } if (config == null) { throw new ArgumentNullException(nameof(config)); } _connection = connection; _identity = identity; _seq = seq; _config = config; }
public MavlinkPacketTransponder(IMavlinkV2Connection connection, MavlinkServerIdentity identityConfig, IPacketSequenceCalculator seq) { if (connection == null) { throw new ArgumentNullException(nameof(connection)); } if (identityConfig == null) { throw new ArgumentNullException(nameof(identityConfig)); } if (seq == null) { throw new ArgumentNullException(nameof(seq)); } _connection = connection; _identityConfig = identityConfig; _seq = seq; _payloadContent = new byte[new TPacket().Payload.GetMaxByteSize() + 1]; _payloadSize = new TPacket().Payload.GetMaxByteSize(); }
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()); }
public Pv2ServerDeviceBase(IMavlinkV2Connection connection, IConfiguration cfgSvc, IEnumerable <Pv2ParamType> paramList, Pv2DeviceClass @class, IEnumerable <IWorkModeFactory> workModes, IChunkStore rttStore, IEnumerable <Pv2RttRecordDesc> rttRecords, IEnumerable <Pv2RttFieldDesc> rttFields, string configSuffix = "PV2") { var systemId = (byte)Pv2DeviceParams.SystemId.ReadFromConfigValue(cfgSvc, configSuffix); var componentId = (byte)Pv2DeviceParams.ComponentId.ReadFromConfigValue(cfgSvc, configSuffix); var networkId = (byte)Pv2DeviceParams.NetworkId.ReadFromConfigValue(cfgSvc, configSuffix); Seq = new PacketSequenceCalculator(); var mavlinkServer = new MavlinkServerBase(connection, new MavlinkServerIdentity { ComponentId = componentId, SystemId = systemId }, Seq).DisposeItWith(Disposable); _payloadServer = new PayloadV2Server(mavlinkServer, networkId).DisposeItWith(Disposable); var defaultParamsList = Pv2DeviceParams.Params.Concat(paramList) .Concat(Pv2BaseInterface.Params) .Concat(Pv2PowerInterface.Params) .Concat(Pv2RttInterface.Params) .Concat(Pv2MissionInterface.Params); _params = new Pv2ServerParamsInterface(_payloadServer, cfgSvc, defaultParamsList, configSuffix) .DisposeItWith(Disposable); _base = new Pv2ServerBaseInterface(_payloadServer, @class, workModes).DisposeItWith(Disposable); _power = new Pv2ServerPowerInterface(_payloadServer, _params).DisposeItWith(Disposable); _rtt = new Pv2ServerRttInterface(_payloadServer, _base, _params, rttStore, rttRecords, rttFields) .DisposeItWith(Disposable); _mission = new Pv2ServerMissionInterface(_payloadServer, _rtt, _base).DisposeItWith(Disposable); _base.OnLogMessage.Merge(_params.OnLogMessage).Merge(_power.OnLogMessage).Merge(_mission.OnLogMessage) .Subscribe(_ => Server.Server.StatusText.Log(_)).DisposeItWith(Disposable); }
public LoggingClient(IMavlinkV2Connection connection, MavlinkClientIdentity identity) { if (connection == null) { throw new ArgumentNullException(nameof(connection)); } if (identity == null) { throw new ArgumentNullException(nameof(identity)); } connection .FilterVehicle(identity) .Where(_ => _.MessageId == LoggingDataPacket.PacketMessageId) .Cast <LoggingDataPacket>() .Where(_ => _.Payload.TargetSystem == identity.SystemId && _.Payload.TargetComponent == identity.ComponentId) .Select(_ => _.Payload) .Subscribe(_loggingData, _disposeCancel.Token); _disposeCancel.Token.Register(() => _loggingData.Dispose()); }
public MavlinkServerBase(IMavlinkV2Connection connection, MavlinkServerIdentity identity, IPacketSequenceCalculator sequenceCalculator = null, bool disposeConnection = true) { _seq = sequenceCalculator ?? new PacketSequenceCalculator(); _heartbeat = new MavlinkHeartbeatServer(connection, _seq, identity, new MavlinkHeartbeatServerConfig { HeartbeatRateMs = 1000 }); _statusText = new StatusTextServer(connection, _seq, identity, new StatusTextLoggerConfig { MaxQueueSize = 100, MaxSendRateHz = 10 }); _commandLong = new CommandLongServer(connection, _seq, identity); _debug = new DebugServer(connection, _seq, identity); _logging = new LoggingServer(connection, _seq, identity); _v2Extension = new V2ExtensionServer(connection, _seq, identity); _params = new MavlinkParamsServer(connection, _seq, identity); MavlinkV2Connection = connection; _identity = identity; _disposeConnection = disposeConnection; }
public DebugClient(IMavlinkV2Connection connection, MavlinkClientIdentity identity) { _identity = identity; var inputPackets = connection.FilterVehicle(identity); inputPackets .Where(_ => _.MessageId == NamedValueFloatPacket.PacketMessageId) .Cast <NamedValueFloatPacket>() .Select(_ => new KeyValuePair <string, float>(ConvertToKey(_.Payload.Name), _.Payload.Value)) .Subscribe(_onFloatSubject, _disposeCancel.Token); _disposeCancel.Token.Register(() => { _onFloatSubject.OnCompleted(); _onFloatSubject.Dispose(); }); inputPackets .Where(_ => _.MessageId == NamedValueIntPacket.PacketMessageId) .Cast <NamedValueIntPacket>() .Select(_ => new KeyValuePair <string, int>(ConvertToKey(_.Payload.Name), _.Payload.Value)) .Subscribe(_onIntSubject, _disposeCancel.Token); _disposeCancel.Token.Register(() => { _onIntSubject.OnCompleted(); _onIntSubject.Dispose(); }); inputPackets .Where(_ => _.MessageId == DebugFloatArrayPacket.PacketMessageId) .Cast <DebugFloatArrayPacket>() .Select(_ => _.Payload) .Subscribe(_debugFloatArray, _disposeCancel.Token); _disposeCancel.Token.Register(() => _debugFloatArray.Dispose()); }
/// <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); }
public MavlinkParamsServer(IMavlinkV2Connection connection, IPacketSequenceCalculator seq, MavlinkServerIdentity identity) : base(connection, seq, identity) { Subscribe <ParamRequestListPacket, ParamRequestListPayload>(OnRequestList); }
public MavlinkOffboardMode(IMavlinkV2Connection connection, MavlinkClientIdentity config) { _connection = connection; _config = config; }
public static IObservable <IPacketV2 <IPayload> > FilterVehicle(this IMavlinkV2Connection src, MavlinkClientIdentity identity) { return(src.Where(_ => FilterVehicle(_, identity))); }