How does the Ardupilot servo control work?

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

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!

You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –


  • Developer


        The APM_RC library is the one that we use in ArduPilot/ArduCopter to read/write pwm values used by the servos.  It uses low level avr hardware calls (although i suspect the servo lib you're using does as well) which makes it fast.  When you update a pwm value that you want sent out to the servos, our code only needs to update one register in the avr.  The rest is all handled by the hardware.  So I guess your question is why is that servo lib so slow...I'm not sure but we've certainly seen other situations where the arduino libraries are not optimised for speed because, in the beginning at least, people tend to use them for simple things so they're optimised for ease of use and simplicity instead of speed.

         I don't think it's because of the relative speed of the nano vs the 2560.
 guess is that it's actually the pulse in that is slow in your code.  I'll bet that it's waiting for the next pulse...if you replace that with a single call to pulseIn and save it to a variable and then use that value to output to both servos i'll bet it speeds up.

This reply was deleted.