public byte[] 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)
{
break;
}
readcount++;
}
// read header
ReadWithTimeout(BaseStream, buffer, 1, 5);
// packet length
int lengthtoread = buffer[1] + 6 + 2 - 2; // data + header + checksum - STX - length
//read rest of packet
ReadWithTimeout(BaseStream, buffer, 6, lengthtoread - 4);
// check message length vs table
if (buffer[1] != MAVLINK_MESSAGE_LENGTHS[buffer[5]])
{
badLength++;
// bad or unknown packet
return(null);
}
// resize the packet to the correct length
Array.Resize <byte>(ref buffer, lengthtoread + 2);
// calc crc
ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2);
// calc extra bit of crc for mavlink 1.0
if (buffer.Length > 5 && buffer[0] == 254)
{
crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[buffer[5]], crc);
}
// check crc
if (buffer.Length < 5 || buffer[buffer.Length - 1] != (crc >> 8) ||
buffer[buffer.Length - 2] != (crc & 0xff))
{
badCRC++;
// crc fail
return(null);
}
return(buffer);
}
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);
}
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);
}
public MAVLinkMessage ReadPacket()
{
byte[] buffer = new byte[GlobalAppData.MAX_PACKET_LEN + 25];
int count = 0;
int length = 0;
int readcount = 0;
MAVLinkMessage message = null;
DateTime start = DateTime.Now;
lock (readlock)
{
while ((BaseStream != null && BaseStream.IsOpen) || logreadmode)
{
try
{
if (readcount > 300)
{
break;
}
readcount++;
if (logreadmode)
{
message = ReadLog();
buffer = message.buffer;
if (buffer == null || buffer.Length == 0)
{
return(MAVLinkMessage.Invalid);
}
}
else
{
if (BaseStream.ReadTimeout != 1200)
{
BaseStream.ReadTimeout = 1200; // 1200 ms between chars - the gps detection requires this.
}
DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout);
while (BaseStream.IsOpen && BaseStream.BytesToRead <= 0)
{
if (DateTime.Now > to)
{
throw new TimeoutException("Timeout");
}
Task.Delay(1);
}
if (BaseStream.IsOpen)
{
BaseStream.Read(buffer, count, 1);
if (rawlogfile != null && rawlogfile.CanWrite)
{
rawlogfile.WriteByte(buffer[count]);
}
}
}
}
catch (Exception ex)
{
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]);
buildplaintxtline += (char)buffer[0];
}
//_bytesReceivedSubj.OnNext(1);
count = 0;
buffer[1] = 0;
continue;
}
// reset count on valid packet
readcount = 0;
// check for a header
if (buffer[0] == 0xfe || buffer[0] == 0xfd || buffer[0] == 'U')
{
var mavlinkv2 = buffer[0] == GlobalAppData.MAVLINK_STX ? true : false;
int headerlength = mavlinkv2 ? GlobalAppData.MAVLINK_CORE_HEADER_LEN : GlobalAppData.MAVLINK_CORE_HEADER_MAVLINK1_LEN;
int headerlengthstx = headerlength + 1;
// 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)
{
throw new TimeoutException("Timeout");
}
Task.Delay(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] == GlobalAppData.MAVLINK_STX)
{
length = buffer[1] + headerlengthstx +
GlobalAppData.MAVLINK_NUM_CHECKSUM_BYTES; // data + header + checksum - magic - length
if ((buffer[2] & GlobalAppData.MAVLINK_IFLAG_SIGNED) > 0)
{
length += GlobalAppData.MAVLINK_SIGNATURE_BLOCK_LEN;
}
}
else
{
length = buffer[1] + headerlengthstx +
GlobalAppData.MAVLINK_NUM_CHECKSUM_BYTES; // 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 - (headerlengthstx)))
{
if (DateTime.Now > to)
{
break;
}
Task.Delay(1);
}
if (BaseStream.IsOpen)
{
int read = BaseStream.Read(buffer, headerlengthstx, length - (headerlengthstx));
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;
}
catch
{
break;
}
break;
}
}
count++;
if (count == 299)
{
break;
}
}
} // 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;
}
_bps2 = _bps1; // prev sec
_bps1 = 0; // current sec
_bpstime = DateTime.Now;
_mavlink1count = 0;
_mavlink2count = 0;
_mavlink2signed = 0;
}
_bps1 += buffer.Length;
if (buffer.Length == 0)
{
return(MAVLinkMessage.Invalid);
}
if (message == null)
{
message = new MAVLinkMessage(buffer, DateTime.UtcNow);
}
uint msgid = message.msgid;
GlobalAppData.message_info msginfo = GlobalAppData.GetMessageInfo(msgid);
// calc crc
var sigsize = (message.sig != null) ? GlobalAppData.MAVLINK_SIGNATURE_BLOCK_LEN : 0;
ushort crc = MavlinkCRC.crc_calculate(buffer, message.Length - sigsize - GlobalAppData.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 size vs table (mavlink1 explicit size check | mavlink2 allow all, undersize 0 trimmed, and oversize unknown extension)
if (!message.ismavlink2 && message.payloadlength != msginfo.minlength)
{
if (msginfo.length == 0) // pass for unknown packets
{
}
else
{
return(MAVLinkMessage.Invalid);
}
}
// check crc
if ((message.crc16 >> 8) != (crc >> 8) ||
(message.crc16 & 0xff) != (crc & 0xff))
{
return(MAVLinkMessage.Invalid);
}
byte sysid = message.sysid;
byte compid = message.compid;
byte packetSeqNo = message.seq;
// stat count
if (message.buffer[0] == GlobalAppData.MAVLINK_STX)
{
_mavlink2count++;
}
else if (message.buffer[0] == GlobalAppData.MAVLINK_STX_MAVLINK1)
{
_mavlink1count++;
}
// if its a gcs packet - dont process further
if (buffer.Length >= 5 && (sysid == 255 || sysid == 253) && logreadmode) // gcs packet
{
return(message);
}
return(message);
}
public object ReadPacket(Stream BaseStream)
{
byte[] buffer = new byte[270];
int readcount = 0;
while (readcount < 200)
{
// read STX byte
BaseStream.Read(buffer, 0, 1);
if (buffer[0] == MAVLink.MAVLINK_STX)
{
break;
}
readcount++;
}
// read header
int read = BaseStream.Read(buffer, 1, 5);
// packet length
int lengthtoread = buffer[1] + 6 + 2 - 2; // data + header + checksum - STX - length
//read rest of packet
read = BaseStream.Read(buffer, 6, lengthtoread - 4);
// resize the packet to the correct length
Array.Resize <byte>(ref buffer, lengthtoread + 2);
// calc crc
ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2);
// calc extra bit of crc for mavlink 1.0
if (buffer.Length > 5 && buffer[0] == 254)
{
crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[buffer[5]], crc);
}
// check message length vs table
if (buffer.Length > 5 && buffer[1] != MAVLINK_MESSAGE_LENGTHS[buffer[5]])
{
// bad or unknown packet
return(null);
}
// check crc
if (buffer.Length < 5 || buffer[buffer.Length - 1] != (crc >> 8) || buffer[buffer.Length - 2] != (crc & 0xff))
{
// crc fail
return(null);
}
byte header = buffer[0];
byte length = buffer[1];
byte seq = buffer[2];
byte sysid = buffer[3];
byte compid = buffer[4];
byte messid = buffer[5];
//create the object spcified by the packet type
object data = Activator.CreateInstance(MAVLINK_MESSAGE_INFO[messid]);
// fill in the data of the object
MavlinkUtil.ByteArrayToStructure(buffer, ref data, 6);
return(data);
}
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());
}
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);
}
/// <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);
}
}
