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