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();
}

 


Posted

in

,

by

Tags:

Comments

  1. Nelson Tye avatar
    Nelson Tye

    Hello,
    First off, thank you for sharing this code, it works great.
    I’m currently building a small UAV for the University of California, using an Arduino Pro Mini 5V because the goal is a small, low-cost UAV.
    I need 5 channels from the receiver and then I will be sending 4 of those readings to the motor and the servos. However, I’m already using pins 2 & 3 for GPS serial. Is there a simple way to include pins from another port in your code? I can use any pins above pin 3 and A1. I also have code working with the PinChangeInt library, but your code is much more succinct and I prefer to use it if I can.
    Thanks again,

    1. ceptimus avatar
      ceptimus

      Sorry for the delay in approving/replying – somehow the real comments went missing amongst the spam and my email notification about comments wasn’t working.

      I use pins A0 and A1 to communicate with my GPS – not using software serial library – just some custom code that works off the pinchange interrupt.

      I’ve modified my code now to use cppm signal for input – this is easy when using FrSKY radio gear – the small D4R-II receiver provides 8 radio channels into the Arduino this way and it only uses one of the Arduino pins for input. The FrSKY receiver is physically small but still has excellent range; a bonus is that it also allows RS232 telemetry to be sent from the Arduino back to the transmitter module for display/logging.

      1. Lenny avatar
        Lenny

        Ceptimus…
        Can you send me a pdf of up~to~date arduino codes so that i can follow your sketch and learn at the same time.
        This is fascinating for a learner like me! I trust that you code works, and I’m extremely interested to know why it does. I have arduino references but I’d love you to break down each element to see effectiveness of each detail.

  2. Simon avatar

    Hello,

    Thank you very much for sharing this code. It works great for me. I have recently started playing about with RC quadcopters and this code will help me very much in adding extra features to my quadcopter, such as controlling lights on it amongst other things I have yet to think of.

    Again thank you. 🙂

    Simon.

  3. Moni avatar

    It works great for me. I have recently started playing about with RC quadcopters and this code will help me very much in adding extra features to my quadcopter, such as controlling lights on it amongst other things I have yet to think of.

Leave a Reply

Your email address will not be published. Required fields are marked *