byte[] readlogPacketMavlink()
{
byte[] temp = new byte[300];
sysid = 0;
//byte[] datearray = BitConverter.GetBytes((ulong)(DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalMilliseconds);
byte[] datearray = new byte[8];
logplaybackfile.BaseStream.Read(datearray, 0, datearray.Length);
Array.Reverse(datearray);
DateTime date1 = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc);
UInt64 dateint = BitConverter.ToUInt64(datearray, 0);
date1 = date1.AddMilliseconds(dateint / 1000);
lastlogread = date1.ToLocalTime();
MainV2.cs.datetime = lastlogread;
int length = 5;
int a = 0;
while (a < length)
{
temp[a] = (byte)logplaybackfile.ReadByte();
if (temp[0] != 'U' && temp[0] != 254)
{
log.InfoFormat("lost sync byte {0} pos {1}", temp[0], logplaybackfile.BaseStream.Position);
a = 0;
continue;
}
if (a == 1)
{
length = temp[1] + 6 + 2; // 6 header + 2 checksum
}
a++;
}
if (temp[5] == 0) {
mavlink_heartbeat_t hb = temp.ByteArrayToStructure<mavlink_heartbeat_t>(6);
mavlinkversion = hb.mavlink_version;
aptype = (MAV_TYPE)hb.type;
}
return temp;
}
private void OpenBg(bool getparams, ProgressWorkerEventArgs progressWorkerEventArgs)
{
frmProgressReporter.UpdateProgressAndStatus(-1, "Mavlink Connecting...");
// reset
sysid = 0;
compid = 0;
param = new Hashtable();
packets.Initialize();
try
{
MainV2.giveComport = true;
BaseStream.ReadBufferSize = 4 * 1024;
lock (objlock) // so we dont have random traffic
{
log.Info("Open port with " + BaseStream.PortName + " " + BaseStream.BaudRate);
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;
//if (Progress != null)
// Progress(-1, string.Format("Trying to connect.\nTimeout in {0}", secondsRemaining));
frmProgressReporter.UpdateProgressAndStatus(-1, string.Format("Trying to connect.\nTimeout in {0}", 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();
MainV2.giveComport = false;
return;
}
// incase we are in setup mode
BaseStream.WriteLine("planner\rgcs\r");
log.Info(DateTime.Now.Millisecond + " Start connect loop ");
if (lastbad[0] == '!' && lastbad[1] == 'G' || lastbad[0] == 'G' && lastbad[1] == '!') // waiting for gps lock
{
//if (Progress != null)
// Progress(-1, "Waiting for GPS detection..");
frmProgressReporter.UpdateProgressAndStatus(-1, "Waiting for GPS detection..");
deadline = deadline.AddSeconds(5); // each round is 1.1 seconds
}
if (DateTime.Now > deadline)
{
//if (Progress != null)
// Progress(-1, "No Heatbeat Packets");
this.Close();
progressWorkerEventArgs.ErrorMessage = "No Heatbeat Packets Received";
throw new Exception("No Mavlink Heartbeat Packets where read from this port - Verify Baud Rate and setup\nIt might also be waiting for GPS Lock\nAPM Planner waits for 2 valid heartbeat packets before connecting");
}
System.Threading.Thread.Sleep(1);
// incase we are in setup mode
BaseStream.WriteLine("planner\rgcs\r");
// can see 2 heartbeat packets at any time, and will connect - was one after the other
if (buffer.Length == 0)
buffer = getHeartBeat();
// incase we are in setup mode
BaseStream.WriteLine("planner\rgcs\r");
System.Threading.Thread.Sleep(1);
if (buffer1.Length == 0)
buffer1 = getHeartBeat();
try
{
log.Debug("MAv Data: len " + buffer.Length + " btr " + BaseStream.BytesToRead);
}
catch { }
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);
mavlinkversion = hb.mavlink_version;
aptype = (MAV_TYPE)hb.type;
sysid = buffer[3];
compid = buffer[4];
recvpacketcount = buffer[2];
log.InfoFormat("ID sys {0} comp {1} ver{2}", sysid, compid, mavlinkversion);
break;
}
}
countDown.Stop();
// if (Progress != null)
// Progress(-1, "Getting Params.. (sysid " + sysid + " compid " + compid + ") ");
frmProgressReporter.UpdateProgressAndStatus(0, "Getting Params.. (sysid " + sysid + " compid " + compid + ") ");
if (getparams)
getParamListBG();
if (frmProgressReporter.doWorkArgs.CancelAcknowledged == true)
{
MainV2.giveComport = false;
if (BaseStream.IsOpen)
BaseStream.Close();
return;
}
}
catch (Exception e)
{
try
{
BaseStream.Close();
}
catch { }
MainV2.giveComport = false;
// if (Progress != null)
// Progress(-1, "Connect Failed\n" + e.Message);
if (string.IsNullOrEmpty(progressWorkerEventArgs.ErrorMessage))
progressWorkerEventArgs.ErrorMessage = "Connect Failed";
throw e;
}
//frmProgressReporter.Close();
MainV2.giveComport = false;
frmProgressReporter.UpdateProgressAndStatus(100, "Done.");
log.Info("Done open " + sysid + " " + compid);
packetslost = 0;
synclost = 0;
}
