/elec/quadcopter

To get this branch, use:
bzr branch http://bzr.ed.am/elec/quadcopter
18 by Tim Marston
completed (hopefully) the RC interface software
1
//
2
// comms.cc
3
//
4
5
6
#include "comms.h"
7
#include "config.h"
8
#include <Arduino.h>
9
10
11
// the width of the display of a single channel (in chars)
12
#define GRAPH_SIZE 7
13
14
15
void Comms::setup()
16
{
17
	Serial.begin( 9600 );
18
}
19
20
21
static void draw_graph( int channel_values[] )
22
{
23
	// init graph
24
	static char graph[ GRAPH_SIZE + 2 ];
25
	static char inited_graph = false;
26
	if( !inited_graph ) {
27
		for( int a = 1; a < GRAPH_SIZE + 1; a++ )
28
			graph[ a ] = '_';
29
		graph[ 0 ] = '|';
30
		graph[ GRAPH_SIZE + 1 ] = 0;
31
		inited_graph = true;
32
	}
33
34
	// draw channels
35
	for( int a = 0; a < NUM_CHANNELS; a++ ) {
36
		unsigned long value = min( channel_values[ a ], MAX_CHANNEL_VALUE );
37
		int pos = ( GRAPH_SIZE ) * value / MAX_CHANNEL_VALUE;
38
		graph[ pos + 1 ] = '^';
39
		Serial.print( graph );
40
		graph[ pos + 1 ] = '_';
41
	}
42
	Serial.println( "|" );
43
}
44
45
28 by Tim Marston
rc-interface: added code to read from serial and set motor channel values
46
void Comms::write_channels( int channel_values[] )
18 by Tim Marston
completed (hopefully) the RC interface software
47
{
28 by Tim Marston
rc-interface: added code to read from serial and set motor channel values
48
//	draw_graph( channel_values );
49
18 by Tim Marston
completed (hopefully) the RC interface software
50
	Serial.write( -1 );
51
	Serial.write( -1 );
52
	unsigned char *data = reinterpret_cast< unsigned char * >( channel_values );
53
	Serial.write( data, sizeof( channel_values ) * NUM_CHANNELS );
54
}
55
56
28 by Tim Marston
rc-interface: added code to read from serial and set motor channel values
57
bool Comms::read_channels( int channel_values[] )
18 by Tim Marston
completed (hopefully) the RC interface software
58
{
28 by Tim Marston
rc-interface: added code to read from serial and set motor channel values
59
	static unsigned char buf[ sizeof( int ) * ( NUM_MOTORS + 1 ) ];
60
	static unsigned int buflen = 0;
61
62
	bool frame_complete = false;
63
64
	while( buflen < sizeof( buf ) && Serial.available() )
65
	{
66
		// read byte
67
		buf[ buflen++ ] = Serial.read();
68
69
		// check if we've got a frame
70
		if( buflen > 1 &&
71
			buf[ buflen - 1 ] == 0xff && buf[ buflen - 2 ] == 0xff )
72
		{
73
			// got a complete frame?
74
			if( buflen == sizeof( buf ) ) {
75
				frame_complete = true;
76
77
				// copy channel values
78
				unsigned char *data =
79
					reinterpret_cast< unsigned char * >( channel_values );
80
				memcpy( buf, data, sizeof( int ) * NUM_MOTORS );
81
			}
82
83
			// reset read buffer
84
			buflen = 0;
85
		}
86
	}
87
88
	// reset read buffer in the case where the buffer is full, but there's been
89
	// no end-of-frame marker (i.e., bad data)
90
	if( buflen >= sizeof( buf ) )
91
		buflen = 0;
92
93
	return frame_complete;
18 by Tim Marston
completed (hopefully) the RC interface software
94
}