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