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