Ejemplo n.º 1
0
        // __ Impl ____________________________________________________________


        private void Parse()
        {
            try
            {
                using (FileStream s = new FileStream(mLogFileName, FileMode.Open))
                {
                    using (BinaryReader reader = new BinaryReader(s))
                    {
                        while (true)
                        {
                            SyncStream(reader);
                            var packet = MavLinkPacket.Deserialize(reader, 0);

                            if (packet.IsValid)
                            {
                                HandlePacketReceived(this, packet);
                            }
                        }
                    }
                }
            }
            catch (EndOfStreamException)
            {
            }

            HandleReceptionEnded(this);
        }
Ejemplo n.º 2
0
        protected void NotifyPacketDiscarded(MavLinkPacket packet)
        {
            if (packet == null || PacketDiscarded == null)
            {
                return;
            }

            PacketDiscarded(this, packet);
        }
Ejemplo n.º 3
0
        /// <summary>
        /// Generates the buffer bytes to be sent on the wire for given message.
        /// </summary>
        /// <param name="msg"></param>
        /// <param name="systemId"></param>
        /// <param name="componentId"></param>
        /// <param name="includeSignalMark">Whether to include the Packet signal in the buffer or not.</param>
        /// <param name="sequenceNumber">A sequence number for the message, if needed.</param>
        /// <returns></returns>
        public byte[] SerializeMessage(
            UasMessage msg, byte systemId, byte componentId,
            bool includeSignalMark, byte sequenceNumber = 1)
        {
            byte mark = includeSignalMark ? PacketSignalByte : (byte)0;

            return(MavLinkPacket.GetBytesForMessage(
                       msg, systemId, componentId, sequenceNumber, mark));
        }
Ejemplo n.º 4
0
 protected void Complete(MavLinkPacket msg)
 {
     if (Sessions.TryGetValue(msg.MessageId, out var tcs))
     {
         if (!tcs.CompletionSource.Task.IsCompleted)
         {
             tcs.CompletionSource.SetResult(msg);
         }
     }
 }
Ejemplo n.º 5
0
 private void OnMavLinkPacketReceived(object sender, MavLinkPacket packet)
 {
     if (packet.Message is UasCommandLong)
     {
         PrintCommandLong(packet.Message as UasCommandLong);
     }
     else
     {
         PrintStandardMessage(packet.Message);
     }
 }
Ejemplo n.º 6
0
 static void logReader_OnPacketReceived(object sender, MavLinkPacket packet)
 {
     count++;
     if (packet.Message is UasHeartbeat)
     {
         IMessageProcessor <UasHeartbeat> heartbeatProcessor = container.GetMessageProcessor <UasHeartbeat>();
         if (heartbeatProcessor != null)
         {
             heartbeatProcessor.Process((UasHeartbeat)packet.Message);
         }
         else
         {
             Console.WriteLine("Heartbeat: {0}", ((UasHeartbeat)packet.Message).Type);
         }
     }
     else if (packet.Message is UasBatteryStatus)
     {
         IMessageProcessor <UasBatteryStatus> processor = container.GetMessageProcessor <UasBatteryStatus>();
         if (processor != null)
         {
             processor.Process((UasBatteryStatus)packet.Message);
         }
         else
         {
             Console.WriteLine("Battery: {0}", ((UasBatteryStatus)packet.Message).CurrentBattery);
         }
     }
     else if (packet.Message is UasGpsStatus)
     {
         Console.WriteLine("GPS Status: {0}", ((UasGpsStatus)packet.Message).SatellitesVisible);
     }
     else if (packet.Message is UasStatustext)
     {
         IMessageProcessor <UasStatustext> processor = container.GetMessageProcessor <UasStatustext>();
         if (processor != null)
         {
             processor.Process((UasStatustext)packet.Message);
         }
         else
         {
             Console.WriteLine("Status: {0}", new string(((UasStatustext)packet.Message).Text));
         }
     }
     else
     {
         Console.WriteLine("Type: {0}, ID: {1}", packet.Message.GetType().Name, packet.Message.MessageId);
     }
 }
Ejemplo n.º 7
0
        public async Task <InvokeResult <TMavlinkPacket> > WaitForMessageAsync <TMavlinkPacket>(UasMessages messageId, TimeSpan timeout) where TMavlinkPacket : class
        {
            try
            {
                var wor = new WaitOnRequest(messageId);
                Sessions[messageId] = wor;

                MavLinkPacket message = null;

                for (var idx = 0; (idx < timeout.TotalMilliseconds / 100) && message == null; ++idx)
                {
                    if (wor.CompletionSource.Task.IsCompleted)
                    {
                        message = wor.CompletionSource.Task.Result;
                    }
                    await Task.Delay(100);
                }

                if (message == null)
                {
                    return(InvokeResult <TMavlinkPacket> .FromError("Timeout waiting for message."));
                }
                else
                {
                    if (message.Message is TMavlinkPacket)
                    {
                        return(InvokeResult <TMavlinkPacket> .Create(message.Message as TMavlinkPacket));
                    }
                    else
                    {
                        return(InvokeResult <TMavlinkPacket> .FromError("Invalid type."));
                    }
                }
            }
            catch (Exception ex)
            {
                return(InvokeResult <TMavlinkPacket> .FromException("AsyncCoupler_WaitOnAsync", ex));
            }
            finally
            {
                Sessions.TryRemove(messageId, out WaitOnRequest obj);
            }
        }
Ejemplo n.º 8
0
        private void PacketProcessingWorker(object state)
        {
            using (var reader = MavLinkPacket.GetBinaryReader(mProcessStream))
            {
                while (true)
                {
                    SyncStream(reader);
                    var packet = MavLinkPacket.Deserialize(reader, 0);

                    if (packet.IsValid)
                    {
                        NotifyPacketReceived(packet);
                    }
                    else
                    {
                        Debug.WriteLine("PACKET IS INVALID! => " + packet.MessageId);
                        NotifyPacketDiscarded(packet);
                    }
                }
            }
        }
Ejemplo n.º 9
0
 private void OnMavLinkPacketReceived(object sender, MavLinkPacket e)
 {
     PrintPacket(e);
 }
Ejemplo n.º 10
0
 protected override void HandlePacketReceived(object sender, MavLinkPacket e)
 {
 }
Ejemplo n.º 11
0
 private void ParserOnPacketReceived(object sender, MavLinkPacket packet)
 {
     mPackets.Add(packet);
 }
Ejemplo n.º 12
0
        protected virtual void HandlePacketReceived(object sender, MavLinkPacket e)
        {
            _dispatcher.Invoke(() =>
            {
                MessagesReceived++;
                BytesReceived += e.PayLoadLength + 7;
                switch (e.MessageId)
                {
                case UasMessages.Statustext:
                    var txt = System.Text.ASCIIEncoding.ASCII.GetString(e.Payload);
                    if (txt.ToLower().Contains("prearm"))
                    {
                        var existingMessage = PreArmMessages.Where(m => m.Message == txt).FirstOrDefault();
                        if (existingMessage != null)
                        {
                            PreArmMessages.Remove(existingMessage);
                        }

                        PreArmMessages.Insert(0, new StatusMessage(txt));
                        var oldItems = PreArmMessages.Where(oldMsg => oldMsg.IsExpired).ToList();
                        foreach (var itm in oldItems)
                        {
                            PreArmMessages.Remove(itm);
                        }
                    }
                    else
                    {
                        var existingMessage = StatusMessages.Where(m => m.Message == txt).FirstOrDefault();
                        if (existingMessage != null)
                        {
                            StatusMessages.Remove(existingMessage);
                        }

                        StatusMessages.Insert(0, new StatusMessage(txt));

                        var oldItems = StatusMessages.Where(oldMsg => oldMsg.IsExpired).ToList();
                        foreach (var itm in oldItems)
                        {
                            StatusMessages.Remove(itm);
                        }
                    }

                    break;

                case UasMessages.Heartbeat:
                    var hb      = e.Message as MavLink.UasHeartbeat;
                    SystemId    = e.SystemId;
                    ComponentId = e.ComponentId;
                    break;

                default:
                    var msg = Messages.Where(m => m.MessageId == e.MessageId).FirstOrDefault();
                    if (msg != null)
                    {
                        msg.ReceiptCount++;
                    }
                    else
                    {
                        e.ReceiptCount++;
                        Messages.Add(e);
                    }
                    break;
                }

                OnPacketReceived?.Invoke(this, e);
                OnMessageReceived?.Invoke(this, e.Message);
                Complete(e);
            });
        }
        //public abstract void SendRawPacket(MavLinkPacket packet);


        // __ MavLink events __________________________________________________


        protected void HandlePacketReceived(object sender, MavLinkPacket e)
        {
            if (OnPacketReceived != null) OnPacketReceived(sender, e);
        }
Ejemplo n.º 14
0
        private void OnMavLinkPacketReceived(object sender, MavLinkPacket packet)
        {
            switch (packet.MessageId)
            {
            case (byte)MavlinkMessageID.HEARTBEAT:
            {
                HeartbeatEvent?.Invoke(packet.Message as UasHeartbeat);
                break;
            }

            case (byte)MavlinkMessageID.SYS_STATUS:
            {
                SysStatusEvent?.Invoke(packet.Message as UasSysStatus);
                break;
            }

            case (byte)MavlinkMessageID.GPS_RAW_INT:
            {
                GpsRawIntEvent?.Invoke(packet.Message as UasGpsRawInt);
                break;
            }

            case (byte)MavlinkMessageID.ATTITUDE:
            {
                AttitudeEvent?.Invoke(packet.Message as UasAttitude);
                break;
            }

            case (byte)MavlinkMessageID.LOCAL_POSITION_NED:
            {
                LocalPositionNedEvent?.Invoke(packet.Message as UasLocalPositionNed);
                break;
            }

            case (byte)MavlinkMessageID.SERVO_OUTPUT_RAW:
            {
                ServoOutputRawEvent?.Invoke(packet.Message as UasServoOutputRaw);
                break;
            }

            case (byte)MavlinkMessageID.VFR_HUD:
            {
                VfrHudEvent?.Invoke(packet.Message as UasVfrHud);
                break;
            }

            case (byte)MavlinkMessageID.POSITION_TARGET_GLOBAL_INT:
            {
                PositionTargetGlobalIntEvent?.Invoke(packet.Message as UasPositionTargetGlobalInt);
                break;
            }

            case (byte)MavlinkMessageID.HIGHRES_IMU:
            {
                HighresImuEvent?.Invoke(packet.Message as UasHighresImu);
                break;
            }

            case (byte)MavlinkMessageID.ALTITUDE:
            {
                AltitudeEvent?.Invoke(packet.Message as UasAltitude);
                break;
            }

            case (byte)MavlinkMessageID.BATTERY_STATUS:
            {
                BatteryStatusEvent?.Invoke(packet.Message as UasBatteryStatus);
                break;
            }

            default:
                Debug.WriteLine(packet.MessageId);
                break;
            }

            MavlinkReceivedEvent?.Invoke(packet.Message);
        }
Ejemplo n.º 15
0
 protected override void HandlePacketReceived(object sender, MavLinkPacket e)
 {
     throw new NotImplementedException();
 }
Ejemplo n.º 16
0
        private static void PrintPacket(MavLinkPacket p)
        {
            WL("FROM {0} _______________________________________________________________", p.SystemId);

            PrintMessage(p.Message);
        }
Ejemplo n.º 17
0
        // __ Packet processing _______________________________________________


        static void OnMavLinkPacketReceived(object sender, MavLinkPacket e)
        {
            PrintPacket(e);
            //PrintMessage(e.Message);
            PrintPacketCount();
        }
Ejemplo n.º 18
0
 private void PrintPacket(MavLinkPacket p)
 {
     PrintMessage(p.Message);
 }
        protected void NotifyPacketDiscarded(MavLinkPacket packet)
        {
            if (packet == null || PacketDiscarded == null) return;

            PacketDiscarded(this, packet);
        }
Ejemplo n.º 20
0
 void mMavlinkClient_OnPacketReceived(object sender, MavLinkPacket packet)
 {
 }