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