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 DebugServer(IMavlinkV2Connection connection, IPacketSequenceCalculator seq, MavlinkServerIdentity identity)
{
_connection = connection;
_seq = seq;
_identity = identity;
_bootTime = DateTime.Now;
}
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 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 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 GroundControlStation(GroundControlStationIdentity config, IPacketSequenceCalculator sequenceCalculator = null, int sendHeartBeatMs = 0)
{
if (config == null)
{
throw new ArgumentNullException(nameof(config));
}
_seq = sequenceCalculator ?? new PacketSequenceCalculator();
_config = config;
MavlinkV2 = new MavlinkV2Connection(Ports, _ =>
{
_.RegisterMinimalDialect();
_.RegisterCommonDialect();
_.RegisterArdupilotmegaDialect();
_.RegisterIcarousDialect();
_.RegisterUavionixDialect();
});
_cancel.Token.Register(() =>
{
_foundDeviceSubject.OnCompleted();
_foundDeviceSubject.Dispose();
});
_cancel.Token.Register(() =>
{
_lostDeviceSubject.OnCompleted();
_lostDeviceSubject.Dispose();
});
MavlinkV2.Where(_ => _.MessageId == HeartbeatPacket.PacketMessageId).Cast <HeartbeatPacket>().Subscribe(DeviceFounder, _cancel.Token);
Observable.Timer(TimeSpan.FromSeconds(3), TimeSpan.FromSeconds(3)).Subscribe(_ => RemoveOldDevice(), _cancel.Token);
if (sendHeartBeatMs != 0)
{
_transponder = new MavlinkPacketTransponder <HeartbeatPacket, HeartbeatPayload>(MavlinkV2, new MavlinkPayloadIdentity {
ComponenId = _config.ComponentId, SystemId = _config.SystemId
}, _seq);
_transponder.Set(_ =>
{
_.Autopilot = MavAutopilot.MavAutopilotInvalid;
_.BaseMode = 0;
_.CustomMode = 0;
_.MavlinkVersion = 3;
_.SystemStatus = MavState.MavStateActive;
_.Type = MavType.MavTypeGcs;
});
_transponder.Start(TimeSpan.FromMilliseconds(sendHeartBeatMs));
}
}
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 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 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 MavlinkPayloadServer(MavlinkPayloadIdentity identity, IDataStream dataStream, bool sendHeartBeatMessages = true, IPacketSequenceCalculator sequenceCalculator = null)
{
_identity = identity;
_logger.Info($"Create mavlink payload server: dataStream:{dataStream}, comId:{identity.ComponenId}, sysId:{identity.SystemId}, netId:{identity.NetworkId}");
_conn = new MavlinkV2Connection(dataStream, _ =>
{
_.RegisterMinimalDialect();
_.RegisterCommonDialect();
_.RegisterArdupilotmegaDialect();
_.RegisterIcarousDialect();
_.RegisterUavionixDialect();
});
_srv = new MavlinkServerBase(_conn, identity, sequenceCalculator);
_srv.Heartbeat.Set(_ =>
{
_.Autopilot = MavAutopilot.MavAutopilotGeneric;
_.Type = MavType.MavTypeGeneric;
_.SystemStatus = MavState.MavStateActive;
_.MavlinkVersion = 3;
_.CustomMode = PayloadHelper.HeartbeatMagicDigit;
});
if (sendHeartBeatMessages)
{
_srv.Heartbeat.Start();
}
_srv.V2Extension.OnData.Where(CheckPacketTarget).Subscribe(OnData, _disposeCancel.Token);
}
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 MavlinkCommon(IMavlinkV2Connection connection, MavlinkClientIdentity identity, IPacketSequenceCalculator seq) : base(connection, identity, seq, "COMMON")
{
}
public MavlinkOffboardMode(IMavlinkV2Connection connection, MavlinkClientIdentity config,
IPacketSequenceCalculator seq) : base(connection, config, seq, "OFFBOARD")
{
}
public MavlinkHeartbeatServer(IMavlinkV2Connection connection, IPacketSequenceCalculator seq, MavlinkServerIdentity identity, MavlinkHeartbeatServerConfig config)
{
_config = config;
_transponder = new MavlinkPacketTransponder <HeartbeatPacket, HeartbeatPayload>(connection, identity, seq);
}
public DgpsClient(IMavlinkV2Connection connection, IPacketSequenceCalculator seq, MavlinkClientIdentity identity)
{
_connection = connection;
_seq = seq;
_identity = identity;
}
public V2ExtensionClient(IMavlinkV2Connection connection, IPacketSequenceCalculator seq, MavlinkClientIdentity identity) : base(connection, identity, seq, "V2EXT")
{
_identity = identity;
Filter <V2ExtensionPacket>().Subscribe(_onData).DisposeItWith(Disposable);
}
public MavlinkCommandClient(IMavlinkV2Connection connection, MavlinkClientIdentity identity,
IPacketSequenceCalculator seq, CommandProtocolConfig config) : base(connection, identity, seq, "COMMAND")
{
_config = config ?? throw new ArgumentNullException(nameof(config));
}
protected MavlinkMicroserviceBase(IMavlinkV2Connection connection, IPacketSequenceCalculator seq, MavlinkServerIdentity identity)
{
_connection = connection ?? throw new ArgumentNullException(nameof(connection));
_seq = seq ?? throw new ArgumentNullException(nameof(seq));
_identity = identity ?? throw new ArgumentNullException(nameof(identity));
}
public MavlinkParamsServer(IMavlinkV2Connection connection, IPacketSequenceCalculator seq, MavlinkServerIdentity identity)
: base(connection, seq, identity)
{
Subscribe <ParamRequestListPacket, ParamRequestListPayload>(OnRequestList);
}
public DgpsClient(IMavlinkV2Connection connection, MavlinkClientIdentity identity, IPacketSequenceCalculator seq) : base(connection, identity, seq, "DGPS")
{
}
