Sunday, November 4, 2012

How to read RC Channels - The RCArduinoFastLib

Background - The problem we are solving.

Your Arduino can only do one thing at a time, when one interrupt occurs no others can run until the current one finishes. This can cause problems in RC Projects which use interrupts for three key functions -


1) The Servo Library uses an internal interrupt to generate the servo signals
2) The Interrupts we use to read incoming RC Signals
3) The Arduino interrupt that drives the timing functions millis() and micros()

When two of our interrupts are triggered at the same time, one will be held waiting until the first one finishes. This introduces errors into our input readings and our output servo pulses.

In the example below an interrupt occurs which blocks the servo library from being able to generate the ideal pulses for our Servos and ESCs, instead we end up with an error - the length of the error is directly determined by the length of the interrupt function.  

Timing clash between an interrupt and the servo library interrupt

To reduce glitches, ticks and measurement errors in our RC Projects we have to reduce the time we spend in the input and output interrupts.

RCArduinoFastLib

This post provides an improved approach to reading multiple RC Channels and introduces a new servo library which can be used for smoother, faster RC Projects.

The new library has the following features -

1) Upto 18 Servos available on an Arduino UNO - 50% more than the standard Servo library.

2) Does not reset Timer1 allowing for fast and precise timing in RC Projects using a minimal input interrupt routine

3) Support for higher refresh rates of anywhere from 50 to 500Hz depending on the number of servos

4) Uses a more direct method than digitalWrite for faster ISR Execution

5) Reduces servo glitch frequency by 75% and glitch size by a factor of 2 when used with the RCArduino channel reading code.

6) Provides two banks of upto 10 servos with independent refresh rates for each bank, allowing servos and ESCs to be refreshed at different rates in the same project.



How is the new library able to reduce glitches ?

As described in the introduction typical RC Projects include at least three sets of interrupts -

1) The internal interrupt used by the Arduino Servo library
2) To read incoming RC Signals
3) The internal interrupts used by the Arduino millis and micros functions

Interrupt clashes effect the measurement of incoming RC Signals and the generation of the output signal. The worst case is an input signal being used to generate an output where both input and output have been delayed - an error on an error.

The following plot compares an original sketch published on RCArduino with an optimised version using RCArduinoFastLib. The optimised version (red) reduces interrupt clashes by about 75% and reduces glitches result from interrupt clashes to half their original size. The result is a smoother, more stable Project.



The blue line represents the sketch previously published here on RCArduino -
http://rcarduino.blogspot.com/2012/04/how-to-read-multiple-rc-channels-draft.html

The red line represents the current version using RCArduinoFastLib and outlined in this post.

An Optimised example sketch

Using the RCArduinoFastLib combined with the following optimisations will provide you with smoother glitch free RC Projects.


1) PinChangeInt Version 2.01 - The latest version of the PinChangeInt library includes an optimization which saves 2us over previous versions of the library. Download and install PinChangeInt version 2.01 or later from the library home page here - http://code.google.com/p/arduino-pinchangeint/

Original Interrupt Service Routing For Reading An RC Channel Pulse

void calcThrottle()
{
  if(digitalRead(THROTTLE_IN_PIN))
  {
    ulThrottleStart = micros();
  }
  else
  {
    unThrottleInShared = (uint16_t)(micros() - ulThrottleStart);
    bUpdateFlagsShared |= THROTTLE_FLAG;
  }
}

2) PCintPort::pinState - The Arduino digitalRead function takes around 1.4 us to complete. The pin change int library gives us a member variable PCintPort::pinState which we use to replace digitalRead inside our ISR and save over 1us of processing time.

3) Timer1 - Steps 1) and 2) each gave us a saving of over 1us, and so will step 3. By accessing timer1 directly to measure the incoming pulse width, we can save a few more micros. In order to do this we need to use the RCArduinoFastServo library in place of the standard servo library, swapping the servo libraries also gets us a speed boost in the servo interrupts.

4) By using timer1 directly we can use a two byte value to store our intermediate times instead of a 4 byte long. This halves the number of read,update,store operations in the ISR leading to another speed boost. It also provides a small boost to the loop function.

Updated ISR


void calcThrottlePulse()
{
  if(PCintPort::pinState)
  {
    unThrottleInStart = TCNT1;
  }
  else
  {
    unThrottleInShared = (TCNT1 - unThrottleInStart)>>1;
    bUpdateFlagsShared |= THROTTLE_FLAG;
  }
}



Update 04/11/2012- In a forum topic I mentioned to Arduino forum users robtillaart and greygnome that the pin change int library could be improved if a certain part of the code was made optional. The guys took this onboard and as a result the ISR is much now faster while retaining the original functionality. Expect pinchangeint 2.02 to be released shortly.

Forum Topic
http://arduino.cc/forum/index.php/topic,87195.15.html

Resulting improvement in RC signal quality - 

The green plot shows the results of repeating the previous test with the modified pinchangeint library combined with RCArduinoFastLib. The graph shows the maximum glitch within a ten second period, the green line hovers between 1 and 2% this means that in many cases the maximum error encountered in a ten second period is only 1%, very few errors over 2% occur. Compare this to the original blue line showing an average error of 3.5% with occasional errors of 4 and 5%.


The RCArduinoFastLib

Over the next few weeks, I will provide some examples of the different ways in which the RCArduinoFastLib can be used. For now, here is a test sketch for reading and outputting three channels.

The sharp eyed will also notice that RCArduinoFastLib includes a PPM Reading class, next week we will look at using this to access the PPM stream inside standard RC receivers for some ultra smooth RC Projects.

Stay tuned ...
    Duane B

The test sketch -

#include <RCArduinoFastLib.h>

 // MultiChannels
//
// rcarduino.blogspot.com
//
// A simple approach for reading three RC Channels using pin change interrupts
//
// See related posts -
// http://rcarduino.blogspot.co.uk/2012/01/how-to-read-rc-receiver-with.html
// http://rcarduino.blogspot.co.uk/2012/03/need-more-interrupts-to-read-more.html
// http://rcarduino.blogspot.co.uk/2012/01/can-i-control-more-than-x-servos-with.html
//
// rcarduino.blogspot.com
//

// include the pinchangeint library - see the links in the related topics section above for details
#include <PinChangeInt.h>

// Assign your channel in pins
#define THROTTLE_IN_PIN 5
#define STEERING_IN_PIN 6
#define AUX_IN_PIN 7

// Assign your channel out pins
#define THROTTLE_OUT_PIN 8
#define STEERING_OUT_PIN 9
#define AUX_OUT_PIN 10

// Assign servo indexes
#define SERVO_THROTTLE 0
#define SERVO_STEERING 1
#define SERVO_AUX 2
#define SERVO_FRAME_SPACE 3

// These bit flags are set in bUpdateFlagsShared to indicate which
// channels have new signals
#define THROTTLE_FLAG 1
#define STEERING_FLAG 2
#define AUX_FLAG 4

// holds the update flags defined above
volatile uint8_t bUpdateFlagsShared;

// shared variables are updated by the ISR and read by loop.
// In loop we immediatley take local copies so that the ISR can keep ownership of the
// shared ones. To access these in loop
// we first turn interrupts off with noInterrupts
// we take a copy to use in loop and the turn interrupts back on
// as quickly as possible, this ensures that we are always able to receive new signals
volatile uint16_t unThrottleInShared;
volatile uint16_t unSteeringInShared;
volatile uint16_t unAuxInShared;

// These are used to record the rising edge of a pulse in the calcInput functions
// They do not need to be volatile as they are only used in the ISR. If we wanted
// to refer to these in loop and the ISR then they would need to be declared volatile
uint16_t unThrottleInStart;
uint16_t unSteeringInStart;
uint16_t unAuxInStart;

uint16_t unLastAuxIn = 0;
uint32_t ulVariance = 0;
uint32_t ulGetNextSampleMillis = 0;
uint16_t unMaxDifference = 0;

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

  Serial.println("multiChannels");

  // attach servo objects, these will generate the correct
  // pulses for driving Electronic speed controllers, servos or other devices
  // designed to interface directly with RC Receivers
  CRCArduinoFastServos::attach(SERVO_THROTTLE,THROTTLE_OUT_PIN);
  CRCArduinoFastServos::attach(SERVO_STEERING,STEERING_OUT_PIN);
  CRCArduinoFastServos::attach(SERVO_AUX,AUX_OUT_PIN);
 
  // lets set a standard rate of 50 Hz by setting a frame space of 10 * 2000 = 3 Servos + 7 times 2000
  CRCArduinoFastServos::setFrameSpaceA(SERVO_FRAME_SPACE,7*2000);

  CRCArduinoFastServos::begin();
 
  // using the PinChangeInt library, attach the interrupts
  // used to read the channels
  PCintPort::attachInterrupt(THROTTLE_IN_PIN, calcThrottle,CHANGE);
  PCintPort::attachInterrupt(STEERING_IN_PIN, calcSteering,CHANGE);
  PCintPort::attachInterrupt(AUX_IN_PIN, calcAux,CHANGE);
}

void loop()
{
  // create local variables to hold a local copies of the channel inputs
  // these are declared static so that thier values will be retained
  // between calls to loop.
  static uint16_t unThrottleIn;
  static uint16_t unSteeringIn;
  static uint16_t unAuxIn;
  // local copy of update flags
  static uint8_t bUpdateFlags;

  // check shared update flags to see if any channels have a new signal
  if(bUpdateFlagsShared)
  {
    noInterrupts(); // turn interrupts off quickly while we take local copies of the shared variables

    // take a local copy of which channels were updated in case we need to use this in the rest of loop
    bUpdateFlags = bUpdateFlagsShared;
  
    // in the current code, the shared values are always populated
    // so we could copy them without testing the flags
    // however in the future this could change, so lets
    // only copy when the flags tell us we can.
  
    if(bUpdateFlags & THROTTLE_FLAG)
    {
      unThrottleIn = unThrottleInShared;
    }
  
    if(bUpdateFlags & STEERING_FLAG)
    {
      unSteeringIn = unSteeringInShared;
    }
  
    if(bUpdateFlags & AUX_FLAG)
    {
      unAuxIn = unAuxInShared;
    }
   
    // clear shared copy of updated flags as we have already taken the updates
    // we still have a local copy if we need to use it in bUpdateFlags
    bUpdateFlagsShared = 0;
  
    interrupts(); // we have local copies of the inputs, so now we can turn interrupts back on
    // as soon as interrupts are back on, we can no longer use the shared copies, the interrupt
    // service routines own these and could update them at any time. During the update, the
    // shared copies may contain junk. Luckily we have our local copies to work with :-)
  }

  // do any processing from here onwards
  // only use the local values unAuxIn, unThrottleIn and unSteeringIn, the shared
  // variables unAuxInShared, unThrottleInShared, unSteeringInShared are always owned by
  // the interrupt routines and should not be used in loop

  // the following code provides simple pass through
  // this is a good initial test, the Arduino will pass through
  // receiver input as if the Arduino is not there.
  // This should be used to confirm the circuit and power
  // before attempting any custom processing in a project.

  // we are checking to see if the channel value has changed, this is indicated
  // by the flags. For the simple pass through we don't really need this check,
  // but for a more complex project where a new signal requires significant processing
  // this allows us to only calculate new values when we have new inputs, rather than
  // on every cycle.
  if(bUpdateFlags & THROTTLE_FLAG)
  {
    CRCArduinoFastServos::writeMicroseconds(SERVO_THROTTLE,unThrottleIn);
  }

  if(bUpdateFlags & STEERING_FLAG)
  {
    CRCArduinoFastServos::writeMicroseconds(SERVO_STEERING,unSteeringIn);
  }

  if(bUpdateFlags & AUX_FLAG)
  {
   CRCArduinoFastServos::writeMicroseconds(SERVO_AUX,unAuxIn);
   }

  bUpdateFlags = 0;
}


// simple interrupt service routine
void calcThrottle()
{
  if(PCintPort::pinState)
  {
    unThrottleInStart = TCNT1;
  }
  else
  {
    unThrottleInShared = (TCNT1 - unThrottleInStart)>>1;
    bUpdateFlagsShared |= THROTTLE_FLAG;
  }
}

void calcSteering()
{
  if(PCintPort::pinState)
  {
    unSteeringInStart = TCNT1;
  }
  else
  {
    unSteeringInShared = (TCNT1 - unSteeringInStart)>>1;

    bUpdateFlagsShared |= STEERING_FLAG;
  }
}

void calcAux()
{
  if(PCintPort::pinState)
  {
    unAuxInStart = TCNT1;
  }
  else
  {
    unAuxInShared = (TCNT1 - unAuxInStart)>>1;
    bUpdateFlagsShared |= AUX_FLAG;  }
}


The RCArduinoFastLib.h file

/*****************************************************************************************************************************
// RCArduinoFastLib by DuaneB is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License.
//
// http://rcarduino.blogspot.com
//
*****************************************************************************************************************************/

#include "Arduino.h"

// COMMENT OR UNCOMMENT THIS LINE TO ENABLE THE SECOND BANK OF SERVOS
//#define MORE_SERVOS_PLEASE 1

// the first bank of servos uses OC1A - this will disable PWM on digital pin 9 - a small price for 10 fast and smooth servos
// the second bank of servos uses OC1B - this will disable PWM on digital pin 10 - a small price for 10 more fast and smooth servos

// The library blindly pulses all ten servos one and after another
// If you change the RC_CHANNEL_OUT_COUNT to 4 servos, the library will pulse them more frequently than
// it can ten -
// 10 servos at 1500us = 15ms = 66Hz
// 4 Servos at 1500us = 6ms = 166Hz
// if you wanted to go even higher, run two servos on each timer
// 2 Servos at 1500us = 3ms = 333Hz
//
// You might not want a high refresh rate though, so the setFrameSpace function is provided for you to
// add a pause before the library begins its next run through the servos
// for 50 hz, the pause should be to (20,000 - (RC_CHANNEL_OUT_COUNT * 2000))

// Change to set the number of servos/ESCs
#define RC_CHANNEL_OUT_COUNT 4

#if defined (MORE_SERVOS_PLEASE)
#define RCARDUINO_MAX_SERVOS (RC_CHANNEL_OUT_COUNT*2)
#else
#define RCARDUINO_MAX_SERVOS (RC_CHANNEL_OUT_COUNT)
#endif

// Minimum and Maximum servo pulse widths, you could change these,
// Check the servo library and use that range if you prefer
#define RCARDUINO_SERIAL_SERVO_MIN 1000
#define RCARDUINO_SERIAL_SERVO_MAX 2000
#define RCARDUINO_SERIAL_SERVO_DEFAULT 1500

#define RC_CHANNELS_NOPORT 0
#define RC_CHANNELS_PORTB 1
#define RC_CHANNELS_PORTC 2
#define RC_CHANNELS_PORTD 3
#define RC_CHANNELS_NOPIN 255

//////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// CRCArduinoFastServos
//
// A class for generating signals in combination with a 4017 Counter
//
// Output upto 10 Servo channels using just digital pins 9 and 12
// 9 generates the clock signal and must be connected to the clock pin of the 4017
// 12 generates the reset pulse and must be connected to the master reset pin of the 4017
//
// The class uses Timer1, as this prevents use with the servo library
// The class uses pins 9 and 12
// The class does not adjust the servo frame to account for variations in pulse width,
// on the basis that many RC transmitters and receivers designed specifically to operate with servos
// output signals between 50 and 100hz, this is the same range as the library
//
// Use of an additional pin would provide for error detection, however using pin 12 to pulse master reset
// at the end of every frame means that the system is essentially self correcting
//
// Note
// This is a simplified derivative of the Arduino Servo Library created by Michael Margolis
// The simplification has been possible by moving some of the flexibility provided by the Servo library
// from software to hardware.
//
////////////////////////////////////////////////////////////////////////////////////////////////////////////

 
class CRCArduinoFastServos
{
public:
    static void setup();

    // configures timer1
    static void begin();

    // called by the timer interrupt service routine, see the cpp file for details.
    static void OCR1A_ISR();
   
#if defined(MORE_SERVOS_PLEASE)
    static void OCR1B_ISR();
#endif

    // called to set the pulse width for a specific channel, pulse widths are in microseconds - degrees are for wimps !
    static void attach(uint8_t nChannel,uint8_t nPin);
    static void writeMicroseconds(uint8_t nChannel,uint16_t nMicroseconds);
    static void setFrameSpaceA(uint8_t sChannel,uint16_t unMicroseconds);
    static void setFrameSpaceB(uint8_t sChannel,uint16_t unMicroseconds);
   
protected:
    class CPortPin
    {
        public:
            //uint8_t m_sPort;
            volatile unsigned char *m_pPort;
            uint8_t m_sPinMask;
            uint16_t m_unPulseWidth;
    };

    // this sets the value of the timer1 output compare register to a point in the future
    // based on the required pulse with for the current servo
    static void setOutputTimerForPulseDurationA() __attribute__((always_inline));
   
   
    static void setChannelPinLowA(uint8_t sChannel) __attribute__((always_inline));
    static void setCurrentChannelPinHighA();
       
        // Easy to optimise this, but lets keep it readable instead, its short enough.
    static volatile uint8_t*  getPortFromPin(uint8_t sPin) __attribute__((always_inline));
    static uint8_t getPortPinMaskFromPin(uint8_t sPin) __attribute__((always_inline));

    // Records the current output channel values in timer ticks
    // Manually set by calling writeChannel, the function adjusts from
    // user supplied micro seconds to timer ticks
    volatile static CPortPin m_ChannelOutA[RC_CHANNEL_OUT_COUNT];
    // current output channel, used by the timer ISR to track which channel is being generated
    static uint8_t m_sCurrentOutputChannelA;
   
#if defined(MORE_SERVOS_PLEASE)
    // Optional channel B for servo number 10 to 19
    volatile static CPortPin m_ChannelOutB[RC_CHANNEL_OUT_COUNT];
    static uint8_t m_sCurrentOutputChannelB;
    static void setOutputTimerForPulseDurationB();
   
    static void setChannelPinLowB(uint8_t sChannel) __attribute__((always_inline));
    static void setCurrentChannelPinHighB() __attribute__((always_inline));
#endif

    // two helper functions to convert between timer values and microseconds
    static uint16_t ticksToMicroseconds(uint16_t unTicks) __attribute__((always_inline));
    static uint16_t microsecondsToTicks(uint16_t unMicroseconds) __attribute__((always_inline));
};

// Change to set the number of channels in PPM Input stream
#define RC_CHANNEL_IN_COUNT 3
// two ticks per us, 3000 us * 2 ticks = 6000 minimum frame space
#define MINIMUM_FRAME_SPACE 6000
#define MAXIMUM_PULSE_SPACE 5000

class CRCArduinoPPMChannels
{
public:
 static void begin();
 static void INT0ISR();
 static uint16_t getChannel(uint8_t nChannel);
 static uint8_t getSynchErrorCounter();

protected:
 static void forceResynch();

 static volatile uint16_t m_unChannelSignalIn[RC_CHANNEL_IN_COUNT];
 static uint8_t m_sCurrentInputChannel;

 static uint16_t m_unChannelRiseTime;
 static volatile uint8_t m_sOutOfSynchErrorCounter;
};

The RCArduinoFastLib.cpp file


/*****************************************************************************************************************************
// RCArduinoFastLib by DuaneB is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License.
//
// http://rcarduino.blogspot.com
//
*****************************************************************************************************************************/

#include "arduino.h"
#include "RCArduinoFastLib.h"

/*----------------------------------------------------------------------------------------

This is essentially a derivative of the Arduino Servo Library created by Michael Margolis

As the technique is very similar to the Servo class, it can be useful to study in order
to understand the servo class.

What does the library do ? It uses a very inexpensive and common 4017 Counter IC
To generate pulses to independently drive up to 10 servos from two Arduino Pins

As previously mentioned, the library is based on the techniques used in the Arduino Servo
library created by Michael Margolis. This means that the library uses Timer1 and Timer1 output
compare register A.

OCR1A is linked to digital pin 9 and so we use digital pin 9 to generate the clock signal
for the 4017 counter.

Pin 12 is used as the reset pin.

*/

void CRCArduinoFastServos::setup()
{
 m_sCurrentOutputChannelA = 0;
 while(m_sCurrentOutputChannelA < RC_CHANNEL_OUT_COUNT)
 {
  m_ChannelOutA[m_sCurrentOutputChannelA].m_unPulseWidth = microsecondsToTicks(RCARDUINO_SERIAL_SERVO_MAX);

#if defined (MORE_SERVOS_PLEASE)
  m_ChannelOutB[m_sCurrentOutputChannelA].m_unPulseWidth = microsecondsToTicks(RCARDUINO_SERIAL_SERVO_MAX);
#endif

   m_sCurrentOutputChannelA++;
  }
}


// Timer1 Output Compare A interrupt service routine
// call out class member function OCR1A_ISR so that we can
// access out member variables
ISR(TIMER1_COMPA_vect)
{
    CRCArduinoFastServos::OCR1A_ISR();
}

void CRCArduinoFastServos::OCR1A_ISR()
{
    // If the channel number is >= 10, we need to reset the counter
    // and start again from zero.
    // to do this we pulse the reset pin of the counter
    // this sets output 0 of the counter high, effectivley
    // starting the first pulse of our first channel
  if(m_sCurrentOutputChannelA >= RC_CHANNEL_OUT_COUNT)
  {
    // reset our current servo/output channel to 0
    m_sCurrentOutputChannelA = 0;
   
    CRCArduinoFastServos::setChannelPinLowA(RC_CHANNEL_OUT_COUNT-1);
  }
  else
  {  
    CRCArduinoFastServos::setChannelPinLowA(m_sCurrentOutputChannelA-1);
  }
  
  CRCArduinoFastServos::setCurrentChannelPinHighA();
   
    // set the duration of the output pulse
    CRCArduinoFastServos::setOutputTimerForPulseDurationA();

    // done with this channel so move on.
    m_sCurrentOutputChannelA++;
}

void CRCArduinoFastServos::setChannelPinLowA(uint8_t sChannel)
{
    volatile CPortPin *pPortPin = m_ChannelOutA + sChannel;

    if(pPortPin->m_sPinMask)
     *pPortPin->m_pPort ^= pPortPin->m_sPinMask;
}

void CRCArduinoFastServos::setCurrentChannelPinHighA()
{
    volatile CPortPin *pPortPin = m_ChannelOutA + m_sCurrentOutputChannelA;

    if(pPortPin->m_sPinMask)
     *pPortPin->m_pPort |= pPortPin->m_sPinMask;
}

// After we set an output pin high, we need to set the timer to comeback for the end of the pulse
void CRCArduinoFastServos::setOutputTimerForPulseDurationA()
{
  OCR1A = TCNT1 + m_ChannelOutA[m_sCurrentOutputChannelA].m_unPulseWidth;
}

#if defined(MORE_SERVOS_PLEASE)
// Timer1 Output Compare B interrupt service routine
// call out class member function OCR1B_ISR so that we can
// access out member variables
ISR(TIMER1_COMPB_vect)
{
    CRCArduinoFastServos::OCR1B_ISR();
}

void CRCArduinoFastServos::OCR1B_ISR()
{
  if(m_sCurrentOutputChannelB >= RC_CHANNEL_OUT_COUNT)
  {
    // reset our current servo/output channel to 0
    m_sCurrentOutputChannelB = 0;
    CRCArduinoFastServos::setChannelPinLowB(RC_CHANNEL_OUT_COUNT-1);
  }
  else
  {  
    CRCArduinoFastServos::setChannelPinLowB(m_sCurrentOutputChannelB-1);
  }
  
  CRCArduinoFastServos::setCurrentChannelPinHighB();
   
    // set the duration of the output pulse
    CRCArduinoFastServos::setOutputTimerForPulseDurationB();

    // done with this channel so move on.
    m_sCurrentOutputChannelB++;
}

void CRCArduinoFastServos::setChannelPinLowB(uint8_t sChannel)
{
    volatile CPortPin *pPortPin = m_ChannelOutB + sChannel;

    if(pPortPin->m_sPinMask)
     *pPortPin->m_pPort ^= pPortPin->m_sPinMask;
}

void CRCArduinoFastServos::setCurrentChannelPinHighB()
{
    volatile CPortPin *pPortPin = m_ChannelOutB + m_sCurrentOutputChannelB;
   
    if(pPortPin->m_sPinMask)
     *pPortPin->m_pPort |= pPortPin->m_sPinMask;
}



// After we set an output pin high, we need to set the timer to comeback for the end of the pulse
void CRCArduinoFastServos::setOutputTimerForPulseDurationB()
{
  OCR1B = TCNT1 + m_ChannelOutB[m_sCurrentOutputChannelB].m_unPulseWidth;
}
#endif

// updates a channel to a new value, the class will continue to pulse the channel
// with this value for the lifetime of the sketch or until writeChannel is called
// again to update the value
void CRCArduinoFastServos::writeMicroseconds(uint8_t nChannel,uint16_t unMicroseconds)
{
    // dont allow a write to a non existent channel
    if(nChannel > RCARDUINO_MAX_SERVOS)
        return;

  // constraint the value just in case
  unMicroseconds = constrain(unMicroseconds,RCARDUINO_SERIAL_SERVO_MIN,RCARDUINO_SERIAL_SERVO_MAX);

#if defined(MORE_SERVOS_PLEASE)
  if(nChannel >= RC_CHANNEL_OUT_COUNT)
  {
    unMicroseconds = microsecondsToTicks(unMicroseconds);
    unsigned char sChannel = nChannel-RC_CHANNEL_OUT_COUNT;
    // disable interrupts while we update the multi byte value output value
    uint8_t sreg = SREG;
    cli();
     
    m_ChannelOutB[sChannel].m_unPulseWidth = unMicroseconds;

    // enable interrupts
    SREG = sreg;
    return;
  }
#endif
 
  unMicroseconds = microsecondsToTicks(unMicroseconds);
 
  // disable interrupts while we update the multi byte value output value
  uint8_t sreg = SREG;
  cli();
 
  m_ChannelOutA[nChannel].m_unPulseWidth = unMicroseconds;

  // enable interrupts
  SREG = sreg;
}

uint16_t CRCArduinoFastServos::ticksToMicroseconds(uint16_t unTicks)
{
    return unTicks / 2;
}

uint16_t CRCArduinoFastServos::microsecondsToTicks(uint16_t unMicroseconds)
{
 return unMicroseconds * 2;
}

void CRCArduinoFastServos::attach(uint8_t sChannel,uint8_t sPin)
{
    if(sChannel >= RCARDUINO_MAX_SERVOS)
        return;

  #if defined(MORE_SERVOS_PLEASE)
  if(sChannel >= RC_CHANNEL_OUT_COUNT)
  {
    // disable interrupts while we update the multi byte value output value
    uint8_t sreg = SREG;
    cli();
     
    m_ChannelOutB[sChannel-RC_CHANNEL_OUT_COUNT].m_unPulseWidth = microsecondsToTicks(RCARDUINO_SERIAL_SERVO_DEFAULT);
    m_ChannelOutB[sChannel-RC_CHANNEL_OUT_COUNT].m_pPort = getPortFromPin(sPin);
    m_ChannelOutB[sChannel-RC_CHANNEL_OUT_COUNT].m_sPinMask = getPortPinMaskFromPin(sPin);
    // enable interrupts
    SREG = sreg;
    pinMode(sPin,OUTPUT);
    return;
  }
  #endif
 
  // disable interrupts while we update the multi byte value output value
  uint8_t sreg = SREG;
  cli();
 
  m_ChannelOutA[sChannel].m_unPulseWidth = microsecondsToTicks(RCARDUINO_SERIAL_SERVO_DEFAULT);
  m_ChannelOutA[sChannel].m_pPort = getPortFromPin(sPin);
  m_ChannelOutA[sChannel].m_sPinMask = getPortPinMaskFromPin(sPin);

  // enable interrupts
  SREG = sreg;
 
  pinMode(sPin,OUTPUT);
}

// this allows us to run different refresh frequencies on channel A and B
// for example servos at a 70Hz rate on A and ESCs at 250Hz on B
void CRCArduinoFastServos::setFrameSpaceA(uint8_t sChannel,uint16_t unMicroseconds)
{
  // disable interrupts while we update the multi byte value output value
  uint8_t sreg = SREG;
  cli();
 
  m_ChannelOutA[sChannel].m_unPulseWidth = microsecondsToTicks(unMicroseconds);
  m_ChannelOutA[sChannel].m_pPort = 0;
  m_ChannelOutA[sChannel].m_sPinMask = 0;

  // enable interrupts
  SREG = sreg;
}

#if defined (MORE_SERVOS_PLEASE)
void CRCArduinoFastServos::setFrameSpaceB(uint8_t sChannel,uint16_t unMicroseconds)
{
  // disable interrupts while we update the multi byte value output value
  uint8_t sreg = SREG;
  cli();
 
  m_ChannelOutB[sChannel].m_unPulseWidth = microsecondsToTicks(unMicroseconds);
  m_ChannelOutB[sChannel].m_pPort = 0;
  m_ChannelOutB[sChannel].m_sPinMask = 0;

  // enable interrupts
  SREG = sreg;
}
#endif
// Easy to optimise this, but lets keep it readable instead, its short enough.
volatile uint8_t* CRCArduinoFastServos::getPortFromPin(uint8_t sPin)
{
    volatile uint8_t* pPort = RC_CHANNELS_NOPORT;
   
    if(sPin <= 7)
    {
        pPort = &PORTD;
    }
    else if(sPin <= 13)
    {
        pPort = &PORTB;
    }
    else if(sPin <= A5) // analog input pin 5
    {
        pPort = &PORTC;
    }
   
    return pPort;
}

// Easy to optimise this, but lets keep it readable instead, its short enough.
uint8_t CRCArduinoFastServos::getPortPinMaskFromPin(uint8_t sPin)
{
    uint8_t sPortPinMask = RC_CHANNELS_NOPIN;
   
    if(sPin <= A5)
    {
        if(sPin <= 7)
        {
            sPortPinMask = (1 << sPin);
        }
        else if(sPin <= 13)
        {
            sPin -= 8;
            sPortPinMask = (1 << sPin);
        }
        else if(sPin <= A5)
        {
            sPin -= A0;
            sPortPinMask = (1 << sPin);
        }
    }
   
    return sPortPinMask;
}  

void CRCArduinoFastServos::begin()
{
    TCNT1 = 0;              // clear the timer count 

    // Initilialise Timer1
    TCCR1A = 0;             // normal counting mode
    TCCR1B = 2;     // set prescaler of 64 = 1 tick = 4us

    // ENABLE TIMER1 OCR1A INTERRUPT to enabled the first bank (A) of ten servos
    TIFR1 |= _BV(OCF1A);     // clear any pending interrupts;
    TIMSK1 |=  _BV(OCIE1A) ; // enable the output compare interrupt

#if defined(MORE_SERVOS_PLEASE)

    // ENABLE TIMER1 OCR1B INTERRUPT to enable the second bank (B) of 10 servos
    TIFR1 |= _BV(OCF1B);     // clear any pending interrupts;
    TIMSK1 |=  _BV(OCIE1B) ; // enable the output compare interrupt

#endif

    OCR1A = TCNT1 + 4000; // Start in two milli seconds
   
    for(uint8_t sServo = 0;sServo<RC_CHANNEL_OUT_COUNT;sServo++)
    {
        Serial.println(m_ChannelOutA[sServo].m_unPulseWidth);

#if defined(MORE_SERVOS_PLEASE)
        Serial.println(m_ChannelOutB[sServo].m_unPulseWidth);
#endif
        }
}

volatile CRCArduinoFastServos::CPortPin CRCArduinoFastServos::m_ChannelOutA[RC_CHANNEL_OUT_COUNT];
uint8_t CRCArduinoFastServos::m_sCurrentOutputChannelA;

#if defined(MORE_SERVOS_PLEASE)  
volatile CRCArduinoFastServos::CPortPin CRCArduinoFastServos::m_ChannelOutB[RC_CHANNEL_OUT_COUNT];
uint8_t CRCArduinoFastServos::m_sCurrentOutputChannelB;
#endif

volatile uint16_t CRCArduinoPPMChannels::m_unChannelSignalIn[RC_CHANNEL_IN_COUNT];
uint8_t CRCArduinoPPMChannels::m_sCurrentInputChannel = 0;

uint16_t CRCArduinoPPMChannels::m_unChannelRiseTime = 0;
uint8_t volatile CRCArduinoPPMChannels::m_sOutOfSynchErrorCounter = 0;

void CRCArduinoPPMChannels::begin()
{
 m_sOutOfSynchErrorCounter = 0;
 attachInterrupt(0,CRCArduinoPPMChannels::INT0ISR,RISING);
}

// we could save a few micros by writting this directly in the signal handler rather than using attach interrupt
void CRCArduinoPPMChannels::INT0ISR()
{
  // only ever called for rising edges, so no need to check the pin state
 
  // calculate the interval between this pulse and the last one we received which is recorded in m_unChannelRiseTime
  uint16_t ulInterval = TCNT1 - m_unChannelRiseTime;
 
  // if all of the channels have been received we should be expecting the frame space next, lets check it
  if(m_sCurrentInputChannel == RC_CHANNEL_IN_COUNT)
  {
    // we have received all the channels we wanted, this should be the frame space
    if(ulInterval < MINIMUM_FRAME_SPACE)
    {
     // it was not so we need to resynch
     forceResynch();
    }
    else
    {
      // it was the frame space, next interval will be channel 0
      m_sCurrentInputChannel = 0;
    }
  }
  else
  {
    // if we were expecting a channel, but found a space instead, we need to resynch
    if(ulInterval > MAXIMUM_PULSE_SPACE)
    {
      forceResynch();
    }
    else
    {
     // its a good signal, lets record it and move onto the next channel
     m_unChannelSignalIn[m_sCurrentInputChannel++] = ulInterval;
    }
  }
  // record the current time
  m_unChannelRiseTime = TCNT1; 
}

// if we force a resynch we set the channel
void CRCArduinoPPMChannels::forceResynch()
{
    m_sCurrentInputChannel = RC_CHANNEL_IN_COUNT;
   
    if(m_sOutOfSynchErrorCounter<255)
     m_sOutOfSynchErrorCounter++;
}

uint8_t CRCArduinoPPMChannels::getSynchErrorCounter()
{
  uint8_t sErrors = m_sOutOfSynchErrorCounter;
 
  m_sOutOfSynchErrorCounter = 0;
 
  return sErrors;
}

uint16_t CRCArduinoPPMChannels::getChannel(uint8_t sChannel)
{
 uint16_t ulPulse;
 unsigned char sreg = SREG;

 cli();

 ulPulse = m_unChannelSignalIn[sChannel];
 m_unChannelSignalIn[sChannel] = 0;
 
 SREG = sreg;

 return ulPulse>>1;
}


201 comments:

  1. Hello.
    Thanks for all the great information. I have licensing question. You state in the text of the file RCArduinoFastLib.cpp that it "is essentially a derivative of the Arduino Servo Library created by Michael Margolis ".
    The Arduino server library is released as LGPL.
    http://code.google.com/p/arduino/source/browse/trunk/libraries/Servo/Servo.cpp?r=1088 .

    You have released your library under a more restrictive license. I am , probably mistakenly , under the impression that under the LGPL any deriviates can not be licensed under a more restrictive license.

    Can you point me to the errors of my assumptions or relicense your code ?

    My project to create a module to allow a standard RC TX (turnigy 9x with er9x firmware), to control an IR helicopter (syma S107). I have no plans to commercialize this project but want the code that ultimately will be released to not be burdened by a non commercial clause.

    Thanks again for all your tutorials.

    Randy Perkins

    ReplyDelete
    Replies
    1. Rcarduino: How To Read Rc Channels - The Rcarduinofastlib >>>>> Download Now

      >>>>> Download Full

      Rcarduino: How To Read Rc Channels - The Rcarduinofastlib >>>>> Download LINK

      >>>>> Download Now

      Rcarduino: How To Read Rc Channels - The Rcarduinofastlib >>>>> Download Full

      >>>>> Download LINK rQ

      Delete

    2. Really Work Fast,******************

      Fast and reliable solution for Herpes Cure

      I was cured from Herpes with herbal med..

      Contact him for Relationship/marital problem,

      He will give you the best..

      Thanks to [[[robinsonbucler @ gmail com]]]

      Delete
  2. Hi, I wrote the code to be used, so carry on, as long as the code retains a link to rcarduino. I will adjust the license over the weekend.

    Duane B

    ReplyDelete
  3. thanks so much.
    I'll post a link when I am finished. It will be much easier now as I wont have to understand all the gritty details of the interrupts and write code. All I have to do is use the good libraries you wrote.
    Randy Perkins

    ReplyDelete
  4. trying to compile the test sketch. lowercase a in arduino.h

    rperkins@htpc:~/sketchbook/libraries/RCArduinoFastLib$ diff -u RCArduinoFastLib.cpp.orig RCArduinoFastLib.cpp
    --- RCArduinoFastLib.cpp.orig 2012-12-01 01:08:07.216789371 -0500
    +++ RCArduinoFastLib.cpp 2012-12-01 01:10:40.612787485 -0500
    @@ -5,7 +5,7 @@
    //
    *****************************************************************************************************************************/

    -#include "arduino.h"
    +#include "Arduino.h"
    #include "RCArduinoFastLib.h"

    ReplyDelete
  5. While working my way through the code was struggling with the timer speed. Pretty sure the comment about the prescaler is wrong. The code is fine, just the comment through me off. Thanks again for everything. Here's a quick patch to update the comment to what i believe is correct.

    --- RCArduinoFastLib.cpp.orig 2012-12-05 17:27:55.294157471 -0500
    +++ RCArduinoFastLib.cpp 2012-12-05 17:29:02.510156648 -0500
    @@ -330,7 +330,7 @@

    // Initilialise Timer1
    TCCR1A = 0; // normal counting mode
    - TCCR1B = 2; // set prescaler of 64 = 1 tick = 4us
    + TCCR1B = 2; // set prescaler of 8 = 1 tick = .5us

    // ENABLE TIMER1 OCR1A INTERRUPT to enabled the first bank (A) of ten servos
    TIFR1 |= _BV(OCF1A); // clear any pending interrupts;

    ReplyDelete
  6. Hi, Thanks, I will update the comment, I also need to update the comments around the ISR. There are two ways to use the ISR, either comment it out and use attachInterrupt or cut and copy it into your main sketch file commenting out the attachInterrupt in the library. There is a slight performance advantage to the second approach. I will have another look at the assembly and decide whether the performance is worth the cutting and pasting. Duane B

    ReplyDelete
  7. Hi Randy, I am not sure if you require PPM output as well as input for your project ? If so let me know and I will release a library I have been holding back which provides PPM output instead of individual servo output - it would not be difficult to provide both styles of output - PPM on one pin and individual servo signals on others.

    Duane B

    ReplyDelete
  8. Thanks Duane.
    My output is to control and infrared LED. The syma s107g is an IR helicopter. I have a rough draft of a class for the s107g that uses this GPL licensed library.
    http://www.open.com.au/mikem/arduino/IRrc/

    Also recently found this post which uses the ICP (input capture pin ) to decode ppmsum. It may be a closer match for what I need, since I dont need ppm out, then no dependency on the pinchangeint library. This ICP uses TIMER1 so it would interfere with the PPM output of your library.

    http://diydrones.com/profiles/blogs/705844:BlogPost:39393

    No matter how I proceed I gained so much knowledge with your tutorials and code. That's what it's all about for me. thanks Duane

    ReplyDelete
  9. Hello ace blog I'm a noob so love the well commented code learning loads just reading
    code. Tired to compile RCArduinoFastLib ppm example and i get the following error
    "core.a(WInterrupts.c.o): In function `__vector_1':
    C:\Documents and Settings\Stan\My Documents\arduino-1.0.1\hardware\arduino\cores\arduino/WInterrupts.c:273: multiple definition of `__vector_1'
    sketch_jan04a.cpp.o:C:\DOCUME~1\Stan\LOCALS~1\Temp\build3426807490179573658.tmp/sketch_jan04a.cpp:78: first defined here" Thanks
    for any help Stan

    ReplyDelete
  10. Hi Stan, A bit confusing that your comment relates to a different post -

    http://rcarduino.blogspot.com/2012/11/how-to-read-rc-receiver-ppm-stream.html

    Will answer there

    Duane B

    ReplyDelete
  11. Hi Duane,
    Double posting, but this is probably the right thread for this question. I'm looking to run one 760 us frame rate servo (refresh rate of 560 hz) Is that easily doable with the FastLib? I've downloaded it all, but honestly can't tell what I should be setting the library and frame rate settings to. Any help (on top of all the helpful info already here:) would be much appreciated.
    Cheers,
    Jon

    ReplyDelete
  12. Hi, From what i can see, your servo has a refresh rate of 560hz and a center pulsewidth of 760us this is roughly half the usual 1500us center pulsewidth and is knowN as 'narrow pulse width'. It wont take much to adapt the code to drive a small number of these servos - i dont have any to test, so ifyou want to do the testing part, contact me DuaneB through the Arduino forum and i will send you something to try.

    Duane B

    ReplyDelete
  13. Hallo Duane,

    Thanks for this really amazing blog. It really saved my a** after using the too slow pulsein() function. The code worked very fine with me but there is noise over time for about 20-40 microseconds on each channel. Is that normal? cause signal almost didn't change with the pulsein() but ofcourse too slow as well

    ReplyDelete
  14. Another question. This code worked on arduino uno only but didn't work on mega 2560. All readings printed are zeros. I couldn't figure out why.Is it made to work on arduino uno only?

    ReplyDelete
  15. The variation you are seeing is probably down to your RC Equipment, what are you using and in what environment ? See here - http://rcarduino.blogspot.com/2012/03/reading-from-rc-reveiver-do-you-need.html

    As for UNO's and Mega's, the Mega has more interrupts so you can read upto 6 channels using the hardware interrupts directly, how many channels do you need ?

    Duane B

    ReplyDelete
  16. Well, I'm making a quadcopter and using a turnigy 9x with firmware upgraded to er9x. I tested it in my room with a distance of 1 meter between transmitter and receiver. I read the link u sent and will try to test it in another envieronment as getting a new Tx would be too difficult for me. but why this didn't happen with the pulsein() function under the same conditions. Signal was very stable even with far distances.

    Yes I've seen that in previous topics of yours, I only use 4 interrupts at the moment. So, if I use attachinterrupt directly without using the pinchangeint library, will it give me the same performance?

    ReplyDelete
  17. If you use attachInterrupt directly on the Mega it will give you better performance. There is an overhead to using the pinchageint library which is only worth it if you need more interrupts than the hardware supplies i.e. if your using UNO or standalone 328's. I recommend you try using the built in interrupts as you have them. An alternative which is slightly more efficient again is to read the PPM Stream using this - http://rcarduino.blogspot.com/2012/11/how-to-read-rc-channels-rcarduinofastlib.html?showComment=1359966482974#c7522495716409361241

    Dont try it just yet, I plan to update it this evening to remove a clash with this library, it should give you smoother readings than this approach when I have updated it.

    Duane

    ReplyDelete
  18. Hi,
    I have updated the PPM Post to remove the clash. As for the signal variation, you will get a variation of around 20us due to clashes with other interrupts. These include the timer interrupts used by Arduino for millis and micros functions and also the timer1 interrupts used by the library to generate the servo pulses. If you switch to using the Mega's hardware interrupts this effect will be reduced as you remove the pinchangeint library overhead, also if you switch to the PPM Library you will have a similar improvement.

    If you do try the Mega's external interrupts remember that the pins are different and so you will need to update the pin tests in the ISRs. I might find time to put together an example this week.

    Duane B

    ReplyDelete
  19. Thanks a lot for ur efforts :) I'll test that and feed you back.

    ReplyDelete
  20. Check your power as well, someone else described a similar problem which they later found was power.

    http://rcarduino.blogspot.com/2012/04/servo-problems-with-arduino-part-1.html

    In case you haven't see it before.

    ReplyDelete
  21. I've never thought about this power thing. I think it's an interesting post to read when I have the time.
    With the code above, noise only decrease of about 20 microseconds (so the maximum noise becomes 20 microseconds) when I put a delay in each loop of about 3 ms which I think is a big waste of time.
    Does the pcinport::pinstate which is used instead of digitalwrite improve the performance if only you don't use attachinterrupt directly or it works in both cases cause as u I mentioned before I'm using mega 2560 so I removed the pinchange library and so I don't get the benefit of pinstate.
    Second, motors don't work with the rcarduinofastlib library and I don't know why. ESCs are connected to pins 8,9 and 10.They work fine with the servo library. Do u have any idea what I might have done worng?

    ReplyDelete
  22. What value are you arming the ESCs with ? Also how many servos and ESCs do you have attached and what framespace have you set ?

    As for pin state, to get similar performance, you can use direct port manipulation. I intend to post a Mega example using this approach soon.

    Duane B

    ReplyDelete
  23. Duane,
    I'm a noob and trying to understand the PPM code. Especially, using it stand alone without the FastServos class.
    I understand the theory. But, I'm lost on how the timer TCNT1 gets started/reset, etc. Also, how do you deal with TCNT1 overflows? Just reading the code, it seems like the following is happening:
    (1) INT0 is attached in begin() to INT0ISR on rising edge - great! Is there some sort of setup required for TCNT1?
    (2) on each PPM pulse on pin 2 (INT0) the INT0ISR is called and TCNT1 is read and compared with the previous value stored in m_unChannelRiseTime - OK. But it seems like this will eventually result in a negative number once the new TCNT1 has overflowed back to 0 and m_unChannelRiseTime was a value near but below 0xFFFF. What happens then? Is this just a resynch situation? Seems like there could be lots of resynchs.

    Any insight you can give on how TCNT1 is set up and scaled in relation to the PPM scheme you've come up with would be very helpful!

    Thanks,
    TheGipp

    ReplyDelete
  24. To answer 2) First, we are using unsigned numbers so nothing we can do will result in a negative number. As for overflows, why not write yourself a test sketch and take unsigned 65000 from unsigned 1500, what do you get ? would that be useful for measuring PPM Pulses ?

    For 1) the timer is set up in the two begin functions. If you try to use the PPM code with the standard servo class you will have a problem because the standard servo class resets timer1 corrupting it for any other code that could otherwise share the timer - this is one of the reasons for creating the RCArduino servo class.

    Duane B

    ReplyDelete
  25. Thanks, Duane.
    I see the TCNT1 setup in the FastServos::begin(). I suppose that I need to duplicate that in the PPM class if I want to use it stand alone - as it's not in the PPMChannels::begin() function.

    I'll do the math as you suggest - unsigned is probably the key to my question.

    Thanks for your thoughtful approach to these servo routines. It's a much tidier approach.

    One interesting thing that I found was that my Futaba 7 ch Rx actually has PPM for 8 channels before the frame spacing. I had to use my scope to discover that. Maybe that factoid will come in handy for someone else someday trying to read the PPM and having synching difficulties.

    TheGipp

    ReplyDelete
  26. Hi, are you sure its PPM for 8 channels ? that would require 9 pulses in a frame. There is always 1 more pulse than the number of channels, this pulse indicates the start of the frame. The time from this pulse to the next one is the pulse width for the first channel. Let me know. Duane.

    ReplyDelete
    Replies
    1. Yes, indeed. I have 9 pulses. I can watch all 7 sub-frames lengthen and shorten on the scope as I move the 7 channels on the transmitter. But, the 8th sub-frame doesn't change size. Then following that 8th sub-frame there is a 9th longer framing pulse is completely consistent with your description in the blog. The total frame length is approx 20ms and doesn't change with any channel setting.

      My Tx is an old Futaba FP-T7FG/K
      Rx is FP-R127DF

      This isn't any trouble for me. Just letting people know that there is a possibility that the PPM signal that you grab from the Rx may have a phantom channel.

      Thanks for your very useful tutorials.

      Delete
    2. Edit to above: the total frame length is approx 22ms for the radio I mention.

      Delete
  27. Hi, Its probably cheaper for manufacturers to design, build and maintain one set of radio internals and then just vary the case design to support more or less channels. If you needed to you could probably hack the radio to have full access to the additional channel.

    I would be interested to hear about other radios with hidden channels, it would be nice to be able to buy a three channel radio and find you could enable it for 6 or 8 channels.

    Something else I have seen is that the channels are not always in the same order within the PPM Stream as the channel numbers printed on the receiver suggest.

    Duane B

    ReplyDelete
  28. Hallo Duane,

    I'm the one who was asking you about the ESCs just before the last one asking. Well, it was my mistake. I modified something in the code that affected badly on the results. After removing this modification, signal has become so perfect with noise only 1 microsecond which is nothing to mention.

    Motors worked fine with the code but I have to initialize the ESCs first using a servo library, then uploading ur code.

    Finally, I'm trying to add another channel. Once the fourth channel is added, noise appears on the other three channels. Here is the modification I made to add another channel.

    http://textsave.de/?p=158585

    Samir

    ReplyDelete
  29. hi, Did you update RC_CHANNEL_OUT_COUNT to 5. ? See the FAQ post I added to the blog in the past few days for background.

    Duane B

    ReplyDelete
  30. Hi Duane,

    I've been reading through your code to determine how to apply a failsafe routine (i.e. if a valid PWM signal is not received within the last 250 ms) then set throttle to neutral.

    My initial thinking is to introduce a 'lastValidPWMTime' variable that's set when a valid PWM signal (pulse time between 1000 & 2000) is received. The main loop would then check this variable against the current time and set throttle to neutral if that gap is greater than 250 ms.

    Do you think this will work? Alternatively, do you have any suggestions for a more efficient mechanism?

    Thanks,
    Yogi

    ReplyDelete
  31. Hi,
    I have tried to stay away from adding failsafe functionality as part of the code I post. The reason is that receivers can behave differently and I do not want to post something that people will use without first figuring out what thier receiver does when it gets out of range. So my first suggestion to you is to check what you receiver outputs when the transmitter is switched off, then when it is switched on but out of range, also check what it does when you bring it back into range - I am guessing it will recover itself, but you need you know what happens in these three cases in order to 1) Detected them 2) Handle them 3) self recover from them.

    Let me know what receiver your using and what you discover in each case.

    Duane.

    ReplyDelete
    Replies
    1. Hi,

      I'm currently using the Traxxas TQi receiver. According to the manual, "Your Traxxas radio system is equipped with a built-in failsafe function that returns the throttle to its last saved neutral position in the event of signal loss.".

      I haven't been able to test this yet (plus what happens during recovery) as I'm having some issues with the Arduino Oscilloscope setup. I'll try to get that fixed and measure the signal over the next couple of days.

      I plan to use the 'FrSky D8R-II plus' receiver in the future. According to its manual, the failsafe signal is user adjustable. It can be set to a 'no pulse mode' on signal loss. I'll try to test this function as well and let you know how I go.

      Yogi

      Delete
  32. Hi Duane,

    I've been able to perform some measurements in the Traxxas TQi behaviour in case of signal loss. As advertised, the PWM signal is set back to the neutral position (1.5 ms pulse width). It auto recovers when signal is back. Results of measurements can be found in the link below.
    http://dl.dropbox.com/u/2906812/Arduino/Traxxas%20TQi%20PWM%20measurements.pdf

    Based on the above, I suppose the Arduino can't really detect signal loss. So, if I wanted to apply the brakes and bring the car to a stop when signal is lost it'll not be possible.

    Other interesting pwm measurement observations are:
    - pulse width is about 10 ms (as opposed to the standard Arduino PWM width of 22 ms). Am I right in understanding that your library will simply adapt to the pulse width of the incoming signal?
    - the pulse height is 3.3. V (as opposed to the standard Arduino output of 5 V). Do you know how I can set this in code?

    Thanks,
    Yogi

    ReplyDelete
  33. Hi,
    Your transmitter and receiver seem to be using a traxxas specifiic standard. It isntgoing to be rocket science to work with, but a lotof what you see on the web about arduino and servos/esc might not apply.

    The first thing you want to sort out is converting your Arduino outputfrom 5 volts down to 3.3 volts - you cannot do this in software. There are many ways to do it in hardware.

    When you have that figured out, the library will read the incoming signals fine, but needs a simple mod to output signals in the traxxas range. Let me know when your there and I will advise the changes.

    Duane B

    ReplyDelete
    Replies
    1. Hi Duane,

      I am using a Traxxas as well and am interested in hearing about this mod for the output if you have the time.

      Thanks!
      Mark

      Delete
    2. Hi,
      In the PM you sent on Arduino forum, you mention that you can read the incoming signals, can you confirm the maximum and minimum pulse width that you read for the channels. Also what is the model of the receiver and what are you sending the output signals to ? Servos, ESCs, make and model ?

      Duane B

      Delete
    3. Hi Duane,

      Thanks for the quick reply! I don't have the setup on hand, but I recall that without any input it was around 1500, full throttle pushed it above 2000. Reverse is not really a concern at this point so it wasn't tested. I am using the EVX-2 ESC motor controller, the Titan 550 motors, and the Traxxas TQi receiver from the latest (I believe) Traxxas E-maxx.

      For those interested, my roommate and I are converting the E-maxx into a parallel hybrid and trying to control both the engine and the motors via Arduino to simulate several propulsion control schemes by running the vehicle through EPA driving cycles.

      Thank you so much for all of your help!

      Mark

      Delete
    4. Hi,
      It looks like its easy enough to work with, standard servo pulse widths, but at a frequency of around 100Hz rather than the usual 50Hz.

      I can tell you what to modify if you tell me -

      1) How many channels are you reading ?
      2) I assume your reading them directly rather than through PPM ?
      3) How many channels do you need to output and are they all on devices which you have previously connected to the receiver and see to function correctly ?

      Duane B

      Delete
    5. The plan is to read from the TQi receiver, at least for the drive motors, so I had assumed that was PPM? The only channel I have to read is the accelerator request from the driver, my Raspberry Pi is handling shifting and the Arduino is interfacing directly to the engine sans receiver.

      The end goal is to control each motor and the engine independently from a single channel in based on a pre-defined propulsion mapping, ie. when reach a certain speed/RPM turn on the engine. For now though, just getting the Arduino to pass a signal straight thru to the motor would be great.

      Thank you for your patience!
      Mark

      Delete
    6. So I was reading through the code and feel that if I want to change from 50 to 100 Hz output I need to be looking at this line in the code:

      CRCArduinoFastServos::setFrameSpaceA(SERVO_FRAME_SPACE,7*2000);

      or this line in the library:

      (20,000 - (RC_CHANNEL_OUT_COUNT * 2000))

      in my case, my optimal RC_CHANNEL_OUT_COUNT is 3, though I'd like to start with 1 and understand it so I can add or subtract in the future, depending on how far we get with the project. My question is are these the only lines I have to modify to change the frequency, and do I adjust the 20,000 or the multiplier or something else entirely?

      I apologize if this response adds more work to you helping me, I just wish to understand the code better. Thanks!

      Delete
    7. Thats the one - CRCArduinoFastServos::setFrameSpaceA(SERVO_FRAME_SPACE,7*2000);

      I suggest you work with three channels from the beginning, it will save time and the additional channels will have no detectable impact on your project until you physically connect servos to them.

      To run three channels at 100Hz, you would want -
      CRCArduinoFastServos::setFrameSpaceA(SERVO_FRAME_SPACE,7*1000);

      Duane B

      Delete
    8. It still does not seem to be working when I try to power only the input to the EVX-2 motor controller. I have the signal output from the receiver going into the Arduino and the output signal going into the signal to the motor controller. Could I PM you my code on the Arduino forum?

      Thanks!
      Mark

      Delete
    9. Sure, send the code to DuaneB, thats me.

      Also send a picture of your set up and confirm that the receiver drives the motor controller when the two are connected directly together.

      Duane B

      Delete
    10. The strangest thing happened today. Just plugged the Arduino in after changing the value to 1000 and the car shot off my desk at full speed without any controller input. Looking at the serial monitor, the input is now going between 10, 600, and 8000, rather than 1500-2000 like before. I just grabbed the code above again (the sketch, .h, .cpp) and it still does that. Have you heard of this before?

      Delete
    11. Luckily, the output works fine though! I just had the Arduino read a pot and spin the servos and it works like a charm! For now, this is acceptable for the project. What I'm curious about now is the speed sensitivity by microseconds. The program that I have running the speed now maps MPH from 0-60 to 1500-2000, but I'm wondering if you know what kind of resolution I'll get with this, or if there's a way I should do this, other than map.

      Thanks!

      Delete
  34. maybe i missed a post, but any update on a mega version of the library that uses regular interrupts. i can get PPM to work fine on a mega using the routing but having issues driving the servos.

    ReplyDelete
    Replies
    1. See the comment below or if you really want to read the individual channels directly, use the code in this post -

      http://rcarduino.blogspot.ae/2012/11/how-to-read-rc-channels-rcarduinofastlib.html

      But change the following -

      // Assign your channel in pins
      #define THROTTLE_IN_PIN 5
      #define STEERING_IN_PIN 6
      #define AUX_IN_PIN 7

      // Assign your channel out pins
      #define THROTTLE_OUT_PIN 8
      #define STEERING_OUT_PIN 9
      #define AUX_OUT_PIN 10

      to

      // Assign your channel in pins
      #define THROTTLE_IN_PIN 10
      #define STEERING_IN_PIN 11
      #define AUX_IN_PIN 12

      // Assign your channel out pins
      #define THROTTLE_OUT_PIN 5
      #define STEERING_OUT_PIN 6
      #define AUX_OUT_PIN 7

      Note that pinchangeint interrupts are not supported on all of the Mega pins, refer to the pin mapping here

      http://arduino.cc/en/Hacking/PinMapping2560

      pcintX is available on 10,11 and 12 so this modification to the original sketch should get you going - or refer to the below comment and the new FAQ linked below to fix whatever issue your having and use the PPM -

      http://rcarduino.blogspot.ae/2013/02/rcarduino-libraries-faq.html

      I will find some time this week to post a Mega specific version using external interrupts in place of pinchangeint, but I would be greatful if you could test this in the meantime

      Duane B

      Delete
  35. Hi,
    If your reading the PPM Fine, the issue with the servos is probably of of the following -

    1) Insufficient current or too much voltage to drive the servos
    2) The number of servos or servo framespace is not set correctly
    3) The number of PPM Input channels is not set correctly

    How many channels are you reading and how many servos are you outputting ?

    What values have you set for RC_CHANNEL_IN_COUNT and RC_CHANNEL_OUT_COUNT ?


    Duane B

    ReplyDelete
  36. Hi Duane,
    Thank you for this nice post. I'm trying to read signals from an 8 Channel Futaba receiver connected to an Arduino UNO R3. Both my original written code and your code here work OK when I'm reading up to 4 or may be 5 channels. When I add a 6th channel everything start failing. All the values start fluctuating even out of range. I guess it is because the code is missing some of the rising or falling edges.
    I'm not able to read the PPM stream from my receiver, since it has an S.BUS pin instead.
    Do you know how many simultaneous channels it is possible to read with this code and hardware? Have you seen this problem before?

    Thank you.

    ReplyDelete
    Replies
    1. I just noticed that you mention Arduino UNO R3 in your original comment. Can you also send me the code you are using, I am user DuaneB on the arduino forum, if you copy your main sketch into a PM I will have a look through it.

      Duane B

      Delete
  37. Hi,
    I can't see any reason why the code would not work for 10 or 20 channels assuming they are consecutive.

    What Arduino are you using ?

    What values have you set for the channel in/out count ?

    Are you using serial.print to output the channel values or have you connected a servo ?

    Also can you try a test using the exact code above, connecting the aux input to channel 3 from your receiver. Assuming this works as expected, move the aux input to channel 4,5,6 etc just to see if the higher number channels are the cause rather than the number of channels.

    If you can let me know the answers we can work out a solution.

    Duane B

    ReplyDelete
    Replies
    1. Hi Duane,

      Thanks for your reply. I've run many tests and it is no a matter of channel number, but a matter of the number of simultaneous channels. If I use your exact code here, everything works fine. If I move the Aux or any other channel to channel #4, 5 or event 8 (still reading 3 channels) it works ok. If I add just one more channel, everything ok. If I add a 5th or 6th channels, the values in ALL the channels start fluctuating. Some times they jump from 1500 to 250 and then go to a ridiculous 65365 or something like that.

      I use Serial.print to output channel values or a plotting library I've developed to work with Processing. Not controlling any servo yet. (Here's my plotting library in case you are interested: https://github.com/saientzg/ardu-plot/)

      I'm sending you the code through Arduino's forum.

      Thanks a lot!

      Delete
    2. Hi,
      Your comments relate to a different post linked below. I will add a few more channels to the code in the link and get back to you, use this link for further correspondence -

      http://rcarduino.blogspot.ae/2013/04/problem-reading-rc-channels-rcarduino.html

      Duane B

      Delete
  38. Hello Duane

    Sorry if i have posted in wrong Area.But will really appreciate your help in this matter.I am trying to control 4 servo motors with remote control using Ken Shirriff library.I have achieved required output but with lots of SERVO TWICTHING ((That is servos are making lot of noise(kirrrr kattttt) and shaking) which is really frustating...

    Arduino USER community will be really greatful to you if you provide working solution as there are many frustrated Arduino users facing this issue and still waiting for solution.I tried to find solution but I thought this is place were i may get answer

    I am using Arduino Mega 2050
    My connection are Grounded properly
    Individual servos code works fine
    Individual Remote control code works fine
    Problem occurs when i combine both codes.
    After lot of searching on Internet, I came to conclusion that it is issue related to Interrupt which is addressed in your blog.But I am not sure whether this code can be used to remove Twitching of servo.(Complexity level is bit high)

    Please advice and try to provide working solution from your busy schedule
    My email address : shail363@yahoo.com

    Thank you very much in advance
    Shail


    Code for reference:
    ====================
    #include
    #include


    int RECV_PIN = 11;
    IRrecv irrecv(RECV_PIN);
    decode_results results;

    Servo motorTop;

    int ButtonValue;
    int vspeed =75;


    void setup()
    {
    irrecv.enableIRIn(); // Start the receiver
    motorTop.attach(8);
    }

    void loop() {


    if (irrecv.decode(&results)) {
    ButtonValue = translateIR();
    irrecv.resume();
    }
    motorTop.write(vspeed);


    }


    int translateIR() // returns value of "Car MP3 remote IR code received

    {
    switch(results.value)
    {
    case 57:
    case 2105:
    vspeed = vspeed + 1;
    return results.value; // Increase angle
    break;

    case 56:
    case 2104:
    vspeed = vspeed - 1;
    return results.value; // Decrease Angle
    break;
    default:
    return results.value; // Other Button / Bad Code

    }
    } //END translateIR


    Sorry if I have posted in wrong place

    ReplyDelete
  39. Hi! I am triyng to compile your test scetch and geting errors (below). Can you tell what's wrong, please?

    sketch_jun25a.cpp.o: In function `loop':
    C:\arduino\152/sketch_jun25a.ino:159: undefined reference to `CRCArduinoFastServos::writeMicroseconds(unsigned char, unsigned int)'
    C:\arduino\152/sketch_jun25a.ino:164: undefined reference to `CRCArduinoFastServos::writeMicroseconds(unsigned char, unsigned int)'
    C:\arduino\152/sketch_jun25a.ino:169: undefined reference to `CRCArduinoFastServos::writeMicroseconds(unsigned char, unsigned int)'
    sketch_jun25a.cpp.o: In function `setup':
    C:\arduino\152/sketch_jun25a.ino:76: undefined reference to `CRCArduinoFastServos::attach(unsigned char, unsigned char)'
    C:\arduino\152/sketch_jun25a.ino:77: undefined reference to `CRCArduinoFastServos::attach(unsigned char, unsigned char)'
    C:\arduino\152/sketch_jun25a.ino:78: undefined reference to `CRCArduinoFastServos::attach(unsigned char, unsigned char)'
    C:\arduino\152/sketch_jun25a.ino:81: undefined reference to `CRCArduinoFastServos::setFrameSpaceA(unsigned char, unsigned int)'
    C:\arduino\152/sketch_jun25a.ino:83: undefined reference to `CRCArduinoFastServos::begin()'

    ReplyDelete
  40. It looks as if you have not installed the library.

    Duane B

    ReplyDelete
  41. Hi Duane,

    I am working on a project where I will need to output a sinusoidal motion to a couple servos, where throttle controls the frequency (it's for moving a pair of flapping wings).

    I'm creating an array that contains a generic sinusoid, then I'm trying to figure out a way to use the throttle input to control the rate that I read from the array, effectively letting me vary flapping rate.

    My question is, how can I implement this efficiently? Right now I'm trying to do something like this: timer1()%throttleinput as a sort of speed regulator, but won't this result in constantly checking the timer interrupt, resulting in poor performance?

    Also one other random question, why are you bitshifting in each of the ISRs in this post, when you are timing the pulse width?

    Thanks,
    John

    ReplyDelete
  42. The bit shift is there to convert from timer ticks (2 per microsecond) to microseconds and back. It just to make the library easier to use for people who are used to using writeMicroseconds with the standard servo library.

    For outputting the sine wave, I would borrow from Direct Digital Synthesis and simply map your throttle signal to a 'phase increment', its all explained here -

    http://rcarduino.blogspot.ae/2012/12/arduino-due-dds-part-1-sinewaves-and.html

    You could use a timer or micros() to figure out when you need to increment the 'phase accumulator' by the 'phase increment'

    For an idea of how to use micros to figure out when you need to do something, have a look for 'blink without delay'

    Duane

    ReplyDelete
  43. Great article, thanks! Has been a reference I keep coming back to, while tediously duplicating some of your experiences!

    Speed-wise, you can do a LOT better than PCintPort by working with the known structure of the signals from the receiver.

    ReplyDelete
  44. For ease of those working with this; I have added it to github; https://github.com/scottjgibson/RCArduinoFastLib includes the source and examples; I'm using it as the basis for my RC Gokart experiment https://github.com/scottjgibson/rcBabyKart

    ReplyDelete
  45. Awesome blog! Just getting into all this and building a QuadCopter. I was getting variable values from the RC Receiver (With no stick movement) i think this will help get stable values out.

    Did you get around to posting an example code base for the MEGA?

    Cheers

    James

    ReplyDelete
  46. hi
    Duane, I have used your code to read one rc channel many times with no probs,
    Im using this because I want to use a second channel to activate pass through or process modded output,

    I noticed that the servo position only refreshes every half second, and there is a half second delay in the code?
    is it needed? I have reduced it and the servo jitters but updates quickly, im going to comment it, but I wanted to check if its needed or not,
    thanks

    m00se

    ReplyDelete
    Replies
    1. EDIT:
      I commented the delay, the code now jitters all the time,
      I have the input on the aux channel,
      any ideas what might be causing the jitter? its not the rc system, the stock servo never jitters at all

      Delete
    2. What code are you using ? there is no delay in the example code in this post

      Duane

      Delete
  47. In the example folder-src-sketch

    this section , where it looks like the servo ppm is updated has a delay
    ---------------------------------------------------------------------------
    if(bUpdateFlags & AUX_FLAG)
    {
    CRCArduinoFastServos::writeMicroseconds(SERVO_AUX,unAuxIn);
    }

    delay(500);
    bUpdateFlags = 0;
    }
    --------------------------------------------------------------------------------

    I noticed you offer some people on the arduino forum a modded version to read 9 channels, do you have a link to that? Do you have a version with a failsafe? I have made a failsafe by taking 6 readings, and comparing them, then if they are all the same acting as a failsafe, that is the only way I can think of acheiving it with a turnigy 9x.

    Thanks for the code, I have been able to get a working auto stable flight mode on my rc plane with it. I say working, I havent flown it, but on the bench it looks good.

    ReplyDelete
  48. Dear Duane B/Can_I_Trade,
    Thank you very much for writing this software. I am building a robot and I wrote my own polling code which sucked until I found your explanation of PWM and interrupt code. I was trying to get it to compile on a raspberry pi on my robot (really a large glorified RC power wheels at this point) and noticed that I had to add "pins_" to the #include in RCArduinoFastLib.cpp file in order to get it to compile in raspbian; #include . It complained that "arduino.h" was not found. I don't know if this information can help you somehow make a more robust implementation of your program; (with some sort of case/switch) or simply help other beginners to use it. Thank you for your awesome software. I has really made my robot a lot more fun, and much safer.
    Sincerely,
    Liam

    ReplyDelete
  49. "pins_arduino.h", the forum software eats my post when I use greater than and less than... > <

    ReplyDelete
  50. Hi Duane,

    First of all I would like to thank you for all this amazing piece of information you gathered up here which I am finding really helpful as I am seeking to introduce active steering on my RC car.

    I am still a newbie so your help would be highly appreciated and was wondering whether you uploaded the sketch for the Mega-2560 which makes use of the external interupts (attachInterrupt)?

    Also, since the external interupts are enough for my purpose should I avoid making use of the PinChangeInt library?

    Thanks alot again.

    ReplyDelete
  51. Hi,
    Remind me in a day or two or contact me DuaneB through the Arduino forum and I will post it here or send you the code.

    Duane B.

    ReplyDelete
    Replies
    1. Hi Duane

      I want to use your RC to RC library . but i have 6 channels to read and write, what is the steps of making more channels available is it just a case of copy and past all the relevant lines untill there is enough ch? and update the FRAME to 7 ?

      Delete
  52. Hi. RC works great, but... I use above original code on Mega256. Throttle on A11, Steering on A12, Aux on A13 pins. I get various values reported from 0 to 32768 with average 0 on all three channels. If transmitter is OFF the bUpdateFlags is never set - OK, so the code and connection works...
    How am I suppose to get things I want?!? I only want Throttle UP/DOWN, Steering LEFT/RIGHT, AUX UP/DOWN. How to get them from original code (example from unThrottleIn)?!?!
    Part of code I use:

    if (bUpdateFlags & THROTTLE_FLAG) {
    if (i < 1000) {
    arr[i] = unThrottleIn;
    i++;
    } else {
    unsigned long sum = 0;
    for (int r=0; r < 1000; r++) {
    sum = sum + arr[r];
    if (arr[r] < minn) minn = arr[r];
    if (arr[r] > maxx) maxx = arr[r];
    }
    sum = sum / 1000;
    Serial.println("DONE");
    Serial.print("Avrg: ");
    Serial.print(avrg);
    Serial.print(" Min:");
    Serial.print(minn);
    Serial.print(" Max:");
    Serial.println(maxx);
    while (1<2) {delay(19);}
    }
    }

    ReplyDelete
  53. word arr[1000], avrg, minn, maxx;

    ReplyDelete
    Replies
    1. Found the error. Instead of TCNT1 on mega2560 one should use micros(). I.e. TCNT0?

      Delete
  54. Hi, I have a question. would this library work with an attiny 45 or 85?
    I need the board to be small and need to control only about 3 or 4 servos that would listen only to specific channels. The attiny would run a 8Mhz. I'm a beginner at coding. really, I can only blink a couple of LEDS. and only one at the time :-) that's how bad I am, so please be gentle. I have a friend that is helping me with the project. Thank you in advance for your answer..

    ReplyDelete
  55. I have tried the fastlib with EIGHT servos, but i can not seem to get any spacing,
    regardless of the setting in "CRCArduinoFastServos::setFrameSpaceA(SERVO_FRAME_SPACE,7*1000);"

    Regardless of the setting, the servo pulses comes one after another,
    with no interframe space after nr8, and before nr1.

    ReplyDelete
  56. If you want 8 servos and a frame space on the end, you need to set

    // Change to set the number of servos/ESCs
    #define RC_CHANNEL_OUT_COUNT 9

    Internally the library treats the framespace as another servo that is not connected to a pin. So set the number to 8 (Servos) + 1 (Framespace) = 9.

    You can then define your 8 channels -

    // Assign servo indexes
    #define SERVO_THROTTLE 0
    #define SERVO_STEERING 1
    #define SERVO_AUX 2
    ...

    and finally the frame space
    #define SERVO_FRAME_SPACE 8

    Remember that the 8 channels start from 0 and run to 7, the final entry is the framespace.

    Hope that helps

    Duane.

    ReplyDelete
  57. I do have "#define SERVO_FRAME_SPACE 8",
    but no "#define RC_CHANNEL_OUT_COUNT 9"

    (was it in the example and i somehow deleted it?)

    ReplyDelete
  58. Its in the RCArduinoFastLib.h file. To get a frameframe space you need to add one to the number of servos, so for you it needs to be 9

    Duane.

    ReplyDelete
  59. Excellent
    I can even finetune to "CRCArduinoFastServos::setFrameSpaceA(SERVO_FRAME_SPACE,7920);"
    And have plenty of time to do a sh**load floating point operations.

    http://i.imgur.com/2D6HGjS.png

    Thanks a lot

    ReplyDelete
    Replies
    1. If you floating point maths takes up too much of the Arduino processing, I have a write up on fixed point maths with Arduino that I will publish as part of an upcoming Arduino synth post.

      Duane.

      Delete
  60. First of all congrats for this job. I`m newbie with Arduino but this program ran without problem.

    My application needs to command a Lego EV3 by RC converting the sinal to a IR Transmitter. Could you help me to explain how can I use a IR diode in you sketch? I tried to understand the code but it`s so advanced for my level at this moment.

    By the way I have changed this line: #define RC_CHANNEL_IN_COUNT 8 to increase the PPM channel from 3 to 8 but It didn`t work. Did I forget anything?

    Thanks a lot.

    ReplyDelete
  61. Hi!

    If I want to simply read three servo positions from a receiver and have the Arduino act on the info, which library/code is required? (I assume that RCArduinoFastLib is for driving servos, not reading?)


    Thanks!

    ReplyDelete
  62. Basic approach here - http://rcarduino.blogspot.ae/2012/04/how-to-read-multiple-rc-channels-draft.html
    RCArduinoFastLib improves on this, but for a quick start the link above will get you going.

    Duane.

    ReplyDelete
    Replies
    1. Hi Duane,

      I have a question about the values returned by the test sketch on this webpage.

      When reading the PWM signal on pin 5 and 6 strange values are returned, always around the same PWM range.
      On pin 5 around a PWM value of 1765 strange values are read, on pin 6 around a PWM value of 1245 strange values are returned. Both about 265 away from 1500.

      I used two different PWM sources with the same result.

      Can you explain this behaviour, or do you need more information?

      Best regards,

      Jan Willem
      The Netherlands

      Delete
    2. Yes, The code above does not output any values to serial, my guess is that you added a line to output values, but did not test the value of the channel flag first. Put your output inside the if statement for whichever channel you want to look at.

      Duane

      Delete
  63. Hello. Trying to use your code on an Arduino Micro to control a live steam locomotive. I don't have any RC equipment yet, but am trying to get code to work using an infrared photo switch and servo. I am getting some numbers from the switch but servo isn't doing anything. I realize w/ what i have it won't act like it's getting a signal from a RC RX. Here is a couple lines that i'm trying to use to get servo to move to its "closed" position:
    #define AUX_OUT_PIN 6 //servo on pin 6
    int pauseButton = 7; //wire from button goes pin #7
    pinMode (pauseButton, INPUT_PULLUP);
    if (digitalRead(pauseButton) == LOW) //if button pressed, pin 7 goes to ground
    {
    CRCArduinoFastServos::writeMicroseconds(SERVO_AUX,1472);
    }

    Am i off base w/ this?

    I copied the RCArduinoFastLib stuff from webpage and pasted into a .txt file. Is that OK?

    Not sure where to start in figuring it out.
    Thx for any help you can send my way.
    Marty

    ReplyDelete
  64. is your IR link sending pulses at over 1000 pulses a second? the library is designed to use the standard 1000-2000ms pulse speeds, and a servo may not respond to anything under or over that,

    I personally had problems with this library introducing jitter into servos, Duanes previous efforts with reading just one rc channel seemed to work without jitter

    ReplyDelete
  65. IR only sending 50 pulses.
    Just trying to test to see if servo is getting signal at this point. Don't have a RX to connect to Arduino.

    ReplyDelete
  66. Hi, As you only have one channel at this point and you are not planning to do lots of high frequency calculations, use this code, its a lot simpler - http://rcarduino.blogspot.ae/2012/04/how-to-read-multiple-rc-channels-draft.html

    If you need a more efficient approach later on in your project you can come back to this library.

    Duane B

    ReplyDelete
    Replies
    1. Thx. Curious though why it doesn't work. Something in the fastLib .h or .cpp files? It's OK to copy from webpage and paste into .txt file?

      Delete
  67. Does it compile?
    the Reading of RC channels I assume is set up to the 1000-2000 ms range, 50 pulses a second probly wont get close to controlling anything, try the code mentioned above, it worked well for me with one channel.

    if all you want to do is test that the servo works, use the arduino servo library, and make a toggled state from a switch that moves it from 1000 to 2000 on each press.

    ReplyDelete
  68. Im trying to use your library on an atmel 328p based arduino to control a Pan tilt bracket with two servos, and two incoming PWM (not ppm) signals from a paired arduino atmel 328p based board. The first board has 433 mhz receiver to catch incoming analog joystick inputs from another joystick + arduino + transmitter setup. The receiving arduino board translates those into a PWM signal that is paired via pins to the servo controlling arduino board.

    I used two boards on the receiving end because virtual wire library did not allow for smooth servo control via software servo or servotimer2, and of course is incompatible with the servo library.

    Would your library allow for me to simplify to only two board for the transmit/receive setup?

    Also when I compile the above example I get 'CRCArduinoFastServos' has not been declared. What am I doing wrong?

    p.s. The arduino boards I am using are nice Chinese clones of the Pro Mini Atmega328p 5v boards (and I did verify they actually use the Atmega328P chip). They cost me $2.50 a piece, and I buy them in slews of 10 at a time. So using multiple is not really a big deal, but since this is mostly a learning endeavor for me I'd of course like to simplify as much as possible.

    Thanks for your help:)

    ReplyDelete
    Replies
    1. Hi, 328 based boards only have one 16 bit timer which is why you find some libraries that cannot work together as they both want to claim this single resource. Part of the fun of 8-bit micro controllers is finding ways to simplify problems down to fit the available resources, I would look for a simpler protocol that virtual wire, why not just send PPM ?

      Duane B

      Delete
    2. No reason at all other than I am new to signal processing. Not afraid just new. Could you direct me to some good resources about PPM signals and understanding them? Is it possible to use the 328 based boards to send a multilayered PPM signal? I have 3 channels in this particular project. X, Y, and a push button input. If I understand you correctly you are saying a PPM would be a simpler signal to decode while still allowing smooth servo control on the receiver side. I mean it seems like you are doing that with your projects.

      Delete
    3. Here
      http://rcarduino.blogspot.com/2012/11/how-to-read-rc-receiver-ppm-stream.html

      and here
      http://rcarduino.blogspot.com/2013/07/ppm-output-draft-looking-for-testers.html

      Duane B

      Delete
    4. Thanks! Sometimes starting out can be overwhelming, just a little guidance is all it takes;) I'll look at implementing this in my project and report back how it goes.

      Where would I download the modified RCArduino Fast lib for PPM output that you reference in the last link you shared with me?

      Thanks again! Major help.

      Delete
  69. Hi Duane,

    I used your test sketch sucessfully with arduino uno, the thing is I'm working on a Simulink-Arduino project, so I need to combine the test sketch and the RCarduinoFast library with a Simulink model , Can this be done through adding it in a Simulink S block within my model ?

    Thanks,
    Nancy

    ReplyDelete
  70. Hi,

    No idea what Simulink is, sorry

    Duane.

    ReplyDelete
  71. Hello Duane

    Thanks for your answer from "How To Read Multiple RC Channels" page. In fact I saw the "RCArduinoFastLib" page alone immediate after I post the last question and I guess you will send me to RCArduinoFastLib.

    I run the example from "RCArduinoFastLib" page and indeed it works great and with very low servo tickle and this is great. You make a very nice job!

    After that I modify the example to 4 servos and unfortunately I can't get the right 50 Hz refresh rate for the servos when I want to control 4 servos. What I get is around 166 Hz as the h file says. I try to put on code the following command "CRCArduinoFastServos::setFrameSpaceA(4,6*2000);" and I get 166 Hz. If I put the following command :"CRCArduinoFastServos::setFrameSpaceA(3,7*2000);" I get 50 Hz if I do not make "writeMicroseconds" to the 4th servo. If I make the "writeMicroseconds" to the 4th servo, even just one time on setup function, a get 175 Hz. All the time I execute the attach command for the 4th servo.

    So what I make wrong and why I can't control 4 servos? Why I need 4 servos? Of course to control an RC plane, and in fact I'll need more, but at least till now I need 4. :)

    Thanks a lot
    Sorin

    ReplyDelete
  72. To add another channel you would need to change this -
    // Assign servo indexes
    #define SERVO_THROTTLE 0
    #define SERVO_STEERING 1
    #define SERVO_AUX 2
    #define SERVO_FRAME_SPACE 3

    to this

    // Assign servo indexes
    #define SERVO_THROTTLE 0
    #define SERVO_STEERING 1
    #define SERVO_AUX 2
    #define SERVO_MYNEWCHANNEL 3 // add an index for the new channel
    #define SERVO_FRAME_SPACE 4 // increase the framespace index by one, should always equal the number of channels i.e. 4 for 4 channels

    then

    CRCArduinoFastServos::setFrameSpaceA(SERVO_FRAME_SPACE,6*2000);

    Duane B

    ReplyDelete
    Replies
    1. Hello Duane

      I do as you indicate from the beginning and I get 166 Hz refresh rate. I may send my code to you on email in order to look at at it and see what I do wrong ?

      Thanks a lot
      Sorin

      Delete
    2. I only need to see your setup and loop functions, so post them here

      Duane.

      Delete
    3. void setup()
      {
      Serial.begin(115200);

      Serial.println("multiChannels");

      // attach servo objects, these will generate the correct
      // pulses for driving Electronic speed controllers, servos or other devices
      // designed to interface directly with RC Receivers
      CRCArduinoFastServos::attach(SERVO_THROTTLE,THROTTLE_OUT_PIN);
      CRCArduinoFastServos::attach(SERVO_STEERING,STEERING_OUT_PIN);
      CRCArduinoFastServos::attach(SERVO_AUX,AUX_OUT_PIN);
      CRCArduinoFastServos::attach(SERVO_RD,RD_OUT_PIN);

      // lets set a standard rate of 50 Hz by setting a frame space of 10 * 2000 = 3 Servos + 7 times 2000
      CRCArduinoFastServos::setFrameSpaceA(SERVO_FRAME_SPACE,6*2000);

      CRCArduinoFastServos::begin();

      // using the PinChangeInt library, attach the interrupts
      // used to read the channels
      PCintPort::attachInterrupt(THROTTLE_IN_PIN, calcThrottle,CHANGE);
      PCintPort::attachInterrupt(STEERING_IN_PIN, calcSteering,CHANGE);
      PCintPort::attachInterrupt(AUX_IN_PIN, calcAux,CHANGE);
      PCintPort::attachInterrupt(RD_IN_PIN, calcRd,CHANGE);

      }

      void loop()
      {
      // create local variables to hold a local copies of the channel inputs
      // these are declared static so that thier values will be retained
      // between calls to loop.
      static uint16_t unThrottleIn;
      static uint16_t unSteeringIn;
      static uint16_t unAuxIn;
      static uint16_t unRdIn;
      // local copy of update flags
      static uint8_t bUpdateFlags;

      // check shared update flags to see if any channels have a new signal
      if(bUpdateFlagsShared)
      {
      noInterrupts(); // turn interrupts off quickly while we take local copies of the shared variables

      // take a local copy of which channels were updated in case we need to use this in the rest of loop
      bUpdateFlags = bUpdateFlagsShared;

      // in the current code, the shared values are always populated
      // so we could copy them without testing the flags
      // however in the future this could change, so lets
      // only copy when the flags tell us we can.

      if(bUpdateFlags & THROTTLE_FLAG)
      {
      unThrottleIn = unThrottleInShared;
      }

      if(bUpdateFlags & STEERING_FLAG)
      {
      unSteeringIn = unSteeringInShared;
      }

      if(bUpdateFlags & AUX_FLAG)
      {
      unAuxIn = unAuxInShared;
      }

      if(bUpdateFlags & RD_FLAG)
      {
      unRdIn = unRdInShared;
      }

      // clear shared copy of updated flags as we have already taken the updates
      // we still have a local copy if we need to use it in bUpdateFlags
      bUpdateFlagsShared = 0;

      interrupts(); // we have local copies of the inputs, so now we can turn interrupts back on

      }

      // do any processing from here onwards

      if(bUpdateFlags & THROTTLE_FLAG)
      {
      CRCArduinoFastServos::writeMicroseconds(SERVO_THROTTLE,unThrottleIn);
      }

      if(bUpdateFlags & STEERING_FLAG)
      {
      CRCArduinoFastServos::writeMicroseconds(SERVO_STEERING,unSteeringIn);
      }

      if(bUpdateFlags & AUX_FLAG)
      {
      CRCArduinoFastServos::writeMicroseconds(SERVO_AUX,unAuxIn);
      }

      //CRCArduinoFastServos::writeMicroseconds(SERVO_RD,unRdIn);

      if(bUpdateFlags & RD_FLAG)
      {
      CRCArduinoFastServos::writeMicroseconds(SERVO_RD,unRdIn);
      }

      bUpdateFlags = 0;
      }

      Delete
    4. Hi, You might need to update this line in the header file -

      // Change to set the number of servos/ESCs
      #define RC_CHANNEL_OUT_COUNT 4

      As you have four channels plus a frame space on the end, set this to 5 (4+1) rather than 4.

      Let me know how you get on

      Duane.

      Delete
    5. Replace 4 with 5 and still 166Hz refresh rate. :(

      Delete
    6. How are you measuring the refresh rate, if you change RC_CHANNEL_OUT_COUNT the refresh rate is going to change. Set it to 2, set it to 10, you should see a change in the refresh rate, if you do not, I would look at the measurement technique.

      Duane

      Delete
    7. Hello Duane

      Sorry for giving you problems, but what can I do. Its not working. A have a digital oscilloscope and I see the servo output signal. so I can measure the frequency. for 3 servos ( RC_CHANNEL_OUT_COUNT = 3 ) the servo signal frequency is correct around 50Hz. For RC_CHANNEL_OUT_COUNT = 4 or 5 the refresh rate is 166Hz. I think that maybe it is a bug on the library ?

      Delete
  73. Its never a bug in the library.

    4 channels * 1500 microseconds = 6000 microseconds.

    1 second / 6000 microseconds = 166Hz

    So whats going on ? when you change the RC_CHANNEL_OUT_COUNT in the header file, the compiler is probably not picking this up. If you add Serial.println(RC_CHANNEL_OUT_COUNT); to your setup function my guess is that you will see 4 printed out even if you have 5 in the header file.

    The arduino compiler tries to compile as little code as possible to save time. Normally this is what you want it to do - use previously compiled versions of the libraries. However when you change the library you want to ensure that the compiler picks this up. To do this you need to choose 'build all' or 'compile all'. This should force the compiler to read all of the project files and make sure that the final sketch includes all changes including any from libraries. Do this and you should see the correct value printed in setup and the refresh rate that you want on the oscilloscope. I will add a comment to the library to remind others to do this whenever changing the number of channels. Thanks Duane.

    ReplyDelete
  74. Hello Duane,

    OK. So how you make "compile all" with arduino ide ?

    On the other hand, I go to another type of design. I use 2 arduino pro mini boards, one for RC signals sample and one for servo signal generation. The link between them is I2C at 400K clock frequency. It works great with 0 servo tickling and very fast response.

    Thanks
    Sorin

    ReplyDelete
    Replies
    1. Looks like there is no option to compile all or build all. One way of forcing the compiler to do this would be to delete all of the temporary files it creates, but even these are in odd places. I have been messing about with a quick and dirty hack for someone that wants the library to run on a Mega, to do this I have added to library files directly to my project rather than using the libraries folder. The compiler seems to be picking up all of the changes I make with this approach.

      Duane B

      Delete
    2. That's the way I do it too. Works well. I use class modules a lot to structure my projects in order to keep the .ino file size down. If you had to search around your computer looking for object files and delete them before every compile, you would certainly go nuts.

      Delete
  75. Hello Duane,

    I'm building a 1/10 scale truck which has an autonomous GPS/Compass PID nav system. I'm using a Mega (2560) and I've got a 3 channel radio. I read all three channels, but only need to drive the ESC and Steering servo. The 3rd channel is used internally in my code.

    So far I used your read method and PinChangeInt.h and it worked great. Resolved some serious glitching.

    Next I wanted to try the RCArduinoFastLib and eliminate Servo.h. I tried the code in this article and am having some problem. It is reading all three channels fine, but I get no response from the ESC or steering.

    In the test sketch....
    if(bUpdateFlags & THROTTLE_FLAG)
    {
    CRCArduinoFastServos::writeMicroseconds(SERVO_THROTTLE,unThrottleIn);
    }

    I added a Serial.print after writeMicroseconds and get proper values for unThrottleIn on my serial monitor. All channels read fine. My problem seems to be with writeMicroseconds ().

    I'm a novice microcontroller programmer, so I can vaguely follow what's happening in the .cpp file, but am stumped as far as to why it's not working. Any ideas?

    P.S. I'm running the test sketch in this article standalone.

    ReplyDelete
  76. Hi, The library is designed for an Arduino UNO, there are two internal functions that map from Arduino UNO digital pin numbers to internal microcontroller ports and pins, these functions will be performing the wrong mapping for a mega. I will have a quick look tonight to see if there is a quick work around you could paste in to support the mega.

    Duane.

    ReplyDelete
    Replies
    1. Great! Thanks for your help.

      Delete
    2. I don't have access to an Arduino Mega but here is a quick and dirty hack you can try. If also goes to plan you should be able to attach servos to pins 22 to 29 of your mega, its a hack and will not support attaching servos to any other pins for now. Let me know if it works and if it does I will eventually tidy the code up to support all Mega digital pins.

      Copy the two functions below replacing the originals in the .cpp file -

      // Easy to optimise this, but lets keep it readable instead, its short enough.
      volatile uint8_t* CRCArduinoFastServos::getPortFromPin(uint8_t sPin)
      {
      volatile uint8_t* pPort = RC_CHANNELS_NOPORT;

      if(sPin <= 7)
      {
      pPort = &PORTD;
      }
      else if(sPin <= 13)
      {
      pPort = &PORTB;
      }
      else if(sPin <= A5) // analog input pin 5
      {
      pPort = &PORTC;
      }
      // QUICK AND DIRTY HACK TO TEST MEGA SUPPORT FOR SERVO OUTPUT ON DIGITAL PINS 22 to 29
      else if(sPin >= 22 and sPIN <= 29)
      {
      pPort = &PORTA;
      }

      return pPort;
      }

      // Easy to optimise this, but lets keep it readable instead, its short enough.
      uint8_t CRCArduinoFastServos::getPortPinMaskFromPin(uint8_t sPin)
      {
      uint8_t sPortPinMask = RC_CHANNELS_NOPIN;

      if(sPin <= A5)
      {
      if(sPin <= 7)
      {
      sPortPinMask = (1 << sPin);
      }
      else if(sPin <= 13)
      {
      sPin -= 8;
      sPortPinMask = (1 << sPin);
      }
      else if(sPin <= A5)
      {
      sPin -= A0;
      sPortPinMask = (1 << sPin);
      }
      // QUICK AND DIRTY HACK TO TEST MEGA SUPPORT FOR SERVO OUTPUT ON DIGITAL PINS 22 to 29
      else if(sPin >= 22 and sPIN <= 29)
      {
      sPin -= 22;
      sPortPinMask = (1 << sPin);
      }
      }

      return sPortPinMask;
      }

      Let me know how you get on

      Duane.

      Delete
  77. No luck. I'm reading all the channels as usual, but no output to the servos. Note in your post above a typo - sPIN. I fixed it.

    On the Mega pins 22 thru 29 (PortA) are just straight digital pins - not PWM. Don't know if that matters here, but I think it does with the Arduino Servo lib.

    Anyway, I'm all set up here and can quickly try anything else you may want to try.

    ReplyDelete
  78. I dug out an Arduino Mega and got the library to work. You need to update 2 lines in the begin function within the cpp file as follows -

    // ENABLE TIMER1 OCR1A INTERRUPT to enabled the first bank (A) of ten servos
    TIFR1 = 2; //|= _BV(OCF1A); // clear any pending interrupts;
    TIMSK1 = 2;//_BV(OCIE1A);

    Let me know how you get on with this change in place.

    Duane.

    ReplyDelete
    Replies
    1. Still no luck. I read the inputs fine of pins 10 thru 12, which are pcint pins on the Mega. I've left the "Quick and dirty Hack" code in place. I assume then I can go to any pins 22 thru 29 for my PWM outputs. I'm using the 3 servo frame setup code in the sketch - I just don't physically attach a servo to aux output(channel3) because I'm not using one. But I think that's ok.

      I guess the other thing I should clarify is I'm assuming if you are only driving two outputs connected to servos, that I don't need the external counter IC. That's just there for lots of servos. I think.

      I'm baffled.

      Delete
  79. I modified the library to suit the Mega as you mentioned in your two previous comments and it's still not working

    ReplyDelete
  80. Mistake is on my part. I have a loop back test up and running with an ArduinoMega right now. To get the hack to work, you need to remove the word 'else' in the two functions as follows - if(sPin >= 22 and sPIN <= 29) with no else before it. Remember that you need to do this for both functions. I missed that A5 has a different value on the Mega than the UNO and this was preventing the Mega hack from running.

    I will tidy the library up to work on the Mega properly now that I have found a Mega to use.

    In the meantime let me know if the hack works for you.

    Duane.

    ReplyDelete
    Replies
    1. Getting closer!

      With these pin assignments

      // Assign your channel in pins
      #define THROTTLE_IN_PIN 10
      #define STEERING_IN_PIN 12
      #define AUX_IN_PIN 11

      // Assign your channel out pins
      #define THROTTLE_OUT_PIN 22
      #define STEERING_OUT_PIN 28
      #define AUX_OUT_PIN 24

      I still get no ESC or steering servo, but when I press my channel 3 button, I can make may my steering toggle full right or full left.

      Delete
  81. If you put a servo on the ESC or Steering outputs, does it resist if you try to turn it with your fingers - if so it is being driven, if not and you can turn it with your fingers it is not being driven. Let me know which you get

    Duane

    ReplyDelete
    Replies
    1. When using a separate servo on pin 22, I get holding torque, but no radio control of rotation.

      Nothing at all on pins 24 or 28.

      Delete
    2. Try this - https://drive.google.com/file/d/0B8WjAvuO3LnIU09ncTJIV0c3NFk/view?usp=sharing

      Duane

      Delete
    3. It works! Tried a small servo on pins 22, 23, 24 and got correct radio control. Then tried using Mega pins 8 and 9 for outputs (I have a wiring harness and connector using these on my vehicle) and they worked too!

      Fantastic. So now I'll integrate this into my code and see if I can speed up my PID Nav controller some more. Thx for all the help.

      Delete
    4. Hello Duane,

      Thought you might like to see your lib in action. Here's a vid of my autonomous monster truck. I set up a GPS waypoint "race track" and Rover races around it using his PID Nav controller. Then I can take over with radio control whenever I want, too.

      http://1drv.ms/1wI5nDV

      Delete
    5. Great project, if you have a page with more details, I will add a link somewhere on RCArduino, if not, no worries and thanks for sharing, always nice to see something in action. Duane.

      Delete
    6. Thx. I don't have any plans for a detailed webpage, but I'll remember the link offer in case I decide to make one. Thx again for your help.

      Delete
  82. Its something trivial, I am out of the country travelling for a few days but will have a look on Sunday.

    Duane.

    ReplyDelete
  83. Dear Duane
    Thank you very much for your codes, your work is very much appreciated.
    I am using an arduino micro on my rc helicopter project and I am having some problems at the moment.
    I read in one of your other articles, that the micro only supports pinchangeint on pins 8,9,10,11, therefore I connected all my inputs to these.
    Printing the input signals in the serial shows that the arduino reads all the channels correctly, so this part is fine.
    I routed the inputs to pin 4,5,6,7 outputs without any processing at this stage. A servo connected to pin 4 works correctly, it follows the input from pin 8.
    However servos connected to pins 5,6,7 are not moving at all,not following inputs 9,10,11.
    Strangely if I connect a servo to pin 12, it follows pin 10 input.

    Using one of your similar codes (the one with servo.h) this setup works perfectly.
    I also modified the RcarduinoFastLib.h to get 5 input channels ( 4+1 servo frame).
    Can you please help me?
    Thank you very much for your work in advance,
    Gergely

    ReplyDelete
  84. Hi,
    The library is designed for the Arduino UNO which is based on an ATMega328 chip, the Micro is based on a different chip, the ATMega32u4. The two chips are very similar which is what allows both to work as Arduino's however there are some differences and a lot of code which hides these differences from the end user. One of the differences which is hidden by Arduino is that the pin mappings are different, this is why you see your outputs in unexpected places when using the library.

    ATMega328 Pin Mapping as used in UNO and Nano
    http://arduino.cc/en/Hacking/PinMapping168

    ATMega32u4 Pin Mapping as used in Leonardo and Micro
    http://arduino.cc/en/Hacking/PinMapping32u4

    If you look at the 328 pin mapping you will see that digital pin 6 of an Arduino UNO is PORTD bit 6 PD6.
    If you look at the 32u4 pin mapping you will see that PD6 is digital pin 12 which is why you see the output you expected from pin 6 is actually generated on pin 12.

    To help users use the library for different models of Arduino I have two choices

    1) Write lots of additional code to deal with pin mappings of the different ATMega Chips and detect which chip is being used

    2) Get the users to do a little work and specify the port and bit which they want their output on in a new version of the attach function.

    I should have something up and running later today.

    Duane

    ReplyDelete
  85. I have shared a sketch with an updated library that 'should' support all major AVR Arduinos (UNO, Leonardo, Mega, Mini, Micro, Nano, maybe others). I do not have access to all Arduinos to test, I have tested on the Mega. To test on any other you would need to update the input and output pins to reflect the model of Arduino you are using, pay particular attention to the pin change interrupt pins for your device, details here - http://rcarduino.blogspot.ae/2012/03/need-more-interrupts-to-read-more.html - I would also suggest that you be ready to disconnect quickly incase there is some timing difference between the models that I have not anticipated. Otherwise, enjoy -

    https://drive.google.com/file/d/0B8WjAvuO3LnIU09ncTJIV0c3NFk/view?usp=sharing

    Duane B

    ReplyDelete
  86. Thank you very much for your help!
    I managed to get it working just by selecting different pins!
    I found that an Arduino Micro can work with 4 channels. Inputs should be connected to pins 8,9,10,11. The servos should be connected to pins 0(TX) 1(RX) 4 12. In the program however you need to define different pins for output:
    To control servo on pin 0, in the complier select pin2 as output
    To control servo on pin 1, in the complier select pin3 as output
    To control servo on pin4, in the complier select pin4 as output
    To control servo on pin12, in the complier select pin6 as output.

    Thanks again for the advice!
    Best regards,
    Gergely

    ReplyDelete
  87. If you try the sketch in the link, you should be able to get the output on whatever pin you select without having to figure out the mappings.

    https://drive.google.com/file/d/0B8WjAvuO3LnIU09ncTJIV0c3NFk/view?usp=sharing

    Duane.

    ReplyDelete
  88. Hi there,

    This code is great and works perfectly. I am now trying to use timmer 1 with a ovf interrupt to accurately ovf every Ms. I have followed your idea of short intterupts by raising a flag. When I introduce this, the results of the reading the rx signals change dramatically and give random value. My timer1 ovf does not seem to fire every 1ms. If I have them as two separate pices of code and run them they function perfectly.
    Any advice you can provide.

    Thanks

    K

    ReplyDelete
  89. The code directly accesses timer 1 to generate the output servo signals. It also uses timer1 to measure the incoming signals. Whereever you see TCNT1 in the code, thats directly accessing timer1 without the need for additional libraries.

    So my advice would be to use the working code as it is and not worry about changing it unless there is something specific that you need that it isn't already doing ?

    Duane B

    ReplyDelete
  90. Hi Duane B,

    Thanks for the reply.

    I am not using the output servo part of the code. I am only using the part to read the incoming PWM.

    Hence why timer1 is free, I think I read on another project that you used timer1 one for precise timing. ie an interrupt at a precise time.

    Outputting to my servos I am using timer 0 and 2 in WGM

    Appreciate any help you can give

    Thanks

    K

    ReplyDelete
    Replies
    1. Not sure what your trying to do but a couple of questions.

      1) Why do you need to interrupt every 1ms ?
      2) Why do you want to use the 8bit timers 0 and 2 for Servo signal generation where timer 1 is so much better for this purpose ?
      3) What is the working example given here not doing that you need to do in your project ?

      Duane.

      Delete
  91. Great job Duane! I found your library on Github, for anyone wanting it there, for easy reading and forking: https://github.com/scottjgibson/RCArduinoFastLib

    ReplyDelete
  92. Duane, it appears to me that with 18 servos, assuming all are maxed out at 2000us, that the frame period would be 2msx18 = 36ms. This would make the max frame rate for this case 1/0.036sec = 27.8Hz. Is this correct, or are you somehow able to maintain 50Hz even with all 18 servos at 2000us??? Even assuming all servos are neutral (1500us), the max frame rate would still be 1/.027sec = 37Hz. Again, am I understanding this correctly? Thanks! ~Gabriel, http://electricrcaircraftguy.blogspot.com/

    ReplyDelete
  93. Hi Gabriel, The library drives the servos using two separate timer channels, each channel has a maximum of nine servos and one optional frame space on the end so divide your maths by 2 and you will see that the frame rate varies from 50Hz and up.

    In AVR Terms, the timer channel is just a compare register, OCR1A drives one bank of servos and OCR1B drives the other.

    Duane.

    ReplyDelete
    Replies
    1. Ah, I see. Makes sense now. So 9 servos on a bank (on a single output compare) means max of 55.6Hz (1/.018sec) assuming they are all at 2000ms and we are not using the frame space. Got it. Does the core Arduino servo library only use one output compare then? I do see one minor problem with using 2: collision. One interrupt must occur before another, so it could induce a small error in one of the outputs. According to Nick Gammon (http://www.gammon.com.au/forum/?id=11488), under his section "How long does it take to execute an ISR?", it takes "2.9375 µS before entering your interrupt handler, and another 2.1875 µS after it returns." This means that the 2nd output if both try to trigger at once could be delayed by up to 5.125us.

      Thanks for the quick response by the way!

      Next question: When you initialize timer1 in your .cpp file, you write:
      "TCCR1B = 2; // set prescaler of 64 = 1 tick = 4us".
      Shouldn't your comment say "set prescaler of 8 = 1 tick = 0.5us"?
      -I am referencing Atmega328 datasheet pg 134-135.

      The 4us resolution seemed too coarse to have good repeatability in the servo, without jitter, so I assumed you were using 0.5us resolution but then your comment confused me, so I checked the datasheet, which does seem to indicate you are using 0.5us resolution.

      Lastly, I see very little activity on your blog since 2012. Have you stopped messing with this Arduino/RC stuff, or have you simply stopped writing about it?

      I think you have a whole lot more to contribute still if you have the time and energy.

      Delete
    2. Ah, I see Randy Perkins above already caught your comment error about 4us vs 0.5us, back in 2012.....

      Delete
    3. Nick knows what he is talking about although the issue is not as big as you think. If you want to use 9 or less servos, there is no need to enable the second bank in which case there will be no clash between banks, the second bank is off by default.

      If you do need additional servos, the time it takes to enter and leave an ISR is about half the number you have referenced, so an occasional 1 to 1.5% error in the output signal if your using more than 9 servos.

      The standard servo library will get you to 12 servos without clashing provided you are not using any other interrupts - for example for reading incoming RC Signals. The reason for me creating this library is that the standard servo library is horribly inefficient internally which leads to large glitches and frequent jittering when you try to use it alongside interrupts for reading incoming signals. It would be easy to extend the library to 12 servos on a single channel but most people are building projects with 8 or less.

      Duane.

      Delete
    4. Duane, thanks for the response. I have seen when using the servo library with only 1 servo that the frequency is right on 50Hz. They must have hard-coded the frame space at the end in to achieve a fixed freq. Can I assume, however, that if I was using 12 servos, all at 2000us, on the core Arduino library, that the freq would be only 41.7Hz? I'll have to try it when I get the chance....

      Also, I didn't know that the Arduino core servo library did not do buffers after *each* servo, to keep each channel at an exact fixed freq, but it seems that is the case according to your info here. That's good to know. Understanding how these servo libraries works has been enlightening. I thought servos and ESCs would be more sensitive to frequency than what you are indicating.

      Delete
  94. "Can I assume, however, that if I was using 12 servos, all at 2000us, on the core Arduino library, that the freq would be only 41.7Hz? I'll have to try it when I get the chance...." Yes, the frequency would drop.

    The standard servo library has a single buffer at the end to fix the frequency, unless you go over 2000us in which case its ignored. I have not added this as I have not seen any need for it however if someone wanted to add a helper function to update the framespace whenever a servo output is changed it would be trivial, but why write, run and maintain code that noone needs.

    Duane

    ReplyDelete
  95. I cant get this to work. I'm trying to display the milliseconds on the serial monitor. I do Serial.println(unThrottleIn) and i keep getting 0 regardless. What am I doing wrong. Could it because I'm using a mega 2560?

    ReplyDelete
    Replies
    1. Yes, it is because you are using a Mega, however I did add support for the Mega in a later version. I will find the link and post it shortly.

      Duane.

      Delete
    2. Code is here - https://drive.google.com/file/d/0B8WjAvuO3LnIU09ncTJIV0c3NFk/view?usp=sharing

      You will need to read the code to find out which pins the inputs and outputs are connected to, its not the same as the UNO version.

      Duane.

      Delete
    3. Hello Duane,

      I'm using the code for the Mega, I can read the signals, but can't steer any servo or ecu. I connected reciever to pin 10 an the servo/eco to pin 22. Where is my fault?

      Greetings Thomas

      Delete
  96. Hello Duane, if you are still listening.

    I've got a GPS/Compass autonomous RC truck that uses your mega version of Fastlib. (I worked with you above on the mega lib mod) It's working fine, but I look at ate big clunky Mega, and my needs being mainly in com (more hardware serial than the one we get in Uno/Nano) I can't help thinking the Mega is mostly useful for attracting a porcupine mate with all its 50 some I/O headers and I am wondering if I could go Nano.

    Listing what the app needs:

    I want to be able to use the one UART for serial monitor debugging

    I have a serial (UART) GPS
    I have a I2C compass

    Flash data logging thru SPI (but very intermittent)

    Currently, I have some buttons using a external interrupt pin (one to get me into a setup menu - then no other code is running)

    I am thinking of going to a Android tablet to replace all my pots and PBs using a lib called Pfod which allows you to create a control menu in Android and control Arduino thru a Bluetooth connection. Need a serial port for that.

    The obvious solution is to use NewSoftwareSerial to get the needed serial RxTx pins.

    I have serious doubts that I can use NewSoftwareSerial with Fastlib however, because I suspect interrupt conflicts.

    Is that so, and if so, any suggestions?

    ReplyDelete
  97. Hi, I am using this library and it works really well for me, thank you very much!

    However, I have been unable to set the servo times to beyond the 1000-2000 µs range. I have set RCARDUINO_SERIAL_SERVO_MIN to 600 and RCARDUINO_SERIAL_SERVO_MAX to 2400. I know my servos can do it, but they don't reach the 600 or 2400 position.
    If I measure the output of the Arduino Nano with a second Nano, I only get the usual range 1000-2000.
    Obviously, I am missing something? The framespace and associated timings maybe?
    Can someone please help me? I'd be very grateful!

    Thank you,
    Jens (sorry, I do not know why my name does not show up, I thought I logged in with google+?!)

    ReplyDelete
    Replies
    1. okay, so here is what happened:

      1) I put the library into c:\programmes\arduino\libraries but Arduino put the library into my c:\users\USERNAME\Arduino\libraries folder. Because of other problems, I have switched to the \users directory now. The new Arduino versions tell you which library they use if there are duplicate entries - mine didn't. Also, if you are wondering, one trick is to enter a syntax error into a library file. If you do not get a build error, you know Arduino is not using the file you thought it would.

      2) after fixing this, the servos were pulsed at 300Hz. My mistake was that I assumed RC_CHANNEL_OUT_COUNT would be the number of channels. It is actually number of channels PLUS 1. This is because the frame space is treated more or less like a servo pulse by the code. So you need to send out "number of channels" servo signals and one pause until the next signals. This IS explained on this page here: http://rcarduino.blogspot.co.uk/2013/02/rcarduino-libraries-faq.html - but not in the code.

      Everything works now, just wanted to update everybody :)

      Delete
  98. Duane,

    I am a beginner with coding, but I am trying to modify/build off of your previous codes. Previously when I attached the Servo objects they were set up as an array that I could loop through. I could also loop through the indices to "writeMicroSeconds(...)", but with the new library it seems like I cannot do this any more?

    What am I missing? Thank you for reading this!

    ReplyDelete
  99. I tried the code link for the Pro Micro but it compiles with errors regarding PORTA. If I comment it out it will compile but still doesn't map to the correct pins.

    ReplyDelete
  100. Can this be implemented using other times, for example OC2A and/or OC2B so that we don't loose PWM analogwrite on pins 9 and 10. This matters for preconfigured shields. I think this is a similar course that the servotimer2 library took: https://github.com/nabontra/ServoTimer2

    ReplyDelete
  101. I really got into this article. I found it to be interesting and loaded with unique points of interest. I like to read material that makes me think. Thank you for writing this great content.

    ReplyDelete
  102. Is it possible to use rcarduinofastlib on a ATTiny85 ? I just want to decode an incoming PPM signal and light leds depending on if the signal is low (sub 1500us) or high (> 1500us). Am I expecting to much from the board? Should I use a nano 3? Weight and size is critical for my project. Thank you.

    ReplyDelete
  103. Hi DuaneB, this is really great code. Thanks to you I managed to successfully finally read out more than 1 channel from the RC and control several servos in a smooth way. I spent before several days with interrupts and pinchangeint, as well as Servo.h, which somehow worked, but with *a lot of glitches* and unpredictable behaviour. Again, thx for this work, additionally also on the comparison of Uno, Mega etc. regarding interrupts.

    Best regards, Raoul

    ReplyDelete
  104. mcafee.com/activate registered trademarks, company names, product names and brand names are the property of their respective owners, and mcafee.com/activate disclaims any ownership in such third-party marks. The use of any third party trademarks, logos, or brand names is for informational purposes only, and does not imply an endorsement by mfmcafee.com or vice versa or that such trademark owner has authorized mfmcafee.com to promote its products or services.


    office.com/setup is an independent support and service provider for the most secure remote technical services for all Office products. Our independent support services offer an instant support for all software related errors in the devices, laptops, desktops and peripherals. We have no link or affiliation with any of the brand or third-party company as we independently offer support service for all the product errors you face while using the Office. If your product is under warranty, then you may also avail our support services for free from manufacturer’s official website office.com/setup.

    mcafee.com/activate is an independent support and service provider for the most secure remote technical services for all norton products. Our independent support services offer an instant support for all software related errors in the devices, laptops, desktops and peripherals. We have no link or affiliation with any of the brand or third-party company as we independently offer support service for all the product errors you face while using the norton. If your product is under warranty, then you may also avail our support services for free from manufacturer’s official website norton.com/setup.

    ReplyDelete
  105. mcafee.com/activate - McAfee Retail Card activation at www.mcafee.com/activate, enter your product key for Activate. Download & Install McAfee Antivirus Software.
    Visit norton.com/setup to install fee or paid version of the software. Know how to install norton setup and Norton core to secure your Wi-Fi network.
    office.com/setup - Download Setup, Redeem Product Key and Install Office on your computer from www.office.com/setup with Microsoft Account.

    ReplyDelete
  106. Hello, I'm Smith Machinist . If you are facing issues with any office product then you can visit my website.
    norton.com/setup | Office.com/setup | McAfee.com/activate

    ReplyDelete
  107. Welcome to the party of my life here you will learn everything about me. RC car steering

    ReplyDelete
  108. Working on Arduino is fun, and knowing about RC channels can be extra fun, Thankyou for sharing this wonderful article .
    mcafee.com/activate

    ReplyDelete
  109. thankyou for sharing this wonderful article it is really nice

    ReplyDelete
  110. You have excellent writing skills and the way you describing things is extraordinary, keep writing, here are some useful links:

    Vpn for mac
    android tv vpn
    apple tv vpn
    vpn download for windows 7
    affordable vpn

    ReplyDelete
  111. This is also a very good post which I really enjoy reading. It is not everyday that I have the possibility to see something like this. Feel free to visit my website;
    일본야동

    ReplyDelete
  112. I learned a lot in this blog and I thought I would continue to follow it, thank you very much for sharing this blog Useful information. Lucky me I found your web site by chance, and I’m shocked why this coincidence did not took place earlier! I bookmarked it. Feel free to visit my website; 국산야동

    ReplyDelete
  113. I’ve seen articles on these topics a few times, but I think your writing is the cleanest I’ve ever seen. I would like to refer to your article on my blog and write it. Feel free to visit my website; 일본야동

    ReplyDelete
  114. Of course, your article is good enough, 카지노사이트 but I thought it would be much better to see professional photos and videos together. There are articles and photos on these topics on my homepage, so please visit and share your opinions.


    ReplyDelete
  115. I’m not that much of a internet reader to be honest but your sites really nice, keep it up! I’ll go ahead and bookmark your site to come back in the future. All the best

    무료야설
    립카페
    마사지블루
    마사지
    카지노

    ReplyDelete
  116. Rcarduino: How To Read Rc Channels - The Rcarduinofastlib >>>>> Download Now

    >>>>> Download Full

    Rcarduino: How To Read Rc Channels - The Rcarduinofastlib >>>>> Download LINK

    >>>>> Download Now

    Rcarduino: How To Read Rc Channels - The Rcarduinofastlib >>>>> Download Full

    >>>>> Download LINK

    ReplyDelete
  117. LEGIT FULLZ & TOOLS STORE

    Hello to All !

    We are offering all types of tools & Fullz on discounted price.
    If you are in search of anything regarding fullz, tools, tutorials, Hack Pack, etc
    Feel Free to contact

    ***CONTACT 24/7***
    **Telegram > @leadsupplier
    **ICQ > 752822040
    **Skype > Peeterhacks
    **Wicker me > peeterhacks

    "SSN LEADS/FULLZ AVAILABLE"
    "TOOLS & TUTORIALS AVAILABLE FOR HACKING, SPAMMING,
    CARDING, CASHOUT, CLONING, SCRIPTING ETC"

    **************************************
    "Fresh Spammed SSN Fullz info included"
    >>SSN FULLZ with complete info
    >>CC With CVV (vbv & non vbv) Fullz USA
    >>FULLZ FOR SBA, PUA & TAX RETURN FILLING
    >>USA I.D Photos Front & Back
    >>High Credit Score fullz (700+ Scores)
    >>DL number, Employee Details, Bank Details Included
    >>Complete Premium Info with Relative Info

    ***************************************
    COMPLETE GUIDE FOR TUTORIALS & TOOLS

    "SPAMMING" "HACKING" "CARDING" "CASH OUT"
    "KALI LINUX" "BLOCKCHAIN BLUE PRINTS" "SCRIPTING"
    "FRAUD BIBLE"

    "TOOLS & TUTORIALS LIST"
    =>Ethical Hacking Ebooks, Tools & Tutorials
    =>Bitcoin Hacking
    =>Kali Linux
    =>Fraud Bible
    =>RAT
    =>Keylogger & Keystroke Logger
    =>Whatsapp Hacking & Hacked Version of Whatsapp
    =>Facebook & Google Hacking
    =>Bitcoin Flasher
    =>SQL Injector
    =>Premium Logs (PayPal/Amazon/Coinbase/Netflix/FedEx/Banks)
    =>Bitcoin Cracker
    =>SMTP Linux Root
    =>Shell Scripting
    =>DUMPS with pins track 1 and 2 with & without pin
    =>SMTP's, Safe Socks, Rdp's brute
    =>PHP mailer
    =>SMS Sender & Email Blaster
    =>Cpanel
    =>Server I.P's & Proxies
    =>Viruses & VPN's
    =>HQ Email Combo (Gmail, Yahoo, Hotmail, MSN, AOL, etc.)

    *Serious buyers will always welcome
    *Price will be reduce in bulk order
    *Discount offers will gives to serious buyers
    *Hope we do a great business together

    ===>Contact 24/7<===
    ==>Telegram > @leadsupplier
    ==>ICQ > 752822040
    ==>Skype > Peeterhacks
    ==>Wicker me > peeterhacks

    ReplyDelete
  118. สล็อต joker สล็อตโรม่า (ROMA SLOT) ถือว่าเป็นเกมสล็อตออนไลน์เกมหนึ่ง ที่ไม่ได้แตกต่างจากเกมสล็อตเกมอื่นๆจำนวนหลายชิ้น ที่ผิดแผกคงจะมีนิดเดียว ซึ่งมีแค่เพียงฟีเจอร์บางตัวใน สล็อตโรม่า เท่านั้น

    รีวิวเกม Streets Of Chicago การคำนวณรูปแบบเส้น จะแตกต่างกันออกไปตามรูปแบบของไลด์ชนะ ทดลองเล่น ได้แล้ววันนี้ ดูสล็อตมากมายเพิ่มเติมทุกเกมได้ที่ Joker123 หรือดูวิธีเล่นเกมอื่นอีกมากมายเพิ่มเติมที่ รีวิวเกมสล็อต

    ReplyDelete
  119. igoal Betting sites do not have to go anywhere. Here, one stop.

    ReplyDelete
  120. sa casino d I thought I would continue to follow it, than

    ReplyDelete
  121. naza สล็อต the best online gaming 24hours to win slot

    ReplyDelete
  122. คาสิโนสด Fast, accurate and very accurate deposit-withdrawal system

    ReplyDelete
  123. amb many rewards are waiting for you. take prize click to get money

    ReplyDelete
  124. Win bonuses all the time so you don't get bored, Press to play automatically เว็บ พนัน 123

    ReplyDelete
  125. ราคาบอล Applying for a slot formula is easy to apply. Open the user with no minimum ready to serve

    ReplyDelete
  126. ดูราคาบอล easy online gaming to get a lot of real money 24hours win

    ReplyDelete
  127. amb many rewards are waiting for you. get rewards & money to you

    ReplyDelete
  128. igoal88เครดิตฟรี Online football betting website That these gamblers use the service, that is Is an online football betting website The best in this era! There are many football betting options, including favorite ball, single ball, step ball, live ball.

    ReplyDelete
  129. เว็บนาซ่า
    Many games, easy to crack, pay for real, online gambling websites.

    ReplyDelete
  130. naza thai
    Asia's number 1 casino Stable, deposit, withdraw, automatic system 24 hours. The easiest bonus.

    ReplyDelete
  131. Mizoram University is a prominent higher education institution located in Aizawl, the capital city of Mizoram, India. Established in 2001, it serves as the central university for the state and plays a crucial role in providing quality education and research opportunities to students in the region. Mizoram University offers a wide range of undergraduate, postgraduate, and doctoral programs in various disciplines, contributing to the academic and socio-economic development of Mizoram and the northeastern region of India. The university is known for its commitment to excellence in education and research, as well as its efforts to promote and preserve the rich cultural heritage of Mizoram and the surrounding areas.

    ReplyDelete