//PC--->>>plane 发送数据 private void generatePacket(byte messageType, object indata) { byte[] data = MavlinkUtil.StructureToByteArray(indata); //Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead); byte[] packet = new byte[data.Length + 6 + 2]; packet[0] = 254; packet[1] = (byte)data.Length; packet[2] = (byte)packet_count; packet_count++; packet[3] = SYS_ID; // this is always 255 - MYGCS packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; packet[5] = messageType; int i = 6; foreach (byte b in data) { packet[i] = b; i++; } ushort checksum = MAVLink.MavlinkCRC.crc_calculate(packet, packet[1] + 6); checksum = MAVLink.MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum); byte ck_a = (byte)(checksum & 0xFF); ///< High byte byte ck_b = (byte)(checksum >> 8); ///< Low byte packet[i] = ck_a; packet[i + 1] = ck_b; //if (this.serial.isComOpen) if (this.serial.IsOpen) { this.serial.Write(packet, 0, packet.Length); } }
void generatePacket(byte messageType, object indata) { if (!BaseStream.IsOpen) { return; } lock (writelock) { byte[] data; data = MavlinkUtil.StructureToByteArray(indata); byte[] packet = new byte[data.Length + 6 + 2]; packet[0] = 254; packet[1] = (byte)data.Length; packet[2] = (byte)packetcount; packetcount++; packet[3] = 255; // this is always 255 - MYGCS packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; packet[5] = 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_CRCS[messageType], 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 (BaseStream.IsOpen) { BaseStream.Write(packet, 0, i); _bytesSentSubj.OnNext(i); } } }
public byte[] GenerateMAVLinkPacket_PX4(MAVLINK_MSG_ID messageType, object indata) { byte[] data; data = MavlinkUtil.StructureToByteArray(indata); byte[] packet = new byte[data.Length + 10 + 2]; packet[0] = 0xfd; packet[1] = (byte)data.Length; packet[2] = 0; packet[3] = 0; packet[4] = 0x75; packet[5] = 255; // this is always 255 - MYGCS packet[6] = 0; packet[7] = (byte)messageType; //messageType packet[8] = 0; //messageType extera packet[9] = 0; //messageType extera packetcount++; //packet[3] = 255; // this is always 255 - MYGCS //packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; //packet[5] = (byte)messageType; int i = 10; foreach (byte b in data) { packet[i] = b; i++; } ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 10); checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[(byte)messageType], 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[] 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[] GenerateMAVLinkPacket(MAVLINK_MSG_ID messageType, object indata) { byte[] data; data = MavlinkUtil.StructureToByteArray(indata); byte[] packet = new byte[data.Length + 6 + 2]; packet[0] = 254; packet[1] = (byte)data.Length; packet[2] = (byte)packetcount; packetcount++; packet[3] = 255; // this is always 255 - MYGCS packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_Application1; 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_CRCS[(byte)messageType], 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); }
/// <summary> /// Serialize this message to MAVLink v2 byte message. /// </summary> public byte[] toBytes(byte [] signingKey = null) { byte[] payload_bytes = MavlinkUtil.StructureToByteArray(payload); // Truncate zero bytes at the end of the payload var length = payload_bytes.Length; while (length > 1 && payload_bytes[length - 1] == 0) { length--; } Span <byte> data = new Span <byte>(payload_bytes, 0, length); int extra = 0; if (signingKey != null) { extra = MAVLink.MAVLINK_SIGNATURE_BLOCK_LEN; } byte[] packet = new byte[data.Length + MAVLink.MAVLINK_NUM_NON_PAYLOAD_BYTES + extra]; packet[0] = MAVLink.MAVLINK_STX; packet[1] = (byte)data.Length; packet[2] = 0; //incompat signing if (signingKey != null) { packet[2] |= MAVLink.MAVLINK_IFLAG_SIGNED; } packet[3] = 0; //compat packet[4] = sequence; packet[5] = system_id; packet[6] = component_id; packet[7] = (byte)message_id; packet[8] = (byte)(message_id >> 8); packet[9] = (byte)(message_id >> 16); int i = MAVLink.MAVLINK_NUM_HEADER_BYTES; foreach (byte b in data) { packet[i] = b; i++; } ushort checksum = MAVLink.MavlinkCRC.crc_calculate(packet, data.Length + MAVLink.MAVLINK_NUM_HEADER_BYTES); checksum = MAVLink.MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_INFOS.GetMessageInfo(message_id).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 (signature != null) { //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 timebytes = BitConverter.GetBytes(signature_timestamp); var sig = new byte[7]; // 13 includes the outgoing hash sig[0] = signature_link_id; Array.Copy(timebytes, 0, sig, 1, 6); // timestamp 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.ToArray()); // 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); }
/// <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); } }