public void ByteArrayToStructure()
{
var array = new byte[100];
for (var l = 0; l < array.Length; l++)
{
array[l] = (byte)l;
}
var obj = (object)new MAVLink.mavlink_heartbeat_t();
int a = 0;
for (a = 0; a < 1000000; a++)
{
MavlinkUtil.ByteArrayToStructure(array, ref obj, 6, 5);
}
}
public MavLinkMessage(byte[] buffer)
{
// fill out standard fields
header = buffer[0];
length = (int)buffer[1];
seq = (int)buffer[2];
sysid = (int)buffer[3];
compid = (int)buffer[4];
messid = (MAVLink.MAVLINK_MSG_ID)buffer[5];
//create the object specified by the packet type
object data = Activator.CreateInstance(getMessageType());
// fill in the data of the object
MavlinkUtil.ByteArrayToStructure(buffer, ref data, 6);
data_struct = data;
}
private void but_structtest_Click(object sender, EventArgs e)
{
var array = new byte[100];
for (var l = 0; l < array.Length; l++)
{
array[l] = (byte)l;
}
var a = 0;
var start = DateTime.MinValue;
var end = DateTime.MinValue;
start = DateTime.Now;
for (a = 0; a < 1000000; a++)
{
var obj = (object)new MAVLink.mavlink_heartbeat_t();
MavlinkUtil.ByteArrayToStructure(array, ref obj, 6);
}
end = DateTime.Now;
Console.WriteLine("ByteArrayToStructure " + (end - start).TotalMilliseconds);
start = DateTime.Now;
for (a = 0; a < 1000000; a++)
{
var ans1 = MavlinkUtil.ByteArrayToStructureT <MAVLink.mavlink_heartbeat_t>(array, 6);
}
end = DateTime.Now;
Console.WriteLine("ByteArrayToStructureT<> " + (end - start).TotalMilliseconds);
start = DateTime.Now;
for (a = 0; a < 1000000; a++)
{
var ans2 = MavlinkUtil.ReadUsingPointer <MAVLink.mavlink_heartbeat_t>(array, 6);
}
end = DateTime.Now;
Console.WriteLine("ReadUsingPointer " + (end - start).TotalMilliseconds);
start = DateTime.Now;
for (a = 0; a < 1000000; a++)
{
var ans3 = MavlinkUtil.ByteArrayToStructureGC <MAVLink.mavlink_heartbeat_t>(array, 6);
}
end = DateTime.Now;
Console.WriteLine("ByteArrayToStructureGC " + (end - start).TotalMilliseconds);
}
public MAVLinkMessage ReadPacketObj(Stream BaseStream)
{
byte[] buffer = ReadPacket(BaseStream);
byte header = buffer[0];
byte length = buffer[1];
byte seq = buffer[2];
byte sysid = buffer[3];
byte compid = buffer[4];
byte messid = buffer[5];
//create the object specified by the packet type
object data = Activator.CreateInstance(MAVLINK_MESSAGE_INFO[messid]);
// fill in the data of the object
MavlinkUtil.ByteArrayToStructure(buffer, ref data, 6);
return(new MAVLinkMessage(header, length, seq, sysid, compid, messid, data));
}
public async Task <InvokeResult <TMavlinkPacket> > WaitForMessageAsync <TMavlinkPacket>(MAVLINK_MSG_ID messageId, TimeSpan timeout) where TMavlinkPacket : struct
{
try
{
Debug.WriteLine("Start Waiting" + DateTime.Now.ToString());
var wor = new WaitOnRequest <object>((uint)messageId);
Sessions[(uint)messageId] = wor;
MAVLinkMessage message = null;
for (var idx = 0; (idx < timeout.TotalMilliseconds / 100) && message == null; ++idx)
{
if (wor.CompletionSource.Task.IsCompleted)
{
Debug.WriteLine("TASK IS COMPLETED!!!!");
message = wor.CompletionSource.Task.Result as MAVLinkMessage;
}
await Task.Delay(100);
}
Debug.WriteLine("End Waiting" + DateTime.Now.ToString());
if (message == null)
{
return(InvokeResult <TMavlinkPacket> .FromError("Timeout waiting for message."));
}
else
{
var result = MavlinkUtil.ByteArrayToStructure <TMavlinkPacket>(message.buffer);
return(InvokeResult <TMavlinkPacket> .Create(result));
}
}
catch (Exception ex)
{
return(InvokeResult <TMavlinkPacket> .FromException("AsyncCoupler_WaitOnAsync", ex));
}
finally
{
Sessions.TryRemove((uint)messageId, out WaitOnRequest <Object> obj);
}
}
public object ReadPacket(Stream BaseStream)
{
byte[] buffer = new byte[270];
int readcount = 0;
while (readcount < 200)
{
// read STX byte
BaseStream.Read(buffer, 0, 1);
if (buffer[0] == MAVLink.MAVLINK_STX)
{
break;
}
readcount++;
}
// read header
int read = BaseStream.Read(buffer, 1, 5);
// packet length
int lengthtoread = buffer[1] + 6 + 2 - 2; // data + header + checksum - STX - length
//read rest of packet
read = BaseStream.Read(buffer, 6, lengthtoread - 4);
// resize the packet to the correct length
Array.Resize <byte>(ref buffer, lengthtoread + 2);
// calc crc
ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2);
// calc extra bit of crc for mavlink 1.0
if (buffer.Length > 5 && buffer[0] == 254)
{
crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[buffer[5]], crc);
}
// check message length vs table
if (buffer.Length > 5 && buffer[1] != MAVLINK_MESSAGE_LENGTHS[buffer[5]])
{
// bad or unknown packet
return(null);
}
// check crc
if (buffer.Length < 5 || buffer[buffer.Length - 1] != (crc >> 8) || buffer[buffer.Length - 2] != (crc & 0xff))
{
// crc fail
return(null);
}
byte header = buffer[0];
byte length = buffer[1];
byte seq = buffer[2];
byte sysid = buffer[3];
byte compid = buffer[4];
byte messid = buffer[5];
//create the object spcified by the packet type
object data = Activator.CreateInstance(MAVLINK_MESSAGE_INFO[messid]);
// fill in the data of the object
MavlinkUtil.ByteArrayToStructure(buffer, ref data, 6);
return(data);
}
public Task UpdateDroneAsync(IDrone drone, MAVLink.MAVLinkMessage msg)
{
switch ((MAVLink.MAVLINK_MSG_ID)msg.msgid)
{
case MAVLink.MAVLINK_MSG_ID.HEARTBEAT:
var hb = MavlinkUtil.ByteArrayToStructure <mavlink_heartbeat_t>(msg.payload);
break;
case MAVLINK_MSG_ID.RAW_IMU:
var imu = MavlinkUtil.ByteArrayToStructure <mavlink_raw_imu_t>(msg.payload);
drone.Acc.X = imu.xacc;
drone.Acc.Y = imu.yacc;
drone.Acc.Z = imu.zacc;
drone.Gyro.X = imu.xgyro;
drone.Gyro.Y = imu.ygyro;
drone.Gyro.Z = imu.zgyro;
drone.Magnometer.X = imu.xmag;
drone.Magnometer.Y = imu.xmag;
drone.Magnometer.Z = imu.zmag;
break;
case MAVLINK_MSG_ID.ATTITUDE:
var att = MavlinkUtil.ByteArrayToStructure <mavlink_attitude_t>(msg.payload);
drone.Pitch = att.pitch;
drone.Roll = att.roll;
drone.Yaw = att.yaw;
drone.PitchSpeed = att.pitchspeed;
drone.RollSpeed = att.rollspeed;
drone.YawSpeed = att.yawspeed;
break;
case MAVLINK_MSG_ID.RC_CHANNELS:
var rc = MavlinkUtil.ByteArrayToStructure <mavlink_rc_channels_raw_t>(msg.payload);
drone.Channels[0].Value = rc.chan1_raw;
drone.Channels[1].Value = rc.chan2_raw;
drone.Channels[2].Value = rc.chan3_raw;
drone.Channels[3].Value = rc.chan4_raw;
drone.Channels[4].Value = rc.chan5_raw;
drone.Channels[5].Value = rc.chan6_raw;
drone.Channels[6].Value = rc.chan7_raw;
drone.Channels[7].Value = rc.chan8_raw;
break;
case MAVLINK_MSG_ID.GLOBAL_POSITION_INT:
var gpsint = MavlinkUtil.ByteArrayToStructure <mavlink_global_position_int_t>(msg.payload);
var location = new GeoLocation()
{
Longitude = gpsint.lon / 1000000.0f,
Latitude = gpsint.lat / 1000000.0f,
Altitude = gpsint.alt / 1000.0f
};
drone.Location = location;
break;
case MAVLINK_MSG_ID.SERVO_OUTPUT_RAW:
var srvout = MavlinkUtil.ByteArrayToStructure <mavlink_servo_output_raw_t>(msg.payload);
drone.ServoOutputs[0].Value = srvout.servo1_raw;
drone.ServoOutputs[1].Value = srvout.servo2_raw;
drone.ServoOutputs[2].Value = srvout.servo3_raw;
drone.ServoOutputs[3].Value = srvout.servo4_raw;
drone.ServoOutputs[4].Value = srvout.servo5_raw;
drone.ServoOutputs[5].Value = srvout.servo6_raw;
drone.ServoOutputs[6].Value = srvout.servo7_raw;
drone.ServoOutputs[7].Value = srvout.servo8_raw;
drone.ServoOutputs[8].Value = srvout.servo9_raw;
drone.ServoOutputs[9].Value = srvout.servo10_raw;
drone.ServoOutputs[10].Value = srvout.servo11_raw;
drone.ServoOutputs[11].Value = srvout.servo12_raw;
drone.ServoOutputs[12].Value = srvout.servo13_raw;
drone.ServoOutputs[13].Value = srvout.servo14_raw;
drone.ServoOutputs[14].Value = srvout.servo15_raw;
drone.ServoOutputs[15].Value = srvout.servo16_raw;
break;
case MAVLINK_MSG_ID.MISSION_ACK:
break;
case MAVLINK_MSG_ID.MISSION_CLEAR_ALL:
break;
case MAVLINK_MSG_ID.MISSION_COUNT:
break;
case MAVLINK_MSG_ID.MISSION_CURRENT:
break;
case MAVLINK_MSG_ID.MISSION_ITEM:
break;
case MAVLINK_MSG_ID.MISSION_ITEM_INT:
break;
case MAVLINK_MSG_ID.MISSION_ITEM_REACHED:
break;
case MAVLINK_MSG_ID.MISSION_REQUEST:
break;
case MAVLINK_MSG_ID.MISSION_REQUEST_INT:
break;
case MAVLINK_MSG_ID.MISSION_REQUEST_LIST:
break;
case MAVLINK_MSG_ID.MISSION_REQUEST_PARTIAL_LIST:
break;
case MAVLINK_MSG_ID.MISSION_SET_CURRENT:
break;
case MAVLINK_MSG_ID.MISSION_WRITE_PARTIAL_LIST:
break;
}
//msg.MessageInfo.Type
return(Task.FromResult(default(Object)));
}
// Parse a single Mavnet message from a slice of a byte array
private MavnetMessage ParseSinglePacket(Span <byte> pkt)
{
MavnetMessage msg = new MavnetMessage();
// Parse out the header
if (pkt[0] == MAVLink.MAVLINK_STX)
{
msg.is_mavlink_v2 = true;
msg.payload_length = pkt[1];
msg.incompat_flags = pkt[2];
msg.compat_flags = pkt[3];
msg.sequence = pkt[4];
msg.system_id = pkt[5];
msg.component_id = pkt[6];
msg.message_id = (uint)((pkt[9] << 16) + (pkt[8] << 8) + pkt[7]);
if ((msg.incompat_flags & MAVLink.MAVLINK_IFLAG_SIGNED) > 0)
{
Span <byte> sig_block = pkt.Slice(pkt.Length - MAVLink.MAVLINK_SIGNATURE_BLOCK_LEN);
msg.signature_link_id = sig_block[0];
msg.signature_timestamp = BitConverter.ToUInt64(sig_block.Slice(1, 6));
msg.signature = sig_block.Slice(7).ToArray();
}
}
else
{
msg.is_mavlink_v2 = false;
msg.payload_length = pkt[1];
msg.sequence = pkt[2];
msg.system_id = pkt[3];
msg.component_id = pkt[4];
msg.message_id = pkt[5];
}
// Lookup the message type info.
MAVLink.message_info messageInfo;
if (!_messageInfoTable.TryGetValue(msg.message_id, out messageInfo))
{
log("Unknown MAVLink message ID: " + msg.message_id);
unknownMessageTypes++;
return(null);
}
msg.message_name = messageInfo.name;
// Check the CRC
var headerLen = msg.is_mavlink_v2 ? MAVLink.MAVLINK_CORE_HEADER_LEN : MAVLink.MAVLINK_CORE_HEADER_MAVLINK1_LEN;
var crc1 = headerLen + msg.payload_length + 1;
var crc2 = crc1 + 1;
ushort crc16 = (ushort)((pkt[crc2] << 8) + pkt[crc1]);
ushort calcCrc = MAVLink.MavlinkCRC.crc_calculate(pkt.ToArray(), pkt.Length - 2);
calcCrc = MAVLink.MavlinkCRC.crc_accumulate(messageInfo.crc, calcCrc);
if (crc16 != calcCrc)
{
log(string.Format("Bad message CRC for {0} message: expected: {1}, found: {2} ", msg.message_name, calcCrc, crc16));
crcErrors++;
return(null);
}
// Deserialize the payload
try
{
object payload = Activator.CreateInstance(messageInfo.type);
if (msg.is_mavlink_v2)
{
MavlinkUtil.ByteArrayToStructure(pkt.ToArray(), ref payload, MAVLink.MAVLINK_NUM_HEADER_BYTES, msg.payload_length);
}
else
{
MavlinkUtil.ByteArrayToStructure(pkt.ToArray(), ref payload, 6, msg.payload_length);
}
msg.payload = payload;
}
catch (Exception ex)
{
log(ex.ToString());
return(null);
}
return(msg);
}
