public async Task <InvokeResult> SendMessage(IDrone drone, MAVLINK_MSG_ID messageId, object req)
{
var buffer = MavlinkUtil.GeneratePacket(drone, MAVLINK_MSG_ID.MISSION_REQUEST_LIST, req);
await _serialPort.WriteAsync(buffer);
return(InvokeResult.Success);
}
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[] 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_MISSIONPLANNER;
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 KeyValuePair<MAVLINK_MSG_ID, Func<MAVLinkMessage, bool>> SubscribeToPacketType(MAVLINK_MSG_ID type,
Func<MAVLinkMessage, bool> function, bool exclusive = false)
{
var item = new KeyValuePair<MAVLINK_MSG_ID, Func<MAVLinkMessage, bool>>(type, function);
lock (Subscriptions)
{
if (exclusive)
{
foreach (var subitem in Subscriptions)
{
if (subitem.Key == item.Key)
{
Subscriptions.Remove(subitem);
break;
}
}
}
log.Info("SubscribeToPacketType " + item.Key + " " + item.Value);
Subscriptions.Add(item);
}
return item;
}
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 byte[] GenerateMAVLinkPacket20(MAVLINK_MSG_ID messageType, object indata, bool sign = false)
{
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;
packetcount++;
packet[5] = 255; // this is always 255 - MYGCS
packet[6] = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
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 (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;
}
private void generatePacket(MAVLINK_MSG_ID messageType, object indata)
{
generatePacket((int)messageType, indata);
}
public void UnSubscribeToPacketType(MAVLINK_MSG_ID msgtype, Func<MAVLinkMessage, bool> item)
{
lock (Subscriptions)
{
log.Info("UnSubscribeToPacketType " + msgtype + " " + item);
var ans = Subscriptions.Where(a => { return a.Key == msgtype && a.Value == item; });
Subscriptions.Remove(ans.First());
}
}
public static byte[] GeneratePacket(IDrone drone, MAVLINK_MSG_ID messageType, object indata)
{
return(GeneratePacket(drone, (int)messageType, indata));
}
public async Task <InvokeResult <TMavlinkPacket> > RequestDataAsync <TMavlinkPacket>(IDrone drone, MAVLINK_MSG_ID outgoingMessageId, object req, MAVLINK_MSG_ID incomingMessageId, TimeSpan timeout) where TMavlinkPacket : struct
{
await SendMessage(drone, outgoingMessageId, req);
return(await WaitForMessageAsync <TMavlinkPacket>(incomingMessageId, timeout));
}
public async Task <InvokeResult <TMavlinkPacket> > WaitForMessageAsync <TMavlinkPacket>(MAVLINK_MSG_ID messageId, TimeSpan timeout) where TMavlinkPacket : struct
{
try
{
Debug.WriteLine("Start Waiting" + DateTime.Now.ToString());
var wor = new WaitOnRequest <object>((uint)messageId);
Sessions[(uint)messageId] = wor;
MAVLinkMessage message = null;
for (var idx = 0; (idx < timeout.TotalMilliseconds / 100) && message == null; ++idx)
{
if (wor.CompletionSource.Task.IsCompleted)
{
Debug.WriteLine("TASK IS COMPLETED!!!!");
message = wor.CompletionSource.Task.Result as MAVLinkMessage;
}
await Task.Delay(100);
}
Debug.WriteLine("End Waiting" + DateTime.Now.ToString());
if (message == null)
{
return(InvokeResult <TMavlinkPacket> .FromError("Timeout waiting for message."));
}
else
{
var result = MavlinkUtil.ByteArrayToStructure <TMavlinkPacket>(message.buffer);
return(InvokeResult <TMavlinkPacket> .Create(result));
}
}
catch (Exception ex)
{
return(InvokeResult <TMavlinkPacket> .FromException("AsyncCoupler_WaitOnAsync", ex));
}
finally
{
Sessions.TryRemove((uint)messageId, out WaitOnRequest <Object> obj);
}
}
