public void Open()
{
// reset
MAV.sysid = 0;
MAV.compid = 0;
MAV.packets.Initialize();
MAV.VersionString = "";
try
{
lock (writelock)
{
BaseStream.Open();
BaseStream.DiscardInBuffer();
Thread.Sleep(1000);
}
byte[] buffer = new byte[0];
byte[] buffer1 = new byte[0];
DateTime start = DateTime.Now;
DateTime deadline = start.AddSeconds(CONNECT_TIMEOUT_SECONDS);
var countDown = new System.Timers.Timer {
Interval = 1000, AutoReset = false
};
countDown.Elapsed += (sender, e) =>
{
int secondsRemaining = (deadline - e.SignalTime).Seconds;
form.ClearTextBox();
form.UpdateTextBox(string.Format("Trying to connect.\nTimeout in {0}", secondsRemaining));
if (secondsRemaining > 0)
{
countDown.Start();
}
};
countDown.Start();
int count = 0;
bool hbseen = false;
// Connect loop
while (true)
{
if (DateTime.Now > deadline)
{
if (hbseen)
{
form.UpdateTextBox("Only one Mavlink heatbeat packet received.");
throw new Exception("Only one Mavlink heart beat packet read from this port.");
}
else
{
form.UpdateTextBox("No Mavlink heatbeat packets received.");
throw new Exception("No Mavlink heart beat packets read from this port.");
}
}
System.Threading.Thread.Sleep(1);
if (buffer.Length == 0)
{
buffer = GetHeartBeat();
}
System.Threading.Thread.Sleep(1);
if (buffer1.Length == 0)
{
buffer1 = GetHeartBeat();
}
if (buffer.Length > 0 || buffer1.Length > 0)
{
hbseen = true;
}
count++;
if (buffer.Length > 5 && buffer1.Length > 5 && buffer[3] == buffer1[3] && buffer[4] == buffer1[4])
{
mavlink_heartbeat_t hb = buffer.ByteArrayToStructure <mavlink_heartbeat_t>(6);
if (hb.type != (byte)MAVLink.MAV_TYPE.GCS)
{
mavlinkversion = hb.mavlink_version;
MAV.aptype = (MAV_TYPE)hb.type;
MAV.apname = (MAV_AUTOPILOT)hb.autopilot;
//setAPType();
MAV.sysid = buffer[3];
MAV.compid = buffer[4];
MAV.recvpacketcount = buffer[2];
//log.InfoFormat("ID sys {0} comp {1} ver{2}", MAV.sysid, MAV.compid, mavlinkversion);
break;
}
}
}
countDown.Stop();
}
catch
{
}
form.UpdateTextBox("Done open " + MAV.sysid + " " + MAV.compid);
packetslost = 0;
synclost = 0;
}
