/elec/quadcopter

To get this branch, use:
bzr branch http://bzr.ed.am/elec/quadcopter
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
//
// main.ino
//
// Testing to see if we can read all 8 channels, multiplexed together on to two
// interrupts (odd channels on one, and even on the other).  We read the
// channels by measuring the pulse width.
//
// Ideally, it would be nice if we could read the raw PPM stream from the
// receiver.  Sometimes you can open up your receiver and get to this stream,
// but we have been unable.  And since this isn't guaranteed to ever be
// available, reading the analogue outputs is certainly more portable.  So, we
// are multiplexing the separate channels to two streams of pulses, like this:
//
//                  ch1  ch3  ch5  ch7  ch2  ch4  ch6  ch8
//                   |    |    |    |    |    |    |    |
//                   ▼    ▼    ▼    ▼    ▼    ▼    ▼    ▼
//                   ¯    ¯    ¯    ¯    ¯    ¯    ¯    ¯
//            ___    |    |    |    |    |    |    |    |     ___
//    GND ---|___|---+----+----+----+    +----+----+----+----|___|--- GND
//             R                    |    |                     R
//             ____________________ | __ | _____________________
//                                  |    |
//                           pin 2  o    o  pin 3
//                    (interrupt 0)        (interrupt 1)
//
// The two resistors in the circuit are pull-down resistors (so, say 100kΩ).
// Without them, the Arduino is unable to read the pulse wave at all.
//
// (Note that on our receiver, and contrary to the above diagram, the channels
// are not actually sent in order.  Channels 2 and 3 are reversed.  To this end,
// the order that channels are received can be adjusted below.)


#include <limits.h>


// minimum pulse width (in ms), used to weed out crappy signals
#define MIN_PULSE_WIDTH 1000UL

// maximum pulse width (in ms), used to weed out crappy signals
#define MAX_PULSE_WIDTH 2000UL

// number of channels
#define NUM_CHANNELS 8

// minimum frame gap time (in ms), used to check that the frame gap is where we
// expect it to be and that we have read the channels properly
#define MIN_FRAME_GAP_WIDTH ( 4000UL + MIN_PULSE_WIDTH )

// the width of the display of a single channel (in chars)
#define GRAPH_SIZE 7

// the channel's expected range for use in drawing (should be similar to
// {MAX,MIN}_PULSE_WIDTH values)
#define GRAPH_MIN 1000
#define GRAPH_MAX 2000


// channel sequence order
const int channel_order_[] = { 1, 3, 2, 4, 5, 6, 7, 8 };

// set to the time of the last pulse edges
static unsigned long new_pulse_up_[2] = { 0, 0 };
static unsigned long new_pulse_down_ = 0;
static char new_pulse_interrupt_;


// ISR to handle the PPM signals
inline void signal_handler( int interrupt, int pin )
{
	// record rising/falling edge
	if( digitalRead( pin ) )
		new_pulse_up_[ interrupt ] = micros();
	else {
		new_pulse_down_ = micros();

		// record which interrupt just had a falling edge
		new_pulse_interrupt_ = interrupt;
	}
}
void signal_handler_0()
{
	signal_handler( 0, 2 );
}
void signal_handler_1()
{
	signal_handler( 1, 3 );
}


void setup()
{
	// set up an interrupts
	attachInterrupt( 0, signal_handler_0, CHANGE );
	attachInterrupt( 1, signal_handler_1, CHANGE );
	digitalWrite( 2, LOW );
	digitalWrite( 3, LOW );

	Serial.begin( 9600 );
}


unsigned long calculate_duration( unsigned long then, unsigned long now )
{
	return now - then;
	
	if( now < then )
		return now + ( ULONG_MAX - then );
	else
		return now - then;
}


bool read_channels( unsigned long channel_values[] )
{
	static unsigned long last_pulse_down = 0;
	static int next_channel = 0;

	// capture pulse values atomically
	noInterrupts();
	unsigned long pulse_up = new_pulse_up_[ new_pulse_interrupt_ ];
	unsigned long pulse_down = new_pulse_down_;
	char new_pulse_interrupt = new_pulse_interrupt_;
	interrupts();

	// if the amount of time that has passed since the last falling edge is
	// greater than the frame gap, reset the next channel so that we can start
	// reading them again
	if( next_channel &&
		new_pulse_interrupt == 1 &&
		calculate_duration( pulse_down, micros() ) > MIN_FRAME_GAP_WIDTH )
	{
		// reset the next channel (which restarts reading them)
		next_channel = 0;
	}

	// check for a new complete pulse
	if( pulse_down != last_pulse_down )
	{
		// are there still pulses to read?
		if( next_channel < NUM_CHANNELS )
		{
			unsigned long duration =
				calculate_duration( pulse_up, pulse_down );

			// does this pulse look ok?
			if( duration >= MIN_PULSE_WIDTH &&
				duration <= MAX_PULSE_WIDTH )
			{
				// store channel value
				int channel = channel_order_[ next_channel ] - 1;
				channel_values[ channel ] = duration;

				// we got a channel
				next_channel++;
			}
			else {
				// set invalid channel number (to indicate error)
				next_channel = NUM_CHANNELS + 1;
			}
		}

		last_pulse_down = pulse_down;
	}

	// if we've read a frame, invalidate the frame (so we don't report it a
	// second time) and return true
	if( next_channel == NUM_CHANNELS ) {
		next_channel++;
		return true;
	}
	
	return false;
}


void draw_graph( unsigned long 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 = max( 0,
			min( channel_values[ a ], GRAPH_MAX ) - GRAPH_MIN );
		int pos = ( GRAPH_SIZE ) * value / ( GRAPH_MAX - GRAPH_MIN );
		graph[ pos + 1 ] = '^';
		Serial.print( graph );
		graph[ pos + 1 ] = '_';
	}
	Serial.println( "|" );
}


void loop()
{
	unsigned long channel_values[ NUM_CHANNELS ];

	while( true )
	{
		if( read_channels( channel_values ) )
			draw_graph( channel_values );
	}
}