Hello everyone!

Currently I'm working on a project that will attempt to use additional control surfaces to induce stresses in the wings of an aircraft that some of my colleagues are developing.  At the moment I'm using an Arduino Nano as a prototype - I want it to have six inputs (from the rx) and eight outputs (four extra control surfaces).  I would use the Ardupilot for this project, but we need 8 inputs and 14 outputs in the final device, so I decided to use an Arduino Mega 2560 board (it's in the mail right now, so I'm using the Nano until then).

Anyway, I'm trying to understand how the Ardupilot servo outputs are written, because I'm not having much luck with my code on the Nano.  I've made use of the Arduino servo library, as well as the function pulseIn() to save having to develop those functions from scratch, but as soon as I add more than one servo to the code, it seems that the processor is unable to keep up with outputting two PWM signals at once.  For example, my code currently looks like this:

// Required header files
#include <Servo.h>

// Input pins
#define inPWM1 0

// Output pins
#define outServoPin1 4
#define outServoPin2 5

// Other Magic Numbers
#define SERVO_MIDPOINT 1500

// Create servo control objects
Servo outServo1;
Servo outServo2;

void setup()
{
    // Configure inputs
    pinMode(inPWM1, INPUT);

    // Configure outputs
    outServo1.attach(outServoPin1);
    outServo2.attach(outServoPin2);
}

void loop()
{
    // Direct pass through to outputs
    outServo1.writeMicroseconds(pulseIn(inPWM1, HIGH));
    outServo2.writeMicroseconds(pulseIn(inPWM1, HIGH));
}

When I cause the servos to move moderately quickly, they freeze up or lag.  When I remove the line of code for outServo2, Servo1 then responds immediately.  If I add code for even more servos (say, eight), then they barely respond at all.

I've glanced over the cpp files for both the servo library and the relevant portions of code in the Ardupilot libraries - they appear to make similar use of interrupts and timers to read and write the pwm signals, so can somebody tell me what magic there is in the Ardupilot that makes it capable of smoothly controlling all those servos at once?

Or is it that the clock speed of the Ardupilot is just a lot faster than the 16Mhz of the Arduinos?

Thanks for reading and answering!