286 lines
11 KiB
C#
286 lines
11 KiB
C#
#if NET45
|
|
|
|
using System;
|
|
using System.Runtime.InteropServices;
|
|
|
|
//using log4net;
|
|
|
|
namespace Plane.Protocols
|
|
{
|
|
/// <summary>
|
|
/// Static methods and helpers for creation and manipulation of Mavlink packets
|
|
/// </summary>
|
|
internal static class MavlinkUtil
|
|
{
|
|
//private static readonly ILog log =
|
|
// LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
|
|
|
|
/// <summary>
|
|
/// Create a new mavlink packet object from a byte array as recieved over mavlink
|
|
/// Endianess will be detetected using packet inspection
|
|
/// </summary>
|
|
/// <typeparam name="TMavlinkPacket">The type of mavlink packet to create</typeparam>
|
|
/// <param name="bytearray">The bytes of the mavlink packet</param>
|
|
/// <param name="startoffset">The position in the byte array where the packet starts</param>
|
|
/// <returns>The newly created mavlink packet</returns>
|
|
internal static TMavlinkPacket ByteArrayToStructure<TMavlinkPacket>(this byte[] bytearray, int startoffset = 6) where TMavlinkPacket : struct
|
|
{
|
|
if (bytearray[0] == 'U')
|
|
{
|
|
throw new NotSupportedException("bytearray[0] == 'U' is not supported.");
|
|
//ByteArrayToStructureEndian(bytearray, ref obj, startoffset);
|
|
}
|
|
else
|
|
{
|
|
int len = MarshalEx.SizeOf<TMavlinkPacket>();
|
|
|
|
IntPtr i = Marshal.AllocHGlobal(len);
|
|
TMavlinkPacket packet;
|
|
|
|
try
|
|
{
|
|
// copy byte array to ptr
|
|
Marshal.Copy(bytearray, startoffset, i, len);
|
|
packet = MarshalEx.PtrToStructure<TMavlinkPacket>(i);
|
|
}
|
|
// 王海, 20151026, 改为不吞异常了。
|
|
//catch
|
|
//{
|
|
// //log.Error("ByteArrayToStructure FAIL", ex);
|
|
//}
|
|
finally
|
|
{
|
|
Marshal.FreeHGlobal(i);
|
|
}
|
|
|
|
return packet;
|
|
}
|
|
}
|
|
|
|
internal static TMavlinkPacket ByteArrayToStructureBigEndian<TMavlinkPacket>(this byte[] bytearray, int startoffset = 6) where TMavlinkPacket : struct
|
|
{
|
|
object newPacket = new TMavlinkPacket();
|
|
throw new NotSupportedException("ByteArrayToStructureBigEndian is not supported.");
|
|
//ByteArrayToStructureEndian(bytearray, ref newPacket, startoffset);
|
|
//return (TMavlinkPacket)newPacket;
|
|
}
|
|
|
|
//internal static void ByteArrayToStructureEndian(byte[] bytearray, ref object obj, int startoffset)
|
|
//{
|
|
// int len = Marshal.SizeOf(obj);
|
|
// IntPtr i = Marshal.AllocHGlobal(len);
|
|
// byte[] temparray = (byte[])bytearray.Clone();
|
|
|
|
// // create structure from ptr
|
|
// obj = Marshal.PtrToStructure(i, obj.GetType());
|
|
|
|
// // do endian swap
|
|
// object thisBoxed = obj;
|
|
// Type test = thisBoxed.GetType();
|
|
|
|
// int reversestartoffset = startoffset;
|
|
|
|
// // Enumerate each structure field using reflection.
|
|
// foreach (var field in test.GetRuntimeFields())
|
|
// {
|
|
// // field.Name has the field's name.
|
|
// object fieldValue = field.GetValue(thisBoxed); // Get value
|
|
|
|
// // Get the TypeCode enumeration. Multiple types get mapped to a common typecode.
|
|
// TypeCode typeCode = Type.GetTypeCode(fieldValue.GetType());
|
|
|
|
// if (typeCode != TypeCode.Object)
|
|
// {
|
|
// Array.Reverse(temparray, reversestartoffset, Marshal.SizeOf(fieldValue));
|
|
// reversestartoffset += Marshal.SizeOf(fieldValue);
|
|
// }
|
|
// else
|
|
// {
|
|
// reversestartoffset += ((byte[])fieldValue).Length;
|
|
// }
|
|
|
|
// }
|
|
|
|
// try
|
|
// {
|
|
// // copy byte array to ptr
|
|
// Marshal.Copy(temparray, startoffset, i, len);
|
|
// }
|
|
// catch
|
|
// {
|
|
// //log.Error("ByteArrayToStructure FAIL", ex);
|
|
// }
|
|
|
|
// obj = Marshal.PtrToStructure(i, obj.GetType());
|
|
|
|
// Marshal.FreeHGlobal(i);
|
|
|
|
//}
|
|
|
|
/// <summary>
|
|
/// Convert a struct to an array of bytes, struct fields being reperesented in
|
|
/// little endian (LSB first)
|
|
/// </summary>
|
|
/// <remarks>Note - assumes little endian host order</remarks>
|
|
internal static byte[] StructureToByteArray<TMavlinkPacket>(TMavlinkPacket packet)
|
|
{
|
|
int len = MarshalEx.SizeOf(packet);
|
|
byte[] arr = new byte[len];
|
|
IntPtr ptr = Marshal.AllocHGlobal(len);
|
|
try
|
|
{
|
|
Marshal.StructureToPtr(packet, ptr, true);
|
|
Marshal.Copy(ptr, arr, 0, len);
|
|
}
|
|
finally
|
|
{
|
|
Marshal.FreeHGlobal(ptr);
|
|
}
|
|
return arr;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert a struct to an array of bytes, struct fields being reperesented in
|
|
/// big endian (MSB first)
|
|
/// </summary>
|
|
internal static byte[] StructureToByteArrayBigEndian<TMavlinkPacket>(TMavlinkPacket packet)
|
|
{
|
|
//// The copy is made becuase SetValue won't work on a struct.
|
|
//// Boxing was used because SetValue works on classes/objects.
|
|
//// Unfortunately, it results in 2 copy operations.
|
|
//object thisBoxed = list[0]; // Why make a copy?
|
|
|
|
object thisBoxed = packet;
|
|
Type test = thisBoxed.GetType();
|
|
|
|
int offset = 0;
|
|
byte[] data = new byte[Marshal.SizeOf(packet)];
|
|
|
|
object fieldValue;
|
|
//TypeCode typeCode;
|
|
|
|
byte[] temp;
|
|
|
|
// Enumerate each structure field using reflection.
|
|
foreach (var field in test.GetFields())
|
|
{
|
|
// field.Name has the field's name.
|
|
|
|
fieldValue = field.GetValue(thisBoxed); // Get value
|
|
|
|
//// Get the TypeCode enumeration. Multiple types get mapped to a common typecode.
|
|
//typeCode = Type.GetTypeCode(fieldValue.GetType());
|
|
|
|
//switch (typeCode)
|
|
switch (fieldValue.GetType().Name)
|
|
{
|
|
case nameof(Single): // float
|
|
{
|
|
temp = BitConverter.GetBytes((Single)fieldValue);
|
|
Array.Reverse(temp);
|
|
Array.Copy(temp, 0, data, offset, sizeof(Single));
|
|
offset += MarshalEx.SizeOf<Single>();
|
|
break;
|
|
}
|
|
case nameof(Int32):
|
|
{
|
|
temp = BitConverter.GetBytes((Int32)fieldValue);
|
|
Array.Reverse(temp);
|
|
Array.Copy(temp, 0, data, offset, sizeof(Int32));
|
|
offset += MarshalEx.SizeOf<Int32>();
|
|
break;
|
|
}
|
|
case nameof(UInt32):
|
|
{
|
|
temp = BitConverter.GetBytes((UInt32)fieldValue);
|
|
Array.Reverse(temp);
|
|
Array.Copy(temp, 0, data, offset, sizeof(UInt32));
|
|
offset += MarshalEx.SizeOf<UInt32>();
|
|
break;
|
|
}
|
|
case nameof(Int16):
|
|
{
|
|
temp = BitConverter.GetBytes((Int16)fieldValue);
|
|
Array.Reverse(temp);
|
|
Array.Copy(temp, 0, data, offset, sizeof(Int16));
|
|
offset += MarshalEx.SizeOf<Int16>();
|
|
break;
|
|
}
|
|
case nameof(UInt16):
|
|
{
|
|
temp = BitConverter.GetBytes((UInt16)fieldValue);
|
|
Array.Reverse(temp);
|
|
Array.Copy(temp, 0, data, offset, sizeof(UInt16));
|
|
offset += MarshalEx.SizeOf<UInt16>();
|
|
break;
|
|
}
|
|
case nameof(Int64):
|
|
{
|
|
temp = BitConverter.GetBytes((Int64)fieldValue);
|
|
Array.Reverse(temp);
|
|
Array.Copy(temp, 0, data, offset, sizeof(Int64));
|
|
offset += MarshalEx.SizeOf<Int64>();
|
|
break;
|
|
}
|
|
case nameof(UInt64):
|
|
{
|
|
temp = BitConverter.GetBytes((UInt64)fieldValue);
|
|
Array.Reverse(temp);
|
|
Array.Copy(temp, 0, data, offset, sizeof(UInt64));
|
|
offset += MarshalEx.SizeOf<UInt64>();
|
|
break;
|
|
}
|
|
case nameof(Double):
|
|
{
|
|
temp = BitConverter.GetBytes((Double)fieldValue);
|
|
Array.Reverse(temp);
|
|
Array.Copy(temp, 0, data, offset, sizeof(Double));
|
|
offset += MarshalEx.SizeOf<Double>();
|
|
break;
|
|
}
|
|
case nameof(Byte):
|
|
{
|
|
data[offset] = (Byte)fieldValue;
|
|
offset += MarshalEx.SizeOf<Byte>();
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
int length = ((byte[])fieldValue).Length;
|
|
Array.Copy(((byte[])fieldValue), 0, data, offset, length);
|
|
offset += length;
|
|
|
|
//System.Diagnostics.Debug.Fail("No conversion provided for this type : " + typeCode.ToString());
|
|
break;
|
|
}
|
|
}; // switch
|
|
} // foreach
|
|
|
|
return data;
|
|
} // Swap
|
|
|
|
/// <summary>
|
|
/// 为 .NET 4.5.1 以下版本提供 SizeOf<T>、PtrToStructure<T> 等方法。
|
|
/// </summary>
|
|
private static class MarshalEx
|
|
{
|
|
public static T PtrToStructure<T>(IntPtr ptr)
|
|
{
|
|
return (T)Marshal.PtrToStructure(ptr, typeof(T));
|
|
}
|
|
|
|
public static int SizeOf<T>()
|
|
{
|
|
return Marshal.SizeOf(typeof(T));
|
|
}
|
|
|
|
public static int SizeOf<T>(T structure)
|
|
{
|
|
return Marshal.SizeOf(structure.GetType());
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
#endif
|