private void postMessageToLocalEventQueue(MavLinkMessage message)
{
// don't post any messages if we don't know who we're communicating with
if (-1 == this.systemId)
{
return;
}
MavLinkMessages(this, new MavLinkEventArgs(message));
}
// parses data from stream into MavLinkMessage objects
MavLinkMessage ReadMessage(Stream BaseStream)
{
byte[] buffer = this.mavlinkParse.ReadPacket(BaseStream);
if (null == buffer)
{
return(null);
}
if (buffer.Length >= 6)
{
MavLinkMessage message = new MavLinkMessage(buffer);
return(message);
}
logger.Error("Bad Message recieved with size less than 6: ", System.Text.Encoding.Default.GetString(buffer));
return(null);
}
// reads for readtime_ms and calls ReadMessage() on each packet from connection, stores messages in the object member 'readQueue'
void readFromStream()
{
DateTime deadline = DateTime.Now.AddMilliseconds(MavLinkConnection.readtime_ms);
// read the current buffered bytes
while (DateTime.Now < deadline)
{
MavLinkMessage message = ReadMessage(port.BaseStream);
if (message == null)
{
continue;
}
if (message.messid.Equals(MAVLink.MAVLINK_MSG_ID.HEARTBEAT))
{
logger.Trace("{1} Message read from target system: {0}, component id {3}, of type {2}", message.sysid, message.messid, message.getMessageType().ToString(), message.compid);
}
logger.Trace("{1} Message read from target system: {0}, component id {3}, of type {2}", message.sysid, message.messid, message.getMessageType().ToString(), message.compid);
this.systemId = message.sysid;
this.componentId = message.compid;
this.postMessageToLocalEventQueue(message);
}
}
public MavLinkEventArgs(MavLinkMessage message)
{
this.message = message;
}