public byte[] GetHeartBeat() { DateTime start = DateTime.Now; int readcount = 0; while (true) { byte[] buffer = ReadPacket(); readcount++; if (buffer.Length > 5) { if (buffer[5] == (byte)MAVLINK_MSG_ID.HEARTBEAT) { mavlink_heartbeat_t hb = buffer.ByteArrayToStructure <mavlink_heartbeat_t>(6); if (hb.type != (byte)MAVLink.MAV_TYPE.GCS) { return(buffer); } } } if (DateTime.Now > start.AddMilliseconds(2200) || readcount > 200) { return(new byte[0]); } } }
void SetupMavConnect(MAVLinkMessage message, mavlink_heartbeat_t hb) { sysidcurrent = message.sysid; compidcurrent = message.compid; mavlinkversion = hb.mavlink_version; MAV.aptype = (MAV_TYPE) hb.type; MAV.apname = (MAV_AUTOPILOT) hb.autopilot; setAPType(message.sysid, message.compid); MAV.sysid = message.sysid; MAV.compid = message.compid; MAV.recvpacketcount = message.seq; log.InfoFormat("ID sys {0} comp {1} ver{2} type {3} name {4}", MAV.sysid, MAV.compid, mavlinkversion, MAV.aptype.ToString(), MAV.apname.ToString()); }
void SetupMavConnect(byte sysid, byte compid,byte mgsno, mavlink_heartbeat_t hb) { sysidcurrent = sysid; mavlinkversion = hb.mavlink_version; MAV.aptype = (MAV_TYPE)hb.type; MAV.apname = (MAV_AUTOPILOT)hb.autopilot; setAPType(sysid); MAV.sysid = sysid; MAV.compid = compid; MAV.recvpacketcount = mgsno; log.InfoFormat("ID sys {0} comp {1} ver{2}", MAV.sysid, MAV.compid, mavlinkversion); }
public byte[] ReadPacket() { byte[] buffer = new byte[260]; int count = 0; int length = 0; int readcount = 0; lastbad = new byte[2]; BaseStream.ReadTimeout = 1200; // 1200 ms between chars - the gps detection requires this. DateTime start = DateTime.Now; lock (readlock) { while (BaseStream.IsOpen) { try { if (readcount > 300) { //No valid mavlink package break; } readcount++; //MAV.cs.datetime = DateTime.Now; DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout); // Console.WriteLine(DateTime.Now.Millisecond + " SR1a " + BaseStream.BytesToRead); while (BaseStream.IsOpen && BaseStream.BytesToRead <= 0) { if (DateTime.Now > to) { //log.InfoFormat("MAVLINK: 1 wait time out btr {0} len {1}", BaseStream.BytesToRead, length); throw new Exception("Timeout"); } System.Threading.Thread.Sleep(1); //Console.WriteLine(DateTime.Now.Millisecond + " SR0b " + BaseStream.BytesToRead); } if (BaseStream.IsOpen) { BaseStream.Read(buffer, count, 1); } } catch (Exception e) { break; } // check if looks like a mavlink packet and check for exclusions and write to console if (buffer[0] != 254) { if (buffer[0] >= 0x20 && buffer[0] <= 127 || buffer[0] == '\n' || buffer[0] == '\r') { // check for line termination if (buffer[0] == '\r' || buffer[0] == '\n') { // check new line is valid if (buildplaintxtline.Length > 3) { plaintxtline = buildplaintxtline; } // reset for next line buildplaintxtline = ""; } form.UpdateTextBox(((char)buffer[0]).ToString()); //TCPConsole.Write(buffer[0]); //Console.Write((char)buffer[0]); buildplaintxtline += (char)buffer[0]; } _bytesReceivedSubj.OnNext(1); count = 0; lastbad[0] = lastbad[1]; lastbad[1] = buffer[0]; buffer[1] = 0; continue; } // reset count on valid packet readcount = 0; // check for a header if (buffer[0] == 254) { if (count == 0) { DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout); while (BaseStream.IsOpen && BaseStream.BytesToRead < 5) { if (DateTime.Now > to) { throw new Exception("Timeout"); } System.Threading.Thread.Sleep(1); } int read = BaseStream.Read(buffer, 1, 5); count = read; } // packet length length = buffer[1] + 6 + 2 - 2; // data + header + checksum - U - length if (count >= 5) { if (MAV.sysid != 0) { if (MAV.sysid != buffer[3] || MAV.compid != buffer[4]) { if (buffer[3] == '3' && buffer[4] == 'D') { // this is a 3dr radio rssi packet } else { //log.InfoFormat("Mavlink Bad Packet (not addressed to this MAV) got {0} {1} vs {2} {3}", buffer[3], buffer[4], MAV.sysid, MAV.compid); return(new byte[0]); } } } try { DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout); while (BaseStream.IsOpen && BaseStream.BytesToRead < (length - 4)) { if (DateTime.Now > to) { //log.InfoFormat("MAVLINK: 3 wait time out btr {0} len {1}", BaseStream.BytesToRead, length); break; } System.Threading.Thread.Sleep(1); } if (BaseStream.IsOpen) { int read = BaseStream.Read(buffer, 6, length - 4); } count = length + 2; } catch { break; } break; } } count++; if (count == 299) { break; } } } // end readlock Array.Resize <byte>(ref buffer, count); _bytesReceivedSubj.OnNext(buffer.Length); if (packetlosttimer.AddSeconds(5) < DateTime.Now) { packetlosttimer = DateTime.Now; packetslost = (packetslost * 0.8f); packetsnotlost = (packetsnotlost * 0.8f); } if (bpstime.Second != DateTime.Now.Second && BaseStream.IsOpen) { Console.Write("bps {0} loss {1} left {2} mem {3} \n", bps1, synclost, BaseStream.BytesToRead, System.GC.GetTotalMemory(false) / 1024 / 1024.0); bps2 = bps1; // prev sec bps1 = 0; // current sec bpstime = DateTime.Now; } bps1 += buffer.Length; bps = (bps1 + bps2) / 2; ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2); if (buffer.Length > 5 && buffer[0] == 254) { crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[buffer[5]], crc); } if (buffer.Length > 5 && buffer[1] != MAVLINK_MESSAGE_LENGTHS[buffer[5]]) { if (MAVLINK_MESSAGE_LENGTHS[buffer[5]] == 0) // pass for unknown packets { } else { //log.InfoFormat("Mavlink Bad Packet (Len Fail) len {0} pkno {1}", buffer.Length, buffer[5]); if (buffer.Length == 11 && buffer[0] == 'U' && buffer[5] == 0) { string message = "Mavlink 0.9 Heartbeat, Please upgrade your AP, This planner is for Mavlink 1.0\n\n"; MessageBox.Show(message); throw new Exception(message); } return(new byte[0]); } } if (buffer.Length < 5 || buffer[buffer.Length - 1] != (crc >> 8) || buffer[buffer.Length - 2] != (crc & 0xff)) { int packetno = -1; if (buffer.Length > 5) { packetno = buffer[5]; } if (packetno != -1 && buffer.Length > 5 && MAVLINK_MESSAGE_INFO[packetno] != null) { //log.InfoFormat("Mavlink Bad Packet (crc fail) len {0} crc {1} vs {4} pkno {2} {3}", buffer.Length, crc, packetno, MAVLINK_MESSAGE_INFO[packetno].ToString(), BitConverter.ToUInt16(buffer, buffer.Length - 2)); form.UpdateTextBox("Mavlink Bad Packet."); } return(new byte[0]); } try { if ((buffer[0] == 'U' || buffer[0] == 254) && buffer.Length >= buffer[1]) { if (buffer[3] == '3' && buffer[4] == 'D') { // Do nothing } else { byte packetSeqNo = buffer[2]; int expectedPacketSeqNo = ((MAV.recvpacketcount + 1) % 0x100); if (packetSeqNo != expectedPacketSeqNo) { synclost++; // actualy sync loss's int numLost = 0; if (packetSeqNo < ((MAV.recvpacketcount + 1))) // recvpacketcount = 255 then 10 < 256 = true if was % 0x100 this would fail { numLost = 0x100 - expectedPacketSeqNo + packetSeqNo; } else { numLost = packetSeqNo - MAV.recvpacketcount; } packetslost += numLost; WhenPacketLost.OnNext(numLost); //log.InfoFormat("lost pkts new seqno {0} pkts lost {1}", packetSeqNo, numLost); } packetsnotlost++; MAV.recvpacketcount = packetSeqNo; WhenPacketReceived.OnNext(1); } if (double.IsInfinity(packetspersecond[buffer[5]])) { packetspersecond[buffer[5]] = 0; } packetspersecond[buffer[5]] = (((1000 / ((DateTime.Now - packetspersecondbuild[buffer[5]]).TotalMilliseconds) + packetspersecond[buffer[5]]) / 2)); packetspersecondbuild[buffer[5]] = DateTime.Now; lock (writelock) { MAV.packets[buffer[5]] = buffer; MAV.packetseencount[buffer[5]]++; } if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT) // status text { var msg = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT].ByteArrayToStructure <MAVLink.mavlink_statustext_t>(6); byte sev = msg.severity; string logdata = Encoding.ASCII.GetString(msg.text); int ind = logdata.IndexOf('\0'); if (ind != -1) { logdata = logdata.Substring(0, ind); } //log.Info(DateTime.Now + " " + logdata); form.UpdateTextBox(logdata); //MAV.cs.messages.Add(logdata); if (sev >= 3) { } } // set ap type if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT) { 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(); } } } } catch { } if (buffer[3] == '3' && buffer[4] == 'D') { // dont update last packet time for 3dr radio packets } else { lastvalidpacket = DateTime.Now; } return(buffer); }
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; }