public byte[] GenerateMAVLinkPacket10(MAVLINK_MSG_ID messageType, object indata, byte sysid = 255, byte compid = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER, int sequence = -1) { byte[] data; data = MavlinkUtil.StructureToByteArray(indata); byte[] packet = new byte[data.Length + 6 + 2]; packet[0] = MAVLINK_STX_MAVLINK1; packet[1] = (byte)data.Length; packet[2] = (byte)packetcount; if (sequence != -1) { packet[2] = (byte)sequence; } packetcount++; packet[3] = sysid; // this is always 255 - MYGCS packet[4] = compid; packet[5] = (byte)messageType; int i = 6; foreach (byte b in data) { packet[i] = b; i++; } ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6); checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo((uint)messageType).crc, checksum); byte ck_a = (byte)(checksum & 0xFF); ///< High byte byte ck_b = (byte)(checksum >> 8); ///< Low byte packet[i] = ck_a; i += 1; packet[i] = ck_b; i += 1; return(packet); }
public byte[] GenerateMAVLinkPacket20(MAVLINK_MSG_ID messageType, object indata, bool sign = false, byte sysid = 255, byte compid = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER, int sequence = -1) { byte[] data; data = MavlinkUtil.StructureToByteArray(indata); MavlinkUtil.trim_payload(ref data); int extra = 0; if (sign) { extra = MAVLINK_SIGNATURE_BLOCK_LEN; } byte[] packet = new byte[data.Length + MAVLINK_NUM_NON_PAYLOAD_BYTES + extra]; packet[0] = MAVLINK_STX; packet[1] = (byte)data.Length; packet[2] = 0;//incompat signing if (sign) { packet[2] |= MAVLINK_IFLAG_SIGNED; } packet[3] = 0;//compat packet[4] = (byte)packetcount; if (sequence != -1) { packet[4] = (byte)sequence; } packetcount++; packet[5] = sysid; packet[6] = compid; packet[7] = (byte)((UInt32)messageType); packet[8] = (byte)((UInt32)messageType >> 8); packet[9] = (byte)((UInt32)messageType >> 16); int i = MAVLINK_NUM_HEADER_BYTES; foreach (byte b in data) { packet[i] = b; i++; } ushort checksum = MavlinkCRC.crc_calculate(packet, data.Length + MAVLINK_NUM_HEADER_BYTES); checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo((uint)messageType).crc, checksum); byte ck_a = (byte)(checksum & 0xFF); ///< High byte byte ck_b = (byte)(checksum >> 8); ///< Low byte packet[i] = ck_a; i += 1; packet[i] = ck_b; i += 1; if (sign) { //https://docs.google.com/document/d/1ETle6qQRcaNWAmpG2wz0oOpFKSF_bcTmYMQvtTGI8ns/edit /* * 8 bits of link ID * 48 bits of timestamp * 48 bits of signature */ // signature = sha256_48(secret_key + header + payload + CRC + link-ID + timestamp) var timestamp = (UInt64)((DateTime.UtcNow - new DateTime(2015, 1, 1)).TotalMilliseconds * 100); if (timestamp == lasttimestamp) { timestamp++; } lasttimestamp = timestamp; var timebytes = BitConverter.GetBytes(timestamp); var sig = new byte[7]; // 13 includes the outgoing hash sig[0] = sendlinkid; Array.Copy(timebytes, 0, sig, 1, 6); // timestamp //Console.WriteLine("gen linkid {0}, time {1} {2} {3} {4} {5} {6} {7}", sig[0], sig[1], sig[2], sig[3], sig[4], sig[5], sig[6], timestamp); if (signingKey == null || signingKey.Length != 32) { signingKey = new byte[32]; } using (SHA256 signit = SHA256.Create()) { MemoryStream ms = new MemoryStream(); ms.Write(signingKey, 0, signingKey.Length); ms.Write(packet, 0, i); ms.Write(sig, 0, sig.Length); var ctx = signit.ComputeHash(ms.GetBuffer()); // trim to 48 Array.Resize(ref ctx, 6); foreach (byte b in sig) { packet[i] = b; i++; } foreach (byte b in ctx) { packet[i] = b; i++; } } } return(packet); }
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); }
/// <summary> /// Generate a Mavlink Packet and write to serial /// </summary> /// <param name="messageType">type number = MAVLINK_MSG_ID</param> /// <param name="indata">struct of data</param> public static byte[] GeneratePacket(IDrone drone, int messageType, object indata, bool forcemavlink2 = false, bool forcesigning = false) { lock (objLock) { var data = MavlinkUtil.StructureToByteArray(indata); var packet = new byte[0]; int i = 0; // are we mavlink2 enabled for this sysid/compid if (!drone.MavLink2 && messageType < 256 && !forcemavlink2) { var info = MAVLink.MAVLINK_MESSAGE_INFOS.SingleOrDefault(p => p.MsgId == messageType); if (data.Length != info.minlength) { Array.Resize(ref data, (int)info.minlength); } //Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead); packet = new byte[data.Length + 6 + 2]; packet[0] = MAVLINK1_STX; packet[1] = (byte)data.Length; packet[2] = (byte)_packetCount; _packetCount++; packet[3] = gcssysid; packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; packet[5] = (byte)messageType; i = 6; foreach (byte b in data) { packet[i] = b; i++; } ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6); checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo((uint)messageType).crc, checksum); byte ck_a = (byte)(checksum & 0xFF); ///< High byte byte ck_b = (byte)(checksum >> 8); ///< Low byte packet[i] = ck_a; i += 1; packet[i] = ck_b; i += 1; } else { // trim packet for mavlink2 MavlinkUtil.TrimePayload(ref data); packet = new byte[data.Length + MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES + MAVLINK_SIGNATURE_BLOCK_LEN]; packet[0] = MAVLINK2_STX; packet[1] = (byte)data.Length; packet[2] = 0; // incompat if (drone.Signing || forcesigning) // current mav { packet[2] |= MAVLINK_IFLAG_SIGNED; } packet[3] = 0; // compat packet[4] = (byte)_packetCount; _packetCount++; packet[5] = gcssysid; packet[6] = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; packet[7] = (byte)(messageType & 0xff); packet[8] = (byte)((messageType >> 8) & 0xff); packet[9] = (byte)((messageType >> 16) & 0xff); i = 10; foreach (byte b in data) { packet[i] = b; i++; } ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + MAVLINK_NUM_HEADER_BYTES); checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo((uint)messageType).crc, checksum); byte ck_a = (byte)(checksum & 0xFF); ///< High byte byte ck_b = (byte)(checksum >> 8); ///< Low byte packet[i] = ck_a; i += 1; packet[i] = ck_b; i += 1; if (drone.Signing || forcesigning) { //https://docs.google.com/document/d/1ETle6qQRcaNWAmpG2wz0oOpFKSF_bcTmYMQvtTGI8ns/edit /* * 8 bits of link ID * 48 bits of timestamp * 48 bits of signature */ // signature = sha256_48(secret_key + header + payload + CRC + link-ID + timestamp) var timestamp = (UInt64)((DateTime.UtcNow - new DateTime(2015, 1, 1)).TotalMilliseconds * 100); if (timestamp == drone.TimeStamp) { timestamp++; } drone.TimeStamp = timestamp; var timebytes = BitConverter.GetBytes(timestamp); var sig = new byte[7]; // 13 includes the outgoing hash sig[0] = drone.SendLinkId; Array.Copy(timebytes, 0, sig, 1, 6); // timestamp //Console.WriteLine("gen linkid {0}, time {1} {2} {3} {4} {5} {6} {7}", sig[0], sig[1], sig[2], sig[3], sig[4], sig[5], sig[6], timestamp); var signingKey = drone.SigningKey; if (signingKey == null || signingKey.Length != 32) { signingKey = new byte[32]; } using (SHA256Managed signit = new SHA256Managed()) { signit.TransformBlock(signingKey, 0, signingKey.Length, null, 0); signit.TransformBlock(packet, 0, i, null, 0); signit.TransformFinalBlock(sig, 0, sig.Length); var ctx = signit.Hash; // trim to 48 Array.Resize(ref ctx, 6); foreach (byte b in sig) { packet[i] = b; i++; } foreach (byte b in ctx) { packet[i] = b; i++; } } } } return(packet); } }