// __ 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); }
protected void NotifyPacketDiscarded(MavLinkPacket packet) { if (packet == null || PacketDiscarded == null) { return; } PacketDiscarded(this, packet); }
/// <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)); }
protected void Complete(MavLinkPacket msg) { if (Sessions.TryGetValue(msg.MessageId, out var tcs)) { if (!tcs.CompletionSource.Task.IsCompleted) { tcs.CompletionSource.SetResult(msg); } } }
private void OnMavLinkPacketReceived(object sender, MavLinkPacket packet) { if (packet.Message is UasCommandLong) { PrintCommandLong(packet.Message as UasCommandLong); } else { PrintStandardMessage(packet.Message); } }
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); } }
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); } }
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); } } } }
private void OnMavLinkPacketReceived(object sender, MavLinkPacket e) { PrintPacket(e); }
protected override void HandlePacketReceived(object sender, MavLinkPacket e) { }
private void ParserOnPacketReceived(object sender, MavLinkPacket packet) { mPackets.Add(packet); }
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); }
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); }
protected override void HandlePacketReceived(object sender, MavLinkPacket e) { throw new NotImplementedException(); }
private static void PrintPacket(MavLinkPacket p) { WL("FROM {0} _______________________________________________________________", p.SystemId); PrintMessage(p.Message); }
// __ Packet processing _______________________________________________ static void OnMavLinkPacketReceived(object sender, MavLinkPacket e) { PrintPacket(e); //PrintMessage(e.Message); PrintPacketCount(); }
private void PrintPacket(MavLinkPacket p) { PrintMessage(p.Message); }
protected void NotifyPacketDiscarded(MavLinkPacket packet) { if (packet == null || PacketDiscarded == null) return; PacketDiscarded(this, packet); }
void mMavlinkClient_OnPacketReceived(object sender, MavLinkPacket packet) { }