public void SendMessage(MavLinkMessage msg)
{
msg.SequenceNumber = seqno++;
byte[] buffer = msg.Pack();
if (this.port == null)
{
throw new Exception("Please call Start to provide the mavlink Port we are using");
}
this.port.Write(buffer, buffer.Length);
}
public MavlinkChannel()
{
// plug in our custom mavlink_simulator_telemetry message
int id = MAVLink.mavlink_telemetry.MessageId;
Type info = MavLinkMessage.GetMavlinkType((uint)id);
if (info != null && typeof(MAVLink.mavlink_telemetry) != info)
{
throw new Exception("The custom messageid " + id + " is already defined, so we can't use it for mavlink_simulator_telemetry");
}
else
{
MAVLink.MAVLINK_MESSAGE_INFO[id] = typeof(MAVLink.mavlink_telemetry);
}
}
void ReceiveThread()
{
byte[] buffer = new byte[1];
MavLinkMessage msg = null;
ReadState state = ReadState.Init;
int payloadPos = 0;
ushort crc = 0;
System.Diagnostics.Stopwatch watch = new System.Diagnostics.Stopwatch();
watch.Start();
try
{
while (this.port != null)
{
int len = this.port.Read(buffer, 1);
if (len == 1)
{
byte b = buffer[0];
switch (state)
{
case ReadState.Init:
if (b == MagicMarker)
{
state = ReadState.GotMagic;
msg = new MavLinkMessage();
msg.Time = (ulong)watch.ElapsedMilliseconds * 1000; // must be in microseconds
msg.Magic = MagicMarker;
payloadPos = 0;
crc = 0;
}
break;
case ReadState.GotMagic:
msg.Length = b;
msg.Payload = new byte[msg.Length];
state = ReadState.GotLength;
break;
case ReadState.GotLength:
msg.SequenceNumber = b;
state = ReadState.GotSequenceNumber;
break;
case ReadState.GotSequenceNumber:
msg.SystemId = b;
state = ReadState.GotSystemId;
break;
case ReadState.GotSystemId:
msg.ComponentId = b;
state = ReadState.GotComponentId;
break;
case ReadState.GotComponentId:
msg.MsgId = (MAVLink.MAVLINK_MSG_ID)b;
if (msg.Length == 0)
{
// done!
state = ReadState.GotPayload;
}
else
{
state = ReadState.GotMessageId;
}
break;
case ReadState.GotMessageId:
// read in the payload.
msg.Payload[payloadPos++] = b;
if (payloadPos == msg.Length)
{
state = ReadState.GotPayload;
}
break;
case ReadState.GotPayload:
crc = b;
state = ReadState.GotCrc1;
break;
case ReadState.GotCrc1:
crc = (ushort)((b << 8) + crc);
// ok, let's see if it's good.
msg.Crc = crc;
ushort found = msg.crc_calculate();
if (found != crc)
{
// bad crc!!
}
else
{
// try and deserialize the payload.
msg.Deserialize();
if (MessageReceived != null)
{
MessageReceived(this, msg);
}
}
state = ReadState.Init;
break;
}
}
}
}
catch (Exception)
{
// port was closed
}
}
void ReceiveThread()
{
byte[] buffer = new byte[1];
const byte MAVLINK_IFLAG_SIGNED = 0x01;
const int MAVLINK_SIGNATURE_BLOCK_LEN = 13;
MavLinkMessage msg = null;
ReadState state = ReadState.Init;
int payloadPos = 0;
ushort crc = 0;
int signaturePos = 0;
uint msgid = 0;
int msgIdPos = 0;
int MaxPayloadLength = 255;
bool messageComplete = false;
System.Diagnostics.Stopwatch watch = new System.Diagnostics.Stopwatch();
watch.Start();
try
{
while (this.port != null)
{
int len = this.port.Read(buffer, 1);
if (len == 1)
{
byte b = buffer[0];
switch (state)
{
case ReadState.Init:
if (b == MavLinkMessage.Mavlink1Stx)
{
state = ReadState.GotMagic;
msg = new MavLinkMessage();
msg.Time = (ulong)watch.ElapsedMilliseconds * 1000; // must be in microseconds
msg.Magic = MavLinkMessage.Mavlink1Stx;
payloadPos = 0;
msgIdPos = 0;
msgid = 0;
crc = 0;
messageComplete = false;
}
else if (b == MavLinkMessage.Mavlink2Stx)
{
state = ReadState.GotMagic;
msg = new MavLinkMessage();
msg.Time = (ulong)watch.ElapsedMilliseconds * 1000; // must be in microseconds
msg.Magic = MavLinkMessage.Mavlink2Stx;
payloadPos = 0;
msgIdPos = 0;
msgid = 0;
crc = 0;
messageComplete = false;
}
break;
case ReadState.GotMagic:
if (b > MaxPayloadLength)
{
state = ReadState.Init;
}
else
{
if (msg.Magic == MavLinkMessage.Mavlink1Stx)
{
msg.IncompatFlags = 0;
msg.CompatFlags = 0;
state = ReadState.GotCompatFlags;
}
else
{
msg.Length = b;
msg.Payload = new byte[msg.Length];
state = ReadState.GotLength;
}
}
break;
case ReadState.GotLength:
msg.IncompatFlags = b;
state = ReadState.GotIncompatFlags;
break;
case ReadState.GotIncompatFlags:
msg.CompatFlags = b;
state = ReadState.GotCompatFlags;
break;
case ReadState.GotCompatFlags:
msg.SequenceNumber = b;
state = ReadState.GotSequenceNumber;
break;
case ReadState.GotSequenceNumber:
msg.SystemId = b;
state = ReadState.GotSystemId;
break;
case ReadState.GotSystemId:
msg.ComponentId = b;
state = ReadState.GotComponentId;
break;
case ReadState.GotComponentId:
if (msg.Magic == MavLinkMessage.Mavlink1Stx)
{
msg.MsgId = (MAVLink.MAVLINK_MSG_ID)b;
if (msg.Length == 0)
{
// done!
state = ReadState.GotPayload;
}
else
{
state = ReadState.GotMessageId;
}
}
else
{
// msgid is 24 bits
switch (msgIdPos)
{
case 0:
msgid = b;
break;
case 1:
msgid |= ((uint)b << 8);
break;
case 2:
msgid |= ((uint)b << 16);
msg.MsgId = (MAVLink.MAVLINK_MSG_ID)msgid;
state = ReadState.GotMessageId;
break;
}
msgIdPos++;
}
break;
case ReadState.GotMessageId:
// read in the payload.
msg.Payload[payloadPos++] = b;
if (payloadPos == msg.Length)
{
state = ReadState.GotPayload;
}
break;
case ReadState.GotPayload:
crc = b;
state = ReadState.GotCrc1;
break;
case ReadState.GotCrc1:
crc = (ushort)((b << 8) + crc);
// ok, let's see if it's good.
msg.Crc = crc;
ushort found = msg.crc_calculate();
if (found != crc)
{
// bad crc!!
}
else
{
if ((msg.IncompatFlags & MAVLINK_IFLAG_SIGNED) == MAVLINK_IFLAG_SIGNED)
{
signaturePos = 0;
msg.Signature = new byte[MAVLINK_SIGNATURE_BLOCK_LEN];
state = ReadState.GetSignature;
}
else
{
messageComplete = true;
}
}
break;
case ReadState.GetSignature:
msg.Signature[signaturePos++] = b;
if (signaturePos == MAVLINK_SIGNATURE_BLOCK_LEN)
{
// todo: check the signature.
messageComplete = true;
}
break;
}
if (messageComplete)
{
// try and deserialize the payload.
msg.Deserialize();
if (MessageReceived != null)
{
MessageReceived(this, msg);
}
// reset for next message.
state = ReadState.Init;
}
}
}
}
catch (Exception)
{
// port was closed
}
}
