Decoding 6 servo channel inputs with an Arduino UNO

Standard radio control receivers drive their connected servos by outputting a train of pulses.  The pulses usually repeat at a 50 Hz rate but the pulse width is the important thing – the standard is a 1.5 ms pulse width for a servo at its center position varying from about 1.0 ms up to 2.0 ms for servo’s nominal travel range. The pulses are positive going, usually about 5V in amplitude.

Searching on the web finds lots of examples of code to read the pulse widths for several servo channels, but for my application the Arduino is looking after several sensors besides the servo signals and also running software serial ports.  The examples I found all gave too much jitter and inaccuracy.

I wired the six channels to the Arduino digital pins 2 to 7.  Pins 0 and 1 are used by the UART (serial communication to PC or other device) so it’s best to avoid those.  Conveniently this means that all six channels are contained on a single Arduino input port, (Port D) so this makes the code to capture the pulse widths very clean.

We enable an interrupt that occurs when any of the 6 pins change state. To do this we set one bit for each of the 6 channels in the PCMSK register.  Then to allow changing input states on Port D to generate interrupts there is just one bit in the PCICR register to set:

PCMSK2 |= 0xFC;
PCICR |= 0x04;

The interrupt handler, declared as ISR(PCINT2_vect) checks each of the six pins. If a pin has changed state since the previous interrupt, we do one of two things:  If the pin has gone high we just remember the current time; if the pin has gone low we store the pulse width by subtracting the remembered time from the current one.  The Arduino has a function that returns microseconds (with a resolution of 4 microseconds on the UNO) convenient for this task.  The microsecond timer wraps around back to zero roughly every 70 minutes but this doesn’t cause any problems – by doing the subtractions using unsigned long integers the pulse widths are still correct even when a ‘wrap around’ occurs.  Here’s the complete program including interrupt handler and simple test output that just prints the current pulse widths to the serial port (PC).

volatile uint8_t prev; // remembers state of input bits from previous interrupt
volatile uint32_t risingEdge[6]; // time of last rising edge for each channel
volatile uint32_t uSec[6]; // the latest measured pulse width for each channel

ISR(PCINT2_vect) { // one or more of pins 2~7 have changed state
  uint32_t now = micros();
  uint8_t curr = PIND; // current state of the 6 input bits
  uint8_t changed = curr ^ prev;
  int channel = 0;
  for (uint8_t mask = 0x04; mask; mask <<= 1) {
    if (changed & mask) { // this pin has changed state
      if (curr & mask) { // +ve edge so remember time
        risingEdge[channel] = now;
      }
      else { // -ve edge so store pulse width
        uSec[channel] = now - risingEdge[channel];
      }
    }
    channel++;
  }
  prev = curr;
}

void setup() {
  Serial.begin(9600);

  for (int pin = 2; pin <= 7; pin++) { // enable pins 2 to 7 as our 6 input bits
    pinMode(pin, INPUT);
  }

  PCMSK2 |= 0xFC; // set the mask to allow those 6 pins to generate interrupts
  PCICR |= 0x04;  // enable interupt for port D
}

void loop() {
  Serial.flush();
  for (int channel = 0; channel < 6; channel++) {
    Serial.print(uSec[channel]);
    Serial.print("\t");
  }
  Serial.println();
}