public void OnMavMessageData(MAVLinkMessage message) { if (MavMessageHandler != null) { MavMessageHandler(this, new MavMessageEventArgs(message)); } }
private bool messageReceived(MAVLinkMessage arg) { //accept any compid, but filter sysid if (arg.sysid != _parent.sysid) { return(true); } if (arg.msgid == (uint)MAVLINK_MSG_ID.DISTANCE_SENSOR) { var dist = arg.ToStructure <mavlink_distance_sensor_t>(); if (dist.current_distance >= dist.max_distance) { return(true); } if (dist.current_distance <= dist.min_distance) { return(true); } _dS.Add((MAV_SENSOR_ORIENTATION)dist.orientation, dist.current_distance, DateTime.Now, 1); } return(true); }
public void addPacket(MAVLinkMessage msg) { lock (packetslock) { packets[msg.msgid] = msg; } }
protected void Complete(MAVLinkMessage msg) { if (Sessions.TryGetValue(msg.msgid, out var tcs)) { Debug.WriteLine("hit " + msg.msgid); tcs.CompletionSource.SetResult(msg); } else { Debug.WriteLine("miss " + msg.msgid + " " + Sessions.Count); if (Sessions.Count > 0) { Debug.WriteLine("miss and first key > " + Sessions.First().Key + " " + msg.msgid + " " + (Sessions.First().Key == msg.msgid).ToString()); } } }
public void addPacket(MAVLinkMessage msg) { lock (packetslock) { // create queue if it does not exist if (!packets.ContainsKey(msg.msgid)) { packets[msg.msgid] = new Queue <MAVLinkMessage>(); } // cleanup queue if not polling this message while (packets[msg.msgid].Count > 5) { packets[msg.msgid].Dequeue(); } packets[msg.msgid].Enqueue(msg); } }
private bool messageReceived(MAVLinkMessage arg) { //accept any compid, but filter sysid if (arg.sysid != _parent.sysid) { return(true); } if (arg.msgid == (uint)MAVLINK_MSG_ID.DISTANCE_SENSOR) { var dist = arg.ToStructure <mavlink_distance_sensor_t>(); if (dist.current_distance >= dist.max_distance) { return(true); } if (dist.current_distance <= dist.min_distance) { return(true); } _dS.Add((MAV_SENSOR_ORIENTATION)dist.orientation, dist.current_distance, DateTime.Now, 3); //Console.WriteLine("DIST " + (MAV_SENSOR_ORIENTATION)dist.orientation + " " + dist.current_distance); if (temp.IsHandleCreated) { //temp.Text = arg.sysid.ToString(); temp.Invalidate(); //temp.Refresh(); } else { if (!temp.IsDisposed) { MainV2.instance.Invoke((MethodInvoker) delegate { temp.Show(); }); } } } return(true); }
public async Task <InvokeResult <TMavlinkPacket> > WaitForMessageAsync <TMavlinkPacket>(MAVLINK_MSG_ID messageId, TimeSpan timeout) where TMavlinkPacket : struct { try { Debug.WriteLine("Start Waiting" + DateTime.Now.ToString()); var wor = new WaitOnRequest <object>((uint)messageId); Sessions[(uint)messageId] = wor; MAVLinkMessage message = null; for (var idx = 0; (idx < timeout.TotalMilliseconds / 100) && message == null; ++idx) { if (wor.CompletionSource.Task.IsCompleted) { Debug.WriteLine("TASK IS COMPLETED!!!!"); message = wor.CompletionSource.Task.Result as MAVLinkMessage; } await Task.Delay(100); } Debug.WriteLine("End Waiting" + DateTime.Now.ToString()); if (message == null) { return(InvokeResult <TMavlinkPacket> .FromError("Timeout waiting for message.")); } else { var result = MavlinkUtil.ByteArrayToStructure <TMavlinkPacket>(message.buffer); return(InvokeResult <TMavlinkPacket> .Create(result)); } } catch (Exception ex) { return(InvokeResult <TMavlinkPacket> .FromException("AsyncCoupler_WaitOnAsync", ex)); } finally { Sessions.TryRemove((uint)messageId, out WaitOnRequest <Object> obj); } }
public object DebugPacket(MAVLinkMessage datin, bool PrintToConsole) { string text = ""; return DebugPacket(datin, ref text, PrintToConsole); }
public MAVLinkMessage ReadPacket(Stream BaseStream) { byte[] buffer = new byte[MAVLink.MAVLINK_MAX_PACKET_LEN]; DateTime packettime = DateTime.MinValue; if (hasTimestamp) { byte[] datearray = new byte[8]; int tem = 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); if ((dateint / 1000 / 1000 / 60 / 60) < 9999999) { date1 = date1.AddMilliseconds(dateint / 1000); packettime = date1.ToLocalTime(); } } int readcount = 0; while (readcount <= MAVLink.MAVLINK_MAX_PACKET_LEN) { // read STX byte ReadWithTimeout(BaseStream, buffer, 0, 1); if (buffer[0] == MAVLink.MAVLINK_STX || buffer[0] == MAVLINK_STX_MAVLINK1) { break; } readcount++; } if (readcount >= MAVLink.MAVLINK_MAX_PACKET_LEN) { return(null); throw new InvalidDataException("No header found in data"); } var headerlength = buffer[0] == MAVLINK_STX ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN; var headerlengthstx = headerlength + 1; // read header try { ReadWithTimeout(BaseStream, buffer, 1, headerlength); } catch (EndOfStreamException) { return(null); } // packet length int lengthtoread = 0; if (buffer[0] == MAVLINK_STX) { lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - magic - length if ((buffer[2] & MAVLINK_IFLAG_SIGNED) > 0) { lengthtoread += MAVLINK_SIGNATURE_BLOCK_LEN; } } else { lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - U - length } try { //read rest of packet ReadWithTimeout(BaseStream, buffer, headerlengthstx, lengthtoread - (headerlengthstx - 2)); } catch (EndOfStreamException) { return(null); } // resize the packet to the correct length Array.Resize <byte>(ref buffer, lengthtoread + 2); MAVLinkMessage message = new MAVLinkMessage(buffer, packettime); // calc crc ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2); // calc extra bit of crc for mavlink 1.0+ if (message.header == MAVLINK_STX || message.header == MAVLINK_STX_MAVLINK1) { crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo(message.msgid).crc, crc); } // check crc if ((message.crc16 >> 8) != (crc >> 8) || (message.crc16 & 0xff) != (crc & 0xff)) { badCRC++; // crc fail return(null); } return(message); }
public MAVLinkMessage ReadPacket(byte[] buffer) { int readcount = 0; while (readcount <= MAVLink.MAVLINK_MAX_PACKET_LEN) { // read STX byte if (buffer[0] == MAVLink.MAVLINK_STX || buffer[0] == MAVLINK_STX_MAVLINK1) { break; } readcount++; } if (readcount >= MAVLink.MAVLINK_MAX_PACKET_LEN) { throw new InvalidDataException("No header found in data"); } var headerlength = buffer[0] == MAVLINK_STX ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN; var headerlengthstx = headerlength + 1; // read header // packet length int lengthtoread = 0; if (buffer[0] == MAVLINK_STX) { lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - magic - length if ((buffer[2] & MAVLINK_IFLAG_SIGNED) > 0) { lengthtoread += MAVLINK_SIGNATURE_BLOCK_LEN; } } else { lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - U - length } //read rest of packet // resize the packet to the correct length Array.Resize <byte>(ref buffer, lengthtoread + 2); MAVLinkMessage message = new MAVLinkMessage(buffer); // calc crc ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2); // calc extra bit of crc for mavlink 1.0+ if (message.header == MAVLINK_STX || message.header == MAVLINK_STX_MAVLINK1) { crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo(message.msgid).crc, crc); } // check crc if ((message.crc16 >> 8) != (crc >> 8) || (message.crc16 & 0xff) != (crc & 0xff)) { badCRC++; // crc fail return(null); } return(message); }
public MAVLinkMessage ReadPacket(Stream BaseStream) { byte[] buffer = new byte[270]; int readcount = 0; while (readcount < 200) { // read STX byte ReadWithTimeout(BaseStream, buffer, 0, 1); if (buffer[0] == MAVLink.MAVLINK_STX || buffer[0] == MAVLINK_STX_MAVLINK1) { break; } readcount++; } var headerlength = buffer[0] == MAVLINK_STX ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN; var headerlengthstx = headerlength + 1; // read header ReadWithTimeout(BaseStream, buffer, 1, headerlength); // packet length int lengthtoread = 0; if (buffer[0] == MAVLINK_STX) { lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - magic - length if ((buffer[2] & MAVLINK_IFLAG_SIGNED) > 0) { lengthtoread += MAVLINK_SIGNATURE_BLOCK_LEN; } } else { lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - U - length } //read rest of packet ReadWithTimeout(BaseStream, buffer, 6, lengthtoread - (headerlengthstx - 2)); // resize the packet to the correct length Array.Resize <byte>(ref buffer, lengthtoread + 2); MAVLinkMessage message = new MAVLinkMessage(buffer); // calc crc ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2); // calc extra bit of crc for mavlink 1.0+ if (message.header == MAVLINK_STX || message.header == MAVLINK_STX_MAVLINK1) { crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo(message.msgid).crc, crc); } // check crc if ((message.crc16 >> 8) != (crc >> 8) || (message.crc16 & 0xff) != (crc & 0xff)) { badCRC++; // crc fail return(null); } return(message); }
MAVLinkMessage readlogPacketMavlink() { byte[] datearray = new byte[8]; bool missingtimestamp = false; if (logplaybackfile.BaseStream is FileStream) { if (((FileStream) _logplaybackfile.BaseStream).Name.ToLower().EndsWith(".rlog")) missingtimestamp = true; } if (!missingtimestamp) { int tem = 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); try { // array is reversed above if (datearray[7] == 254 || datearray[7] == 253) { //rewind 8bytes logplaybackfile.BaseStream.Seek(-8, SeekOrigin.Current); } else { if ((dateint/1000/1000/60/60) < 9999999) { date1 = date1.AddMilliseconds(dateint/1000); lastlogread = date1.ToLocalTime(); } } } catch { } } byte[] temp = new byte[0]; byte byte0 = 0; byte byte1 = 0; byte byte2 = 0; var filelength = logplaybackfile.BaseStream.Length; var filepos = logplaybackfile.BaseStream.Position; if(filelength == filepos) return MAVLinkMessage.Invalid; int length = 5; int a = 0; while (a < length) { if (filelength == filepos) return MAVLinkMessage.Invalid; var tempb = (byte) logplaybackfile.ReadByte(); filepos++; switch (a) { case 0: byte0 = tempb; if (byte0 != 'U' && byte0 != 254 && byte0 != 253) { log.DebugFormat("logread - lost sync byte {0} pos {1}", byte0, logplaybackfile.BaseStream.Position); // seek to next valid do { byte0 = logplaybackfile.ReadByte(); } while (byte0 != 'U' && byte0 != 254 && byte0 != 253); a = 1; continue; } break; case 1: byte1 = tempb; // handle length { int headerlength = byte0 == MAVLINK_STX ? 9 : 5; int headerlengthstx = headerlength + 1; length = byte1 + headerlengthstx + 2; // header + 2 checksum } break; case 2: byte2 = tempb; // handle signing and mavlink2 if (byte0 == MAVLINK_STX) { if ((byte2 & MAVLINK_IFLAG_SIGNED) > 0) length += MAVLINK_SIGNATURE_BLOCK_LEN; } // handle rest { temp = new byte[length]; temp[0] = byte0; temp[1] = byte1; temp[2] = byte2; var readto = a + 1; var readlength = length - (a + 1); logplaybackfile.Read(temp, readto, readlength); a = length; } break; } a++; } MAVLinkMessage tmp = new MAVLinkMessage(temp); MAVlist[tmp.sysid, tmp.compid].cs.datetime = lastlogread; return tmp; }
/// <summary> /// Used to extract mission from log file - both sent or received /// </summary> /// <param name="buffer">packet</param> void getWPsfromstream(ref MAVLinkMessage buffer, byte sysid, byte compid) { if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_COUNT) { // clear old mavlink_mission_count_t wp = buffer.ToStructure<mavlink_mission_count_t>(); if (wp.target_system == gcssysid) { wp.target_system = sysid; wp.target_component = compid; } MAVlist[wp.target_system, wp.target_component].wps.Clear(); } if (buffer.msgid == (byte)MAVLink.MAVLINK_MSG_ID.MISSION_ITEM) { mavlink_mission_item_t wp = buffer.ToStructure<mavlink_mission_item_t>(); if (wp.target_system == gcssysid) { wp.target_system = sysid; wp.target_component = compid; } if (wp.current == 2) { // guide mode wp MAVlist[wp.target_system, wp.target_component].GuidedMode = wp; } else { MAVlist[wp.target_system, wp.target_component].wps[wp.seq] = wp; } Console.WriteLine("WP # {7} cmd {8} p1 {0} p2 {1} p3 {2} p4 {3} x {4} y {5} z {6}", wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y, wp.z, wp.seq, wp.command); } if (buffer.msgid == (byte) MAVLINK_MSG_ID.RALLY_POINT) { mavlink_rally_point_t rallypt = buffer.ToStructure<mavlink_rally_point_t>(); if (rallypt.target_system == gcssysid) { rallypt.target_system = sysid; rallypt.target_component = compid; } MAVlist[rallypt.target_system, rallypt.target_component].rallypoints[rallypt.idx] = rallypt; Console.WriteLine("RP # {0} {1} {2} {3} {4}", rallypt.idx, rallypt.lat, rallypt.lng, rallypt.alt, rallypt.break_alt); } if (buffer.msgid == (byte)MAVLINK_MSG_ID.CAMERA_FEEDBACK) { mavlink_camera_feedback_t camerapt = buffer.ToStructure<mavlink_camera_feedback_t>(); if (MAVlist[sysid, compid].camerapoints.Count == 0 || MAVlist[sysid, compid].camerapoints.Last().time_usec != camerapt.time_usec) { MAVlist[sysid, compid].camerapoints.Add(camerapt); } } if (buffer.msgid == (byte) MAVLINK_MSG_ID.FENCE_POINT) { mavlink_fence_point_t fencept = buffer.ToStructure<mavlink_fence_point_t>(); if (fencept.target_system == gcssysid) { fencept.target_system = sysid; fencept.target_component = compid; } MAVlist[fencept.target_system, fencept.target_component].fencepoints[fencept.idx] = fencept; } if (buffer.msgid == (byte) MAVLINK_MSG_ID.PARAM_VALUE) { mavlink_param_value_t value = buffer.ToStructure<mavlink_param_value_t>(); string st = System.Text.ASCIIEncoding.ASCII.GetString(value.param_id); int pos = st.IndexOf('\0'); if (pos != -1) { st = st.Substring(0, pos); } if (MAV.apname == MAV_AUTOPILOT.ARDUPILOTMEGA) { MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, value.param_value, MAV_PARAM_TYPE.REAL32, (MAV_PARAM_TYPE)value.param_type); } else { MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, value.param_value, (MAV_PARAM_TYPE) value.param_type); } } }
/// <summary> /// Serial Reader to read mavlink packets. POLL method /// </summary> /// <returns></returns> public MAVLinkMessage readPacket() { byte[] buffer = new byte[MAVLINK_MAX_PACKET_LEN + 25]; int count = 0; int length = 0; int readcount = 0; BaseStream.ReadTimeout = 1200; // 1200 ms between chars - the gps detection requires this. DateTime start = DateTime.Now; //Console.WriteLine(DateTime.Now.Millisecond + " SR0 " + BaseStream.BytesToRead); lock (readlock) { lastbad = new byte[2]; //Console.WriteLine(DateTime.Now.Millisecond + " SR1 " + BaseStream.BytesToRead); while (BaseStream.IsOpen || logreadmode) { try { if (readcount > 300) { log.Info("MAVLink readpacket No valid mavlink packets"); break; } readcount++; if (logreadmode) { buffer = readlogPacketMavlink().buffer; if (buffer.Length == 0) return new MAVLinkMessage(); } else { // time updated for internal reference 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 TimeoutException("Timeout"); } System.Threading.Thread.Sleep(1); //Console.WriteLine(DateTime.Now.Millisecond + " SR0b " + BaseStream.BytesToRead); } //Console.WriteLine(DateTime.Now.Millisecond + " SR1a " + BaseStream.BytesToRead); if (BaseStream.IsOpen) { BaseStream.Read(buffer, count, 1); if (rawlogfile != null && rawlogfile.CanWrite) rawlogfile.WriteByte(buffer[count]); } //Console.WriteLine(DateTime.Now.Millisecond + " SR1b " + BaseStream.BytesToRead); } } catch (Exception e) { log.Info("MAVLink readpacket read error: " + e.ToString()); break; } // check if looks like a mavlink packet and check for exclusions and write to console if (buffer[0] != 0xfe && buffer[0] != 'U' && buffer[0] != 0xfd) { 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 = ""; } 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; //Console.WriteLine(DateTime.Now.Millisecond + " SR2 " + BaseStream.BytesToRead); mavlinkv2 = buffer[0] == MAVLINK_STX ? true : false; int headerlength = mavlinkv2 ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN; int headerlengthstx = headerlength + 1; // check for a header if (buffer[0] == 0xfe || buffer[0] == 0xfd || buffer[0] == 'U') { // if we have the header, and no other chars, get the length and packet identifiers if (count == 0 && !logreadmode) { DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout); while (BaseStream.IsOpen && BaseStream.BytesToRead < headerlength) { if (DateTime.Now > to) { log.InfoFormat("MAVLINK: 2 wait time out btr {0} len {1}", BaseStream.BytesToRead, length); throw new Exception("Timeout"); } System.Threading.Thread.Sleep(1); } int read = BaseStream.Read(buffer, 1, headerlength); count = read; if (rawlogfile != null && rawlogfile.CanWrite) rawlogfile.Write(buffer, 1, read); } // packet length if (buffer[0] == 0xfd) { length = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - magic - length if ((buffer[2] & MAVLINK_IFLAG_SIGNED) > 0) { length += MAVLINK_SIGNATURE_BLOCK_LEN; } } else { length = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - U - length } if (count >= headerlength || logreadmode) { try { if (logreadmode) { } else { DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout); while (BaseStream.IsOpen && BaseStream.BytesToRead < (length - count)) { 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, headerlengthstx, length - (headerlengthstx-2)); if (rawlogfile != null && rawlogfile.CanWrite) { // write only what we read, temp is the whole packet, so 6-end rawlogfile.Write(buffer, headerlengthstx, read); } } } count = length + 2; } catch { break; } break; } } count++; if (count == 299) break; } //Console.WriteLine(DateTime.Now.Millisecond + " SR3 " + BaseStream.BytesToRead); } // end readlock // resize the packet to the correct length Array.Resize<byte>(ref buffer, count); // add byte count _bytesReceivedSubj.OnNext(buffer.Length); // update bps statistics if (bpstime.Second != DateTime.Now.Second) { long btr = 0; if (BaseStream != null && BaseStream.IsOpen) { btr = BaseStream.BytesToRead; } else if (logreadmode) { btr = logplaybackfile.BaseStream.Length - logplaybackfile.BaseStream.Position; } Console.Write("bps {0} loss {1} left {2} mem {3} \n", bps1, MAV.synclost, btr, System.GC.GetTotalMemory(false)/1024/1024.0); bps2 = bps1; // prev sec bps1 = 0; // current sec bpstime = DateTime.Now; } bps1 += buffer.Length; if (buffer.Length == 0) return new MAVLinkMessage(); MAVLinkMessage message = new MAVLinkMessage(buffer); uint msgid = message.msgid; message_info msginfo; msginfo = MAVLINK_MESSAGE_INFOS.SingleOrDefault(p => p.msgid == msgid); // calc crc var sigsize = (message.sig != null) ? MAVLINK_SIGNATURE_BLOCK_LEN : 0; ushort crc = MavlinkCRC.crc_calculate(buffer, message.Length - sigsize-MAVLINK_NUM_CHECKSUM_BYTES); // calc extra bit of crc for mavlink 1.0/2.0 if (message.header == 0xfe || message.header == 0xfd) { crc = MavlinkCRC.crc_accumulate(msginfo.crc, crc); } // check message length vs table if (message.payloadlength != msginfo.length) { if (msginfo.length == 0) // pass for unknown packets { log.InfoFormat("unknown packet type {0}", message.msgid); } else { log.InfoFormat("Mavlink Bad Packet (Len Fail) len {0} pkno {1}", buffer.Length, message.msgid); if (buffer.Length == 11 && message.header == 'U' && message.msgid == 0) // check for 0.9 hb packet { string messagehb = "Mavlink 0.9 Heartbeat, Please upgrade your AP, This planner is for Mavlink 1.0\n\n"; Console.WriteLine(messagehb); if (logreadmode) logplaybackfile.BaseStream.Seek(0, SeekOrigin.End); throw new Exception(messagehb); } return new MAVLinkMessage(); } } // check crc if ((message.crc16 >> 8) != (crc >> 8) || (message.crc16 & 0xff) != (crc & 0xff)) { if (message.msgid != -1 && buffer.Length > 5 && msginfo.name != null) log.InfoFormat("Mavlink Bad Packet (crc fail) len {0} crc {1} vs {4} pkno {2} {3}", buffer.Length, crc, message.msgid, msginfo.name.ToString(), message.crc16); if (logreadmode) log.InfoFormat("bad packet pos {0} ", logplaybackfile.BaseStream.Position); return new MAVLinkMessage(); } byte sysid = message.sysid; byte compid = message.compid; byte packetSeqNo = message.seq; //check if sig was included in packet, and we are not ignoring the signature (signing isnt checked else we wont enable signing) if (message.sig != null && !MAVlist[sysid,compid].signingignore) { using (SHA256Managed signit = new SHA256Managed()) { signit.TransformBlock(signingKey, 0, signingKey.Length, null, 0); signit.TransformFinalBlock(message.buffer, 0, message.Length - MAVLINK_SIGNATURE_BLOCK_LEN + 7); var ctx = signit.Hash; // trim to 48 Array.Resize(ref ctx, 6); //Console.WriteLine("linkid {0}, time {1} {2} {3} {4} {5} {6} - {7}", message.sig[0], message.sig[1], message.sig[2], message.sig[3], message.sig[4], message.sig[5], message.sig[6], message.sigTimestamp); for (int i = 0; i < ctx.Length; i++) { if (ctx[i] != message.sig[7 + i]) { log.InfoFormat("Packet failed signature but passed crc"); return new MAVLinkMessage(); } } MAVlist[sysid, compid].linkid = message.sig[0]; enableSigning(); } } // packet is now verified // update packet loss statistics if (!logreadmode && MAVlist[sysid, compid].packetlosttimer.AddSeconds(5) < DateTime.Now) { MAVlist[sysid, compid].packetlosttimer = DateTime.Now; MAVlist[sysid, compid].packetslost = (MAVlist[sysid, compid].packetslost*0.8f); MAVlist[sysid, compid].packetsnotlost = (MAVlist[sysid, compid].packetsnotlost*0.8f); } else if (logreadmode && MAVlist[sysid, compid].packetlosttimer.AddSeconds(5) < lastlogread) { MAVlist[sysid, compid].packetlosttimer = lastlogread; MAVlist[sysid, compid].packetslost = (MAVlist[sysid, compid].packetslost*0.8f); MAVlist[sysid, compid].packetsnotlost = (MAVlist[sysid, compid].packetsnotlost*0.8f); } // extract wp's from stream if (buffer.Length >= 5) { getWPsfromstream(ref message, sysid, compid); } // if its a gcs packet - dont process further if (buffer.Length >= 5 && (sysid == 255 || sysid == 253) && logreadmode) // gcs packet { return message; } // create a state for any sysid/compid if (!MAVlist.Contains(sysid, compid)) { // create an item - hidden MAVlist.AddHiddenList(sysid, compid); } try { if ((message.header == 'U' || message.header == 0xfe || message.header == 0xfd) && buffer.Length >= message.payloadlength) { // check if we lost pacakets based on seqno int expectedPacketSeqNo = ((MAVlist[sysid, compid].recvpacketcount + 1)%0x100); { // the seconds part is to work around a 3dr radio bug sending dup seqno's if (packetSeqNo != expectedPacketSeqNo && packetSeqNo != MAVlist[sysid, compid].recvpacketcount) { MAVlist[sysid, compid].synclost++; // actualy sync loss's int numLost = 0; if (packetSeqNo < ((MAVlist[sysid, compid].recvpacketcount + 1))) // recvpacketcount = 255 then 10 < 256 = true if was % 0x100 this would fail { numLost = 0x100 - expectedPacketSeqNo + packetSeqNo; } else { numLost = packetSeqNo - expectedPacketSeqNo; } MAVlist[sysid, compid].packetslost += numLost; WhenPacketLost.OnNext(numLost); log.InfoFormat("mav {2}-{4} seqno {0} exp {3} pkts lost {1}", packetSeqNo, numLost, sysid, expectedPacketSeqNo,compid); } MAVlist[sysid, compid].packetsnotlost++; //Console.WriteLine("{0} {1}", sysid, packetSeqNo); MAVlist[sysid, compid].recvpacketcount = packetSeqNo; } WhenPacketReceived.OnNext(1); // packet stats per mav if (double.IsInfinity(MAVlist[sysid, compid].packetspersecond[msgid])) MAVlist[sysid, compid].packetspersecond[msgid] = 0; MAVlist[sysid, compid].packetspersecond[msgid] = (((1000 / ((DateTime.Now - MAVlist[sysid, compid] .packetspersecondbuild[msgid]) .TotalMilliseconds) + MAVlist[sysid, compid].packetspersecond[ msgid]) / 2)); MAVlist[sysid, compid].packetspersecondbuild[msgid] = DateTime.Now; //Console.WriteLine("Packet {0}",temp[5]); // store packet history lock (objlock) { MAVlist[sysid, compid].packets[msgid] = message; // 3dr radio status packet are injected into the current mav if (msgid == (byte)MAVLink.MAVLINK_MSG_ID.RADIO_STATUS || msgid == (byte)MAVLink.MAVLINK_MSG_ID.RADIO) { MAVlist[sysidcurrent, compidcurrent].packets[msgid] = message; } // adsb packets are forwarded and can be from any sysid/compid if (msgid == (byte)MAVLink.MAVLINK_MSG_ID.ADSB_VEHICLE) { var adsb = message.ToStructure<MAVLink.mavlink_adsb_vehicle_t>(); MainV2.instance.adsbPlanes[adsb.ICAO_address.ToString("X5")] = new MissionPlanner.Utilities.adsb.PointLatLngAltHdg(adsb.lat / 1e7, adsb.lon / 1e7, adsb.altitude / 1000, adsb.heading * 0.01f, adsb.ICAO_address.ToString("X5"), DateTime.Now); } } // set seens sysid's based on hb packet - this will hide 3dr radio packets if (msgid == (byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT) { mavlink_heartbeat_t hb = message.ToStructure<mavlink_heartbeat_t>(); // not a gcs if (hb.type != (byte) MAV_TYPE.GCS) { // add a seen sysid if (!MAVlist.Contains(sysid, compid, false)) { // ensure its set from connect or log playback MAVlist.Create(sysid, compid); MAVlist[sysid, compid].aptype = (MAV_TYPE) hb.type; MAVlist[sysid, compid].apname = (MAV_AUTOPILOT) hb.autopilot; setAPType(sysid, compid); } // attach to the only remote device. / default to first device seen if (MAVlist.Count == 1) { sysidcurrent = sysid; compidcurrent = compid; } } } // only process for active mav if (sysidcurrent == sysid && compidcurrent == compid) PacketReceived(message); if (debugmavlink) DebugPacket(message); if (msgid == (byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT) // status text { var msg = message.ToStructure<MAVLink.mavlink_statustext_t>(); 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 + " " + sev + " " + logdata); MAVlist[sysid, compid].cs.messages.Add(logdata); bool printit = false; // the change of severity and the autopilot version where introduced at the same time, so any version non 0 can be used // copter 3.4+ // plane 3.4+ if (MAVlist[sysid, compid].cs.version.Major > 0 || MAVlist[sysid, compid].cs.version.Minor >= 4) { if (sev <= (byte) MAV_SEVERITY.WARNING) { printit = true; } } else { if (sev >= 3) { printit = true; } } if (logdata.StartsWith("Tuning:")) printit = true; if (printit) { MAVlist[sysid, compid].cs.messageHigh = logdata; MAVlist[sysid, compid].cs.messageHighTime = DateTime.Now; if (MainV2.speechEngine != null && MainV2.speechEngine.IsReady && Settings.Instance["speechenable"] != null && Settings.Instance["speechenable"].ToString() == "True") { MainV2.speechEngine.SpeakAsync(logdata); } } } if (lastparamset != DateTime.MinValue && lastparamset.AddSeconds(10) < DateTime.Now) { lastparamset = DateTime.MinValue; if (BaseStream.IsOpen) { doCommand(MAV_CMD.PREFLIGHT_STORAGE, 0, 0, 0, 0, 0, 0, 0); } } getWPsfromstream(ref message, sysid, compid); try { if (logfile != null && logfile.CanWrite && !logreadmode) { lock (logfile) { byte[] datearray = BitConverter.GetBytes( (UInt64) ((DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalMilliseconds*1000)); Array.Reverse(datearray); logfile.Write(datearray, 0, datearray.Length); logfile.Write(buffer, 0, buffer.Length); if (msgid == 0) { // flush on heartbeat - 1 seconds logfile.Flush(); rawlogfile.Flush(); } } } } catch (Exception ex) { log.Error(ex); } try { // full rw from mirror stream if (MirrorStream != null && MirrorStream.IsOpen) { MirrorStream.Write(buffer, 0, buffer.Length); while (MirrorStream.BytesToRead > 0) { byte[] buf = new byte[1024]; int len = MirrorStream.Read(buf, 0, buf.Length); if (MirrorStreamWrite) BaseStream.Write(buf, 0, len); } } } catch { } } } catch (Exception ex) { log.Error(ex); } // update last valid packet receive time MAVlist[sysid, compid].lastvalidpacket = DateTime.Now; return new MAVLinkMessage(buffer); }
MAVLinkMessage readlogPacketMavlink() { byte[] temp = new byte[300]; //byte[] datearray = BitConverter.GetBytes((ulong)(DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalMilliseconds); byte[] datearray = new byte[8]; int tem = 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); try { // array is reversed above if (datearray[7] == 254 || datearray[7] == 253) { //rewind 8bytes logplaybackfile.BaseStream.Seek(-8, SeekOrigin.Current); } else { if ((dateint/1000/1000/60/60) < 9999999) { date1 = date1.AddMilliseconds(dateint/1000); lastlogread = date1.ToLocalTime(); } } } catch { } int length = 5; int a = 0; while (a < length) { if (logplaybackfile.BaseStream.Position == logplaybackfile.BaseStream.Length) break; temp[a] = (byte) logplaybackfile.ReadByte(); if (temp[0] != 'U' && temp[0] != 254 && temp[0] != 253) { log.InfoFormat("logread - lost sync byte {0} pos {1}", temp[0], logplaybackfile.BaseStream.Position); a = 0; continue; } if (a == 1) { int headerlength = temp[0] == MAVLINK_STX ? 9 : 5; int headerlengthstx = headerlength + 1; length = temp[1] + headerlengthstx + 2; // header + 2 checksum } if (a == 2 && temp[0] == MAVLINK_STX) { if ((temp[a] & MAVLINK_IFLAG_SIGNED) > 0) length += MAVLINK_SIGNATURE_BLOCK_LEN; } a++; } MAVLinkMessage tmp = new MAVLinkMessage(temp); MAVlist[tmp.sysid, tmp.compid].cs.datetime = lastlogread; return tmp; }
public MavMessageEventArgs(MAVLinkMessage message) { Message = message; }
public string CollectPacket(byte[] data) { int dataIndex = 0; StringBuilder nonMavData = new StringBuilder(); while (dataIndex < data.Length) { byte dataByte = data[dataIndex++]; if (collectCount == 0) { // read STX byte collectBuffer[0] = dataByte; if (collectBuffer[0] == MAVLink.MAVLINK_STX || collectBuffer[0] == MAVLINK_STX_MAVLINK1) { collectCount = 1; collectGotHeader = false; } else { nonMavData.Append(Encoding.ASCII.GetString(new byte[] { dataByte })); } continue; } var headerlength = collectBuffer[0] == MAVLINK_STX ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN; var headerlengthstx = headerlength + 1; if (!collectGotHeader && collectCount < headerlengthstx) { // read header collectBuffer[collectCount++] = dataByte; if (collectCount >= headerlengthstx) { collectGotHeader = true; collectCount = 6; } continue; } // packet length int lengthToRead; if (collectBuffer[0] == MAVLINK_STX) { lengthToRead = collectBuffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - magic - length if ((collectBuffer[2] & MAVLINK_IFLAG_SIGNED) > 0) { lengthToRead += MAVLINK_SIGNATURE_BLOCK_LEN; } } else { lengthToRead = collectBuffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - U - length } //read rest of packet //ReadWithTimeout(BaseStream, buffer, 6, lengthtoread - (headerlengthstx - 2)); if (collectCount < lengthToRead - (headerlengthstx - 2) + 6) { collectBuffer[collectCount++] = dataByte; if (collectCount < lengthToRead - (headerlengthstx - 2) + 6) { continue; } } MAVLinkMessage message = new MAVLinkMessage(collectBuffer); // resize the packet to the correct length //Array.Resize<byte>(ref collectBuffer, lengthToRead + 2); // calc crc ushort crc = MavlinkCRC.crc_calculate(collectBuffer, collectCount - 2); // calc extra bit of crc for mavlink 1.0+ if ((message.header == MAVLINK_STX || message.header == MAVLINK_STX_MAVLINK1) && message.msgid < MAVLINK_MESSAGE_CRCS.Count) { crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[message.msgid], crc); } // check crc if ((message.crc16 >> 8) != (crc >> 8) || (message.crc16 & 0xff) != (crc & 0xff)) { badCRC++; OnMavError(MavError.BAD_CRC); } else { OnMavMessageData(message); } collectCount = 0; } return(nonMavData.ToString()); }
private bool messageReceived(MAVLinkMessage arg) { //accept any compid, but filter sysid if (arg.sysid != _parent.sysid) return true; if (arg.msgid == (uint)MAVLINK_MSG_ID.DISTANCE_SENSOR) { var dist = arg.ToStructure<mavlink_distance_sensor_t>(); if (dist.current_distance >= dist.max_distance) return true; if (dist.current_distance <= dist.min_distance) return true; _dS.Add(dist.id, (MAV_SENSOR_ORIENTATION)dist.orientation, dist.current_distance, DateTime.Now, 3); if (temp.IsHandleCreated) { temp.Invalidate(); } else { if (!temp.IsDisposed) { MainV2.instance.Invoke((MethodInvoker)delegate { temp.Show(); }); } } } return true; }
private void LoadFile(object sender, EventArgs e) { using (OpenFileDialog ofdFile = new OpenFileDialog()) { ofdFile.Filter = GAP.filemask; ofdFile.FilterIndex = 2; ofdFile.RestoreDirectory = true; if (ofdFile.ShowDialog() == System.Windows.Forms.DialogResult.OK) { GAP.AllTlogData.Clear(); foreach (string logfile in ofdFile.FileNames) { string extension = Path.GetExtension(logfile); if (String.Equals(extension.ToLower(), ".tlog")) { progressbar1.Visibility = Visibility.Visible; GAP.FilePath = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + Path.GetFileNameWithoutExtension(logfile) + ".csv"; GAP.FileName = Path.GetFileName(logfile); using (PacketExtractor mine = new PacketExtractor()) { try { mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)); } catch (Exception ex) { return; } mine.logreadmode = true; while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { int percent = (int) ((float)mine.logplaybackfile.BaseStream.Position / (float)mine.logplaybackfile.BaseStream.Length * 100.0f); if (progressbar1.Value != percent) { progressbar1.Dispatcher.Invoke(() => progressbar1.Value = percent, DispatcherPriority.Background); } MAVLinkMessage packet = mine.ReadPacket(); string text = ""; mine.DebugPacket(packet, ref text, false, ","); Parser.addMessage("* " + mine.lastlogread.ToString("yyyy-MM-dd HH:mm:ss.fff")); Parser.addMessage(text); } Parser.parseMessages(); ArrayList headers = Parser.GetHeaders(); foreach (string header in headers) { GlobalAppData.TlogData data = new GlobalAppData.TlogData(); data.HeaderName = header; data.Name = RM.GetString(header); GAP.AddTlogData(data); } progressbar1.Value = 100; EnableSettings(); FileLabel.Visibility = Visibility.Visible; mine.logreadmode = false; mine.logplaybackfile.Close(); mine.logplaybackfile = null; } } else if (String.Equals(extension.ToLower(), ".bin")) { DialogResult Result = System.Windows.Forms.MessageBox.Show("The .bin file does not display any data parameters." + Environment.NewLine + "We will be exporting the .bin file into a .csv file. Are you fine with that?", ".Bin Message", System.Windows.Forms.MessageBoxButtons.YesNo); if (Result == System.Windows.Forms.DialogResult.Yes) { try { using (BinaryReader br = new BinaryReader(new BufferedStream(File.OpenRead(logfile), 1024 * 1024))) { DateTime displaytimer = DateTime.MinValue; var length = br.BaseStream.Length; while (br.BaseStream.Position < length) { string data = BL.ReadMessage(br.BaseStream, length); BP.InputContent(data); } } BP.ParseBIN(); ArrayList OutputMessages = BP.GetMessages(); if (sw != null) { sw.Dispose(); } sw = new StreamWriter(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + Path.GetFileNameWithoutExtension(logfile) + ".csv"); foreach (string message in OutputMessages) { sw.WriteLine(message); } sw.Close(); System.Windows.Forms.MessageBox.Show("You have successfully converted " + Path.GetFileName(logfile) + " into a .csv file"); DisableSettings(); } catch { System.Windows.MessageBox.Show("You currently have the .csv open. Please close your .csv file and retry.", "Open File Error"); } } } else { System.Windows.MessageBox.Show("You've selected the wrong file type.", "Wrong File Type Error"); } } } } }
public MAVLinkMessage ReadPacket(Stream BaseStream) { byte[] buffer = new byte[270]; int readcount = 0; while (readcount < 200) { // read STX byte ReadWithTimeout(BaseStream, buffer, 0, 1); if (buffer[0] == MAVLink.MAVLINK_STX || buffer[0] == MAVLINK_STX_MAVLINK1) break; readcount++; } var headerlength = buffer[0] == MAVLINK_STX ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN; var headerlengthstx = headerlength + 1; // read header ReadWithTimeout(BaseStream, buffer, 1, headerlength); // packet length int lengthtoread = 0; if (buffer[0] == MAVLINK_STX) { lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - magic - length if ((buffer[2] & MAVLINK_IFLAG_SIGNED) > 0) { lengthtoread += MAVLINK_SIGNATURE_BLOCK_LEN; } } else { lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - U - length } //read rest of packet ReadWithTimeout(BaseStream, buffer, 6, lengthtoread - (headerlengthstx-2)); // resize the packet to the correct length Array.Resize<byte>(ref buffer, lengthtoread + 2); MAVLinkMessage message = new MAVLinkMessage(buffer); // calc crc ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2); // calc extra bit of crc for mavlink 1.0+ if (message.header == MAVLINK_STX || message.header == MAVLINK_STX_MAVLINK1) { crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo(message.msgid).crc, crc); } // check crc if ((message.crc16 >> 8) != (crc >> 8) || (message.crc16 & 0xff) != (crc & 0xff)) { badCRC++; // crc fail return null; } return message; }
public object DebugPacket(MAVLinkMessage datin, ref string text) { return DebugPacket(datin, ref text, true); }
/// <summary> /// Print entire decoded packet to console /// </summary> /// <param name="datin">packet byte array</param> /// <returns>struct of data</returns> public object DebugPacket(MAVLinkMessage datin, ref string text, bool PrintToConsole, string delimeter = " ") { string textoutput = ""; try { if (datin.Length > 5) { textoutput = string.Format( "{0,2:X}{8}{1,2:X}{8}{2,2:X}{8}{3,2:X}{8}{4,2:X}{8}{5,2:X}{8}{6,2:X}{8}{7,6:X}{8}", datin.header, datin.payloadlength, datin.incompat_flags, datin.compat_flags, datin.seq, datin.sysid, datin.compid, datin.msgid, delimeter); object data = datin.data; Type test = data.GetType(); if (PrintToConsole) { textoutput = textoutput + test.Name + delimeter; foreach (var field in test.GetFields()) { // field.Name has the field's name. object fieldValue = field.GetValue(data); // Get value if (field.FieldType.IsArray) { textoutput = textoutput + field.Name + delimeter; if (fieldValue.GetType() == typeof (byte[])) { try { byte[] crap = (byte[]) fieldValue; foreach (byte fiel in crap) { if (fiel == 0) { break; } else { textoutput = textoutput + (char) fiel; } } } catch { } } if (fieldValue.GetType() == typeof (short[])) { try { short[] crap = (short[]) fieldValue; foreach (short fiel in crap) { if (fiel == 0) { break; } else { textoutput = textoutput + Convert.ToString(fiel, 16) + "|"; } } } catch { } } textoutput = textoutput + delimeter; } else { textoutput = textoutput + field.Name + delimeter + fieldValue.ToString() + delimeter; } } var sig = ""; if (datin.sig != null) sig = Convert.ToBase64String(datin.sig); textoutput = textoutput + delimeter + "sig " + sig + delimeter + "Len" + delimeter + datin.Length + "\r\n"; if (PrintToConsole) Console.Write(textoutput); if (text != null) text = textoutput; } return data; } } catch { textoutput = textoutput + "\r\n"; } return null; }
private bool messageReceived(MAVLinkMessage arg) { //accept any compid, but filter sysid if (arg.sysid != _parent.sysid) { return(true); } if (arg.msgid == (uint)MAVLINK_MSG_ID.DISTANCE_SENSOR) { var dist = arg.ToStructure <mavlink_distance_sensor_t>(); if (dist.current_distance >= dist.max_distance) { return(true); } if (dist.current_distance <= dist.min_distance) { return(true); } _dS.Add(dist.id, (MAV_SENSOR_ORIENTATION)dist.orientation, dist.current_distance, DateTime.Now, 3); DataAvailable = true; } else if (arg.msgid == (uint)MAVLINK_MSG_ID.OBSTACLE_DISTANCE) { var dists = arg.ToStructure <mavlink_obstacle_distance_t>(); var inc = dists.increment == 0 ? dists.increment_f : dists.increment; var rangestart = dists.angle_offset; if (dists.frame == (byte)MAV_FRAME.BODY_FRD) { // no action } else if (dists.frame == (byte)MAV_FRAME.GLOBAL) { rangestart += _parent.cs.yaw; } var rangeend = rangestart + inc * dists.distances.Length; for (var a = 0; a < dists.distances.Length; a++) { // not used if (dists.distances[a] == ushort.MaxValue) { continue; } if (dists.distances[a] > dists.max_distance) { continue; } if (dists.distances[a] < dists.min_distance) { continue; } var dist = Math.Min(Math.Max(dists.distances[a], dists.min_distance), dists.max_distance); var angle = rangestart + inc * a; _dS.Add(dists.sensor_type, angle, inc, dist, DateTime.Now, 0.2); } DataAvailable = true; } return(true); }
private void PacketReceived(MAVLinkMessage buffer) { MAVLINK_MSG_ID type = (MAVLINK_MSG_ID) buffer.msgid; lock (Subscriptions) { foreach (var item in Subscriptions) { if (item.Key == type) { try { item.Value(buffer); } catch (Exception ex) { log.Error(ex); } } } } }
public void HandleBuffer(byte[] buffer, int readCount) { for (var idx = 0; idx < readCount; ++idx) { var value = (byte)buffer[idx]; if (_currentMessage != null) { _currentMessage.add_byte(value); } switch (_mavLinkParserState) { case MavLinkParserStates.ExpectingStx: if (value == MAVLINK2_STX) { _currentMessage = new MAVLinkMessage(); _currentMessage.add_byte(value); _mavLinkParserState = MavLinkParserStates.Expecting2PayloadLen; } if (value == 254) { _currentMessage = new MAVLinkMessage(); _currentMessage.add_byte(value); _mavLinkParserState = MavLinkParserStates.ExpectingPayloadLen; } break; case MavLinkParserStates.Expecting2PayloadLen: _currentMessage.payloadlength = value; _mavLinkParserState = MavLinkParserStates.Expecting2IncompatFlags; break; case MavLinkParserStates.Expecting2IncompatFlags: _currentMessage.incompat_flags = value; _mavLinkParserState = MavLinkParserStates.Expecting2CompatFlag; break; case MavLinkParserStates.Expecting2CompatFlag: _currentMessage.compat_flags = value; _mavLinkParserState = MavLinkParserStates.Expecting2Seq; break; case MavLinkParserStates.Expecting2Seq: _currentMessage.seq = value; _mavLinkParserState = MavLinkParserStates.Expecting2SysId; break; case MavLinkParserStates.Expecting2SysId: _currentMessage.sysid = value; _mavLinkParserState = MavLinkParserStates.Expecting2ComponentId; break; case MavLinkParserStates.Expecting2ComponentId: _currentMessage.compid = value; _mavLinkParserState = MavLinkParserStates.Expecting2MsgId07; _currentMessage.msgid = 0x00; break; case MavLinkParserStates.Expecting2MsgId07: _currentMessage.msgid = value; _mavLinkParserState = MavLinkParserStates.Expecting2MsgId815; break; case MavLinkParserStates.Expecting2MsgId815: _mavLinkParserState = MavLinkParserStates.Expecting2MsgId1623; _currentMessage.msgid += (UInt32)(value << 8); break; case MavLinkParserStates.Expecting2MsgId1623: _mavLinkParserState = MavLinkParserStates.Expecting2TargetSysId; _currentMessage.msgid += (UInt32)(value << 16); break; case MavLinkParserStates.Expecting2TargetSysId: _currentMessage.targetsysid = value; _mavLinkParserState = MavLinkParserStates.Expecting2TargetComponentId; break; case MavLinkParserStates.Expecting2TargetComponentId: _currentMessage.targetcomponentid = value; _currentMessage.payload_index = 0; _mavLinkParserState = MavLinkParserStates.ReadingPayload2; break; case MavLinkParserStates.ReadingPayload2: if (_currentMessage.payload_index < _currentMessage.payloadlength) { _currentMessage.payload[_currentMessage.payload_index++] = value; } if (_currentMessage.payload_index == _currentMessage.payloadlength) { _mavLinkParserState = MavLinkParserStates.Expecting2CheckSum1; } break; case MavLinkParserStates.Expecting2CheckSum1: _currentMessage.crc16 = value; _mavLinkParserState = MavLinkParserStates.Expecting2CheckSum2; break; case MavLinkParserStates.Expecting2CheckSum2: _currentMessage.crc16 += (UInt16)(value << 8); _mavLinkParserState = MavLinkParserStates.Expecting2Signature; _currentMessage.sig = new byte[13]; _signatureIndex = 0; break; case MavLinkParserStates.Expecting2Signature: _currentMessage.sig[_signatureIndex++] = value; if (_signatureIndex == SIGNATURE_LEN) { Debug.WriteLine($"MAVLINK2 - Message Id: {_currentMessage.msgid} - {_currentMessage.payloadlength} - {_currentMessage.seq} - {_currentMessage.crc16}"); _currentMessage.processBuffer(_currentMessage.buffer); Debug.WriteLine($"MAVLINK2 - Message Id: {_currentMessage.msgid} - {_currentMessage.payloadlength} - {_currentMessage.seq} - {_currentMessage.crc16}"); _mavLinkParserState = MavLinkParserStates.ExpectingStx; _currentMessage = null; } break; case MavLinkParserStates.ExpectingPayloadLen: _currentMessage.payloadlength = value; _mavLinkParserState = MavLinkParserStates.ExpectingSeqNumber; break; case MavLinkParserStates.ExpectingSeqNumber: _currentMessage.seq = value; _mavLinkParserState = MavLinkParserStates.ExpectingSystemId; break; case MavLinkParserStates.ExpectingSystemId: _currentMessage.sysid = value; _mavLinkParserState = MavLinkParserStates.ExpectingComponentId; break; case MavLinkParserStates.ExpectingComponentId: _currentMessage.compid = value; _mavLinkParserState = MavLinkParserStates.ExpectingMessageId; break; case MavLinkParserStates.ExpectingMessageId: _currentMessage.msgid = value; _mavLinkParserState = MavLinkParserStates.ReadingPayloadData; break; case MavLinkParserStates.ReadingPayloadData: if (_currentMessage.payload_index < _currentMessage.payloadlength) { _currentMessage.payload[_currentMessage.payload_index++] = value; } if (_currentMessage.payload_index == _currentMessage.payloadlength) { _currentMessage.MessageInfo = MAVLink.MAVLINK_MESSAGE_INFOS.GetMessageInfo(_currentMessage.msgid); _currentMessage.crc16Calc = MavlinkCRC.crc_calculate(_currentMessage.buffer, _currentMessage.buffer_index); _currentMessage.crc16Calc = MavlinkCRC.crc_accumulate(_currentMessage.MessageInfo.crc, _currentMessage.crc16Calc); _mavLinkParserState = MavLinkParserStates.ExpectingCRC1; } break; case MavLinkParserStates.ExpectingCRC1: _currentMessage.crc16 = value; _mavLinkParserState = MavLinkParserStates.ExpectingCRC2; break; case MavLinkParserStates.ExpectingCRC2: _currentMessage.crc16 += (UInt16)(value << 8); if ((_currentMessage.crc16 >> 8) != (_currentMessage.crc16Calc >> 8) || (_currentMessage.crc16 & 0xff) != (_currentMessage.crc16Calc & 0xff)) { //Debug.WriteLine($"INVALID CHECK SUM MAVLINK1 - Message Id: {_currentMessage.msgid} - {_currentMessage.payloadlength} - {_currentMessage.seq} - {_currentMessage.crc16:x2} - {_currentMessage.crc16Calc:x2}"); } else { MessageParsed?.Invoke(this, _currentMessage); ///Debug.WriteLine($"VALID CHECK SUM MAVLINK1 - Message Id: {_currentMessage.msgid} - {_currentMessage.payloadlength} - {_currentMessage.seq} - {_currentMessage.crc16:x2} - {_currentMessage.crc16Calc:x2}"); } _currentMessage.processBuffer(_currentMessage.buffer); _currentMessage = null; _mavLinkParserState = MavLinkParserStates.ExpectingStx; break; } } }
private void OpenBg(object PRsender, bool getparams, ProgressWorkerEventArgs progressWorkerEventArgs) { frmProgressReporter.UpdateProgressAndStatus(-1, Strings.MavlinkConnecting); giveComport = true; if (BaseStream is SerialPort) { // allow settings to settle - previous dtr System.Threading.Thread.Sleep(1000); } Terrain = new TerrainFollow(this); bool hbseen = false; try { BaseStream.ReadBufferSize = 16*1024; lock (objlock) // so we dont have random traffic { log.Info("Open port with " + BaseStream.PortName + " " + BaseStream.BaudRate); BaseStream.Open(); BaseStream.DiscardInBuffer(); // other boards seem to have issues if there is no delay? posible bootloader timeout issue Thread.Sleep(1000); } MAVLinkMessage buffer = new MAVLinkMessage(); MAVLinkMessage buffer1 = new MAVLinkMessage(); 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; frmProgressReporter.UpdateProgressAndStatus(-1, string.Format(Strings.Trying, secondsRemaining)); if (secondsRemaining > 0) countDown.Start(); }; countDown.Start(); // px4 native BaseStream.WriteLine("sh /etc/init.d/rc.usb"); 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(); if (hbseen) { progressWorkerEventArgs.ErrorMessage = Strings.Only1Hb; throw new Exception(Strings.Only1HbD); } else { progressWorkerEventArgs.ErrorMessage = "No Heartbeat Packets Received"; throw new Exception(@"Can not establish a connection\n Please check the following 1. You have firmware loaded 2. You have the correct serial port selected 3. PX4 - You have the microsd card installed 4. Try a diffrent usb port\n\n" + "No Mavlink Heartbeat Packets where read from this port - Verify Baud Rate and setup\nMission Planner waits for 2 valid heartbeat packets before connecting"); } } System.Threading.Thread.Sleep(1); // can see 2 heartbeat packets at any time, and will connect - was one after the other 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++; // 2 hbs that match if (buffer.Length > 5 && buffer1.Length > 5 && buffer.sysid == buffer1.sysid && buffer.compid == buffer1.compid) { mavlink_heartbeat_t hb = buffer.ToStructure<mavlink_heartbeat_t>(); if (hb.type != (byte) MAVLink.MAV_TYPE.GCS) { SetupMavConnect(buffer, hb); break; } } // 2 hb's that dont match. more than one sysid here if (buffer.Length > 5 && buffer1.Length > 5 && (buffer.sysid == buffer1.sysid || buffer.compid == buffer1.compid)) { mavlink_heartbeat_t hb = buffer.ToStructure<mavlink_heartbeat_t>(); if (hb.type != (byte) MAVLink.MAV_TYPE.ANTENNA_TRACKER && hb.type != (byte) MAVLink.MAV_TYPE.GCS) { SetupMavConnect(buffer, hb); break; } hb = buffer1.ToStructure<mavlink_heartbeat_t>(); if (hb.type != (byte) MAVLink.MAV_TYPE.ANTENNA_TRACKER && hb.type != (byte) MAVLink.MAV_TYPE.GCS) { SetupMavConnect(buffer1, hb); break; } } } countDown.Stop(); getVersion(); frmProgressReporter.UpdateProgressAndStatus(0, "Getting Params.. (sysid " + MAV.sysid + " compid " + MAV.compid + ") "); if (getparams) { getParamListBG(); } byte[] temp = ASCIIEncoding.ASCII.GetBytes("Mission Planner " + Application.ProductVersion + "\0"); Array.Resize(ref temp, 50); generatePacket((byte) MAVLINK_MSG_ID.STATUSTEXT, new mavlink_statustext_t() {severity = (byte) MAV_SEVERITY.INFO, text = temp}); 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); throw; } //frmProgressReporter.Close(); giveComport = false; frmProgressReporter.UpdateProgressAndStatus(100, Strings.Done); log.Info("Done open " + MAV.sysid + " " + MAV.compid); MAV.packetslost = 0; MAV.synclost = 0; }
private void _link_MessageParsed(object sender, MAVLinkMessage e) { }
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()); }
private void _telemeteryLink_MessageParsed(object sender, MAVLinkMessage msg) { _droneAdapter.UpdateDroneAsync(_apmDrone, msg); }