Ejemplo n.º 1
0
        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);
        }
Ejemplo n.º 2
0
        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);
        }
Ejemplo n.º 3
0
        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);
        }
Ejemplo n.º 4
0
        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;
        }
Ejemplo n.º 6
0
        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;
        }
Ejemplo n.º 7
0
        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);
        }
Ejemplo n.º 8
0
        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;
        }
Ejemplo n.º 9
0
 private void generatePacket(MAVLINK_MSG_ID messageType, object indata)
 {
     generatePacket((int)messageType, indata);
 }
Ejemplo n.º 10
0
 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());
     }
 }
Ejemplo n.º 11
0
 public static byte[] GeneratePacket(IDrone drone, MAVLINK_MSG_ID messageType, object indata)
 {
     return(GeneratePacket(drone, (int)messageType, indata));
 }
Ejemplo n.º 12
0
        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));
        }
Ejemplo n.º 13
0
        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);
            }
        }