#include "stdafx.h"
#include "odin.h"

// ************************************************************************************************
//
//	Copyright @ 2015 - Aethon-Technologies
//
//	File:		BUFF.c
//
//	Author:		A. Jones
//
//	Date:		28 Aug/2015
//
//	Brief:		
//
//	Notes:		
//
// ************************************************************************************************

// *** I N C L U D E S ****************************************************************************
//
//#include "BUFF.h"


// ************************************************************************************************
//
//	Function:	BUFF_PutU8
//
//	Usage:		Store a uint8 in a Buffer
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_PutU8(uint8* pu8Buffer, uint8 u8Value, uint8 u8Endian)
{
	(void)u8Endian;

	if (pu8Buffer)
	{
		*pu8Buffer++ = u8Value;
	}

	return pu8Buffer;
}
// BUFF_PutU8()


// ************************************************************************************************
//
//	Function:	BUFF_PutU16
//
//	Usage:		Store a uint16 in a Buffer
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_PutU16(uint8* pu8Buffer, uint16 u16Value, uint8 u8Endian)
{
	return BUFF_PutVariable(pu8Buffer, (uint8*)&u16Value, sizeof(uint16), u8Endian);
}
// BUFF_PutU16()


// ************************************************************************************************
//
//	Function:	BUFF_PutU32
//
//	Usage:		Store a uint32 in a Buffer
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_PutU32(uint8* pu8Buffer, uint32 u32Value, uint8 u8Endian)
{
	return BUFF_PutVariable(pu8Buffer, (uint8*)&u32Value, sizeof(uint32), u8Endian);
}
// BUFF_PutU32()


// ************************************************************************************************
//
//	Function:	BUFF_PutU64
//
//	Usage:		Store a uint64 in a Buffer
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_PutU64(uint8* pu8Buffer, uint64 u64Value, uint8 u8Endian)
{
	return BUFF_PutVariable(pu8Buffer, (uint8*)&u64Value, sizeof(uint64), u8Endian);
}
// BUFF_PutU64()


// ************************************************************************************************
//
//	Function:	BUFF_PutString8
//
//	Usage:		Store a Max 255 Byte String in a Buffer as a P String
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_PutString8(uint8* pu8Buffer, const char* pS, uint8 u8Endian)
{
	uint16	u16Len = 0;
	char*	pX;

	(void)u8Endian;		// It's passed in for consistency in Function calls

	if (pu8Buffer && pS)
	{
		pX = (char*)pS;
		while (*pX++)
			++u16Len;

		if (u16Len > 255)
			u16Len = 255;

		// String Length
		//
		*pu8Buffer++ = (uint8)u16Len;

		// String
		//
		while (*pS && u16Len)
		{
			*pu8Buffer++ = (uint8)*pS++;
			--u16Len;
		}
	}

	return pu8Buffer;
}
// BUFF_PutString8()


// ************************************************************************************************
//
//	Function:	BUFF_PutString16
//
//	Usage:		Store a Max 65535 Byte String in a Buffer as a P String
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_PutString16(uint8* pu8Buffer, const char* pS, uint8 u8Endian)
{
	uint16	u16Len = 0;
	char*	pX;

	if (pu8Buffer && pS)
	{
		pX = (char*)pS;		
		while (*pX++)
			++u16Len;

		// String Length
		//
		pu8Buffer = BUFF_PutU16(pu8Buffer, u16Len, u8Endian);

		// String
		//
		while (*pS && u16Len)
		{
			*pu8Buffer++ = (uint8)*pS++;
			--u16Len;
		}
	}

	return pu8Buffer;
}
// BUFF_PutString16()


// ************************************************************************************************
//
//	Function:	BUFF_PutData
//
//	Usage:		Store binary Data in a Buffer
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_PutData(uint8* pu8Buffer, const uint8* pData, uint32 u32Len, uint8 u8Endian)
{
	(void)u8Endian;

	if (pu8Buffer && pData)
	{
		while (u32Len > 0U)
		{
			*pu8Buffer++ = *pData++;
			--u32Len;
		}
	}

	return pu8Buffer;
}
// BUFF_PutData()


// ************************************************************************************************
//
//	Function:	BUFF_GetU8
//
//	Usage:		Get a uint8 from a Buffer
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_GetU8(uint8* pu8Buffer, uint8* pu8Value, uint16* pu16Len, uint8 u8Endian)
{
	(void)u8Endian;

	if (pu8Buffer && pu8Value && pu16Len && (*pu16Len >= sizeof(uint8)))
	{
		*pu8Value = *pu8Buffer++;
		*pu16Len -= sizeof(uint8);
	}

	return pu8Buffer;
}
// BUFF_GetU8()


// ************************************************************************************************
//
//	Function:	BUFF_GetU16
//
//	Usage:		Get a uint16 from a Buffer
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_GetU16(uint8* pu8Buffer, uint16* pu16Value, uint16* pu16Len, uint8 u8Endian)
{
	return BUFF_GetVariable(pu8Buffer, (uint8*)pu16Value, sizeof(uint16), pu16Len, u8Endian);
}
// BUFF_GetU16()


// ************************************************************************************************
//
//	Function:	BUFF_GetU32
//
//	Usage:		Get a uint32 from a Buffer
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_GetU32(uint8* pu8Buffer, uint32* pu32Value, uint16* pu16Len, uint8 u8Endian)
{
	return BUFF_GetVariable(pu8Buffer, (uint8*)pu32Value, sizeof(uint32), pu16Len, u8Endian);
}
// BUFF_GetU32()


// ************************************************************************************************
//
//	Function:	BUFF_GetU64
//
//	Usage:		Get a uint64 from a Buffer
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_GetU64(uint8* pu8Buffer, uint64* pu64Value, uint16* pu16Len, uint8 u8Endian)
{
	return BUFF_GetVariable(pu8Buffer, (uint8*)pu64Value, sizeof(uint64), pu16Len, u8Endian);
}
// BUFF_GetU64()


// ************************************************************************************************
//
//	Function:	BUFF_GetString8
//
//	Usage:		Get a (up to) 255 Character String from a Buffer
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_GetString8(uint8* pu8Buffer, char* pD, uint16* pu16Len, uint8 u8Endian)
{
	uint8	u8StringLen;

	(void)u8Endian;

	if (pu8Buffer && pD && pu16Len && *pu16Len)
	{
		u8StringLen = *pu8Buffer++;
		*pu16Len -= 1U;
		while (u8StringLen-- && *pu16Len)
		{
			*pD++ = *pu8Buffer++;
			*pu16Len -= 1U;
		}
		*pD = 0U;	// NULL Terminate
	}

	return pu8Buffer;
}
// BUFF_GetString8()


// ************************************************************************************************
//
//	Function:	BUFF_GetString16
//
//	Usage:		Get a (up to) 65535 Character String from a Buffer
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_GetString16(uint8* pu8Buffer, char* pD, uint16* pu16Len, uint8 u8Endian)
{
	uint16 u16StringLen;

	if (pu8Buffer && pD && pu16Len && *pu16Len)
	{
		pu8Buffer = BUFF_GetU16(pu8Buffer, &u16StringLen, pu16Len, u8Endian);
		while (u16StringLen-- && *pu16Len)
		{
			*pD++ = *pu8Buffer++;
			*pu16Len -= 1U;
		}
		if (*pu16Len > 0U)
			*pD = 0U;	// NULL Terminate (But don't move Pointer - Helps for Debugging)
	}

	return pu8Buffer;
}
// BUFF_GetString16()


// ************************************************************************************************
//
//	Function:	BUFF_GetData
//
//	Usage:		Get Data from a Buffer
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_GetData(uint8* pu8Buffer, uint8* pData, uint32 u32Len, uint16* pu16Len, uint8 u8Endian)
{
	(void)u8Endian;

	if (pu8Buffer && pu16Len)
	{
		while (u32Len && *pu16Len)
		{
			if (pData)	// Allow for passing NULL for Data to Skip fields
			{
				*pData++ = *pu8Buffer;
			}
			++pu8Buffer;
			--u32Len;
			*pu16Len -= 1U;
		}
	}

	return pu8Buffer;
}
// BUFF_GetData()


// ************************************************************************************************
//
//	Function:	BUFF_Concate
//
//	Usage:		Concatenate a Buffer onto our Buffer.  This "Puts" at the
//				current Buffer Position (there is no end to move to)
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_Concate(uint8* pu8Buffer, uint8* pData, uint32 u32Len, uint8 u8Endian)
{
	(void)u8Endian;

	if (pu8Buffer && pData && u32Len)
	{
		while (u32Len--)
			*pu8Buffer++ = *pData++;
	}

	return pu8Buffer;
}
// BUFF_Concate()


// ************************************************************************************************
//
//	Function:	BUFF_PutFloat
//
//	Usage:		Store a float in a Buffer
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_PutFloat(uint8* pu8Buffer, float fValue, uint8 u8Endian)
{
	return BUFF_PutVariable(pu8Buffer, (uint8*)&fValue, sizeof(float), u8Endian);
}
// BUFF_PutFloat()


// ************************************************************************************************
//
//	Function:	BUFF_PutDouble
//
//	Usage:		Store a double in a Buffer
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_PutDouble(uint8* pu8Buffer, double dValue, uint8 u8Endian)
{
	return BUFF_PutVariable(pu8Buffer, (uint8*)&dValue, sizeof(double), u8Endian);
}
// BUFF_PutDouble()


// ************************************************************************************************
//
//	Function:	BUFF_PutVariable
//
//	Usage:		Get a memory representing a variable into a buffer for transport
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//				pu8Variable	- Variable Memory
//				u16Len		- Length of Variabel Memory
//				u8Endian	- Target Endianess
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_PutVariable(uint8* pu8Buffer, uint8* pu8Variable, uint16 u16Len, uint8 u8Endian)
{
	uint32	n;
	uint8*	pS = pu8Variable;

	if ((pu8Buffer == NULL) || (pu8Variable == NULL))
		return pu8Buffer;

	if (ENDIAN == u8Endian)
	{
		for (n = 0; n < u16Len; ++n)
			*pu8Buffer++ = *pS++;
	}
	else
	{
		pS += u16Len - 1;
		for (n = 0; n < u16Len; ++n)
			*pu8Buffer++ = *pS--;
	}

	return pu8Buffer;
}
// BUFF_PutVariable()


// ************************************************************************************************
//
//	Function:	BUFF_GetFloat
//
//	Usage:		Get a float from a Buffer
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_GetFloat(uint8* pu8Buffer, float* pfValue, uint16* pu16Len, uint8 u8Endian)
{
	return BUFF_GetVariable(pu8Buffer, (uint8*)pfValue, sizeof(float), pu16Len, u8Endian);
}
// BUFF_GetFloat()


// ************************************************************************************************
//
//	Function:	BUFF_GetDouble
//
//	Usage:		Get a double from a Buffer
//
//	Params:		pu8Buffer	- Pointer to the Buffer
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_GetDouble(uint8* pu8Buffer, double* pdValue, uint16* pu16Len, uint8 u8Endian)
{
	return BUFF_GetVariable(pu8Buffer, (uint8*)pdValue, sizeof(double), pu16Len, u8Endian);
}
// BUFF_GetDouble()


// ************************************************************************************************
//
//	Function:	BUFF_GetVariable
//
//	Usage:		Get Variable memory representing a variable from a buffer for transport
//
//	Params:		pu8Buffer	- Pointer to the Buffer (source)
//				pu8Variable	- Variable Memory (dest)
//				u16Len		- Length of Variable Memory
//				pu16Len		- Remaining length of the Buffer
//				u8Endian	- Target Endianess
//
//	Returns:	New position of the Pointer
//
//	Notes:
//
// ************************************************************************************************
//
uint8* BUFF_GetVariable(uint8* pu8Buffer, uint8* pu8Variable, uint16 u16Len, uint16* pu16Len, uint8 u8Endian)
{
	uint32	n;
	uint8*	pD = pu8Variable;

	if ((pu8Buffer == NULL) || (pu8Variable == NULL) || (pu16Len == NULL) || (*pu16Len == 0))
		return pu8Buffer;

	if (u16Len > *pu16Len)
		u16Len = *pu16Len;

	if (ENDIAN == u8Endian)
	{
		for (n = 0; n < u16Len; ++n)
			*pD++ = *pu8Buffer++;
	}
	else
	{
		pD += u16Len - 1;
		for (n = 0; n < u16Len; ++n)
			*pD-- = *pu8Buffer++;
	}

	*pu16Len -= u16Len;

	return pu8Buffer;
}
// BUFF_GetVariable()

// BUFF.c
// ************************************************************************************************
//
//	Copyright @ 2015 - Aethon-Technologies
//
//	File:		CRC.c
//
//	Author:		A. Jones
//
//	Date:		28 Aug/2015
//
//	Brief:		
//
//	Notes:		
//
// ************************************************************************************************

// *** I N C L U D E S ****************************************************************************
//
//#include "CRC.h"


// *** C O N S T S ********************************************************************************
//

// CRC8 implementation
//
const uint8 crc8tab[256] =
{
	0x00,0x07,0x0E,0x09,0x1C,0x1B,0x12,0x15,
	0x38,0x3F,0x36,0x31,0x24,0x23,0x2A,0x2D,
	0x70,0x77,0x7E,0x79,0x6C,0x6B,0x62,0x65,
	0x48,0x4F,0x46,0x41,0x54,0x53,0x5A,0x5D,
	0xE0,0xE7,0xEE,0xE9,0xFC,0xFB,0xF2,0xF5,
	0xD8,0xDF,0xD6,0xD1,0xC4,0xC3,0xCA,0xCD,
	0x90,0x97,0x9E,0x99,0x8C,0x8B,0x82,0x85,
	0xA8,0xAF,0xA6,0xA1,0xB4,0xB3,0xBA,0xBD,
	0xC7,0xC0,0xC9,0xCE,0xDB,0xDC,0xD5,0xD2,
	0xFF,0xF8,0xF1,0xF6,0xE3,0xE4,0xED,0xEA,
	0xB7,0xB0,0xB9,0xBE,0xAB,0xAC,0xA5,0xA2,
	0x8F,0x88,0x81,0x86,0x93,0x94,0x9D,0x9A,
	0x27,0x20,0x29,0x2E,0x3B,0x3C,0x35,0x32,
	0x1F,0x18,0x11,0x16,0x03,0x04,0x0D,0x0A,
	0x57,0x50,0x59,0x5E,0x4B,0x4C,0x45,0x42,
	0x6F,0x68,0x61,0x66,0x73,0x74,0x7D,0x7A,
	0x89,0x8E,0x87,0x80,0x95,0x92,0x9B,0x9C,
	0xB1,0xB6,0xBF,0xB8,0xAD,0xAA,0xA3,0xA4,
	0xF9,0xFE,0xF7,0xF0,0xE5,0xE2,0xEB,0xEC,
	0xC1,0xC6,0xCF,0xC8,0xDD,0xDA,0xD3,0xD4,
	0x69,0x6E,0x67,0x60,0x75,0x72,0x7B,0x7C,
	0x51,0x56,0x5F,0x58,0x4D,0x4A,0x43,0x44,
	0x19,0x1E,0x17,0x10,0x05,0x02,0x0B,0x0C,
	0x21,0x26,0x2F,0x28,0x3D,0x3A,0x33,0x34,
	0x4E,0x49,0x40,0x47,0x52,0x55,0x5C,0x5B,
	0x76,0x71,0x78,0x7F,0x6A,0x6D,0x64,0x63,
	0x3E,0x39,0x30,0x37,0x22,0x25,0x2C,0x2B,
	0x06,0x01,0x08,0x0F,0x1A,0x1D,0x14,0x13,
	0xAE,0xA9,0xA0,0xA7,0xB2,0xB5,0xBC,0xBB,
	0x96,0x91,0x98,0x9F,0x8A,0x8D,0x84,0x83,
	0xDE,0xD9,0xD0,0xD7,0xC2,0xC5,0xCC,0xCB,
	0xE6,0xE1,0xE8,0xEF,0xFA,0xFD,0xF4,0xF3
};
// crc8tab

// CRC16 implementation according to CCITT standards
//
const uint16 crc16tab[256] =
{
	0x0000,  0x1021,  0x2042,  0x3063,  0x4084,  0x50a5,  0x60c6,  0x70e7,
    0x8108,  0x9129,  0xa14a,  0xb16b,  0xc18c,  0xd1ad,  0xe1ce,  0xf1ef,
    0x1231,  0x0210,  0x3273,  0x2252,  0x52b5,  0x4294,  0x72f7,  0x62d6,
    0x9339,  0x8318,  0xb37b,  0xa35a,  0xd3bd,  0xc39c,  0xf3ff,  0xe3de,
    0x2462,  0x3443,  0x0420,  0x1401,  0x64e6,  0x74c7,  0x44a4,  0x5485,
    0xa56a,  0xb54b,  0x8528,  0x9509,  0xe5ee,  0xf5cf,  0xc5ac,  0xd58d,
    0x3653,  0x2672,  0x1611,  0x0630,  0x76d7,  0x66f6,  0x5695,  0x46b4,
    0xb75b,  0xa77a,  0x9719,  0x8738,  0xf7df,  0xe7fe,  0xd79d,  0xc7bc,
    0x48c4,  0x58e5,  0x6886,  0x78a7,  0x0840,  0x1861,  0x2802,  0x3823,
    0xc9cc,  0xd9ed,  0xe98e,  0xf9af,  0x8948,  0x9969,  0xa90a,  0xb92b,
    0x5af5,  0x4ad4,  0x7ab7,  0x6a96,  0x1a71,  0x0a50,  0x3a33,  0x2a12,
    0xdbfd,  0xcbdc,  0xfbbf,  0xeb9e,  0x9b79,  0x8b58,  0xbb3b,  0xab1a,
    0x6ca6,  0x7c87,  0x4ce4,  0x5cc5,  0x2c22,  0x3c03,  0x0c60,  0x1c41,
    0xedae,  0xfd8f,  0xcdec,  0xddcd,  0xad2a,  0xbd0b,  0x8d68,  0x9d49,
    0x7e97,  0x6eb6,  0x5ed5,  0x4ef4,  0x3e13,  0x2e32,  0x1e51,  0x0e70,
    0xff9f,  0xefbe,  0xdfdd,  0xcffc,  0xbf1b,  0xaf3a,  0x9f59,  0x8f78,
    0x9188,  0x81a9,  0xb1ca,  0xa1eb,  0xd10c,  0xc12d,  0xf14e,  0xe16f,
    0x1080,  0x00a1,  0x30c2,  0x20e3,  0x5004,  0x4025,  0x7046,  0x6067,
    0x83b9,  0x9398,  0xa3fb,  0xb3da,  0xc33d,  0xd31c,  0xe37f,  0xf35e,
    0x02b1,  0x1290,  0x22f3,  0x32d2,  0x4235,  0x5214,  0x6277,  0x7256,
    0xb5ea,  0xa5cb,  0x95a8,  0x8589,  0xf56e,  0xe54f,  0xd52c,  0xc50d,
    0x34e2,  0x24c3,  0x14a0,  0x0481,  0x7466,  0x6447,  0x5424,  0x4405,
    0xa7db,  0xb7fa,  0x8799,  0x97b8,  0xe75f,  0xf77e,  0xc71d,  0xd73c,
    0x26d3,  0x36f2,  0x0691,  0x16b0,  0x6657,  0x7676,  0x4615,  0x5634,
    0xd94c,  0xc96d,  0xf90e,  0xe92f,  0x99c8,  0x89e9,  0xb98a,  0xa9ab,
    0x5844,  0x4865,  0x7806,  0x6827,  0x18c0,  0x08e1,  0x3882,  0x28a3,
    0xcb7d,  0xdb5c,  0xeb3f,  0xfb1e,  0x8bf9,  0x9bd8,  0xabbb,  0xbb9a,
    0x4a75,  0x5a54,  0x6a37,  0x7a16,  0x0af1,  0x1ad0,  0x2ab3,  0x3a92,
    0xfd2e,  0xed0f,  0xdd6c,  0xcd4d,  0xbdaa,  0xad8b,  0x9de8,  0x8dc9,
    0x7c26,  0x6c07,  0x5c64,  0x4c45,  0x3ca2,  0x2c83,  0x1ce0,  0x0cc1,
    0xef1f,  0xff3e,  0xcf5d,  0xdf7c,  0xaf9b,  0xbfba,  0x8fd9,  0x9ff8,
    0x6e17,  0x7e36,  0x4e55,  0x5e74,  0x2e93,  0x3eb2,  0x0ed1,  0x1ef0
};
// crc16tab

// CRC32 (polynomial 0xedb88320)
//
const uint32 crc32tab[] =
{
	0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
	0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
	0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
	0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
	0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
	0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
	0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
	0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
	0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
	0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
	0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
	0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
	0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
	0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
	0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
	0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
	0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
	0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
	0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
	0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
	0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
	0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
	0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
	0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
	0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
	0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
	0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
	0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
	0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
	0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
	0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
	0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
};
// crc32tab


// ************************************************************************************************
//
//	Function:	CRC8Buffer
//
//	Usage:		CRC8 a Buffer
//
//	Params:		u8CRC	- Seed
//				pu8Data	- Pointer to Buffer
//				u32Len	- Length of Buffer
//
//	Returns:	New CRC8
//
//	Notes:
//
// ************************************************************************************************
//
uint8 CRC8Buffer(uint8 u8CRC, uint8* pu8Data, uint32 u32Len)
{
	uint8 u8Byte;

	while (u32Len--)
	{
		u8Byte = *pu8Data++;
		u8CRC = CRC8(u8CRC, u8Byte);
	}

	return u8CRC;
}
// CRC8Buffer()

// ************************************************************************************************
//
//	Function:	CRC16Buffer
//
//	Usage:		CRC16 a Buffer
//
//	Params:		u16CRC	- Seed
//				pu8Data	- Pointer to Buffer
//				u32Len	- Length of Buffer
//
//	Returns:	New CRC16
//
//	Notes:
//
// ************************************************************************************************
//
uint16 CRC16Buffer(uint16 u16CRC, uint8* pu8Data, uint32 u32Len)
{
	uint8 u8Byte;

	while (u32Len--)
	{
		u8Byte = *pu8Data++;
		u16CRC = CRC16(u16CRC, u8Byte);
	}

	return u16CRC;
}
// CRC16Buffer()

// ************************************************************************************************
//
//	Function:	CRC32Buffer
//
//	Usage:		CRC32 a Buffer
//
//	Params:		u32CRC	- Seed
//				pu8Data	- Pointer to Buffer
//				u32Len	- Length of Buffer
//
//	Returns:	New CRC32
//
//	Notes:
//
// ************************************************************************************************
//
uint32 CRC32Buffer(uint32 u32CRC, uint8* pu8Data, uint32 u32Len)
{
	uint8 u8Byte;

	while (u32Len--)
	{
		u8Byte = *pu8Data++;
		u32CRC = CRC32(u32CRC, u8Byte);
	}

	return u32CRC;
}

// *** P R O T O T Y P E S ************************************************************************
//
static BOOL		SLIP_ProcessPacket(SLIPStreamDEF* pSLIP);


// ************************************************************************************************
//
//	Function:		SLIP_Create
//
//	Usage:			Create a SLIP Stream
//
//	Params:			pSLIP				- A SLIP Stream
//					u8OurID				- The ID we intend to Send as (Src)
//					pFunc				- A Callback Routine that will handle Received Messages
//					vpUser				- Any User Data (Context) to associate with Stream
//											(Can be referenced in the Callback routine)
//
//	Returns:		TRUE on Success
//
//	Notes:
//
// ************************************************************************************************
//
BOOL SLIP_Create(SLIPStreamDEF* pSLIP, uint8 u8OurID, SlipFN pFunc, void* vpUser)
{
	if ((pSLIP == NULL) || (pFunc == NULL))
		return FALSE;

	memset(pSLIP, 0, sizeof(SLIPStreamDEF));
	pSLIP->u8OurID = u8OurID;
	pSLIP->pFuncProcessPacket = pFunc;
	pSLIP->vpUser = vpUser;

	return TRUE;
}
// SLIP_Create()


// ************************************************************************************************
//
//	Function:	SLIP_FramePayload
//
//	Usage:		Wrap a Payload Buffer in the proper Framing, calculate Header
//				and Payload CRCs, etc.
//
//	Params:		pSLIP				- A SLIP Stream
//				pu8DestBuffer		- Destination Buffer
//				u8SrcID				- Source Device ID
//				u8DestDevID			- Dest Device ID
//				u8MsgID				- Msg ID (SLIP_MSG_xxx)
//				pu8Payload			- Payload Data
//				u16PayloadLen		- Payload Length
//
//	Returns:	Length of the new packet
//
// ************************************************************************************************
//
uint16 SLIP_FramePayload(SLIPStreamDEF* pSLIP, uint8 u8SrcDevID, uint8 u8DestDevID, uint8 u8MsgID, uint8* pu8Payload, uint16 u16PayloadLen)
{
	uint8*	pu8Byte;
	uint8	u8Byte;
	uint8	u8CheckSum;						// Modulos 8 (Low Overhead) Sanity Check on Header
	uint16	u16CRC;							// CRC16 of Payload

	// Assume we fail
	//
	pSLIP->u16TXPacketSize = 0;

	// Validate Stream Pointer
	//
	if (pSLIP == NULL)
		return 0;

	// Validate Payload Pointer
	//
	if (u16PayloadLen && (pu8Payload == NULL))
		return 0;

	// Validate Payload
	//
	if (u16PayloadLen > SLIP_MAX_PAYLOAD_SIZE)
		return 0;

	// ID Help
	//
	if (u8SrcDevID == SLIP_ID_ANY)
		u8SrcDevID = pSLIP->u8OurID;

	// Statistics
	//
	++pSLIP->u32TXPackets;

	// <u8Sync> <u8Seq> <u8SrcDevID> <u8DestDevID> <u8MsgID> <u16PayloadLen> <u8HeaderChecksum> ... <Payload> .. <u16PayLoadCRC>

	// Point to beginning of TXPacket
	//
	pu8Byte = pSLIP->u8TXPacket;

	// Sync (8 Bits)
	//
	*pu8Byte = SLIP_SYNC;
	u8CheckSum = *pu8Byte++;

	// Seq ID (8 Bits)
	//
	*pu8Byte = pSLIP->u8TXSeqNum++;
	u8CheckSum += *pu8Byte++;

	// Source Device Type (8 Bits)
	//
	*pu8Byte = u8SrcDevID;
	u8CheckSum += *pu8Byte++;

	// Dest Device Type (8 Bits)
	//
	*pu8Byte = u8DestDevID;
	u8CheckSum += *pu8Byte++;

	// Msg ID (8 Bits)
	//
	*pu8Byte = u8MsgID;
	u8CheckSum += *pu8Byte++;

	// Payload Length (Lil Endian) (16 Bits)
	//	
	*pu8Byte = (uint8)u16PayloadLen;			// LSB
	u8CheckSum += *pu8Byte++;
	*pu8Byte = (uint8)(u16PayloadLen >> 8);		// MSB
	u8CheckSum += *pu8Byte++;

	// Header Checksum (8 Bits)
	//
	*pu8Byte++ = u8CheckSum;

	// --- End of fixed length Header -----------------

	// Payload (if any)
	//
	if (u16PayloadLen > 0)
	{
		// Copy Payload / Calc CRC
		//
		u16CRC = 0;
		while (u16PayloadLen--)
		{
			u8Byte = *pu8Payload++;
			u16CRC = CRC16(u16CRC, u8Byte);
			*pu8Byte++ = u8Byte;
		}

		// Payload CRC (Lil Endian) (16 Bits)
		//
		*pu8Byte++ = (uint8)u16CRC;				// LSB
		*pu8Byte++ = (uint8)(u16CRC >> 8);		// MSB
	}

	// Calculate Length of Entire Packet
	//
	pSLIP->u16TXPacketSize = (uint16)(pu8Byte - pSLIP->u8TXPacket);

	return pSLIP->u16TXPacketSize;
}
// SLIP_FramePayload()


// ************************************************************************************************
//
//	Function:	SLIP_Consume
//
//	Usage:		Consume SLIP binary stream data from a source
//
//	Params:		pSLIP				- A SLIP Stream
//				pu8Buffer			- A chunk of stream
//				u16Len				- Length of the chunk
//
//	Returns:	TRUE on Packet detected
//
//	Notes:
//
// ************************************************************************************************
//
BOOL SLIP_Consume(SLIPStreamDEF* pSLIP, uint8* pu8Buffer, uint16 u16Len)
{
	BOOL	bProcessed = FALSE;
	uint8	u8Byte;

	// Validate Params
	//
	if ((pSLIP == NULL) || (pu8Buffer == NULL))
		return FALSE;

	// <u8Sync> <u8Seq> <u8SrcDevID> <u8DestDevID> <u8MsgID> <u16PayloadLen> <u8HeaderChecksum> ... <Payload> .. <u16PayLoadCRC>

	// For each Byte in Buffer
	//
	while (u16Len)
	{
		u8Byte = *pu8Buffer++;												// Next Byte in Buffer
		--u16Len;

		switch (pSLIP->u8State)
		{
			// Waiting for <u8Sync>
			//
			case 0:
				if (u8Byte == SLIP_SYNC)									// Start of Frame
				{
					pSLIP->u8CalcChecksum = u8Byte;							// Init Header Checksum
					++pSLIP->u8State;										// Next state
				}
				else
				{
					++pSLIP->u32RXExtra;									// Track Extraneous Bytes (bytes outside Framing)
				}
				break;

			// Seq ID <u8Seq>
			//
			case 1:
				pSLIP->u8Seq = u8Byte;										// Seq ID
				pSLIP->u8CalcChecksum += u8Byte;							// Calc Header Checksum
				++pSLIP->u8State;											// Next state
				break;

			// Src <u8SrcDevID>
			//
			case 2:
				pSLIP->u8Src = u8Byte;										// Source Device Type
				pSLIP->u8CalcChecksum += u8Byte;							// Calc Header ChecksumpSLIP->u16SLIPCalcCRC = CRC16(pSLIP->u16SLIPCalcCRC, u8Byte);		// Calc running CRC
				++pSLIP->u8State;											// Next state
				break;

			// Dst <u8DestDevID>
			//
			case 3:
				pSLIP->u8Dst = u8Byte;										// Dest Device Type
				pSLIP->u8CalcChecksum += u8Byte;							// Calc Header Checksum
				++pSLIP->u8State;											// Next state
				break;

			// Msg ID <u8MsgID>
			//
			case 4:
				pSLIP->u8Msg = u8Byte;										// Message ID
				pSLIP->u8CalcChecksum += u8Byte;							// Calc Header Checksum					
				++pSLIP->u8State;											// Next state
				break;

			// Payload Size <u16PayloadLen> - LSB
			//
			case 5:
				pSLIP->u16PayloadLen = u8Byte;								// Payload Size (LSB)
				pSLIP->u8CalcChecksum += u8Byte;							// Calc Header Checksum
				++pSLIP->u8State;											// Next state
				break;

			// Payload Size <u16PayloadLen> - MSB
			//
			case 6:
				pSLIP->u16PayloadLen |= (u8Byte << 8);						// Payload Size (LSB)
				if (pSLIP->u16PayloadLen <= SLIP_MAX_PAYLOAD_SIZE)			// Validate that we can store the Payload
				{
					pSLIP->u8CalcChecksum += u8Byte;						// Calc Header Checksum
					pSLIP->u16RXPayloadAcquired = 0;						// Start of Payload
					pSLIP->pu8RXByte = pSLIP->u8RXPayload;					// Point to start of Payload
					++pSLIP->u8State;										// Next state
				}
				else
				{
					++pSLIP->u32RXErrors;									// Track Errors
					pSLIP->u8State = 0;										// TODO: Throwing away Oversized Packets
				}
				break;

			// Header Checksum
			//
			case 7:
				pSLIP->u8HeaderChecksum = u8Byte;							// Header Checksum
				if (pSLIP->u8CalcChecksum == pSLIP->u8HeaderChecksum)		// Correct Checksum?
				{
					if (pSLIP->u16PayloadLen > 0)							// Is there a Payload?
					{
						pSLIP->u16CRCPayloadCalc = 0;						// Init Payload CRC
						++pSLIP->u8State;									// Next state
					}
					else
					{
						bProcessed = SLIP_ProcessPacket(pSLIP);				// Process the (Zero length Payload) Message
						pSLIP->u8State = 0;									// No Payload Reset State Machine
					}
				}
				else // Header Checksum Error
				{
					++pSLIP->u32RXBadHeaderChk;								// Track overall Checksum Errors
					pSLIP->u8State = 0;										// Reset State Machine					
				}
				break;

			// Payload (n bytes)
			//
			case 8:
				*pSLIP->pu8RXByte++ = u8Byte;								// Store Payload Byte
				++pSLIP->u16RXPayloadAcquired;								// Track acquired Payload size
				pSLIP->u16CRCPayloadCalc = CRC16(pSLIP->u16CRCPayloadCalc, u8Byte);	// Calc running CRC
				if (pSLIP->u16RXPayloadAcquired >= pSLIP->u16PayloadLen)	// Payload Complete?
				{
					++pSLIP->u8State;										// Move on to Payload CRC
				}
				else
				{
					// -- We stay in this State until Payload Length Reached --
				}
				break;

			// Payload CRC LSB
			//
			case 9:
				pSLIP->u16CRCPayload = u8Byte;								// Payload CRC (LSB)
				++pSLIP->u8State;											// Next State
				break;

			// Payload CRC MSB
			//
			case 10:
				pSLIP->u16CRCPayload |= (u8Byte << 8);						// Payload CRC (MSB)
				if (pSLIP->u16CRCPayload == pSLIP->u16CRCPayloadCalc)		// Is Payload CRC Correct?
				{
					bProcessed = SLIP_ProcessPacket(pSLIP);					// Process the (Zero length Payload) Message
				}
				else
				{
					++pSLIP->u32RXBadPayloadCRC;							// Track Overall CRC Errors
				}
				pSLIP->u8State = 0;											// Either way, we are done, Reset State Machine
				break;

			// Illegal State
			//
			default:
				pSLIP->u8State = 0;											// Reset State Machine
				break;

		} // switch

	} // while (u16Len

	return bProcessed;
}
// SLIP_Consume()


// ************************************************************************************************
//
//	Function:	SLIP_ProcessPacket
//
//	Usage:		Process a fully qualified SLIP Packet
//
//	Params:		pSLIP				- A SLIP Stream
//
//	Returns:	TRUE on known Msg
//
//	Notes:
//
// ************************************************************************************************
//
BOOL SLIP_ProcessPacket(SLIPStreamDEF* pSLIP)
{
	BOOL	bProcessed = FALSE;
	uint8	u8ExpectedSeq;

	// Validate Params
	//
	if (pSLIP == NULL)
		return FALSE;

	// Statistics
	//
	++pSLIP->u32RXPackets;

	// Routing (Is Msg for 'Us' or Anyone)
	//
	if ((pSLIP->u8Dst != pSLIP->u8OurID) && (pSLIP->u8Dst != 0xFF))
	{
		//LogPrintf(LOG_INFO, "SLIP '%s' - Rejected Packet with Dst:%d from Src:%d", pSLIP->szName, pSLIP->u8Dst, pSLIP->u8Src);
		return FALSE;
	}

	// Sequence Errors
	//
	if (pSLIP->u8RXSeqNum != 0)
	{
		u8ExpectedSeq = pSLIP->u8RXSeqNum + 1;
		if (pSLIP->u8Seq != u8ExpectedSeq)
			++pSLIP->u32RXSeqErrors;
	}
	pSLIP->u8RXSeqNum = pSLIP->u8Seq;

	// Process the Msg
	//
	if (pSLIP->pFuncProcessPacket)
		bProcessed = pSLIP->pFuncProcessPacket(pSLIP);

	return bProcessed;
}
// SLIP_ProcessPacket()

//	-------------------------------------------------------------------
//	xxxMessage_t
//	-------------------------------------------------------------------

const uint8_t endian = ENDIAN;

GimbalStatusMessage_t GimbalStatusMessage;
GimbalCtrlMessage_t GimbalCtrlMessage;
GeoPointMessage_t GeoPointMessage;
GeoPointEvent_t GeoPointEvent;
GeoFocusMessage_t GeoFocusMessage;

//	-------------------------------------------------------------------
//	packGeoFocusMessage
//	-------------------------------------------------------------------

uint16_t packGimbalStatusMessage(uint8_t *payload)
{
	uint8_t *p = payload;

	p = BUFF_PutU16(	p,	GimbalStatusMessage.u16Validity,	endian);
	p = BUFF_PutU8(		p,	GimbalStatusMessage.u8Mode,			endian);
	p = BUFF_PutU16(	p,	GimbalStatusMessage.u16Flags,		endian);
	p = BUFF_PutFloat(	p,	GimbalStatusMessage.fPanAngle,		endian);
	p = BUFF_PutFloat(	p,	GimbalStatusMessage.fTiltAngle,		endian);
	p = BUFF_PutFloat(	p,	GimbalStatusMessage.fRollAngle,		endian);
	p = BUFF_PutFloat(	p,	GimbalStatusMessage.fGyroRate[0],	endian);
	p = BUFF_PutFloat(	p,	GimbalStatusMessage.fGyroRate[1],	endian);
	p = BUFF_PutFloat(	p,	GimbalStatusMessage.fGyroRate[2],	endian);

	uint16_t len = p-payload;
	return len;
};

//	-------------------------------------------------------------------
//	packGimbalCtrlMessage
//	-------------------------------------------------------------------

uint16_t packGimbalCtrlMessage(uint8_t *payload)
{
	uint8_t *p = payload;

	p = BUFF_PutU16(	p,	GimbalCtrlMessage.u16Validity,	endian);
	p = BUFF_PutU8(		p,	GimbalCtrlMessage.u8Mode,		endian);
	p = BUFF_PutU16(	p,	GimbalCtrlMessage.u16Flags,		endian);
	p = BUFF_PutFloat(	p,	GimbalCtrlMessage.fPan,			endian);
	p = BUFF_PutFloat(	p,	GimbalCtrlMessage.fTilt,		endian);
	p = BUFF_PutFloat(	p,	GimbalCtrlMessage.fRoll,		endian);

	uint16_t len = p-payload;
	return len;
};

//	-------------------------------------------------------------------
//	packGeoPointMessage
//	-------------------------------------------------------------------

uint16_t packGeoPointMessage(uint8_t *payload)
{
	uint8_t *p = payload;

	p = BUFF_PutU16(	p,	GeoPointMessage.u16Sequence,	endian);
	p = BUFF_PutU16(	p,	GeoPointMessage.u16Validity,	endian);
	p = BUFF_PutU16(	p,	GeoPointMessage.u16Flags,		endian);
	p = BUFF_PutU16(	p,	GeoPointMessage.u16ClusterID,	endian);
	p = BUFF_PutFloat(	p,	GeoPointMessage.fMinRng,		endian);
	p = BUFF_PutFloat(	p,	GeoPointMessage.fMaxRng,		endian);
	p = BUFF_PutFloat(	p,	GeoPointMessage.fMinAz,			endian);
	p = BUFF_PutFloat(	p,	GeoPointMessage.fMaxAz,			endian);
	p = BUFF_PutU16(	p,	GeoPointMessage.u16Targets,		endian);
	for(int i=0;i<GeoPointMessage.u16Targets;i++){
		p = BUFF_PutU16(	p,	GeoPointMessage.points[i].u16TargetID,	endian);
		p = BUFF_PutDouble(	p,	GeoPointMessage.points[i].dLatitude,	endian);
		p = BUFF_PutDouble(	p,	GeoPointMessage.points[i].dLongitude,	endian);
		p = BUFF_PutFloat(	p,	GeoPointMessage.points[i].fAltELL,		endian);
	}

	uint16_t len = p-payload;
	return len;
};

//	-------------------------------------------------------------------
//	packGeoPointEvent
//	-------------------------------------------------------------------

uint16_t packGeoPointEvent(uint8_t *payload)
{
	uint8_t *p = payload;

	p = BUFF_PutU8(		p, GeoPointEvent.u8Event,		endian);
	p = BUFF_PutU16(	p, GeoPointEvent.u16Sequence,	endian);
	p = BUFF_PutU8(		p, GeoPointEvent.u8Sensor,		endian);
	p = BUFF_PutU32(	p, GeoPointEvent.u32Frame,		endian);
	p = BUFF_PutU16(	p, GeoPointEvent.u16Cluster,	endian);
	p = BUFF_PutU16(	p, GeoPointEvent.u16Target,		endian);
	p = BUFF_PutFloat(	p, GeoPointEvent.fGimRange,		endian);
	p = BUFF_PutFloat(	p, GeoPointEvent.fGimBearing,	endian);
	p = BUFF_PutU16(	p, GeoPointEvent.u16Validity,	endian);
	p = BUFF_PutFloat(	p, GeoPointEvent.fPanAngle,		endian);
	p = BUFF_PutFloat(	p, GeoPointEvent.fTiltAngle,	endian);
	p = BUFF_PutFloat(	p, GeoPointEvent.fRollAngle,	endian);
	p = BUFF_PutU64(	p, GeoPointEvent.u64Validity,	endian);
	p = BUFF_PutU64(	p, GeoPointEvent.u64GPSTime,	endian);
	p = BUFF_PutU16(	p, GeoPointEvent.u16GPSStatus,	endian);
	p = BUFF_PutU8(		p, GeoPointEvent.u8SatV,		endian);
	p = BUFF_PutU8(		p, GeoPointEvent.u8SatT,		endian);
	p = BUFF_PutU8(		p, GeoPointEvent.u8Align,		endian);
	p = BUFF_PutU8(		p, GeoPointEvent.u8Qual,		endian);
	p = BUFF_PutDouble(	p, GeoPointEvent.dLatitude,		endian);
	p = BUFF_PutDouble(	p, GeoPointEvent.dLongitude,	endian);
	p = BUFF_PutDouble(	p, GeoPointEvent.dAltELL,		endian);
	p = BUFF_PutFloat(	p, GeoPointEvent.fPitch,		endian);
	p = BUFF_PutFloat(	p, GeoPointEvent.fRoll,			endian);
	p = BUFF_PutFloat(	p, GeoPointEvent.fYawHdgT,		endian);
	p = BUFF_PutFloat(	p, GeoPointEvent.fTrackT,		endian);
	p = BUFF_PutFloat(	p, GeoPointEvent.fSpeed,		endian);
	p = BUFF_PutFloat(	p, GeoPointEvent.fVelN,			endian);
	p = BUFF_PutFloat(	p, GeoPointEvent.fVelE,			endian);
	p = BUFF_PutFloat(	p, GeoPointEvent.fVelD,			endian);

	uint16_t len = p-payload;
	return len;
};

//	-------------------------------------------------------------------
//	packGeoFocusMessage
//	-------------------------------------------------------------------

uint16_t packGeoFocusMessage(uint8_t *payload)
{
	uint8_t *p = payload;

	p = BUFF_PutU16(	p, GeoFocusMessage.u16Cluster,	endian);
	p = BUFF_PutU16(	p, GeoFocusMessage.u16Cluster,	endian);
	p = BUFF_PutFloat(	p, GeoFocusMessage.fSlantRange,	endian);

	uint16_t len = p-payload;
	return len;
};

//	-------------------------------------------------------------------
//	getGimbalStatusMessage
//	-------------------------------------------------------------------

void getGimbalStatusMessage(uint8_t *payload, uint16_t len)
{
	uint8_t *p = payload;
	uint16_t l = len;
	
	p = BUFF_GetU16(	p,	&GimbalStatusMessage.u16Validity,	&l,	endian);
	p = BUFF_GetU8(		p,	&GimbalStatusMessage.u8Mode,		&l,	endian);
	p = BUFF_GetU16(	p,	&GimbalStatusMessage.u16Flags,		&l,	endian);
	p = BUFF_GetFloat(	p,	&GimbalStatusMessage.fPanAngle,		&l,	endian);
	p = BUFF_GetFloat(	p,	&GimbalStatusMessage.fTiltAngle,	&l,	endian);
	p = BUFF_GetFloat(	p,	&GimbalStatusMessage.fRollAngle,	&l,	endian);
	p = BUFF_GetFloat(	p,	&GimbalStatusMessage.fGyroRate[0],	&l,	endian);
	p = BUFF_GetFloat(	p,	&GimbalStatusMessage.fGyroRate[1],	&l,	endian);
	p = BUFF_GetFloat(	p,	&GimbalStatusMessage.fGyroRate[2],	&l,	endian);
};


//	-------------------------------------------------------------------
//	getGimbalCtrlMessage
//	-------------------------------------------------------------------

void getGimbalCtrlMessage(uint8_t *payload, uint16_t len)
{
	uint8_t *p = payload;
	uint16_t l = len;

	p = BUFF_GetU16(	p, &GimbalCtrlMessage.u16Validity,	&l, endian);
	p = BUFF_GetU8(		p, &GimbalCtrlMessage.u8Mode,		&l, endian);
	p = BUFF_GetU16(	p, &GimbalCtrlMessage.u16Flags,		&l, endian);
	p = BUFF_GetFloat(	p, &GimbalCtrlMessage.fPan,			&l, endian);
	p = BUFF_GetFloat(	p, &GimbalCtrlMessage.fTilt,		&l, endian);
	p = BUFF_GetFloat(	p, &GimbalCtrlMessage.fRoll,		&l, endian);
};


//	-------------------------------------------------------------------
//	getGeoPointMessage
//	-------------------------------------------------------------------

void getGeoPointMessage(uint8_t *payload, uint16_t len)
{
	uint8_t *p = payload;
	uint16_t l = len;

	p = BUFF_GetU16(	p, &GeoPointMessage.u16Sequence,	&l, endian);
	p = BUFF_GetU16(	p, &GeoPointMessage.u16Validity,	&l, endian);
	p = BUFF_GetU16(	p, &GeoPointMessage.u16Flags,		&l, endian);
	p = BUFF_GetU16(	p, &GeoPointMessage.u16ClusterID,	&l, endian);
	p = BUFF_GetFloat(	p, &GeoPointMessage.fMinRng,		&l, endian);
	p = BUFF_GetFloat(	p, &GeoPointMessage.fMaxRng,		&l, endian);
	p = BUFF_GetFloat(	p, &GeoPointMessage.fMinAz,			&l, endian);
	p = BUFF_GetFloat(	p, &GeoPointMessage.fMaxAz,			&l, endian);
	p = BUFF_GetU16(	p, &GeoPointMessage.u16Targets,		&l, endian);
	for(int i=0;i<GeoPointMessage.u16Targets;i++){
		p = BUFF_GetU16(	p, &GeoPointMessage.points[i].u16TargetID,	&l, endian);
		p = BUFF_GetDouble(	p, &GeoPointMessage.points[i].dLatitude,	&l, endian);
		p = BUFF_GetDouble(	p, &GeoPointMessage.points[i].dLongitude,	&l, endian);
		p = BUFF_GetFloat(	p, &GeoPointMessage.points[i].fAltELL,		&l, endian);
	}
};

//	-------------------------------------------------------------------
//	getGeoPointEvent
//	-------------------------------------------------------------------

void getGeoPointEvent(uint8_t *payload, uint16_t len)
{
	uint8_t *p = payload;
	uint16_t l = len;

	p = BUFF_GetU8(		p, &GeoPointEvent.u8Event,		&l, endian);
	p = BUFF_GetU16(	p, &GeoPointEvent.u16Sequence,	&l, endian);
	p = BUFF_GetU8(		p, &GeoPointEvent.u8Sensor,		&l, endian);
	p = BUFF_GetU32(	p, &GeoPointEvent.u32Frame,		&l, endian);
	p = BUFF_GetU16(	p, &GeoPointEvent.u16Cluster,	&l, endian);
	p = BUFF_GetU16(	p, &GeoPointEvent.u16Target,	&l, endian);
	p = BUFF_GetFloat(	p, &GeoPointEvent.fGimRange,	&l, endian);
	p = BUFF_GetFloat(	p, &GeoPointEvent.fGimBearing,	&l, endian);
	p = BUFF_GetU16(	p, &GeoPointEvent.u16Validity,	&l, endian);
	p = BUFF_GetFloat(	p, &GeoPointEvent.fPanAngle,	&l, endian);
	p = BUFF_GetFloat(	p, &GeoPointEvent.fTiltAngle,	&l, endian);
	p = BUFF_GetFloat(	p, &GeoPointEvent.fRollAngle,	&l, endian);
	p = BUFF_GetU64(	p, &GeoPointEvent.u64Validity,	&l, endian);
	p = BUFF_GetU64(	p, &GeoPointEvent.u64GPSTime,	&l, endian);
	p = BUFF_GetU16(	p, &GeoPointEvent.u16GPSStatus,	&l, endian);
	p = BUFF_GetU8(		p, &GeoPointEvent.u8SatV,		&l, endian);
	p = BUFF_GetU8(		p, &GeoPointEvent.u8SatT,		&l, endian);
	p = BUFF_GetU8(		p, &GeoPointEvent.u8Align,		&l, endian);
	p = BUFF_GetU8(		p, &GeoPointEvent.u8Qual,		&l, endian);
	p = BUFF_GetDouble(	p, &GeoPointEvent.dLatitude,	&l, endian);
	p = BUFF_GetDouble(	p, &GeoPointEvent.dLongitude,	&l, endian);
	p = BUFF_GetDouble(	p, &GeoPointEvent.dAltELL,		&l, endian);
	p = BUFF_GetFloat(	p, &GeoPointEvent.fPitch,		&l, endian);
	p = BUFF_GetFloat(	p, &GeoPointEvent.fRoll,		&l, endian);
	p = BUFF_GetFloat(	p, &GeoPointEvent.fYawHdgT,		&l, endian);
	p = BUFF_GetFloat(	p, &GeoPointEvent.fTrackT,		&l, endian);
	p = BUFF_GetFloat(	p, &GeoPointEvent.fSpeed,		&l, endian);
	p = BUFF_GetFloat(	p, &GeoPointEvent.fVelN,		&l, endian);
	p = BUFF_GetFloat(	p, &GeoPointEvent.fVelE,		&l, endian);
	p = BUFF_GetFloat(	p, &GeoPointEvent.fVelD,		&l, endian);
};

//	-------------------------------------------------------------------
//	getGeoFocusMessage
//	-------------------------------------------------------------------

void getGeoFocusMessage(uint8_t *payload, uint16_t len){
	uint8_t *p = payload;
	uint16_t l = len;

	p = BUFF_GetU16(	p, &GeoFocusMessage.u16Cluster,		&l, endian);
	p = BUFF_GetU16(	p, &GeoFocusMessage.u16Cluster,		&l, endian);
	p = BUFF_GetFloat(	p, &GeoFocusMessage.fSlantRange,	&l, endian);
};
