private void OpenBg(object PRsender, bool getparams, ProgressWorkerEventArgs progressWorkerEventArgs)
{
frmProgressReporter.UpdateProgressAndStatus(-1, "正在连接.....");
giveComport = true;
byte[] buffer = new byte[1024];
if (BaseStream is SerialPort)
{
// allow settings to settle - previous dtr
Thread.Sleep(1000);
}
//Terrain = new TerrainFollow(this);
try
{
BaseStream.ReadBufferSize = 1024;
lock (objlock) // so we dont have random traffic
{
log.Info("Open port with " + BaseStream.PortName + " " + BaseStream.BaudRate);
if (BaseStream is UdpSerial)
{
progressWorkerEventArgs.CancelRequestChanged += (o, e) =>
{
((UdpSerial)BaseStream).CancelConnect = true;
((ProgressWorkerEventArgs)o)
.CancelAcknowledged = true;
};
}
BaseStream.Open();
// 读取数据 待修改
//BaseStream.DiscardInBuffer();
// other boards seem to have issues if there is no delay? posible bootloader timeout issue
if (BaseStream is SerialPort)
{
Thread.Sleep(1000);
}
}
//List<MAVLinkMessage> hbhistory = new List<MAVLinkMessage>();
DateTime start = DateTime.Now;
DateTime deadline = start.AddSeconds(CONNECT_TIMEOUT_SECONDS);
var countDown = new Timer {
Interval = 1000, AutoReset = false
};
countDown.Elapsed += (sender, e) =>
{
int secondsRemaining = (deadline - e.SignalTime).Seconds;
frmProgressReporter.UpdateProgressAndStatus(-1, string.Format(Strings.Trying, secondsRemaining));
if (secondsRemaining > 0)
{
countDown.Start();
}
};
countDown.Start();
int count = 0;
while (true)
{
if (progressWorkerEventArgs.CancelRequested)
{
progressWorkerEventArgs.CancelAcknowledged = true;
countDown.Stop();
if (BaseStream.IsOpen)
{
BaseStream.Close();
}
giveComport = false;
return;
}
log.Info(DateTime.Now.Millisecond + " Start connect loop ");
if (DateTime.Now > deadline)
{
//if (Progress != null)
// Progress(-1, "No Heartbeat Packets");
countDown.Stop();
this.Close();
}
Thread.Sleep(1);
//BaseStream.Read(buffer, count, 1);
count++;
// 解析数据，获取 sysid ,comid
sysidcurrent = 1;
compidcurrent = 50;
// if we get no data, try enableing rts/cts
//if (buffer.Length == 0 && BaseStream is SerialPort)
//{
// BaseStream.RtsEnable = !BaseStream.RtsEnable;
//}
if (count > 2)
{
break;
}
SetupMavConnect(1, 50, "AIR");
}
countDown.Stop();
if (frmProgressReporter.doWorkArgs.CancelAcknowledged == true)
{
giveComport = false;
if (BaseStream.IsOpen)
{
BaseStream.Close();
}
return;
}
}
catch (Exception e)
{
try
{
BaseStream.Close();
}
catch
{
}
giveComport = false;
if (string.IsNullOrEmpty(progressWorkerEventArgs.ErrorMessage))
{
progressWorkerEventArgs.ErrorMessage = Strings.ConnectFailed;
}
log.Error(e);
Console.WriteLine(e.ToString());
throw;
}
//frmProgressReporter.Close();
giveComport = false;
frmProgressReporter.UpdateProgressAndStatus(100, Strings.Done);
//log.Info("Done open " + MAV.sysid + " " + MAV.compid);
//MAV.packetslost = 0;
//MAV.synclost = 0;
}
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;
}
