/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
//
// receiver.cc
//


#include "receiver.h"
#include "config.h"
#include <Arduino.h>
#include <limits.h>


// channel sequence order
static const int channel_order_[] = { CHANNEL_ORDER };

// 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;
	}
}


static void signal_handler_0()
{
	signal_handler( 0, 2 );
}


static void signal_handler_1()
{
	signal_handler( 1, 3 );
}


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


static 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 Receiver::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_[ (int)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 ] = MAX_CHANNEL_VALUE *
					( duration - MIN_PULSE_WIDTH ) /
					( MAX_PULSE_WIDTH - MIN_PULSE_WIDTH );

				// 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;
}