Read PWM, Decode RC Receiver Input, and Apply Fail-Safe

1unsigned long now;                        // timing variables to update  data at a regular interval                  
2unsigned long rc_update;
3const  int channels = 6;                   // specify the number of receiver channels
4float  RC_in[channels];                    // an array to store the calibrated input from  receiver 
5
6void setup() {
7    setup_pwmRead();                      
8    Serial.begin(9600);
9}
10
11void loop() {  
12    
13    now = millis();
14    
15    if(RC_avail() || now - rc_update > 25){   // if RC data is available  or 25ms has passed since last update (adjust to be equal or greater than the frame  rate of receiver)
16      
17      rc_update = now;                           
18      
19      print_RCpwm();                        // uncommment to print raw  data from receiver to serial
20      
21      for (int i = 0; i<channels; i++){       // run through each RC channel
22        int CH = i+1;
23        
24        RC_in[i]  = RC_decode(CH);             // decode receiver channel and apply failsafe
25        
26        //print_decimal2percentage(RC_in[i]);   // uncomment to print calibrated  receiver input (+-100%) to serial       
27      }
28      //Serial.println();                       // uncomment when printing calibrated receiver input to serial.
29    }
30}
31

1/*  Kelvin Nelson 24/07/2019
2 *  
3 *  Pulse Width Modulation (PWM)  decoding of RC Receiver with failsafe
4 *  
5 *  This code contains easy to  use functions to measure square wave signals on any arduiuno pro mini, nano or uno  pins, excluding A6 and A7.
6 *  The code is intended to be used with RC receivers,  but could also be used in most other PWM measurement applications as a direct replacement  for pulseIn(PIN, HIGH). 
7 *  (to date it hasn't been tested at a frequency greater  than 1khz or on an arduino mega)
8 *  
9 *  An RC signal pulse can be converted  from a pulse width duration (1000-2000uS) at each input pin into an -+100% (-+1.0)  output for use in a sketch.
10 *  The calibration for this conversion plus a failsafe  setting can be set for each channel. (fail safe tolerances 10-330Hz and 500-2500uS).  
11 *  
12 *  The raw data for each pin can also be extracted, i.e. time of pulse,  pulse width, frame length, duty and frequency.
13 *  
14 *  Set-up is quick, the  organisation of this file is as follows:
15 *  
16 *    - Overview of the code
17  *    - List of functions
18 *    - How to use, including example sketches
19 *    - User defined variables -> specify input pins, transmitter calibration, and  failsafe.
20 *    - Global variables and functions
21 *  
22 *  OVERVIEW OF THE  CODE:
23 *  
24 *  The code enables pin change interrupts on the selected pins  by setting the appropriate registers.
25 *  
26 *  A voltage change on any of  the selected pins will trigger one of three Interrupt Service Routines depending  on which register the pin belongs to.
27 *    - ISR(PCINT0_vect), ISR(PCINT1_vect)  or ISR(PCINT2_vect)
28 *    
29 *  Within each ISR the code determines which pin  has changed, and makes a note of the time before returning back to the main loop().
30  *  
31 *  The time intervals between pin changes are used to calculate pulse width  and frame length. 
32 *  
33 *  Flags are set by the ISR to indicate when new  pulses are received.
34 *  
35 *  The Flags are then used to extract and process  the data collected by each ISR.
36 * 
37 *  Although it's not exactly the same,  this code follows similar principles to those explained in this video: https://youtu.be/bENjl1KQbvo
38  * 
39 */
40// LIST OF FUNCTIONS:
41// OUTPUT TYPE    NAME OF FUNCTION           NOTES
42
43//  void           setup_pwmRead()            initialise the PWM measurement using pin  change interrupts
44
45// RC RECEIVER DECODING
46// boolean        RC_avail()                 returns a HIGH when new RC data is available
47// float          RC_decode(channel  number)  decodes the selected RC channel into the range +-100%, and applies a failsafe.
48//  void           print_RCpwm()              Prints the RC channel raw data to serial  port (used for calibration).
49
50// GENERIC PWM MEASUREMENTS
51// boolean        PWM_read(channel  number)   returns a HIGH when a new pulse has been detected on a particular channel.  
52//                                           The function saves the pulse data  to variables outside the interrupt routines
53//                                           and  must be called just before using the rest of PWM functions.
54// unsigned long  PWM_time()                 returns the time at the start of pulse 
55// float          PWM()                      returns the pulse width
56// float          PWM_period()               returns the time between pulses
57// float          PWM_freq()                 calculates  the frequency
58// float          PWM_duty()                 calculates the duty
59
60//  NOTE: PWM_read(CH) and RC_decode(CH) use the same flags to detect when new data  is available, meaning data could be lost if both are used on the same channel at  the same time.
61// SUGESTION: if you want to use PWM_read(CH) to find the frame  rate of an RC channel call it before RC_decode(CH). The output from RC_decode(CH)  will then default to the failsafe.
62
63// HOW TO USE, including example sketches
64
65//  under the "USER DEFINED VARIABLES" title in the code below:
66//
67//    Step  1: enter the input pins into the array pwmPIN[] = {}. 
68//
69//            -  Any number of pins can be entered into pwmPIN[] (pins available 0 - 13 and A0 -  A5)
70//            - The pins do not need to be in numerical order, for example  pwmPIN[] = {A0,5,6,10,8} for 5 channels, or pwmPIN[] = {A0,5} for 2 channels
71//            - The first element in the array is the pin number for  "channel 1",  and the second is the pin number for "channel 2"... etc.
72//            - All  pins connected to the RC receiver need to be at the start of the array. i.e. the  first 2 channels could be RC inputs and the 3rd channel could be connected to another  device like the echo pin of an ultrasonic sensor.
73//
74//    Step 2: if an RC  receiver is connected to all of the inputs then set RC_inputs to 0, if not specify  the number of channels connected to the receiver i.e. RC_inputs = 2;
75//
76//    Step 3: calibrate your transmitter by uploading a simple sketch with this .ino  file included in the sketch folder, and print the raw PWM values to serial (alternatively  copy and paste the functions needed into the sketch).
77//            Using the  info from the serial monitor manually update the values in arrays RC_min[], RC_mid[],  RC_max[] to suit your transmitter (use full rates to get the best resolution).
78        
79//            an example sketch for printing the RC channel PWM data  to serial. 
80              /* 
81              void setup()  {
82                  setup_pwmRead();
83                  Serial.begin(9600);
84              }
85              void loop()  {
86                  if(RC_avail()) print_RCpwm();
87              }
88               */
89
90//    Step 4: Choose a failsafe position for each channel, in the range -1.0 to +1.0,  and enter it into the array RC_failsafe[] = {}
91//            Note: if you would  like the arduino to respond to the loss of transmitter signal you may need to disable  the failsafe feature on your receiver (if it has one).
92//            an example  sketch to check the operation of the failsafe, and for printing the calibrated channels  to serial:
93/* 
94              unsigned long now;                        //  timing variables to update data at a regular interval                  
95              unsigned  long rc_update;
96              const int channels = 6;                   // specify  the number of receiver channels
97              float RC_in[channels];                    //  an array to store the calibrated input from receiver 
98              
99              void  setup()  {
100                  setup_pwmRead();                      
101                  Serial.begin(9600);
102              }
103              
104              void loop()  {
105                  now  = millis();
106                  
107                  if(RC_avail() || now - rc_update  > 25){   // if RC data is available or 25ms has passed since last update (adjust  to suit frame rate of receiver)
108                    
109                    rc_update  = now;                           
110                    
111                    //print_RCpwm();                        // uncommment to print raw data from receiver to serial
112                    
113                    for (int i = 0; i<channels; i++){       //  run through each RC channel
114                      int CH = i+1;
115                      
116                      RC_in[i] = RC_decode(CH);             // decode receiver channel  and apply failsafe
117                      
118                      print_decimal2percentage(RC_in[i]);   // uncomment to print calibrated receiver input (+-100%) to serial       
119                    }
120                    Serial.println();                       //  uncomment when printing calibrated receiver input to serial.
121                  }
122              }
123               */
124
125// EXAMPLE USE OF GENERIC PWM FUNCTIONS:
126  /*
127 // Print the pulse width of channel 1 to the serial monitor. 
128 // This  is equivelant to the using standard arduino pulseIn(pin, HIGH) function, but without  blocking the code.
129 
130 if (PWM_read(1)){          // if a new pulse is detected  on channel 1, print the pulse width to serial monitor.
131   Serial.println(PWM());
132  } 
133  */
134 // Or 
135 /*
136 // Print RC receiver frame length and frame rate
137  
138 if (PWM_read(1)){                                      // if a new pulse is  detected on channel 1
139   Serial.print(PWM_period(),0);Serial.print("uS ");     
140   Serial.print(PWM_freq());Serial.println("Hz");
141 }
142
143 */
144
145/*
146  *  USER DEFINED VARIABLES (MODIFY TO SUIT YOUR APPLICATION)
147 */
148 
149// PWM  input pins, any of the following pins can be used: digital 0 - 13 or analog A0 -  A5 
150
151const int pwmPIN[]={2,3,4,5,6,7}; // an array to identify the PWM input  pins (the array can be any length) 
152                                  // first  pin is channel 1, second is channel 2...etc
153
154int RC_inputs = 0;                //  The number of pins in pwmPIN that are connected to an RC receiver. Addition pins  not connected to an RC receiver could be used for any other purpose i.e. detecting  the echo pulse on an HC-SR04 ultrasonic distance sensor
155                                  //  When 0, it will automatically update to the number of pins specified in pwmPIN[]  after calling setup_pwmRead().                                                
156//  Calibration of each RC channel:
157 
158// The arrays below are used to calibrate  each RC channel into the range -1 to +1 so that servo direction, mixing, rates,  sub trims...etc can be applied in sketch depending on application. 
159// The arrays  should be modified in order to calibrate the min, middle and max pulse durations  to suit your transmitter (use max rates to get the best resolution). 
160// FYI:  the function print_PWM() will print the raw pulse width data for all the RC channels  to the serial port.
161// if the RC_min[], RC_mid[], RC_max[] are empty or have  missing data the calibration will default to min 1000us, mid 1500us and max 2000us.
162
163//SANWA  6CH 40MHz with corona RP6D1  
164//                THR     RUD     PIT     BAL     SWITCH  SLIDER
165int RC_min[6] = { 988,    1060,   976,    960,    1056,   1116};
166int  RC_mid[6] = { 1472,   1446,   1424,   1398,   1374,   1460};
167int RC_max[6] =  { 1800,   1816,   1796,   1764,   1876,   1796};
168
169// fail safe positions
170
171float  RC_failsafe[] = {0.00, 0.00, 1, 0.00, -0.25, 0.00};
172   
173// enter a failsafe  position (in the range of -+1) for each RC channel in case radio signal is lost
174//  if the array is the incorrect length for the number of RC channels, the failsafe  will default to neutral i.e. 0. 
175// The failsafe tolerances are: 10-330Hz & 500-2500us
176
177/*
178  *    GLOBAL PWM DECODE VARIABLES
179 */
180
181const int num_ch = sizeof(pwmPIN)/sizeof(int);  // calculate the number of input pins (or channels)
182volatile int PW[num_ch];                        // an array to store pulsewidth measurements
183volatile  boolean prev_pinState[num_ch];         // an array used to determine whether a pin  has gone low-high or high-low
184volatile unsigned long pciTime;                 //  the time of the current pin change interrupt
185volatile unsigned long pwmTimer[num_ch];        // an array to store the start time of each PWM pulse
186
187volatile boolean  pwmFlag[num_ch];               // flag whenever new data is available on each pin
188volatile  boolean RC_data_rdy;                   // flag when all RC receiver channels have  received a new pulse
189unsigned long pwmPeriod[num_ch];                 // period,  mirco sec, between two pulses on each pin
190
191byte pwmPIN_reg[num_ch];                        //  each of the input pins expressed as a position on it's associated port register
192byte  pwmPIN_port[num_ch];                       // identify which port each input pin  belongs to (0 = PORTB, 1 = PORTC, 2 = PORTD)
193
194const int size_RC_min = sizeof(RC_min)  / sizeof(int);           // measure the size of the calibration and failsafe arrays
195const  int size_RC_mid = sizeof(RC_mid) / sizeof(int);
196const int size_RC_max = sizeof(RC_max)  / sizeof(int);
197const int size_RC_failsafe = sizeof(RC_failsafe) / sizeof(float);
198
199//  FUNCTION USED TO TURN ON THE INTERRUPTS ON THE RELEVANT PINS
200// code from http://playground.arduino.cc/Main/PinChangeInterrupt
201
202void  pciSetup(byte pin){
203    *digitalPinToPCMSK(pin) |= bit (digitalPinToPCMSKbit(pin));  // enable pin
204    PCIFR  |= bit (digitalPinToPCICRbit(pin));                   //  clear any outstanding interrupt
205    PCICR  |= bit (digitalPinToPCICRbit(pin));                   // enable interrupt for the group
206}
207
208// FUNCTION USED  TO FIND THE PIN POSITION ON EACH PORT REGISTER: helps the interrupt service routines,  ISR, run faster
209
210void pwmPIN_to_port(){
211  for (int i = 0; i < num_ch; i++){
212
213    // determine which port and therefore ISR (PCINT0_vect, PCINT1_vect or PCINT2_vect)  each pwmPIN belongs to.
214                                                                  pwmPIN_port[i]  = 1;    // pin belongs to PCINT1_vect (PORT C)
215    if (pwmPIN[i] >= 0 && pwmPIN[i]  <= 7)                         pwmPIN_port[i] = 2;    // pin belongs to PCINT2_vect  (PORT D)
216    else if (pwmPIN[i] >= 8 && pwmPIN[i] <= 13)                   pwmPIN_port[i]  = 0;    // pin belongs to PCINT0_vect (PORT B)
217
218    // covert the pin number  (i.e. pin 11 or pin A0) to the pin position in the port register. There is most  likely a better way of doing this using a macro...
219    // (Reading the pin state  directly from the port registers speeds up the code in the ISR)
220    
221    if(pwmPIN[i]  == 0 || pwmPIN[i] == A0 || pwmPIN[i] == 8)         pwmPIN_reg[i] = 0b00000001;
222    else if(pwmPIN[i] == 1 || pwmPIN[i] == A1 || pwmPIN[i] == 9)    pwmPIN_reg[i]  = 0b00000010;
223    else if(pwmPIN[i] == 2 || pwmPIN[i] == A2 || pwmPIN[i] == 10)   pwmPIN_reg[i] = 0b00000100;
224    else if(pwmPIN[i] == 3 || pwmPIN[i] == A3  || pwmPIN[i] == 11)   pwmPIN_reg[i] = 0b00001000;
225    else if(pwmPIN[i] == 4  || pwmPIN[i] == A4 || pwmPIN[i] == 12)   pwmPIN_reg[i] = 0b00010000;
226    else  if(pwmPIN[i] == 5 || pwmPIN[i] == A5 || pwmPIN[i] == 13)   pwmPIN_reg[i] = 0b00100000;
227    else if(pwmPIN[i] == 6)                                         pwmPIN_reg[i]  = 0b01000000;
228    else if(pwmPIN[i] == 7)                                         pwmPIN_reg[i]  = 0b10000000;
229    
230  }
231}
232
233// SETUP OF PIN CHANGE INTERRUPTS
234
235void  setup_pwmRead(){
236  
237  for(int i = 0; i < num_ch; i++){              // run  through each input pin
238    pciSetup(pwmPIN[i]);                        // enable  pinchange interrupt for pin
239  }
240  pwmPIN_to_port();                             //  determines the port for each input pin
241                                                //  pwmPIN_to_port() also coverts the pin number in pwmPIN[] (i.e. pin 11 or pin A0)  to the pin position in the port register (i.e. 0b00000001) for use in the ISR.
242  
243  if(RC_inputs == 0 || RC_inputs > num_ch) RC_inputs = num_ch;    // define  the number of pins connected to an RC receiver.                                          
244}  
245
246// INTERRUPT SERVICE ROUTINES (ISR) USED TO READ PWM INPUT
247
248// the  PCINT0_vect (B port register) reacts to any changes on pins D8-13.
249// the PCINT1_vect  (C port register)          ""        ""         A0-A5.
250// the PCINT2_vect  (D port register)          ""        ""         D0-7.
251
252// port registers  are used to speed up if statements in ISR code:
253// https://www.arduino.cc/en/Reference/PortManipulation  http://tronixstuff.com/2011/10/22/tutorial-arduino-port-manipulation/
254// http://harperjiangnew.blogspot.co.uk/2013/05/arduino-port-manipulation-on-mega-2560.html
255
256
257//  READ INTERRUPTS ON PINS D8-D13: ISR routine detects which pin has changed, and returns  PWM pulse width, and pulse repetition period.
258
259ISR(PCINT0_vect){                                                 //  this function will run if a pin change is detected on portB
260  
261  pciTime =  micros();                                             // Record the time of the  PIN change in microseconds
262
263  for (int i = 0; i < num_ch; i++){                               //  run through each of the channels
264    if (pwmPIN_port[i] == 0){                                     //  if the current channel belongs to portB
265      
266      if(prev_pinState[i] ==  0 && PINB & pwmPIN_reg[i]){          // and the pin state has changed from LOW to  HIGH (start of pulse)
267        prev_pinState[i] = 1;                                     //  record pin state
268        pwmPeriod[i] = pciTime - pwmTimer[i];                     //  calculate the time period, micro sec, between the current and previous pulse
269        pwmTimer[i] = pciTime;                                    // record the  start time of the current pulse
270      }
271      else if (prev_pinState[i] ==  1 && !(PINB & pwmPIN_reg[i])){ // or the pin state has changed from HIGH to LOW  (end of pulse)
272        prev_pinState[i] = 0;                                     //  record pin state
273        PW[i] = pciTime - pwmTimer[i];                            //  calculate the duration of the current pulse
274        pwmFlag[i] = HIGH;                                        //  flag that new data is available
275        if(i+1 == RC_inputs) RC_data_rdy = HIGH;                  
276      }
277    }
278  }
279}
280
281// READ INTERRUPTS ON  PINS A0-A5: ISR routine detects which pin has changed, and returns PWM pulse width,  and pulse repetition period.
282
283ISR(PCINT1_vect){                                                 //  this function will run if a pin change is detected on portC
284
285  pciTime = micros();                                             // Record the time of the PIN change  in microseconds
286
287  for (int i = 0; i < num_ch; i++){                               //  run through each of the channels
288    if (pwmPIN_port[i] == 1){                                     //  if the current channel belongs to portC
289      
290      if(prev_pinState[i] ==  0 && PINC & pwmPIN_reg[i]){          // and the pin state has changed from LOW to  HIGH (start of pulse)
291        prev_pinState[i] = 1;                                     //  record pin state
292        pwmPeriod[i] = pciTime - pwmTimer[i];                     //  calculate the time period, micro sec, between the current and previous pulse
293        pwmTimer[i] = pciTime;                                    // record the  start time of the current pulse
294      }
295      else if (prev_pinState[i] ==  1 && !(PINC & pwmPIN_reg[i])){ // or the pin state has changed from HIGH to LOW  (end of pulse)
296        prev_pinState[i] = 0;                                     //  record pin state
297        PW[i] = pciTime - pwmTimer[i];                             //  calculate the duration of the current pulse
298        pwmFlag[i] = HIGH;                                         //  flag that new data is available
299        if(i+1 == RC_inputs) RC_data_rdy = HIGH;
300      }
301    }
302  }
303}
304
305// READ INTERRUPTS ON PINS D0-7: ISR routine  detects which pin has changed, and returns PWM pulse width, and pulse repetition  period.
306
307ISR(PCINT2_vect){                                                 //  this function will run if a pin change is detected on portD
308
309  pciTime = micros();                                             // Record the time of the PIN change  in microseconds
310
311  for (int i = 0; i < num_ch; i++){                               //  run through each of the channels
312    if (pwmPIN_port[i] == 2){                                     //  if the current channel belongs to portD
313      
314      if(prev_pinState[i] ==  0 && PIND & pwmPIN_reg[i]){          // and the pin state has changed from LOW to  HIGH (start of pulse)
315        prev_pinState[i] = 1;                                     //  record pin state
316        pwmPeriod[i] = pciTime - pwmTimer[i];                     //  calculate the time period, micro sec, between the current and previous pulse
317        pwmTimer[i] = pciTime;                                    // record the  start time of the current pulse
318      }
319      else if (prev_pinState[i] ==  1 && !(PIND & pwmPIN_reg[i])){ // or the pin state has changed from HIGH to LOW  (end of pulse)
320        prev_pinState[i] = 0;                                     //  record pin state
321        PW[i] = pciTime - pwmTimer[i];                            //  calculate the duration of the current pulse
322        pwmFlag[i] = HIGH;                                        //  flag that new data is available
323        if(i+1 == RC_inputs) RC_data_rdy = HIGH;
324      }
325    }
326  }
327}
328
329/*
330 *  RC OUTPUT FUNCTIONS
331 */
332
333  boolean RC_avail(){
334    boolean avail = RC_data_rdy;
335    RC_data_rdy = LOW;                          // reset the flag
336    return avail;
337    }
338
339  float RC_decode(int CH){
340  
341  if(CH < 1 || CH > RC_inputs) return 0;     //  if channel number is out of bounds return zero.
342  
343  int i = CH - 1;                     
344
345  // determine the pulse width calibration for the RC channel. The default is 1000,  1500 and 2000us.
346  
347  int Min;
348  if(CH <= size_RC_min) Min = RC_min[CH-1];  else Min = 1000;
349  
350  int Mid;
351  if(CH <= size_RC_mid) Mid = RC_mid[CH-1];  else Mid = 1500;
352  
353  int Max;
354  if(CH <= size_RC_max) Max = RC_max[CH-1];  else Max = 2000;
355
356  float CH_output;
357      
358  if(FAILSAFE(CH) == HIGH){                         // If the RC channel is outside of failsafe tolerances  (10-330hz and 500-2500uS)
359      if(CH > size_RC_failsafe) CH_output = 0;      //  and if no failsafe position has been defined, set output to neutral
360      else  CH_output = RC_failsafe[i];              // or if defined set the failsafe position  
361  }
362  else{                                             // If the RC signal  is valid
363    CH_output = calibrate(PW[i],Min,Mid,Max);       // calibrate the  pulse width to the range -1 to 1.
364  }
365  return CH_output;                                 
366
367  // The signal is mapped from a pulsewidth into the range of -1 to +1, using the  user defined calibrate() function in this code. 
368
369  // 0 represents neutral  or center stick on the transmitter
370  // 1 is full displacement of a control input  is one direction (i.e full left rudder)
371  // -1 is full displacement of the control  input in the other direction (i.e. full right rudder)
372}
373
374/*
375 *  Receiver  Calibration
376 */
377
378 // NEED TO SPEED UP
379
380float calibrate(float Rx,  int Min, int Mid, int Max){
381   float calibrated;
382   if (Rx >= Mid)
383   {
384    calibrated = map(Rx, Mid, Max, 0, 1000);  // map from 0% to 100% in one direction
385   }
386   else if (Rx == 0)
387   {
388    calibrated = 0;                           //  neutral
389   }
390   else
391   {
392    calibrated = map(Rx, Min, Mid, -1000,  0); // map from 0% to -100% in the other direction
393   }
394  return calibrated  * 0.001;
395}
396
397// Basic Receiver FAIL SAFE
398// check for 500-2500us and  10-330Hz (same limits as pololu)
399
400boolean FAILSAFE(int CH){
401
402   int  i = CH-1;
403   boolean failsafe_flag = LOW;
404        
405       if(pwmFlag[i]  == 1)                             // if a new pulse has been measured.
406         {
407            pwmFlag[i] = 0;                            // set flag to zero
408      
409            if(pwmPeriod[i] > 100000)                  // if time between pulses  indicates a pulse rate of less than 10Hz   
410            {
411              failsafe_flag  = HIGH;                       
412            }
413            else if(pwmPeriod[i]  < 3000)               // or if time between pulses indicates a pulse rate greater  than 330Hz   
414            {
415              failsafe_flag = HIGH;                             
416            }
417
418            if(PW[i] < 500 || PW[i] > 2500)           // if  pulswidth is outside of the range 500-2500ms
419            {
420              failsafe_flag  = HIGH;                        
421            }   
422         }
423        else  if (micros() - pwmTimer[i] > 100000)     // if there is no new pulswidth measurement  within 100ms (10hz)
424        {
425          failsafe_flag = HIGH;                      
426        }
427
428    return failsafe_flag;   
429}
430
431/*
432 *  Quick print  function of Rx channel input
433 */
434
435void print_RCpwm(){                             //  display the raw RC Channel PWM Inputs
436  for (int i = 0; i < RC_inputs; i++){
437    //Serial.print(" ch");Serial.print(i+1);
438    Serial.print("  ");
439    if(PW[i] < 1000) Serial.print(" ");
440    Serial.print(PW[i]);
441  }
442  Serial.println("");
443}
444
445void print_decimal2percentage(float dec){
446  int pc = dec*100;
447  // the number and text will take up 6 charactors i.e ___3%_  or -100%_
448  if (pc >= 0) Serial.print(" ");
449  if (abs(pc) < 100) Serial.print("  ");
450  if (abs(pc) < 10) Serial.print(" ");
451  Serial.print(" ");Serial.print(pc);Serial.print("%  ");
452}
453
454/*
455 * GENERIC PWM FUNCTIONS
456 */
457
458unsigned long pin_time;
459float  pin_pwm;
460float pin_period;
461
462boolean PWM_read(int CH){
463  if(CH < 1 &&  CH > num_ch) return false;
464  int i = CH-1;
465  boolean avail = pwmFlag[i];
466  if (avail == HIGH){
467    pwmFlag[i] = LOW;
468    noInterrupts();
469    pin_time  = pwmTimer[i];
470    pin_pwm = PW[i];
471    pin_period = pwmPeriod[i];
472    interrupts();
473  }
474  return avail;
475}
476
477unsigned long PWM_time(){return pin_time;}
478float  PWM_period(){return pin_period;}
479float PWM(){return pin_pwm;}
480
481float PWM_freq(){
482  float freq;
483  return freq = 1000000 / pin_period;  // frequency Hz
484}
485
486float  PWM_duty(){
487  float duty;
488  duty = pin_pwm/pin_period;
489  return duty;
490}
491

1void setup() {
2    setup_pwmRead();                      
3    Serial.begin(9600);
4}
5
6void  loop() {  
7    
8    // Print RC receiver frame length and frame rate
9 
10    if (PWM_read(1)){                                      // if a new pulse is  detected on channel 1
11      Serial.print(PWM_period(),0);Serial.print("uS ");     
12      Serial.print(PWM_freq());Serial.println("Hz");
13    }
14
15}
16

1// servo variables
2
3#include <Servo.h> // include the servo library  to control the servos
4
5Servo servo1;   // name each servo output for use with  the servo library    
6Servo servo2;      
7
8// Each servo must be attached  to a pin that has a PWM output
9// on the arduino uno, nano and pro mini these  pins are 3, 5, 6, 9, 10 and 11
10
11const int servo1_pin = 9;  // identify the  pins that each servo signal wire is connected to
12const int servo2_pin = 10;
13
14//  Select Servo Direction, Rates and Sub-trim (the size of each array must match the  number of servos)
15
16boolean servo_dir[] = {0,1};     // Direction: 0 is normal,  1 is reverse
17float servo_rates[] = {1,0.5};  // Rates: range 0 to 2 (1 = +-500us  (NORMAL), 2 = +-1000us (MAX)): The amount of servo deflection in both directions
18float  servo_subtrim[] = {0.0,0.0};  // Subtrimrange -1 to +1 (-1 = 1000us, 0 = 1500us,  1 = 2000us): The neutral position of the servo
19boolean servo_mix_on = true;
20
21unsigned  long now;                  // timing variables to update data at a regular interval                        
22unsigned long rc_update;
23
24// Receiver variables
25
26const  int channels = 6;                   // specify the number of receiver channels
27float  RC_in[channels];                    // an array to store the calibrated input from  receiver 
28
29void setup() {
30    
31    servo1.attach(servo1_pin, 500, 2500);  // attach the servo library to each servo pin, and define min and max uS values
32    servo2.attach(servo2_pin, 500, 2500);
33    
34    setup_pwmRead();                      
35    Serial.begin(9600);
36}
37
38void loop() {  
39    
40    now = millis();
41    
42    if(RC_avail() || now - rc_update > 25){   // if RC data is available  or 25ms has passed since last update (adjust to > frame rate of receiver)
43      
44      rc_update = now;                           
45      
46      print_RCpwm();                        // uncommment to print raw data from receiver to serial
47      
48      for (int i = 0; i<channels; i++){       // run through each RC channel
49        int CH = i+1;
50        
51        RC_in[i] = RC_decode(CH);             //  decode receiver channel and apply failsafe
52        
53        //print_decimal2percentage(RC_in[i]);   // uncomment to print calibrated receiver input (+-100%) to serial       
54      }
55      //Serial.println();                       // uncomment when printing  calibrated receiver input to serial.
56
57      
58      
59      int servo1_uS;      // variables to store the pulse widths to be sent to the servo
60      int  servo2_uS;      
61      
62      if (servo_mix_on == true){              // MIXING  ON
63        
64        float mix1 = RC_in[1] - RC_in[2];     // Channel 2 (ELV)  - Channel 3 (AIL)
65        float mix2 = RC_in[1] + RC_in[2];     // Channel 2  (ELV) + Channel 3 (AIL)
66  
67        if(mix1 > 1) mix1 = 1;                //  limit mixer output to +-1
68        else if(mix1 < -1) mix1 = -1;
69  
70        if(mix2  > 1) mix2 = 1;                // limit mixer output to +-1
71        else if(mix2  < -1) mix2 = -1;  
72  
73        // Calculate the pulse widths for the servos
74      
75        servo1_uS = calc_uS(mix1, 1);         // apply the servo rates,  direction and sub_trim for servo 1, and convert to a RC pulsewidth (microseconds,  uS)
76        servo2_uS = calc_uS(mix2, 2);         // apply the servo rates, direction  and sub_trim for servo 2, and convert to a RC pulsewidth (microseconds, uS)
77            
78      }
79      else{                                   // MIXING  OFF
80        servo1_uS = calc_uS(RC_in[1],1);      // apply the servo rates, direction  and sub_trim for servo 1, and convert to a RC pulsewidth (microseconds, uS)
81        servo2_uS = calc_uS(RC_in[2],2);      // apply the servo rates, direction  and sub_trim for servo 1, and convert to a RC pulsewidth (microseconds, uS)
82      }
83
84      servo1.writeMicroseconds(servo1_uS);   // write the pulsewidth  to the servo.
85      servo2.writeMicroseconds(servo2_uS);   // write the pulsewidth  to the servo. 
86    }
87}
88
89int calc_uS(float cmd, int servo){                                //  cmd = commanded position +-100% 
90                                                                  //  servo = servo num (to apply correct direction, rates and trim)
91  int i = servo-1;
92  float dir;
93  if(servo_dir[i] == 0) dir = -1; else dir = 1;                   //  set the direction of servo travel
94  
95  cmd = 1500 + (cmd*servo_rates[i]*dir  + servo_subtrim[i])*500;   // apply servo rates and sub trim, then convert to a  uS value
96
97  if(cmd > 2500) cmd = 2500;                                      //  limit pulsewidth to the range 500 to 2500us
98  else if(cmd < 500) cmd = 500;
99
100  return cmd;
101}
102

1void setup() {
2    setup_pwmRead();                      
3    Serial.begin(9600);
4}
5
6void   loop() {  
7    
8    // Print RC receiver frame length and frame rate
9 
10     if (PWM_read(1)){                                      // if a new pulse is   detected on channel 1
11      Serial.print(PWM_period(),0);Serial.print("uS ");      
12      Serial.print(PWM_freq());Serial.println("Hz");
13    }
14
15}
16

1// servo variables
2
3#include <Servo.h> // include the servo library   to control the servos
4
5Servo servo1;   // name each servo output for use with   the servo library    
6Servo servo2;      
7
8// Each servo must be attached   to a pin that has a PWM output
9// on the arduino uno, nano and pro mini these   pins are 3, 5, 6, 9, 10 and 11
10
11const int servo1_pin = 9;  // identify the   pins that each servo signal wire is connected to
12const int servo2_pin = 10;
13
14//   Select Servo Direction, Rates and Sub-trim (the size of each array must match the   number of servos)
15
16boolean servo_dir[] = {0,1};     // Direction: 0 is normal,   1 is reverse
17float servo_rates[] = {1,0.5};  // Rates: range 0 to 2 (1 = +-500us   (NORMAL), 2 = +-1000us (MAX)): The amount of servo deflection in both directions
18float   servo_subtrim[] = {0.0,0.0};  // Subtrimrange -1 to +1 (-1 = 1000us, 0 = 1500us,   1 = 2000us): The neutral position of the servo
19boolean servo_mix_on = true;
20
21unsigned   long now;                  // timing variables to update data at a regular interval                         
22unsigned long rc_update;
23
24// Receiver variables
25
26const   int channels = 6;                   // specify the number of receiver channels
27float   RC_in[channels];                    // an array to store the calibrated input from   receiver 
28
29void setup() {
30    
31    servo1.attach(servo1_pin, 500, 2500);   // attach the servo library to each servo pin, and define min and max uS values
32     servo2.attach(servo2_pin, 500, 2500);
33    
34    setup_pwmRead();                      
35     Serial.begin(9600);
36}
37
38void loop() {  
39    
40    now = millis();
41     
42    if(RC_avail() || now - rc_update > 25){   // if RC data is available   or 25ms has passed since last update (adjust to > frame rate of receiver)
43      
44       rc_update = now;                           
45      
46      print_RCpwm();                         // uncommment to print raw data from receiver to serial
47       
48      for (int i = 0; i<channels; i++){       // run through each RC channel
49         int CH = i+1;
50        
51        RC_in[i] = RC_decode(CH);             //   decode receiver channel and apply failsafe
52        
53        //print_decimal2percentage(RC_in[i]);    // uncomment to print calibrated receiver input (+-100%) to serial       
54       }
55      //Serial.println();                       // uncomment when printing   calibrated receiver input to serial.
56
57      
58      
59      int servo1_uS;       // variables to store the pulse widths to be sent to the servo
60      int   servo2_uS;      
61      
62      if (servo_mix_on == true){              // MIXING   ON
63        
64        float mix1 = RC_in[1] - RC_in[2];     // Channel 2 (ELV)   - Channel 3 (AIL)
65        float mix2 = RC_in[1] + RC_in[2];     // Channel 2   (ELV) + Channel 3 (AIL)
66  
67        if(mix1 > 1) mix1 = 1;                //   limit mixer output to +-1
68        else if(mix1 < -1) mix1 = -1;
69  
70        if(mix2   > 1) mix2 = 1;                // limit mixer output to +-1
71        else if(mix2   < -1) mix2 = -1;  
72  
73        // Calculate the pulse widths for the servos
74       
75        servo1_uS = calc_uS(mix1, 1);         // apply the servo rates,   direction and sub_trim for servo 1, and convert to a RC pulsewidth (microseconds,   uS)
76        servo2_uS = calc_uS(mix2, 2);         // apply the servo rates, direction   and sub_trim for servo 2, and convert to a RC pulsewidth (microseconds, uS)
77             
78      }
79      else{                                   // MIXING   OFF
80        servo1_uS = calc_uS(RC_in[1],1);      // apply the servo rates, direction   and sub_trim for servo 1, and convert to a RC pulsewidth (microseconds, uS)
81         servo2_uS = calc_uS(RC_in[2],2);      // apply the servo rates, direction   and sub_trim for servo 1, and convert to a RC pulsewidth (microseconds, uS)
82       }
83
84      servo1.writeMicroseconds(servo1_uS);   // write the pulsewidth   to the servo.
85      servo2.writeMicroseconds(servo2_uS);   // write the pulsewidth   to the servo. 
86    }
87}
88
89int calc_uS(float cmd, int servo){                                //   cmd = commanded position +-100% 
90                                                                  //   servo = servo num (to apply correct direction, rates and trim)
91  int i = servo-1;
92   float dir;
93  if(servo_dir[i] == 0) dir = -1; else dir = 1;                   //   set the direction of servo travel
94  
95  cmd = 1500 + (cmd*servo_rates[i]*dir   + servo_subtrim[i])*500;   // apply servo rates and sub trim, then convert to a   uS value
96
97  if(cmd > 2500) cmd = 2500;                                      //   limit pulsewidth to the range 500 to 2500us
98  else if(cmd < 500) cmd = 500;
99
100   return cmd;
101}
102

1unsigned long now;                        // timing variables to update
2  data at a regular interval                  
3unsigned long rc_update;
4const
5  int channels = 6;                   // specify the number of receiver channels
6float
7  RC_in[channels];                    // an array to store the calibrated input from
8  receiver 
9
10void setup() {
11    setup_pwmRead();                      
12
13    Serial.begin(9600);
14}
15
16void loop() {  
17    
18    now = millis();
19
20    
21    if(RC_avail() || now - rc_update > 25){   // if RC data is available
22  or 25ms has passed since last update (adjust to be equal or greater than the frame
23  rate of receiver)
24      
25      rc_update = now;                           
26
27      
28      print_RCpwm();                        // uncommment to print raw
29  data from receiver to serial
30      
31      for (int i = 0; i<channels; i++){
32       // run through each RC channel
33        int CH = i+1;
34        
35        RC_in[i]
36  = RC_decode(CH);             // decode receiver channel and apply failsafe
37        
38
39        //print_decimal2percentage(RC_in[i]);   // uncomment to print calibrated
40  receiver input (+-100%) to serial       
41      }
42      //Serial.println();
43                       // uncomment when printing calibrated receiver input to serial.
44
45    }
46}
47