/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: 2014-02-26 22:33:50 UTC
  • Revision ID: tim@ed.am-20140226223350-6618z7cy7yxchsas
rc-interface: added code to read from serial and set motor channel values

Show diffs side-by-side

added added

removed removed

src/rc-interface/comms.cc
43
43
}
44
44
 
45
45
 
46
 
static void write_raw_channel_data( int channel_values[] )
 
46
void Comms::write_channels( int channel_values[] )
47
47
{
 
48
//      draw_graph( channel_values );
 
49
 
48
50
        Serial.write( -1 );
49
51
        Serial.write( -1 );
50
52
        unsigned char *data = reinterpret_cast< unsigned char * >( channel_values );
52
54
}
53
55
 
54
56
 
55
 
void Comms::write_channels( int channel_values[] )
 
57
bool Comms::read_channels( int channel_values[] )
56
58
{
57
 
        // draw_graph( channel_values );
58
 
        write_raw_channel_data( 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;
59
94
}
60