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