// __ API _____________________________________________________________
public void SendMessage(MavlinkMessage msg)
{
mSendQueue.Enqueue(msg);
// Signal send thread
mSendSignal.Set();
}
public override void SendMessage(MavlinkMessage msg)
{
MavlinkPacket packet = CreatePacket(msg);
byte[] bytes = Send(packet);
try { serial.Write(bytes, 0, bytes.Length); }
catch { }
}
private void SendMavlinkMessage(IPEndPoint ep, MavlinkMessage msg)
{
MavlinkPacket p = GetPacketFromMsg(msg);
byte[] buffer = mMavLink.Send(p);
mUdpClient.Send(buffer, buffer.Length, ep);
}
private MavlinkPacket CreatePacket(MavlinkMessage msg)
{
MavlinkPacket packet = new MavlinkPacket();
packet.ComponentId = (int)MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;
packet.SystemId = 0xCE;
packet.Message = msg;
return(packet);
}
private MavlinkPacket GetPacketFromMsg(MavlinkMessage msg)
{
MavlinkPacket p = new MavlinkPacket();
p.Message = msg;
p.TimeStamp = DateTime.Now;
p.SequenceNumber = 1;
p.SystemId = MavlinkSystemId;
p.ComponentId = MavlinkComponentId;
return(p);
}
/// <summary>
/// 序列化
/// </summary>
/// <param name="message"></param>
/// <param name="systemId"></param>
/// <param name="componentId"></param>
/// <returns></returns>
public byte[] Serialize(MavlinkMessage message, int systemId, int componentId)
{
var buff = new byte[256];
buff[0] = (byte)systemId;
buff[1] = (byte)componentId;
var endPos = 3;
var msgId = message.Serialize(buff, ref endPos);
buff[2] = (byte)msgId;
var resultBytes = new byte[endPos];
Array.Copy(buff, resultBytes, endPos);
return(resultBytes);
}
public MavlinkPacket(MavlinkMessage message)
{
Message = message;
}
public abstract void SendMessage(MavlinkMessage msg);
public byte[] Serialize(MavlinkMessage message, int systemId, int componentId)
{
var buff = new byte[256];
buff[0] = (byte)systemId;
buff[1] = (byte)componentId;
var endPos = 3;
var msgId = message.Serialize(buff, ref endPos);
buff[2] = (byte)msgId;
var resultBytes = new byte[endPos];
Array.Copy(buff, resultBytes, endPos);
return resultBytes;
}
