Exemple #1
0
        public AP_GPS_GSOF(string portname)
        {
            var sport = new TcpSerial();

            //new SerialPort(portname, 38400);

            sport.Open();

            port = sport.BaseStream;

            requestBaud();


            sport.BaudRate = 115200;

            requestGSOF();

            //requestPostion();

            gsof_msg.data = new byte[1024 * 20];

            read();
        }
        public static void Start()
        {
            var config = Settings.Instance[SettingsName];

            if (config == null)
            {
                Settings.Instance[SettingsName] = connectionInfos.ToJSON();
                config = Settings.Instance[SettingsName];
            }

            connectionInfos = config.FromJSON <List <ConnectionInfo> >();

            foreach (var connectionInfo in connectionInfos)
            {
                if (connectionInfo.Enabled == false)
                {
                    continue;
                }

                log.Info(connectionInfo.ToJSON());

                if (connectionInfo.Format == ConnectionFormat.MAVLink)
                {
                    if (connectionInfo.Protocol == ProtocolType.Udp && connectionInfo.Direction == Direction.Inbound)
                    {
                        try
                        {
                            var client = new UdpClient(connectionInfo.Port);
                            client.BeginReceive(clientdataMAVLink, client);
                        }
                        catch (Exception ex)
                        {
                            log.Error(ex);
                        }

                        continue;
                    }

                    if (connectionInfo.Protocol == ProtocolType.Udp && connectionInfo.Direction == Direction.Outbound)
                    {
                        try
                        {
                            // create and set default dest
                            var client = new UdpClient(connectionInfo.ConfigString, connectionInfo.Port);
                            client.SendAsync(new byte[] { 0 }, 1);
                            client.BeginReceive(clientdataMAVLink, client);
                        }
                        catch (Exception ex)
                        {
                            log.Error(ex);
                        }

                        continue;
                    }

                    if (connectionInfo.Protocol == ProtocolType.Tcp &&
                        connectionInfo.Direction == Direction.Outbound)
                    {
                        try
                        {
                            // try anything already connected
                            Task.Run(() =>
                            {
                                try
                                {
                                    var serial            = new TcpSerial();
                                    serial.Host           = connectionInfo.ConfigString;
                                    serial.Port           = connectionInfo.Port.ToString();
                                    serial.ReadBufferSize = 1024 * 300;
                                    serial.Open();
                                    // sample 1.2seconds
                                    Thread.Sleep(1200);
                                    var btr    = serial.BytesToRead;
                                    var buffer = new byte[btr];
                                    serial.Read(buffer, 0, buffer.Length);
                                    //serial.Close();
                                    var parse = new MAVLink.MavlinkParse();
                                    var st    = buffer.ToMemoryStream();
                                    while (st.Position < st.Length)
                                    {
                                        var packet = parse.ReadPacket(st);
                                        if (packet != null)
                                        {
                                            if (packet.msgid == (int)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
                                            {
                                                NewMavlinkConnection?.BeginInvoke(null, serial, null, null);
                                                return;
                                            }
                                        }
                                    }
                                }
                                catch
                                {
                                }
                            });
                        }
                        catch (Exception ex)
                        {
                            log.Error(ex);
                        }

                        continue;
                    }

                    if (connectionInfo.Protocol == ProtocolType.Tcp &&
                        connectionInfo.Direction == Direction.Inbound)
                    {
                        try
                        {
                            // try anything already connected
                            Task.Run(() =>
                            {
                                try
                                {
                                    TcpListener listener = new TcpListener(IPAddress.Any, connectionInfo.Port);
                                    listener.Start();
                                    var client            = listener.AcceptTcpClient();
                                    var serial            = new TcpSerial();
                                    serial.client         = client;
                                    serial.ReadBufferSize = 1024 * 300;
                                    serial.Open();
                                    // sample 1.2seconds
                                    Thread.Sleep(1200);
                                    var btr    = serial.BytesToRead;
                                    var buffer = new byte[btr];
                                    serial.Read(buffer, 0, buffer.Length);
                                    //serial.Close();
                                    var parse = new MAVLink.MavlinkParse();
                                    var st    = buffer.ToMemoryStream();
                                    while (st.Position < st.Length)
                                    {
                                        var packet = parse.ReadPacket(st);
                                        if (packet != null)
                                        {
                                            if (packet.msgid == (int)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
                                            {
                                                NewMavlinkConnection?.BeginInvoke(null, serial, null, null);
                                                return;
                                            }
                                        }
                                    }
                                }
                                catch
                                {
                                }
                            });
                        }
                        catch (Exception ex)
                        {
                            log.Error(ex);
                        }

                        continue;
                    }

                    if (connectionInfo.Protocol == ProtocolType.Serial &&
                        connectionInfo.Direction == Direction.Outbound)
                    {
                        try
                        {
                            // try anything already connected
                            Task.Run(() =>
                            {
                                Parallel.ForEach(SerialPort.GetPortNames(), port =>
                                {
                                    try
                                    {
                                        var serial            = new SerialPort(port, connectionInfo.Port);
                                        serial.ReadBufferSize = 1024 * 300;
                                        serial.Open();
                                        // sample 1.2seconds
                                        Thread.Sleep(1200);
                                        var btr    = serial.BytesToRead;
                                        var buffer = new byte[btr];
                                        serial.Read(buffer, 0, buffer.Length);
                                        serial.Close();
                                        var parse = new MAVLink.MavlinkParse();
                                        var st    = buffer.ToMemoryStream();
                                        while (st.Position < st.Length)
                                        {
                                            var packet = parse.ReadPacket(st);
                                            if (packet != null)
                                            {
                                                if (packet.msgid == (int)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
                                                {
                                                    NewMavlinkConnection?.BeginInvoke(null, serial, null, null);
                                                    return;
                                                }
                                            }
                                        }
                                    }
                                    catch
                                    {
                                    }
                                });
                            });
                        }
                        catch (Exception ex)
                        {
                            log.Error(ex);
                        }

                        continue;
                    }
                }
                else if (connectionInfo.Format == ConnectionFormat.Video)
                {
                    if (connectionInfo.Protocol == ProtocolType.Udp && connectionInfo.Direction == Direction.Inbound)
                    {
                        try
                        {
                            var client = new UdpClient(connectionInfo.Port, AddressFamily.InterNetwork);
                            client.BeginReceive(clientdataVideo, client);
                        }
                        catch (Exception ex)
                        {
                            log.Error(ex);
                        }

                        continue;
                    }

                    if (connectionInfo.Protocol == ProtocolType.Tcp && connectionInfo.Direction == Direction.Inbound)
                    {
                        try
                        {
                            var client = new TcpListener(IPAddress.Any, connectionInfo.Port);
                            client.Start();
                            client.BeginAcceptTcpClient(clientdatatcpvideo, client);
                        }
                        catch (Exception ex)
                        {
                            log.Error(ex);
                        }

                        continue;
                    }

                    if (connectionInfo.Direction == Direction.Outbound)
                    {
                        NewVideoStream?.BeginInvoke(null,
                                                    connectionInfos.First(a => a == connectionInfo).ConfigString, null, null);
                        continue;
                    }
                }
            }
        }