/elec/quadcopter

//

// comms.cc

//

#include "comms.h"

#include "config.h"

#include <Arduino.h>

// the width of the display of a single channel (in chars)

#define GRAPH_SIZE 7

void Comms::setup()

{

	Serial.begin( 9600 );

}

static void draw_graph( int channel_values[] )

{

	// init graph

	static char graph[ GRAPH_SIZE + 2 ];

	static char inited_graph = false;

	if( !inited_graph ) {

		for( int a = 1; a < GRAPH_SIZE + 1; a++ )

			graph[ a ] = '_';

		graph[ 0 ] = '|';

		graph[ GRAPH_SIZE + 1 ] = 0;

		inited_graph = true;

	}

	// draw channels

	for( int a = 0; a < NUM_CHANNELS; a++ ) {

		unsigned long value = min( channel_values[ a ], MAX_CHANNEL_VALUE );

		int pos = ( GRAPH_SIZE ) * value / MAX_CHANNEL_VALUE;

		graph[ pos + 1 ] = '^';

		Serial.print( graph );

		graph[ pos + 1 ] = '_';

	}

	Serial.println( "|" );

}

void Comms::write_channels( int channel_values[] )

{

//	draw_graph( channel_values );

	Serial.write( -1 );

	Serial.write( -1 );

	unsigned char *data = reinterpret_cast< unsigned char * >( channel_values );

	Serial.write( data, sizeof( channel_values ) * NUM_CHANNELS );

}

bool Comms::read_channels( int channel_values[] )

{

	static unsigned char buf[ sizeof( int ) * ( NUM_MOTORS + 1 ) ];

	static unsigned int buflen = 0;

	bool frame_complete = false;

	while( buflen < sizeof( buf ) && Serial.available() )

	{

		// read byte

		buf[ buflen++ ] = Serial.read();

		// check if we've got a frame

		if( buflen > 1 &&

			buf[ buflen - 1 ] == 0xff && buf[ buflen - 2 ] == 0xff )

		{

			// got a complete frame?

			if( buflen == sizeof( buf ) ) {

				frame_complete = true;

				// copy channel values

				unsigned char *data =

					reinterpret_cast< unsigned char * >( channel_values );

				memcpy( data, buf, sizeof( int ) * NUM_MOTORS );

			}

			// reset read buffer

			buflen = 0;

		}

	}

	// reset read buffer in the case where the buffer is full, but there's been

	// no end-of-frame marker (i.e., bad data)

	if( buflen >= sizeof( buf ) )

		buflen = 0;

	return frame_complete;

}