Skip to content

Commit

Permalink
MAVLink2 refactoring
Browse files Browse the repository at this point in the history
  • Loading branch information
@meee1
meee1 committed Apr 21, 2016
1 parent c8a0ba9 commit 4307f7a
Show file tree
Hide file tree
Showing 20 changed files with 575 additions and 397 deletions.
2 changes: 1 addition & 1 deletion Comms/MAVLinkSerialPort.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ bool ReceviedPacket(MAVLink.MAVLinkMessage packet)

packetcount++;

MAVLink.mavlink_serial_control_t item = packet.ByteArrayToStructure<MAVLink.mavlink_serial_control_t>();
MAVLink.mavlink_serial_control_t item = packet.ToStructure<MAVLink.mavlink_serial_control_t>();

if (item.count == 0)
return true;
Expand Down
240 changes: 120 additions & 120 deletions CurrentState.cs
View file

Large diffs are not rendered by default.

23 changes: 15 additions & 8 deletions ExtLibs/Mavlink/MAVLinkMessage.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ public object data
}
}

public T ByteArrayToStructure<T>()
public T ToStructure<T>()
{
return (T)data;
}
Expand All @@ -51,14 +51,14 @@ public T ByteArrayToStructure<T>()

public byte[] sig;

/*public byte this[int index]
public int Length
{
get { return buffer[index]; }
}*/

public int Length { get
{
if (buffer == null) return 0; return buffer.Length; } }
get
{
if (buffer == null) return 0;
return buffer.Length;
}
}

public MAVLinkMessage()
{
Expand All @@ -84,6 +84,13 @@ public MAVLinkMessage(byte[] buffer)
var crc2 = 9 + payloadlength+2;

crc16 = (ushort)((buffer[crc2] << 8) + buffer[crc1]);

if ((incompat_flags & MAVLINK_IFLAG_SIGNED) > 0)
{
sig = new byte[MAVLINK_SIGNATURE_BLOCK_LEN];
Array.ConstrainedCopy(buffer, buffer.Length - MAVLINK_SIGNATURE_BLOCK_LEN, sig, 0,
MAVLINK_SIGNATURE_BLOCK_LEN);
}
}
else
{
Expand Down
151 changes: 77 additions & 74 deletions ExtLibs/Mavlink/MavlinkUtil.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,9 @@
using System.Text;


/// <summary>
/// Static methods and helpers for creation and manipulation of Mavlink packets
/// </summary>
/// <summary>
/// Static methods and helpers for creation and manipulation of Mavlink packets
/// </summary>
public static class MavlinkUtil
{
/// <summary>
Expand All @@ -18,19 +18,21 @@ public static class MavlinkUtil
/// <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>
public static TMavlinkPacket ByteArrayToStructure<TMavlinkPacket>(this byte[] bytearray, int startoffset = 6) where TMavlinkPacket : struct
public static TMavlinkPacket ByteArrayToStructure<TMavlinkPacket>(this byte[] bytearray, int startoffset = 6)
where TMavlinkPacket : struct
{
return ReadUsingPointer<TMavlinkPacket>(bytearray,startoffset);
return ReadUsingPointer<TMavlinkPacket>(bytearray, startoffset);
}

public static TMavlinkPacket ByteArrayToStructureBigEndian<TMavlinkPacket>(this byte[] bytearray, int startoffset = 6) where TMavlinkPacket : struct
public static TMavlinkPacket ByteArrayToStructureBigEndian<TMavlinkPacket>(this byte[] bytearray,
int startoffset = 6) where TMavlinkPacket : struct
{
object newPacket = new TMavlinkPacket();
ByteArrayToStructureEndian(bytearray, ref newPacket, startoffset);
return (TMavlinkPacket)newPacket;
return (TMavlinkPacket) newPacket;
}
public static void ByteArrayToStructure(byte[] bytearray, ref object obj, int startoffset)

public static void ByteArrayToStructure(byte[] bytearray, ref object obj, int startoffset)
{
int len = Marshal.SizeOf(obj);

Expand Down Expand Up @@ -67,11 +69,11 @@ public static TMavlinkPacket ByteArrayToStructureT<TMavlinkPacket>(byte[] bytear
Console.WriteLine("ByteArrayToStructure FAIL " + ex.Message);
}

var obj = Marshal.PtrToStructure(i, typeof(TMavlinkPacket));
var obj = Marshal.PtrToStructure(i, typeof (TMavlinkPacket));

Marshal.FreeHGlobal(i);

return (TMavlinkPacket)obj;
return (TMavlinkPacket) obj;
}

public static T ReadUsingPointer<T>(byte[] data, int startoffset) where T : struct
Expand All @@ -80,7 +82,7 @@ public static T ReadUsingPointer<T>(byte[] data, int startoffset) where T : stru
{
fixed (byte* p = &data[startoffset])
{
return (T)Marshal.PtrToStructure(new IntPtr(p), typeof(T));
return (T) Marshal.PtrToStructure(new IntPtr(p), typeof (T));
}
}
}
Expand All @@ -90,7 +92,7 @@ public static T ByteArrayToStructureGC<T>(byte[] bytearray, int startoffset) whe
GCHandle gch = GCHandle.Alloc(bytearray, GCHandleType.Pinned);
try
{
return (T)Marshal.PtrToStructure(new IntPtr(gch.AddrOfPinnedObject().ToInt64() + startoffset) , typeof(T));
return (T) Marshal.PtrToStructure(new IntPtr(gch.AddrOfPinnedObject().ToInt64() + startoffset), typeof (T));
}
finally
{
Expand All @@ -103,7 +105,7 @@ public static void ByteArrayToStructureEndian(byte[] bytearray, ref object obj,

int len = Marshal.SizeOf(obj);
IntPtr i = Marshal.AllocHGlobal(len);
byte[] temparray = (byte[])bytearray.Clone();
byte[] temparray = (byte[]) bytearray.Clone();

// create structure from ptr
obj = Marshal.PtrToStructure(i, obj.GetType());
Expand All @@ -130,7 +132,7 @@ public static void ByteArrayToStructureEndian(byte[] bytearray, ref object obj,
}
else
{
reversestartoffset += ((byte[])fieldValue).Length;
reversestartoffset += ((byte[]) fieldValue).Length;
}

}
Expand Down Expand Up @@ -200,76 +202,77 @@ public static byte[] StructureToByteArrayBigEndian(params object[] list)
switch (typeCode)
{
case TypeCode.Single: // float
{
temp = BitConverter.GetBytes((Single)fieldValue);
Array.Reverse(temp);
Array.Copy(temp, 0, data, offset, sizeof(Single));
break;
}
{
temp = BitConverter.GetBytes((Single) fieldValue);
Array.Reverse(temp);
Array.Copy(temp, 0, data, offset, sizeof (Single));
break;
}
case TypeCode.Int32:
{
temp = BitConverter.GetBytes((Int32)fieldValue);
Array.Reverse(temp);
Array.Copy(temp, 0, data, offset, sizeof(Int32));
break;
}
{
temp = BitConverter.GetBytes((Int32) fieldValue);
Array.Reverse(temp);
Array.Copy(temp, 0, data, offset, sizeof (Int32));
break;
}
case TypeCode.UInt32:
{
temp = BitConverter.GetBytes((UInt32)fieldValue);
Array.Reverse(temp);
Array.Copy(temp, 0, data, offset, sizeof(UInt32));
break;
}
{
temp = BitConverter.GetBytes((UInt32) fieldValue);
Array.Reverse(temp);
Array.Copy(temp, 0, data, offset, sizeof (UInt32));
break;
}
case TypeCode.Int16:
{
temp = BitConverter.GetBytes((Int16)fieldValue);
Array.Reverse(temp);
Array.Copy(temp, 0, data, offset, sizeof(Int16));
break;
}
{
temp = BitConverter.GetBytes((Int16) fieldValue);
Array.Reverse(temp);
Array.Copy(temp, 0, data, offset, sizeof (Int16));
break;
}
case TypeCode.UInt16:
{
temp = BitConverter.GetBytes((UInt16)fieldValue);
Array.Reverse(temp);
Array.Copy(temp, 0, data, offset, sizeof(UInt16));
break;
}
{
temp = BitConverter.GetBytes((UInt16) fieldValue);
Array.Reverse(temp);
Array.Copy(temp, 0, data, offset, sizeof (UInt16));
break;
}
case TypeCode.Int64:
{
temp = BitConverter.GetBytes((Int64)fieldValue);
Array.Reverse(temp);
Array.Copy(temp, 0, data, offset, sizeof(Int64));
break;
}
{
temp = BitConverter.GetBytes((Int64) fieldValue);
Array.Reverse(temp);
Array.Copy(temp, 0, data, offset, sizeof (Int64));
break;
}
case TypeCode.UInt64:
{
temp = BitConverter.GetBytes((UInt64)fieldValue);
Array.Reverse(temp);
Array.Copy(temp, 0, data, offset, sizeof(UInt64));
break;
}
{
temp = BitConverter.GetBytes((UInt64) fieldValue);
Array.Reverse(temp);
Array.Copy(temp, 0, data, offset, sizeof (UInt64));
break;
}
case TypeCode.Double:
{
temp = BitConverter.GetBytes((Double)fieldValue);
Array.Reverse(temp);
Array.Copy(temp, 0, data, offset, sizeof(Double));
break;
}
{
temp = BitConverter.GetBytes((Double) fieldValue);
Array.Reverse(temp);
Array.Copy(temp, 0, data, offset, sizeof (Double));
break;
}
case TypeCode.Byte:
{
data[offset] = (Byte)fieldValue;
break;
}
{
data[offset] = (Byte) fieldValue;
break;
}
default:
{
//System.Diagnostics.Debug.Fail("No conversion provided for this type : " + typeCode.ToString());
break;
}
}; // switch
{
//System.Diagnostics.Debug.Fail("No conversion provided for this type : " + typeCode.ToString());
break;
}
}
; // switch
if (typeCode == TypeCode.Object)
{
int length = ((byte[])fieldValue).Length;
Array.Copy(((byte[])fieldValue), 0, data, offset, length);
int length = ((byte[]) fieldValue).Length;
Array.Copy(((byte[]) fieldValue), 0, data, offset, length);
offset += length;
}
else
Expand Down
7 changes: 7 additions & 0 deletions ExtLibs/Mavlink/message_definitions/common.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3136,5 +3136,12 @@
<field type="uint8_t" name="ind">index of debug variable</field>
<field type="float" name="value">DEBUG value</field>
</message>
<message id="256" name="SETUP_SIGNING">
<description>Setup a MAVLink2 signing key. If called with secret_key of all zero and zero initial_timestamp will disable signing</description>
<field type="uint8_t" name="target_system">system id of the target</field>
<field type="uint8_t" name="target_component">component ID of the target</field>
<field type="uint8_t[32]" name="secret_key">signing key</field>
<field type="uint64_t" name="initial_timestamp">initial timestamp</field>
</message>
</messages>
</mavlink>
Loading

0 comments on commit 4307f7a

Please sign in to comment.