My first attempt quad copter

So my Senior Project for Computer Engineering Technology at the University of Southern Mississippi is giving me fits.  I am attempting to use an Arduino DUE to take in RC from a receiver and pass them through the DUE to an APM 2.6 flight controller on a Quad Copter.  The pass through works fine.   I am now adding ultrasonic sensors to the mix to add some wall avoidance to the quad.  I am currently testing methods to implement the ultrasonic reads as there are no Arduino DUE libraries for ultrasonics out there yet.
Another problem is the Arduino DUE can only handle a 3.3V signal to come from the echo of the sensor.  I am using the HC-SR04 sensors for testing right now due to their cost and fairly decent accuracy.  Just about all sensors and sheilds designed for the Arduino family use 5v signals.  This gives us a problem, well sorta.  I am testing different configurations of voltage dividers to reduce the voltage without compromising the signal.  This alas is my current hangup keeping me from testing my code out.
Below is the code in segments.
Understand that this is heavily influenced by Duane's 8 channel RCArduino work at
http://rcarduino.blogspot.co.uk/2012/04/how-to-read-multiple-rc-channels-draft.html
Without his code to basically copy I would still be very much lost.

UPDATE 3/28/2014 
I am putting my DUE away for now and testing with a mega2560
Pretty much the same code with a few minor changes if it works.  I have to change how the library servo.h is written syntax wise and how to use the attach interrupt statements.  I am highlighting what I forgot and what I changed.

Ok I found a couple of errors from above and changed some things around.  One I forgot to copy my local flag from my global one.  This flag is to tell the rest of the program that there was an update to one of the channels, basically any change triggers the ISR and that triggers the global flag.  I have to turn off the interrupt handler to make a local copy of the flag and the changed incoming values.
I forgot to turn my interrupts back on so of course the code doesnt update... duhh.  I am actually moving back to the Mega2560 arduino for further testing so I dont have to deal with the voltage divider and possible ruining my DUE.  but the code should work for both in theory.
Another change I am making is to move the ultrasonic for loop and mode calculation to inside the area of the code that the interrupts are turned off.  This should not add enough time with the interrupts off to be noticed.

First the includes and defines.
#include <Average.h>
//#include "Servo.h" // DUE
#include <Servo.h>  // big difference here huh  Mega2560

#define PITCH_CHANNEL_IN 2  // attach from receiver elevator channel for pitch control (Mega2560 interrupt 0)
#define ROLL_CHANNEL_IN 3   // attach from receiver ailerons channel for roll control (Mega interrupt 1)

#define PITCH_CHANNEL_OUT 9  // attach to flight controller elevator pin
#define ROLL_CHANNEL_OUT 10  // attach to flight controller aileron pin

Servo servoChannel1;  // setup servo channel for pwm output
Servo servoChannel2;  // setup servo channel for pwm output

#define PITCH_FLAG 1  // flag to determine if new input is detected on pitch channel
#define ROLL_FLAG 2   // flag to determine if new input is detected on roll channel


volatile uint32_t SharedNewFlag;  // global shared flag for determining if any flag has been triggered

volatile uint32_t PITCH_IN_SHARED;  // global shared flag for pitch input trigger
volatile uint32_t ROLL_IN_SHARED;   // global shared flag for roll input trigger

#define pingcnt 6  // define number of pings to loop through for ultrasonic pings
int avgf[pingcnt]; //array for front ultrasonc sensor
int avgb[pingcnt]; //array for rear ultrasonc sensor
int avgr[pingcnt]; //array for right ultrasonc sensor
int avgl[pingcnt]; //array for left ultrasonc sensor

// using digital pins
#define TRIGGER_PINF 4   // trigger pin for front ultrasonic sensor
#define ECHO_PINF 5 // echo pin for front ultrasonic sensor
#define TRIGGER_PINB 6 // trigger pin for rear ultrasonic sensor
#define ECHO_PINB 7 // echo pin for rear ultrasonic sensor
#define TRIGGER_PINL 8 // trigger pin for left ultrasonic sensor
#define ECHO_PINL 9 // echo pin for left ultrasonic sensor
#define TRIGGER_PINR 10 // trigger pin for right ultrasonic sensor
#define ECHO_PINR 11 // echo pin for right ultrasonic sensor

Now that thats done we add in the setup portion

void setup() 
{
  Serial.begin(115200);  // for testing the PWM pass through just add Serial.print(the channel needed); after the servo write 
  
  servoChannel1.attach(PITCH_CHANNEL_OUT);  // attach the servo channel 1 to pitch out pin
  servoChannel2.attach(ROLL_CHANNEL_OUT);   // attach the servo channel 2 to roll out pin

  attachInterrupt(0, calcPitch,CHANGE);  // attach interrupt to the in pins for pass through
  attachInterrupt(1, calcRoll,CHANGE); // attach interrupt to the in pins for pass through 
  //attachInterrupt(PITCH_CHANNEL_IN, calcPitch,CHANGE);  // attach interrupt to the in pins for pass through
  //attachInterrupt(ROLL_CHANNEL_IN, calcRoll,CHANGE); // attach interrupt to the in pins for pass through
// notice above that the only change between DUE attach interrupt statement and the Mega is you can't just assign
// any pin as an interrupt in the Mega like you can in the DUE.
// I am using 2 interrupts so using pin 2 which is interrupt 0 on the mega
// and pin 3 which os interrupt 1 on the mega (arduino.cc datasheet had these wrong when I originally did some research)
// INT5 :        18
// INT4 :        19
// INT3 :        20
// INT2 :        21
// INT1 :        3// INT0 :        2  
// check out http://arduinomega.blogspot.com/2011/05/external-interrupts-buttons-for.html for a good explanation of these


  // set ultrasonic pin modes trigger out and echo in
  pinMode(TRIGGER_PINF, OUTPUT);
  pinMode(TRIGGER_PINB, OUTPUT);
  pinMode(TRIGGER_PINL, OUTPUT);
  pinMode(TRIGGER_PINR, OUTPUT);
  pinMode(ECHO_PINF, INPUT);
  pinMode(ECHO_PINB, INPUT);
  pinMode(ECHO_PINL, INPUT);
  pinMode(ECHO_PINR, INPUT);
}

Now for the main loop

void loop() 
{
// static local copies to be used as output and calculation
  static uint32_t PITCH_IN_LOCAL;
  static uint32_t ROLL_IN_LOCAL;
// local update flag
  static uint32_t LocalNewFlag;
// check if global flag has been triggered then turn off interrupts until output has been completed
if(SharedNewFlag)
  {
    noInterrupts();
       // check which channel was the flagged as updated
    LocalNewFlag = SharedNewFlag; // silly me left this out also without this there is nothing updated ever
    if(LocalNewFlag & PITCH_FLAG)
    {
      PITCH_IN_LOCAL = PITCH_IN_SHARED; // set local copy of input to global shared value
    }
   
    if(LocalNewFlag & ROLL_FLAG)
    {
      ROLL_IN_LOCAL = ROLL_IN_SHARED;  // set local copy of input to global shared value
    }
   
    SharedNewFlag = 0;  // reset global update flag to zero for next loop
}
//force trigger pins low then high for at least 10us to help clear false echos
//save the measured distances to an int variable
            
   for(int i=0; i<pingcnt; i++)
   {
    digitalWrite(TRIGGER_PINF, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIGGER_PINF, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIGGER_PINF, LOW);
    long durationf = pulseIn(ECHO_PINF, HIGH);
    avgf[i] = (durationf/2)/29.1;
    
    digitalWrite(TRIGGER_PINB, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIGGER_PINB, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIGGER_PINB, LOW);
    long durationb = pulseIn(ECHO_PINB, HIGH);
    avgb[i] = (durationb/2)/29.1;
    
    digitalWrite(TRIGGER_PINR, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIGGER_PINR, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIGGER_PINR, LOW);
    long durationr = pulseIn(ECHO_PINR, HIGH);
    avgr[i] = (durationr/2)/29.1;
    
    digitalWrite(TRIGGER_PINL, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIGGER_PINL, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIGGER_PINL, LOW);
    long durationl = pulseIn(ECHO_PINL, HIGH);
    avgl[i] = (durationl/2)/29.1;

    
   }
   int distancef = mode(avgf, pingcnt); // distance equals the most common value of the array currently set to 6
   int distanceb = mode(avgb, pingcnt);
   int distancer = mode(avgr, pingcnt);
   int distancel = mode(avgl, pingcnt);

interrupts();  // oops forgot to turn them back on last post


  //depending on which update flag was triggered output determined on ultrasonic calculations
  //distances are in cm 
if(LocalNewFlag & PITCH_FLAG)
  {     // below the code will limit user input differently at various distances from the wall
        // this should, in theory, limit the user input for a slow deceleration towards an obsticle and avoid it if 
        // the quad copter should drift towards the wall after limiting the users input to nill
        // this still allows the user to perform other movements unchecked.  That means the user could possibly fly at max             //  speed along a wall while maintaining a 1.5 to 2 ft range from the 
        // wall if the user gives some input towards the wall with the other direction.
      if((distancef && distanceb >= 246) || (distancef && distanceb <=0 )) // if outside the range of 1 cm to 245(10 ft)                    //ignore ultrasonic data
{
servoChannel1.writeMicroseconds(PITCH_IN_LOCAL);
}      
      else if(distancef < distanceb) // checks to see which distance is closer that involves this channel
{
       if(distancef < 45) // distances in cm danger close check accounts for drifting and inertia toward the wall
                {
servoChannel1.writeMicroseconds(1400); // back away from the wall
}
                else if(distancef < 60) // this one is the distance from the obsticle that we dont want user input to move that                           // direction
                {
servoChannel1.writeMicroseconds(1500); // no user input
}
                else if(distancef < 122) // closest distance that a user can still control the quad to move toward the wall
                {
   if(PITCH_IN_LOCAL > 1550) // limits the user to max of movement in this direction of very slow
     {
     servoChannel1.writeMicroseconds(1550); 
     }
     else
     {
     servoChannel1.writeMicroseconds(PITCH_IN_LOCAL); // allows the user to slowly close on the obsticle
     }
}
                else if(distancef < 183) // less limited and further away from the wall currently at the 6 ft range
                {
     if(PITCH_IN_LOCAL > 1600)
     {
     servoChannel1.writeMicroseconds(1600); // same as before limits users max to slow approach
     }
     else
     {
     servoChannel1.writeMicroseconds(PITCH_IN_LOCAL);
     }
}
                else if(distancef < 245) // even less limited further still from the wall currently at 8 ft range
                {
     if(PITCH_IN_LOCAL > 1650) 
     {
     servoChannel1.writeMicroseconds(1650); // same as before limits users max to slightly faster approach
     }
     else
     {
     servoChannel1.writeMicroseconds(PITCH_IN_LOCAL);
     }
}
        }
else
{
if(distanceb < 45)
                {
servoChannel1.writeMicroseconds(1600); // back away from the wall
}
       else if(distanceb < 60)
                {
servoChannel1.writeMicroseconds(1500); // no user input
}
                else if(distanceb < 122)
                {
   if(PITCH_IN_LOCAL > 1450)
   {
   servoChannel1.writeMicroseconds(1450); 
   }
   else
   {
   servoChannel1.writeMicroseconds(PITCH_IN_LOCAL);
   }
}
else if(distanceb < 183)
                {
   if(PITCH_IN_LOCAL > 1400)
   {
   servoChannel1.writeMicroseconds(1400); 
   }
   else
   {
   servoChannel1.writeMicroseconds(PITCH_IN_LOCAL);
   }
}
                else if(distanceb < 245)
                {
   if(PITCH_IN_LOCAL > 1350)
   {
   servoChannel1.writeMicroseconds(1350); 
   }
   else
   {
   servoChannel1.writeMicroseconds(PITCH_IN_LOCAL);
   }
}
}   
  }

  if(LocalNewFlag & ROLL_FLAG)
  {
if((distancer && distancel >= 246) || (distancer && distancel <=0 )) // if outside the range of 1 cm to 245(10 ft) ignore ultrasonic data
{
servoChannel2.writeMicroseconds(ROLL_IN_LOCAL);
}
      else if(distancer < distancel)
{
if(distancer < 45)
                {
servoChannel2.writeMicroseconds(1400); // back away from the wall
}
       else if(distancer < 60)
                {
servoChannel2.writeMicroseconds(1500); // no user input
}
                else if(distancer < 122)
                {
                    if(ROLL_IN_LOCAL > 1550)
   {
   servoChannel2.writeMicroseconds(1550); 
   }
   else
   {
   servoChannel2.writeMicroseconds(ROLL_IN_LOCAL);
   }
}
else if(distancer < 183)
                {
   if(ROLL_IN_LOCAL > 1600)
   {
   servoChannel2.writeMicroseconds(1600); 
   }
   else
   {
   servoChannel2.writeMicroseconds(ROLL_IN_LOCAL);
   }
}
                else if(distancer < 245)
                {
   if(ROLL_IN_LOCAL > 1650)
   {
   servoChannel2.writeMicroseconds(1650); 
   }
   else
   {
   servoChannel2.writeMicroseconds(ROLL_IN_LOCAL);
   }
}
}
else
{
if(distancel < 45)
                {
servoChannel2.writeMicroseconds(1600); // back away from the wall
}
       else if(distancel < 60)
                {
servoChannel2.writeMicroseconds(1500); // no user input
}
                else if(distancel < 122)
                {
                    if(ROLL_IN_LOCAL > 1450)
   {
   servoChannel2.writeMicroseconds(1450); 
   }
   else
   {
   servoChannel2.writeMicroseconds(ROLL_IN_LOCAL);
   }
}
else if(distancel < 183)
                {
   if(ROLL_IN_LOCAL > 1400)
   {
   servoChannel2.writeMicroseconds(1400); 
   }
   else
   {
   servoChannel2.writeMicroseconds(ROLL_IN_LOCAL);
   }
}
                else if(distancel < 245)
                {
   if(ROLL_IN_LOCAL > 1350)
   {
   servoChannel2.writeMicroseconds(1350); 
   }
   else
   {
   servoChannel2.writeMicroseconds(ROLL_IN_LOCAL);
   }
}
        }
  }

LocalNewFlag = 0; // reset local flag to zero for next update
}

Last 2 functions

// calculate the PWM pulse lengths and flip flags
void calcPitch()
{
  static uint32_t ulStart;
  
  if(digitalRead(PITCH_CHANNEL_IN))
  {
    ulStart = micros();
  }
  else
  {
    PITCH_IN_SHARED = (uint32_t)(micros() - ulStart);
    SharedNewFlag |= PITCH_FLAG;
  }
}
void calcRoll()
{
  static uint32_t ulStart;
  
  if(digitalRead(ROLL_CHANNEL_IN))
  {
    ulStart = micros();
  }
  else
  {
    ROLL_IN_SHARED = (uint32_t)(micros() - ulStart);
    SharedNewFlag |= ROLL_FLAG;
  }
}