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