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 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 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 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 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 V2ExtensionClient(IMavlinkV2Connection connection, IPacketSequenceCalculator seq, MavlinkClientIdentity 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.ComponentId))
.Subscribe(_onData, _disposeCancel.Token);
}
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 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 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());
}
public static IObservable <IPacketV2 <IPayload> > FilterVehicle(this IMavlinkV2Connection src, MavlinkClientIdentity identity)
{
return(src.Where(_ => FilterVehicle(_, identity)));
}
public MavlinkCommon(IMavlinkV2Connection connection, MavlinkClientIdentity identity, IPacketSequenceCalculator seq) : base(connection, identity, seq, "COMMON")
{
}
public DgpsClient(IMavlinkV2Connection connection, IPacketSequenceCalculator seq, MavlinkClientIdentity identity)
{
_connection = connection;
_seq = seq;
_identity = identity;
}
public static IObservable <IPacketV2 <IPayload> > FilterVehicle(this IObservable <IPacketV2 <IPayload> > src, MavlinkClientIdentity identity)
{
return(src.Where(_ => FilterVehicle(_, identity.TargetSystemId, identity.TargetComponentId)));
}
public MavlinkCommon(IMavlinkV2Connection connection, MavlinkClientIdentity config)
{
_connection = connection;
_config = config;
}
public V2ExtensionClient(IMavlinkV2Connection connection, IPacketSequenceCalculator seq, MavlinkClientIdentity identity) : base(connection, identity, seq, "V2EXT")
{
_identity = identity;
Filter <V2ExtensionPacket>().Subscribe(_onData).DisposeItWith(Disposable);
}
public DgpsClient(IMavlinkV2Connection connection, MavlinkClientIdentity identity, IPacketSequenceCalculator seq) : base(connection, identity, seq, "DGPS")
{
}