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

Multiplexing RC Channels with Arduino

RCArduinoChannels by DuaneB is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License.

RC Transmitters multiplex as many as 9 channels onto a single radio signal. Your RC Receiver decodes this single signal to obtain the individual channel signals for your servos and electronic speed controllers.

If we reverse part of this process we can read all of the incoming channels using just two Arduino Pins.

Not only does this keep our pins free for more important tasks within our projects, but it is also more efficient to read them this way, we simply need to count the pulses to know which channel we are looking at.

This is a technique I have been using in the development of the RCArduino library.


The library uses just two interrupt pins to read as many as six RC Channels. The library is also able to generate upto six RC Channel outputs in its current form. I will soon be adding the capability to drive 9 servos using only 3 Arduino digital pins. 

UPDATE : The library has been reworked so that it now supports three output modes -

1) Generate six outputs using a selected port 

2) Do not generate any outputs, but operate in a mode compatible with the existing servo library and all of the flexibility that it offers - also suitable for working with H-Bridges etc.

3) Serial servos, use less pins and less code to drive more servos - details soon.

So, if you want to be able to do more, with less, with a library that is smaller and faster than the existing general purpose libraries, read on.

The RC Arduino Channel Multiplexer
Some high end receivers offer a 'raw' output, mine don't and yours don't need to either. Instead of modifying the receiver we will simply create a small hardware 'RC Channel multiplexer'. The multiplexer is basically a single diode for each channel.

The key to the multiplexer is that we route all of the even numbered channels 0,2,4 to INT0 (digital pin 2) and all of the odd numbered channels 1,3,5 to INT1 (digital pin 3).

How does it work ?The diodes stop interference between the pulses - if one channel is high and the others are low, the low channels will sink some of the current from the high channel, this would be sufficient to stop the Arduino from detecting the pulse. However adding a diode to each channel prevents any current from the high channel sinking back into the low channels and ensures we can detect all of the channel pulses.

An RC Arduino Channel Multiplexer - 3 Channels into two pins, as simple as that.

Multiplexing the receiver channels into the two Arduino interrupts is as simple as adding a diode to each channel before connecting it to INT0 for 0 and even numbered channels and INT1 for odd numbered channels.

There is an interesting effect where a charge gets trapped between the diodes and the Arduino pins. To overcome this effect a high value resistor can be place between the point where the diodes meet and ground. I am using a 1M resistor. Picture below -.

Schematic - The even half of the channel multiplexer, the odd half is a duplicate but is connected between the odd numbered channels and INT1 (digital pin 3).

The next post in this series will be a conversion of a previous project to use the new library, as always the post will include the full source code of both the library and the project.

Duane B

Never Say Never - The RC Arduino Library

I always told myself I would never write an Arduino library but after looking at the assembly code of some of my recent projects I have changed my mind.

Most of my projects involve reading and writing RC Signals, these are both time critical activities, the difference between full brakes and full throttle is only one thousandth of a second.

Existing solutions such as the code I have posted previously are perfectly good, but they are based on general purpose libraries that sacrifice performance and accuracy for flexibility.

Examples using general purpose libraries -
Servo Library
http://rcarduino.blogspot.com/2012/01/can-i-control-more-than-x-servos-with.html

PinChangeInt library
http://rcarduino.blogspot.com/2012/03/need-more-interrupts-to-read-more.html
http://rcarduino.blogspot.com/2012/04/how-to-read-multiple-rc-channels-draft.html


I am in the testing stages with a dedicated RC Library which sacrifices some flexibility for a big improvement in accuracy, size and performance.

I have a the perfect test bed for the library in my existing projects -

The L293D RC Robot
http://rcarduino.blogspot.com/2012/05/rc-arduino-robot.html

RC Race Car Child Mode
http://rcarduino.blogspot.com/2012/01/traction-control-part-13-we-have.html

Active Yaw Control
http://rcarduino.blogspot.com/2012/07/rcarduino-yaw-control-part-2.html

By converting each project to use the new library I can demonstrate the performance, ease of use and reliability of the library. I also plan to include the existing projects as samples within the library download.

Bench testing is complete, road testing starts tomorrow.

Duane B

RCArduino Yaw Control - Part 2

 

RCArduinoYawControl by DuaneB is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License.
Based on a work at rcarduino.blogspot.com.

The Project Pitch - 

Do you have an RC Car thats just too unpredictable to really enjoy ?

Are you a beginner driver in the rear wheel drive classes and could use some help ?

Do you race in a class where electronic assistance is allowed ?

Existing solutions are not helping you as they fail to address the root cause of RC Car instability. 

Race orientated RC Cars are powerful enough to reach scale speeds of 600 miles per hour. Stability solutions currently available try to address this excess power by actively steering into skids however this approach cannot hope to match up to the vast amounts of power resulting in frustration, broken cars and lost interest.

The RCArduino solution is the first complete solution to RC Car yaw control and provides the perfect introduction to driving the fastest and most challenging RC Race cars -

 - Learn to drive high powered RC Cars using the RCArduino adjustable assistance system.

 - Bring the fun back into high powered RC

 - Reduce the sensitivity as your skill level increases.

 - Avoid unecessary crashes and breakage while learning

40Km/h Arduino -Passing The Controls Over To Get Feedback From Other Track users

The Project

In Yaw Control Part 1 I discussed how gyroscopes can be applied in Radio Controlled cars to reduce yaw under braking and acceleration.

Since this first post I have developed an Arduino based yaw control system which goes beyond the original goals to address the major flaw in the gyro based approach.

Anti-Yaw Control Part 1 -  A Flawed Approach

http://rcarduino.blogspot.com/2011/12/anti-yaw-control-part-1.html

Background - Why do we need Yaw Control ?

Radio controlled cars have many times the power to weight ratio of conventional cars. In front wheel drive and four wheel drive RC Cars this can be seen in the form of wheel spin and power slides. Few of the rear wheel drive RC Car designs are able to cope with this excess power resulting in a car that spins out under acceleration. All of my RWD Cars - Tamiya M04, F103GT, 3Racing F109 and Sand Scorcher will spin out under modest acceleration.

A similar problem exists with braking, in a RWD RC Car, the motor provides braking, but as the motor is only attached to the rear wheels the effect is similar to pulling the handbrake (e-brake) in a road car. Great for j-turns, but its not going to get you around the track very quickly.

The original and flawed concept

My original design was based on an established idea which is to use a RC Helicopter gyroscope to counter steer into a skid before it can develop into a spin. These gyroscopes are readily available and are used in RC Helicopters to automatically increase or decrease the tail rotor speed to compensate for changes in the main rotor speed which would otherwise cause the helicopter to rotate. This type of gyro is known as a rate gyro.

Rate Gryo Fitted To My F103GT RWD RC Race Car

To use a rate gyro in a car, the gyro is connected between the receiver and the steering servo. The gyro listens to the incoming steering signal and generates an output based on this and the rate of rotation. When the model is accelerating or braking with no steering input, the gyro will detect the car starting to rotate and automatically signal the steering servo to steer into the skid and bring it under control.



Sounds good, whats the problem ?

If we go back to the statement under 'Why do we need Yaw Control' -

'Radio controlled cars have many times the power to weight ratio of conventional cars.'

It is fundamentally a power problem. Attempting to address the problem through steering alone will only get us so far.

So what is the solution ?

The solution is to take active control of both steering and throttle channels,  this way we can address the original cause of the problem (reduce the excess power) and the resulting effect (steer into the slide before it becomes a spin).

Unfortunately RC Gyros are primarily designed for use in helicopters and so while we can use them to add active counter steer to our cars, they are not able to interface with the throttle channel and are therefore unable to address the root cause of our problem.

Fortunately there is a interesting micro controller project to build this dual channel active control system.

The RCArduino Solution

Note : For a background on the techniques used to read the RC Receiver, Output the RC Signal and Calibrate with the RC Transmitter refer to the previous RCArduino posts listed below -


http://rcarduino.blogspot.com/2012/01/how-to-read-rc-receiver-with.html

http://rcarduino.blogspot.com/2012/04/how-to-read-multiple-rc-channels-draft.html

http://rcarduino.blogspot.com/2012/05/interfacing-rc-channels-to-l293d-motor.html

The RCArduino approach to yaw control implements tuneable control of both the throttle and steering channels. Separate adjustments are provided for each channel ranging from no intervention to very aggressive intervention.

Features -

 - One touch calibration
 - Fully and independently adjustable throttle intervention
 - Fully and independently adjustable steering  intervention

 - No need to cut or otherwise alter existing connections to your car electronics
 - Fail safe mode
 - Unique software specifically designed for high powered RC Cars

Version 0.1 - Big, but easy to work on.

How does it work ?

The unit reads the incoming signals and generates corresponding output signals. When the unit detects the car rotating it will calculate an intervention component for the output to reduce the rotation and allow the driver to retain control. The degree of intervention is adjustable from no intervention to very aggressive.

A further adjustment is provided to control the 'throttle intervention recovery period', when this is set to a low value throttle intervention is applied something like and on/off switch, with a higher value throttle intervention fades away over a period of upto 2 seconds providing for a gradual transition from full intervention to full throttle.

The intervention recovery period provides for a tuneable degree of drift. With a long recovery period the car will be very stable through the corner and onto the straight. With a minimal recovery period the throttle channel will be continually 'blipped' throughout the corner and a moderate drift can be maintained. With low recovery periods, transistions can be very aggressive, as soon as the system detects that rotation has stopped it will apply full power, this can happen in the transistion of an S bend or when exiting a corner onto the straight. This is a useful tuning technique for learning where a car can be driven hard and which points on the circuit require a more restrained approach.

Whats coming in version 1.0 ?

Version 1.0 is smaller and adds LED level meters which display -
1) The rotation rate being reported by the gyro
2) The level of steering intervention currently being applied
3)  The level of throttle intervention being applied
4) An additional tuning option on the steering channel
5) An additional tuning option for braking

Next Steps -

The system has been road tested on various surfaces and tyre combinations for around 10 hours without any unpredictable behaviour, the next step is to change the motor from the current entry level silver can motor (<>15,000 RPM) to a Sport Tuned Motor (<>22,000 RPM).

Version 0.1 Code - Lots of obvious optimization needed and lacking features being tested in 1.0, but in the interest of sharing, here it is - 

#include <Servo.h>
#include <EEPROM.h>

// RCArduinoYawControl
//
// RCArduinoYawControl by DuaneB is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License.
// Based on a work at rcarduino.blogspot.com.
//
// rcarduino.blogspot.com
//
// A simple approach for reading two RC Channels from a hobby quality receiver
// and outputting to the common motor driver IC the L293D to drive a tracked vehicle
//
// We will use the Arduino to mix the channels to give car like steering using a standard two stick
// or pistol grip transmitter. The Aux channel will be used to switch and optional momentum mode on and off
//
// See related posts -
//
// Reading an RC Receiver - What does this signal look like and how do we read it -
// http://rcarduino.blogspot.co.uk/2012/01/how-to-read-rc-receiver-with.html
//
// The Arduino library only supports two interrupts, the Arduino pinChangeInt Library supports more than 20 -
// http://rcarduino.blogspot.co.uk/2012/03/need-more-interrupts-to-read-more.html
//
// The Arduino Servo Library supports upto 12 Servos on a single Arduino, read all about it here -
// http://rcarduino.blogspot.co.uk/2012/01/can-i-control-more-than-x-servos-with.html
//
// The wrong and then the right way to connect servos to Arduino
// http://rcarduino.blogspot.com/2012/04/servo-problems-with-arduino-part-1.html
// http://rcarduino.blogspot.com/2012/04/servo-problems-part-2-demonstration.html
//
// Using pinChangeInt library and Servo library to read three RC Channels and drive 3 RC outputs (mix of Servos and ESCs)
// http://rcarduino.blogspot.com/2012/04/how-to-read-multiple-rc-channels-draft.html
//
// rcarduino.blogspot.com
//

// Two channels in
// Two channels out
// One program button
// One throttle intervention LED
// One Steering Intervention LED
// One Steering Sensitivity POT
// One Throttle Sensitivity POT
// One decay/damping POT

#define RC_NEUTRAL 1500
#define RC_MAX 2000
#define RC_MIN 1000
#define RC_DEADBAND 1

#define ROTATION_CENTER 380

uint16_t unSteeringMin = RC_MIN;
uint16_t unSteeringMax = RC_MAX;
uint16_t unSteeringCenter = RC_NEUTRAL;

uint16_t unThrottleMin = RC_MIN;
uint16_t unThrottleMax = RC_MAX;
uint16_t unThrottleCenter = RC_NEUTRAL;

uint16_t unRotationCenter = ROTATION_CENTER; // the gyro I am using outputs a center voltage of <> 1.24 volts
// full range is 0 to 2*1.24 = 2.48
// Using the Arduino voltage reference of 3.3 volts gives a center point of (1.24/(3.3/1024)) = 384
// In tests I see 380 so I will use 380 as my default value.

//////////////////////////////////////////////////////////////////
// PIN ASSIGNMENTS
//////////////////////////////////////////////////////////////////
// ANALOG PINS
//////////////////////////////////////////////////////////////////
#define THROTTLE_SENSITIVITY_PIN 0
#define STEERING_SENSITIVITY_PIN 1
#define THROTTLE_DECAY_PIN 2
#define STEERING_DECAY_PIN 3
#define GYRO_PIN 5
//////////////////////////////////////////////////////////////////
// DIGITAL PINS
//////////////////////////////////////////////////////////////////
#define PROGRAM_PIN 9
#define INFORMATION_INDICATOR_PIN 5
#define ERROR_INDICATOR_PIN 6
#define THROTTLE_IN_PIN 2
#define STEERING_IN_PIN 3
#define THROTTLE_OUT_PIN 8
#define STEERING_OUT_PIN 7

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

// 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;

// 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
uint32_t ulThrottleStart;
uint32_t ulSteeringStart;

// used to ensure we are getting regular throttle signals
uint32_t ulLastThrottleIn;

#define MODE_FORCEPROGRAM 0
#define MODE_RUN 1
#define MODE_QUICK_PROGRAM 2
#define MODE_FULL_PROGRAM 3
#define MODE_ERROR 4

uint8_t gMode = MODE_RUN;
uint32_t ulProgramModeExitTime = 0;

// Index into the EEPROM Storage assuming a 0 based array of uint16_t
// Data to be stored low byte, high byte
#define EEPROM_INDEX_STEERING_MIN 0
#define EEPROM_INDEX_STEERING_MAX 1
#define EEPROM_INDEX_STEERING_CENTER 2
#define EEPROM_INDEX_THROTTLE_MIN 3
#define EEPROM_INDEX_THROTTLE_MAX 4
#define EEPROM_INDEX_THROTTLE_CENTER 5
#define EEPROM_INDEX_ROTATION_CENTER 6

Servo servoThrottle;
Servo servoSteering;

uint16_t unThrottleSensitivity = 0;
uint16_t unSteeringSensitivity = 0;
uint16_t unThrottleDecay = 0;
uint16_t unSteeringDecay = 0;

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

  pinMode(PROGRAM_PIN,INPUT);
  pinMode(INFORMATION_INDICATOR_PIN,OUTPUT);
  pinMode(ERROR_INDICATOR_PIN,OUTPUT);

  attachInterrupt(0 /* INT0 = THROTTLE_IN_PIN */,calcThrottle,CHANGE);
  attachInterrupt(1 /* INT1 = STEERING_IN_PIN */,calcSteering,CHANGE);

  readAnalogSettings();
 
  servoThrottle.attach(THROTTLE_OUT_PIN);
  servoSteering.attach(STEERING_OUT_PIN);
   
  if(false == readSettingsFromEEPROM())
  {
    gMode = MODE_FORCEPROGRAM;
  }
 
  ulLastThrottleIn = millis();
}

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;
  // local copy of rotation rate
  static uint16_t unRotation;
  // local copy of update flags
  static uint8_t bUpdateFlags;

  static uint16_t unThrottleInterventionPeak;
  static uint16_t unSteeringInterventionPeak;
 

  uint32_t ulMillis = millis();
 
  // 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;
    }

    // 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 :-)
  }

  if(false == digitalRead(PROGRAM_PIN) && gMode != MODE_FULL_PROGRAM)
  {
    // give 10 seconds to program
    gMode = MODE_QUICK_PROGRAM;
 
    // turn indicators off for QUICK PROGRAM mode
    digitalWrite(INFORMATION_INDICATOR_PIN,LOW);   
    digitalWrite(ERROR_INDICATOR_PIN,LOW);   

    // wait two seconds and test program pin again
    // if pin if still held low, enter full program mode
    // otherwise read sensitivity pots and return to run
    // mode with new sensitivity readings.
    delay(2000);
   
    if(false == digitalRead(PROGRAM_PIN))
    {
      gMode = MODE_FULL_PROGRAM;
      ulProgramModeExitTime = ulMillis + 10000;
      
      unThrottleMin = RC_NEUTRAL;
      unThrottleMax = RC_NEUTRAL;
      unSteeringMin = RC_NEUTRAL;
      unSteeringMax = RC_NEUTRAL;
   
      unThrottleCenter = unThrottleIn;
      unSteeringCenter = unSteeringIn;
    }
    else
    {
      gMode = MODE_RUN; 
      ulMillis = millis();   
      ulLastThrottleIn = ulMillis;
    }
   
    // Take new sensitivity and decay readings for quick program and full program modes here
    readAnalogSettings();
  }
 
  if(gMode == MODE_FULL_PROGRAM)
  {
   if(ulProgramModeExitTime < ulMillis)
   {
     // set to 0 to exit program mode
     ulProgramModeExitTime = 0;
     gMode = MODE_RUN;

     analogRead(GYRO_PIN);
     uint32_t ulTotal = 0;
     for(int nCount = 0;nCount < 50;nCount++)
     {
       ulTotal += analogRead(GYRO_PIN);
     }
     unRotationCenter = ulTotal/50;

     writeSettingsToEEPROM();
    
     ulLastThrottleIn = ulMillis;
   }
   else
   {
     if(unThrottleIn > unThrottleMax && unThrottleIn <= RC_MAX)
     {
       unThrottleMax = unThrottleIn;
     }
     else if(unThrottleIn < unThrottleMin && unThrottleIn >= RC_MIN)
     {
       unThrottleMin = unThrottleIn;
     }
    
     if(unSteeringIn > unSteeringMax && unSteeringIn <= RC_MAX)
     {
       unSteeringMax = unSteeringIn;
     }
     else if(unSteeringIn < unSteeringMin && unSteeringIn >= RC_MIN)
     {
       unSteeringMin = unSteeringIn;
     }
   }
  }
  else if(gMode == MODE_RUN)
  {
    if((ulLastThrottleIn + 500) < ulMillis)
    {
      gMode = MODE_ERROR;
    }
    else
    {
      // 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)
      {
       unRotation = analogRead(GYRO_PIN);
      }

      if(bUpdateFlags & THROTTLE_FLAG)
      {
        ulLastThrottleIn = ulMillis;
       
        // A good idea would be to check the before and after value,
        // if they are not equal we are receiving out of range signals
        // this could be an error, interference or a transmitter setting change
        // in any case its a good idea to at least flag it to the user somehow
        uint16_t unThrottleIntervention = 0;
       
        if(unRotation != unRotationCenter)
        {
          uint32_t ulRotationWithGain = 0;
          if(unRotation > unRotationCenter)
          {
           ulRotationWithGain = (long)(unRotation - unRotationCenter)*unThrottleSensitivity;
           unThrottleIntervention = map(ulRotationWithGain,0,(long)unRotationCenter*128L,0,500);
          }
          else
          {
           ulRotationWithGain = (long)(unRotationCenter - unRotation)*unThrottleSensitivity;
           unThrottleIntervention = map(ulRotationWithGain,0,(long)unRotationCenter*128L,0,500);
          }
        }

        if(unThrottleIntervention > unThrottleInterventionPeak)
        {
          unThrottleInterventionPeak = unThrottleIntervention;
        }
        else
        {
          if(unThrottleInterventionPeak >= unThrottleDecay)
          {
            unThrottleInterventionPeak -= unThrottleDecay;
          }
          else
          {
            unThrottleInterventionPeak = 0;
          }
         
          if(unThrottleIntervention < unThrottleInterventionPeak)
          {
            unThrottleIntervention = unThrottleInterventionPeak;
          }
        }
       
        if(unThrottleIn >= unThrottleCenter)
        {
          unThrottleIn = constrain(unThrottleIn - unThrottleIntervention,unThrottleCenter,unThrottleIn);
        }
        else
        {
          unThrottleIn = constrain(unThrottleIn + unThrottleIntervention,unThrottleMin,unThrottleCenter);
        }

        servoThrottle.writeMicroseconds(unThrottleIn);
      }
    }
   
    if(bUpdateFlags & STEERING_FLAG)
    {
      uint16_t unSteeringIntervention = 0;
      uint8_t bInvert = false;
       
      if(unRotation != unRotationCenter)
      {
        uint32_t ulRotationWithGain = 0;
       
        // note steering offers gain from 0 to 4
        uint16_t unInterventionMaxMinusInput = 0;
        if(unSteeringIn >= unSteeringCenter)
        {
          unInterventionMaxMinusInput = (unSteeringMax-unSteeringCenter) - (unSteeringIn - unSteeringCenter);
          unInterventionMaxMinusInput = constrain(unInterventionMaxMinusInput,0,unSteeringMax-unSteeringCenter);
        }
        else
        {
          unInterventionMaxMinusInput = (unSteeringCenter - unSteeringMin) - (unSteeringCenter - unSteeringIn);
          unInterventionMaxMinusInput = constrain(unInterventionMaxMinusInput,0,unSteeringCenter - unSteeringIn);
        }
      
        if(unRotation > unRotationCenter)
        {
         bInvert = true;
         ulRotationWithGain = (long)(unRotation - unRotationCenter)*unSteeringSensitivity;
         unSteeringIntervention = map(ulRotationWithGain,0,(long)unRotationCenter*128L,0,unInterventionMaxMinusInput);
        }
        else
        {
         ulRotationWithGain = (long)(unRotationCenter - unRotation)*unSteeringSensitivity;
         unSteeringIntervention = map(ulRotationWithGain,0,(long)unRotationCenter*128L,0,unInterventionMaxMinusInput);
        }
      }
       
      if(bInvert)
      {
        unSteeringIn = constrain(unSteeringIn - unSteeringIntervention,unSteeringMin,unSteeringMax);
      }
      else
      {
        unSteeringIn = constrain(unSteeringIn + unSteeringIntervention,unSteeringMin,unSteeringMax);
      }
      servoSteering.writeMicroseconds(unSteeringIn);
    }
  }
  else if(gMode == MODE_ERROR)
  {
    servoThrottle.writeMicroseconds(unThrottleCenter);
   
    // allow steering to get to safety
    if(bUpdateFlags & STEERING_FLAG)
    {
      unSteeringIn = constrain(unSteeringIn,unSteeringMin,unSteeringMax);

      servoSteering.writeMicroseconds(unSteeringIn);
    }
  }
 
  bUpdateFlags = 0;
 
  animateIndicatorsAccordingToMode(gMode,ulMillis);
}

void animateIndicatorsAccordingToMode(uint8_t gMode,uint32_t ulMillis)
{
  static uint32_t ulLastUpdateMillis;
  static boolean bAlternate;

  if(ulMillis > (ulLastUpdateMillis + 1000))
  {
    ulLastUpdateMillis = ulMillis;
    bAlternate = (!bAlternate);
    switch(gMode)
    {
      // flash alternating info and error once a second
      case MODE_FORCEPROGRAM:
         digitalWrite(ERROR_INDICATOR_PIN,bAlternate);   
         digitalWrite(INFORMATION_INDICATOR_PIN,false == bAlternate);   
      break;
      // steady info, turn off error
      case MODE_RUN:
        digitalWrite(ERROR_INDICATOR_PIN,LOW);   
        digitalWrite(INFORMATION_INDICATOR_PIN,HIGH);
      break;
      // flash info once a second, turn off error
      case MODE_FULL_PROGRAM:
        digitalWrite(INFORMATION_INDICATOR_PIN,bAlternate);   
        digitalWrite(ERROR_INDICATOR_PIN,LOW);   
      break;
      // alternate error, turn off info
      // MODE_QUICK_PROGRAM is self contained and should never get here,
      // if it does we have an error.
      default:
      case MODE_ERROR:
        digitalWrite(INFORMATION_INDICATOR_PIN,LOW);
        digitalWrite(ERROR_INDICATOR_PIN,bAlternate);   
      break;
    }
  }
}

// simple interrupt service routine
void calcThrottle()
{
  // if the pin is high, its a rising edge of the signal pulse, so lets record its value
  if(PIND & 4)
  {
    ulThrottleStart = micros();
  }
  else
  {
    // else it must be a falling edge, so lets get the time and subtract the time of the rising edge
    // this gives use the time between the rising and falling edges i.e. the pulse duration.
    unThrottleInShared = (uint16_t)(micros() - ulThrottleStart);
    // use set the throttle flag to indicate that a new throttle signal has been received
    bUpdateFlagsShared |= THROTTLE_FLAG;
  }
}

void calcSteering()
{
  if(PIND & 8)
  {
    ulSteeringStart = micros();
  }
  else
  {
    unSteeringInShared = (uint16_t)(micros() - ulSteeringStart);
    bUpdateFlagsShared |= STEERING_FLAG;
  }
}

uint8_t readSettingsFromEEPROM()
{
  uint8_t bError = false;
 
  unSteeringMin = readChannelSetting(EEPROM_INDEX_STEERING_MIN);
  if(unSteeringMin < RC_MIN || unSteeringMin > RC_NEUTRAL)
  {
    unSteeringMin = RC_MIN;
    bError = true;
  }
  Serial.println(unSteeringMin);

  unSteeringMax = readChannelSetting(EEPROM_INDEX_STEERING_MAX);
  if(unSteeringMax > RC_MAX || unSteeringMax < RC_NEUTRAL)
  {
    unSteeringMax = RC_MAX;
    bError = true;
  }
  Serial.println(unSteeringMax);
 
  unSteeringCenter = readChannelSetting(EEPROM_INDEX_STEERING_CENTER);
  if(unSteeringCenter < unSteeringMin || unSteeringCenter > unSteeringMax)
  {
    unSteeringCenter = RC_NEUTRAL;
    bError = true;
  }
  Serial.println(unSteeringCenter);

  unThrottleMin = readChannelSetting(EEPROM_INDEX_THROTTLE_MIN);
  if(unThrottleMin < RC_MIN || unThrottleMin > RC_NEUTRAL)
  {
    unThrottleMin = RC_MIN;
    bError = true;
  }
  Serial.println(unThrottleMin);

  unThrottleMax = readChannelSetting(EEPROM_INDEX_THROTTLE_MAX);
  if(unThrottleMax > RC_MAX || unThrottleMax < RC_NEUTRAL)
  {
    unThrottleMax = RC_MAX;
    bError = true;
  }
  Serial.println(unThrottleMax);
 
  unThrottleCenter = readChannelSetting(EEPROM_INDEX_THROTTLE_CENTER);
  if(unThrottleCenter < unThrottleMin || unThrottleCenter > unThrottleMax)
  {
    unThrottleCenter = RC_NEUTRAL;
    bError = true;
  }
  Serial.println(unThrottleCenter);
 
  unRotationCenter = readChannelSetting(EEPROM_INDEX_ROTATION_CENTER);
  Serial.println(unRotationCenter);
 
  // ideally we would have 512 as the center
  if(unRotationCenter < 100 || unRotationCenter > 560)
  {
    unRotationCenter = ROTATION_CENTER;
    bError = true;
  }
 
  return (false == bError);
}

void writeSettingsToEEPROM()
{
  writeChannelSetting(EEPROM_INDEX_STEERING_MIN,unSteeringMin);
  writeChannelSetting(EEPROM_INDEX_STEERING_MAX,unSteeringMax);
  writeChannelSetting(EEPROM_INDEX_STEERING_CENTER,unSteeringCenter);
  writeChannelSetting(EEPROM_INDEX_THROTTLE_MIN,unThrottleMin);
  writeChannelSetting(EEPROM_INDEX_THROTTLE_MAX,unThrottleMax);
  writeChannelSetting(EEPROM_INDEX_THROTTLE_CENTER,unThrottleCenter);
 
  writeChannelSetting(EEPROM_INDEX_ROTATION_CENTER,unRotationCenter);
           
  Serial.println(unSteeringMin);
  Serial.println(unSteeringMax);
  Serial.println(unSteeringCenter);
  Serial.println(unThrottleMin);
  Serial.println(unThrottleMax);
  Serial.println(unThrottleCenter);
 
  Serial.println(unRotationCenter);
}


uint16_t readChannelSetting(uint8_t nStart)
{
  uint16_t unSetting = (EEPROM.read((nStart*sizeof(uint16_t))+1)<<8);
  unSetting += EEPROM.read(nStart*sizeof(uint16_t));

  return unSetting;
}

void writeChannelSetting(uint8_t nIndex,uint16_t unSetting)
{
  EEPROM.write(nIndex*sizeof(uint16_t),lowByte(unSetting));
  EEPROM.write((nIndex*sizeof(uint16_t))+1,highByte(unSetting));
}

/////////////////////////////////////////////////////////////////////////////
//
// The following function reads the analog settings.
// These adjustments are read at startup and anytime that the program button
// is pressed including QUICK_PROGRAM and FULL_PROGRAM
//
// Note the 1-1023 range for sensitivity and 1-100 range for decay 1 = 500/(50*1) per sec = 10 seconds. 100 = 500/(50*100) per sec = .1 seconds
//
/////////////////////////////////////////////////////////////////////////////
void readAnalogSettings()
{
 // dummy read to settle ADC
  analogRead(THROTTLE_SENSITIVITY_PIN);
  unThrottleSensitivity  = constrain(analogRead(THROTTLE_SENSITIVITY_PIN),1,1023); 

  // dummy read to settle ADC
  analogRead(STEERING_SENSITIVITY_PIN);
  unSteeringSensitivity = constrain(analogRead(STEERING_SENSITIVITY_PIN),1,1023);
 
  // dummy read to settle ADC
  analogRead(THROTTLE_DECAY_PIN);
  unThrottleDecay = analogRead(THROTTLE_DECAY_PIN);
  // if its at the bottom end of the range, turn it off
  if(unThrottleDecay <= 50) // instant
  {
    unThrottleDecay = 500; // = 500/1
  }
  else if(unThrottleDecay <= 100) // 2 tenths of a second
  {
    unThrottleDecay = 50; // 2 tenths = (2*(1/10))/(1/50)
  }
  else if(unThrottleDecay <= 150) // 3 tenths
  {
    unThrottleDecay = 33;
  }
  else if(unThrottleDecay <= 200) // 4 tenths
  {
    unThrottleDecay = 25;
  }
  else if(unThrottleDecay <= 250) // 5 tenths
  {
    unThrottleDecay = 20;
  }
  else if(unThrottleDecay <= 300) // 6 tenths
  {
    unThrottleDecay = 17;
  }
  else if(unThrottleDecay <= 350) // 7 tenths
  {
    unThrottleDecay = 14;
  }
  else if(unThrottleDecay <= 400) // 8 tenths
  {
    unThrottleDecay = 12;
  }
  else if(unThrottleDecay <= 500) // 9 tenths
  {
    unThrottleDecay = 11; 
  }
  else if(unThrottleDecay <= 600) // 10 tenths
  {
    unThrottleDecay = 10;
  }
  else if(unThrottleDecay <= 700) // 11 tenths
  {
    unThrottleDecay = 9;
  }
  else if(unThrottleDecay <= 800) // 12 tenths
  {
    unThrottleDecay = 8;
  }
  else if(unThrottleDecay <= 850) // 16 tenths
  {
    unThrottleDecay = 7;
  }
  else if(unThrottleDecay <= 900) // 2 seconds
  {
    unThrottleDecay = 5;
  }
  else if(unThrottleDecay <= 1000)
  {
     unThrottleDecay = 4;
  }
  else
  {
    unThrottleDecay = 1;
  }
 
  Serial.println(unThrottleDecay);
 
  // dummy read to settle ADC
  analogRead(STEERING_DECAY_PIN);
  unSteeringDecay = constrain(analogRead(STEERING_DECAY_PIN),1,500);
}