internal void generatePacket(int messageType, object indata, int sysid, int compid, bool forcemavlink2 = false, bool forcesigning = false)
{
if (!BaseStream.IsOpen)
{
return;
}
if (ReadOnly)
{
// allow these messages
if (messageType == (byte) MAVLink.MAVLINK_MSG_ID.MISSION_REQUEST_LIST ||
messageType == (byte) MAVLink.MAVLINK_MSG_ID.MISSION_REQUEST_PARTIAL_LIST ||
messageType == (byte) MAVLink.MAVLINK_MSG_ID.MISSION_REQUEST ||
messageType == (byte) MAVLink.MAVLINK_MSG_ID.PARAM_REQUEST_LIST ||
messageType == (byte) MAVLink.MAVLINK_MSG_ID.PARAM_REQUEST_READ ||
messageType == (byte) MAVLink.MAVLINK_MSG_ID.RALLY_FETCH_POINT ||
messageType == (byte) MAVLink.MAVLINK_MSG_ID.FENCE_FETCH_POINT
)
{
}
else
{
return;
}
}
lock (objlock)
{
byte[] data = MavlinkUtil.StructureToByteArray(indata);
byte[] packet = new byte[0];
int i = 0;
// are we mavlink2 enabled for this sysid/compid
if (!MAVlist[sysid, compid].mavlinkv2 && messageType < 256 && !forcemavlink2)
{
var info = 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] = MAVLINK_STX_MAVLINK1;
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.trim_payload(ref data);
packet = new byte[data.Length + MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES + MAVLINK_SIGNATURE_BLOCK_LEN];
packet[0] = MAVLINK_STX ;
packet[1] = (byte)data.Length;
packet[2] = 0; // incompat
if (MAVlist[sysid, compid].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 (MAVlist[sysid, compid].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 == MAVlist[sysid, compid].timestamp)
timestamp++;
MAVlist[sysid, compid].timestamp = timestamp;
var timebytes = BitConverter.GetBytes(timestamp);
var sig = new byte[7]; // 13 includes the outgoing hash
sig[0] = MAVlist[sysid, compid].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 = MAVlist[sysid, compid].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++;
}
}
}
}
if (BaseStream.IsOpen)
{
BaseStream.Write(packet, 0, i);
_bytesSentSubj.OnNext(i);
}
try
{
if (logfile != null && logfile.CanWrite)
{
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(packet, 0, i);
}
}
}
catch
{
}
}
}
