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