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