/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( "|" );

}

static void write_raw_channel_data( int 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 );

}

void Comms::write_channels( int channel_values[] )

{

	// draw_graph( channel_values );

	write_raw_channel_data( channel_values );

}