public static byte[] GetBytesForMessage(
UasMessage msg, byte systemId, byte componentId, byte sequenceNumber, byte signalMark)
{
MavLinkPacket p = MavLinkPacket.GetPacketForMessage(
msg, systemId, componentId, sequenceNumber);
int bufferSize = p.GetPacketSize();
if (signalMark != 0)
{
bufferSize++;
}
byte[] result = new byte[bufferSize];
using (MemoryStream s = new MemoryStream(result))
{
using (BinaryWriter w = new BinaryWriter(s))
{
if (signalMark != 0)
{
w.Write(signalMark);
}
p.Serialize(w);
}
}
return(result);
}
// __ API _____________________________________________________________
public override void SendMessage(UasMessage msg)
{
mSendQueue.Enqueue(msg);
// Signal send thread
mSendSignal.Set();
}
// __ Serialization ___________________________________________________
public static MavLinkPacket GetPacketForMessage(
UasMessage msg, byte systemId, byte componentId, byte sequenceNumber)
{
MavLinkPacket result = new MavLinkPacket()
{
SystemId = systemId,
InCompatFlags = 0,
CompatFlats = 0,
ComponentId = componentId,
PacketSequenceNumber = sequenceNumber,
MessageId = msg.MessageId,
Message = msg
};
using (MemoryStream ms = new MemoryStream())
{
using (BinaryWriter bw = new BinaryWriter(ms))
{
msg.SerializeBody(bw);
}
result.Payload = ms.ToArray();
result.PayLoadLength = (byte)result.Payload.Length;
result.UpdateCrc();
}
return(result);
}
public override void SendMessage(UasMessage msg)
{
// No messages are sent on this transport (only read from the logfile)
mSendQueue.Enqueue(msg);
// Signal send thread
mSendSignal.Set();
}
/// <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));
}
private void SendMavlinkMessage(UasMessage msg)
{
byte[] buffer = mMavLink.SerializeMessage(msg, MavlinkSystemId, MavlinkComponentId, true);
try
{
mSerialPort.Write(buffer, 0, buffer.Length);
}
catch
{
//throw new Exception("Serialpoart is closed.");
}
}
private void DeserializeMessage()
{
UasMessage result = UasSummary.CreateFromId(MessageId);
if (result == null)
{
return; // Unknown type
}
using (MemoryStream ms = new MemoryStream(Payload)) //在组装包的时候进行解密(未完成)
{
using (BinaryReader br = GetBinaryReader(ms))
{
try
{
result.DeserializeBody(br);
}
catch { }//mavlinkv2会精简为0的包PayLoad从零开始就不发了
}
}
Message = result;
IsValid = true;
}
private void SendMavlinkMessage(IPEndPoint ep, UasMessage msg)
{
byte[] buffer = mMavLink.SerializeMessage(msg, MavlinkSystemId, MavlinkComponentId, true);
//buffer = BtyesToBytes(buffer);
mUdpClient.Send(buffer, buffer.Length, ep);
}
public abstract void SendMessage(UasMessage msg);