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

  • Committer: Tim Marston
  • Date: 2013-11-20 23:58:23 UTC
  • Revision ID: tim@ed.am-20131120235823-q9xheffre1y00lhd
fixed pulse-width test

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/rc-interface/receiver.cc
1
 
//
2
 
// receiver.cc
3
 
//
4
 
 
5
 
 
6
 
#include "receiver.h"
7
 
#include "config.h"
8
 
#include <Arduino.h>
9
 
#include <limits.h>
10
 
 
11
 
 
12
 
// channel sequence order
13
 
static const int channel_order_[] = { CHANNEL_ORDER };
14
 
 
15
 
// set to the time of the last pulse edges
16
 
static unsigned long new_pulse_up_[2] = { 0, 0 };
17
 
static unsigned long new_pulse_down_ = 0;
18
 
static char new_pulse_interrupt_;
19
 
 
20
 
 
21
 
// ISR to handle the PPM signals
22
 
inline void signal_handler( int interrupt, int pin )
23
 
{
24
 
        // record rising/falling edge
25
 
        if( digitalRead( pin ) )
26
 
                new_pulse_up_[ interrupt ] = micros();
27
 
        else {
28
 
                new_pulse_down_ = micros();
29
 
 
30
 
                // record which interrupt just had a falling edge
31
 
                new_pulse_interrupt_ = interrupt;
32
 
        }
33
 
}
34
 
 
35
 
 
36
 
static void signal_handler_0()
37
 
{
38
 
        signal_handler( 0, 2 );
39
 
}
40
 
 
41
 
 
42
 
static void signal_handler_1()
43
 
{
44
 
        signal_handler( 1, 3 );
45
 
}
46
 
 
47
 
 
48
 
void Receiver::setup()
49
 
{
50
 
        // set up an interrupts
51
 
        attachInterrupt( 0, signal_handler_0, CHANGE );
52
 
        attachInterrupt( 1, signal_handler_1, CHANGE );
53
 
        digitalWrite( 2, LOW );
54
 
        digitalWrite( 3, LOW );
55
 
}
56
 
 
57
 
 
58
 
static unsigned long calculate_duration( unsigned long then, unsigned long now )
59
 
{
60
 
        return now - then;
61
 
        
62
 
        if( now < then )
63
 
                return now + ( ULONG_MAX - then );
64
 
        else
65
 
                return now - then;
66
 
}
67
 
 
68
 
 
69
 
bool Receiver::read_channels( unsigned long channel_values[] )
70
 
{
71
 
        static unsigned long last_pulse_down = 0;
72
 
        static int next_channel = 0;
73
 
 
74
 
        // capture pulse values atomically
75
 
        noInterrupts();
76
 
        unsigned long pulse_up = new_pulse_up_[ (int)new_pulse_interrupt_ ];
77
 
        unsigned long pulse_down = new_pulse_down_;
78
 
        char new_pulse_interrupt = new_pulse_interrupt_;
79
 
        interrupts();
80
 
 
81
 
        // if the amount of time that has passed since the last falling edge is
82
 
        // greater than the frame gap, reset the next channel so that we can start
83
 
        // reading them again
84
 
        if( next_channel &&
85
 
                new_pulse_interrupt == 1 &&
86
 
                calculate_duration( pulse_down, micros() ) > MIN_FRAME_GAP_WIDTH )
87
 
        {
88
 
                // reset the next channel (which restarts reading them)
89
 
                next_channel = 0;
90
 
        }
91
 
 
92
 
        // check for a new complete pulse
93
 
        if( pulse_down != last_pulse_down )
94
 
        {
95
 
                // are there still pulses to read?
96
 
                if( next_channel < NUM_CHANNELS )
97
 
                {
98
 
                        unsigned long duration =
99
 
                                calculate_duration( pulse_up, pulse_down );
100
 
 
101
 
                        // does this pulse look ok?
102
 
                        if( duration >= MIN_PULSE_WIDTH &&
103
 
                                duration <= MAX_PULSE_WIDTH )
104
 
                        {
105
 
                                // store channel value
106
 
                                int channel = channel_order_[ next_channel ] - 1;
107
 
                                channel_values[ channel ] = MAX_CHANNEL_VALUE *
108
 
                                        ( duration - MIN_PULSE_WIDTH ) /
109
 
                                        ( MAX_PULSE_WIDTH - MIN_PULSE_WIDTH );
110
 
 
111
 
                                // we got a channel
112
 
                                next_channel++;
113
 
                        }
114
 
                        else {
115
 
                                // set invalid channel number (to indicate error)
116
 
                                next_channel = NUM_CHANNELS + 1;
117
 
                        }
118
 
                }
119
 
 
120
 
                last_pulse_down = pulse_down;
121
 
        }
122
 
 
123
 
        // if we've read a frame, invalidate the frame (so we don't report it a
124
 
        // second time) and return true
125
 
        if( next_channel == NUM_CHANNELS ) {
126
 
                next_channel++;
127
 
                return true;
128
 
        }
129
 
        
130
 
        return false;
131
 
}