public MavlinkPayloadServer(MavlinkPayloadServerConfig cfg, bool sendHeartBeatMessages = true) { _logger.Info($"Create mavlink payload server: cs:{cfg.ConnectionString}, comId:{cfg.Identity.ComponenId}, sysId:{cfg.Identity.SystemId}, netId:{cfg.Identity.NetworkId}"); _conn = new MavlinkV2Connection(cfg.ConnectionString, _ => { _.RegisterMinimalDialect(); _.RegisterCommonDialect(); _.RegisterArdupilotmegaDialect(); _.RegisterIcarousDialect(); _.RegisterUavionixDialect(); }); _srv = new MavlinkServerBase(_conn, cfg.Identity); _identity = cfg.Identity; _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 GroundControlStation() { MavlinkV2 = new MavlinkV2Connection(Ports, _ => { _.RegisterCommonDialect(); _.RegisterArdupilotmegaDialect(); _.RegisterIcarousDialect(); _.RegisterUavionixDialect(); }); _cancel.Token.Register(() => _foundDeviceSubject.Dispose()); _cancel.Token.Register(() => _lostDeviceSubject.Dispose()); MavlinkV2.Where(_ => _.MessageId == HeartbeatPacket.PacketMessageId).Cast <HeartbeatPacket>().Subscribe(DeviceFounder, _cancel.Token); Observable.Timer(TimeSpan.FromSeconds(1), TimeSpan.FromSeconds(1)).Subscribe(_ => RemoveOldDevice(), _cancel.Token); }
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)); } }