/elec/quadcopter

To get this branch, use:
bzr branch http://bzr.ed.am/elec/quadcopter

« back to all changes in this revision

Viewing changes to src/rc-interface/comms.cc

  • Committer: Tim Marston
  • Date: 2013-01-16 21:42:06 UTC
  • Revision ID: tim@ed.am-20130116214206-5mw4cgsya1h0yiyq
updated arduino.mk

Show diffs side-by-side

added added

removed removed

src/rc-interface/comms.cc
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
 
 
46
 
void Comms::write_channels( int channel_values[] )
47
 
{
48
 
//      draw_graph( channel_values );
49
 
 
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
 
 
57
 
bool Comms::read_channels( int channel_values[] )
58
 
{
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( data, buf, 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;
94
 
}