/*
GCS Protocol

4	Ardupilot Header
D
5	Payload length
1	Message ID
1	Message Version
9	Payload byte 1
8	Payload byte 2
7	Payload byte 3
A	Checksum byte 1
B	Checksum byte 2

*/


#if GCS_PORT == 3
# define SendSerw		Serial3.write
# define SendSer		Serial3.print
#else
# define SendSerw		Serial.write
# define SendSer		Serial.print
#endif

byte mess_buffer[54];
byte buff_pointer;

// Unions for getting byte values
union f_out{
	byte bytes[4];
	float value;
} floatOut;

union i_out {
	byte bytes[2];
	int value;
} intOut;

union l_out{
	byte bytes[4];
	long value;
} longOut;

// Add binary values to the buffer
void write_byte(byte val)
{
	mess_buffer[buff_pointer++] = val;
}

void write_int(int val )
{
	intOut.value = val;
	mess_buffer[buff_pointer++] = intOut.bytes[0];
	mess_buffer[buff_pointer++] = intOut.bytes[1];
}

void write_float(float val)
{
	floatOut.value = val;
	mess_buffer[buff_pointer++] = floatOut.bytes[0];
	mess_buffer[buff_pointer++] = floatOut.bytes[1];
	mess_buffer[buff_pointer++] = floatOut.bytes[2];
	mess_buffer[buff_pointer++] = floatOut.bytes[3];
}

void write_long(long val)
{
	longOut.value = val;
	mess_buffer[buff_pointer++] = longOut.bytes[0];
	mess_buffer[buff_pointer++] = longOut.bytes[1];
	mess_buffer[buff_pointer++] = longOut.bytes[2];
	mess_buffer[buff_pointer++] = longOut.bytes[3];
}

void flush(byte id)
{
	byte mess_ck_a = 0;
	byte mess_ck_b = 0;
	byte i;
	
	SendSer("4D");  			// This is the message preamble
	SendSerw(buff_pointer);  	// Length
	SendSerw(2);  				// id
	SendSerw(0x01);  			// Version

	for (i = 0; i < buff_pointer; i++) {
		SendSerw(mess_buffer[i]);
	}
	
	//for (i = 0; i < buff_pointer; i++) {
	//	mess_ck_a += mess_buffer[i];	// Calculates checksums
	//	mess_ck_b += mess_ck_a;
	//}

	//SendSerw(mess_ck_a);
	//SendSerw(mess_ck_b);

	buff_pointer = 0;
}