/************************************************************************

Radio decoder Throttle and two coils

Copyright (c) 2002 Andy Birkett

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You may view the GNU General Public License at http://www.gnu.org/copyleft/gpl.html
or write to:
the Free Software Foundation, Inc.,
59 Temple Place - Suite 330,
Boston, MA  02111-1307, USA.

mailto:AndyBirkett.Planes@gaspode.co.uk

http://www.gaspode.co.uk

************************************************************************/


// 16F84A can be used for testing the code
//#define PIC84

// Invert input for Grahams receiver
//#define INVERT

// Skip tx throttle channel detection for simulation it takes too long!
//#define SIMULATE

/*
16f84:
;            ---------
;     <->RA2 |1    18| RA1<->
;     <->RA3 |2    17| RA0<->
;     <->RA4 |3    16| osc -xtal---||--0V
;     -->CLR |4    15| osc -xtal---||--0V
;         0V |5    14| 5V
; int <->RB0 |6    13| RB7<-> int <RB4-7>
;     <->RB1 |7    12| RB6<-> int <RB4-7>
;     <->RB2 |8    11| RB5<-> int <RB4-7>
;     <->RB3 |9    10| RB4<-> int <RB4-7>
;            ---------

12C508a:

;                  ---------
;               5V |1    18| 0V
; elevatorB <->GP5 |2    17| GP0<-> Throttle
; elevatorA <->GP4 |3    16| GP1<-> RudderA
; rx i/p    -->GP3 |4    15| GP2<-> RudderB
;                  ---------



*/

/*

What a frame looks like:

               <--------------------- 21 ms ------------------------->
580µS pulses   --      --       --    --    ---   --                  --
---------------  ------  -------  ----  ----   ---  ------------------  --
                   ^        ^       ^     ^     ^        ^
                   |        |       |     |     |        |
ch 1 (aileron)  ----        |       |     |     |        |
                            |       |     |     |        |
ch 2 (throttle) -------------       |     |     |        |
                                    |     |     |        |
ch 3 (elevator) ---------------------     |     |        |
                                          |     |        |
ch 4 (rudder)   ---------------------------     |        |
                                                |        |
ch 5 and other channels ...----------------------        |
                                                         |
at least 3ms gap between frames?--------------------------

Thhe 580µS pulse can vary  Grahams is more like 400µS Colins is 460µS

A channel is low for 410µS to 1530µS (applying full trim)
A channel is low for 540µS to 1390µS (no trim)


aileron full left  = 410µS
throttle full down = 410µS (my throttle is inverted!)
elevator stick up  = 410µS
rudder stick left  = 410µS

  NOTE we use aileron as rudder!!!

Typical values:

// NOTE my throttle is reversed to work with GWS speed controller!
Throttle:
  stick up trim up        1530µS
  stick up trim centre    1390µS
  centre stick             920µS
  stick down trim centre   540µS
  stick down trim right    410µS


  Leading edge to leading edge times (these are what we use now):

                Mine,         Colins   worst case   Used ( 1 lot of 35 towards centre)
 full trim       990 (est)    1010      n/a
left / up       1120          1090      1120        1155
centre          1520          1500      1500        1500
right / down    1980          1950      1950        1915
 full trim      2110 (est)    2070      n/a


Compared with the old figures:

Min throttle is     550 + 2*35 + 580 = 1200
Min Pulse widht is         550 + 580 = 1130
Max pulse width is        1390 + 580 = 1970
Center stick is       920 + 35 + 580 = 1535


// old values:
#define MAX_MEASURED_WIDTH      1390/ACTUAL_CYCLE_TIME      // 39
#define MIN_PULSE_WIDTH         550/ACTUAL_CYCLE_TIME       // 15
#define MIN_THROTTLE_WIDTH      (MIN_PULSE_WIDTH+2)
// make the number of counts per frame a bit smaller to make sure full on is always reachable with no trim
#define MAX_FRAME_COUNT         ((MAX_MEASURED_WIDTH - MIN_PULSE_WIDTH) - 3)  //  21
#define CENTER_STICK            ((920 / ACTUAL_CYCLE_TIME)+1)   // 27
*/


#ifdef PIC84

#include "..\compiler\cc5xfree\16f84a.h"
#define CP_off  |= 0x3FF8

#pragma config PWRTE=on, WDTE=off, FOSC=HS, CP_off

#else

#include "..\compiler\cc5xfree\12C508a.h"

/*
bit 0  0}
bit 1  1}=> Internal RC
bit 2  0=>Watchdog disabled
bit 3  1=>No code protecr
bit 4  0=>mclr pin NOT used
*/
#define NO_MCLR     &= ~0x80
#define CP_off      |= 0xFFE8
#define INTERNAL_RX |= 2

#pragma config  INTERNAL_RX, CP_off, WDTE=off

#endif

/***********/
/* DEFINES */
/***********/

#define CYCLE_TIME              32                // 32µS cycle time
#define ACTUAL_CYCLE_TIME       35

/*

For an exact timer interval use the formula;
 256 - (xtal freq / 4) * timer interval) + 3 = timer constant
xtal = 4Mhz

*/

#define CYCLE_RELOAD            ((256 - CYCLE_TIME) + 2)

// High to High pulses longer than this are rejected
#define MAX_ALLOWED_PULSE_WIDTH (2200/ACTUAL_CYCLE_TIME)     // 62

#define MAX_MEASURED_WIDTH      (1950/ACTUAL_CYCLE_TIME - 1) // 54 (-1 => towards centre)
#define MIN_PULSE_WIDTH         (1120/ACTUAL_CYCLE_TIME + 1) // 33 (+1 => towards centre)

#define MIN_THROTTLE_WIDTH      (1200/ACTUAL_CYCLE_TIME)     // 34
#define MAX_THROTTLE_WIDTH      (1870/ACTUAL_CYCLE_TIME)     // 53

// make the number of counts per frame a bit smaller to make sure full on is always reachable with no trim
#define MAX_FRAME_COUNT         ((MAX_MEASURED_WIDTH - MIN_PULSE_WIDTH) - 2)  //  19
#define CENTER_STICK            ((1500 / ACTUAL_CYCLE_TIME)+1)   // 43

#define WAIT_17mS               (17000/ACTUAL_CYCLE_TIME)   // 485
#define WAIT_3mS                (3000/ACTUAL_CYCLE_TIME)    // 85

// High pulses shorter than this are rejected
#define WAIT_300uS              (300/ACTUAL_CYCLE_TIME)     // 8

#define FALSE                   0
#define TRUE                    1

// The number of bad pwm steps before we switch everything off in weve had an error
// Equates to 1 seconds
#define ERROR_THRESHOLD         1000000/ACTUAL_CYCLE_TIME/MAX_FRAME_COUNT


/************/
/* VARIABLES */
/************/

// a bit is set to 1 to turn on the corresponding o/p in the o/p port
// this mask is writted to the o/p port at the start of each PWM frame.
// Each (enabled) channel is then turned off after a calculated period of time.
unsigned char nOutputEnables;

// Hoerver we can't assign the output enables directly or we will blip the o/p off every now and then
// So we have a tempory copy that we fill in bit by bit and then assign to nOutputEnables
unsigned char nTempOutputEnables;

#pragma bit MSB_TIMER_SET   @ TMR0.7

#ifdef PIC84

/* PWM outputs */

// temp assignment for PIC84
#pragma bit OP_RUDDER_A   @ PORTB.0
#pragma bit OP_RUDDER_B   @ PORTB.1
#pragma bit OP_ELEVATOR_A @ PORTB.2
#pragma bit OP_ELEVATOR_B @ PORTB.3
#pragma bit OP_MOTOR      @ PORTA.0

// output enables - these are not written to as individual bits
// instead fTempxxxx is used and then coped on to these variables
#pragma bit xfRudderAOn    @ nOutputEnables.0
#pragma bit xfRudderBOn    @ nOutputEnables.1
#pragma bit xfElevatorAOn  @ nOutputEnables.2
#pragma bit xfElevatorBOn  @ nOutputEnables.3

// can't be just copied for the PIC84 since its on port A
#pragma bit fMotorOn      @ nOutputEnables.5

// Bit settings in temporary output enables
// These must be in the same order as above!
#pragma bit fTempRudderAOn    @ nTempOutputEnables.0
#pragma bit fTempRudderBOn    @ nTempOutputEnables.1
#pragma bit fTempElevatorAOn  @ nTempOutputEnables.2
#pragma bit fTempElevatorBOn  @ nTempOutputEnables.3
#pragma bit fTempMotorOn      @ nTempOutputEnables.5


/* Inputs */
#pragma bit ip_rx @ PORTB.6  // bit stream from receiver


#else

/* PWM outputs */

#pragma bit OP_MOTOR       @ GPIO.0
#pragma bit OP_RUDDER_A    @ GPIO.1
#pragma bit OP_RUDDER_B    @ GPIO.2
#pragma bit OP_ELEVATOR_A  @ GPIO.4
#pragma bit OP_ELEVATOR_B  @ GPIO.5

// output enables
// These must be in the same order as above!
// output enables - these are not written to as individual bits
// instead fTempxxxx is used and then coped on to these variables
#pragma bit xfMotorOn     @ nOutputEnables.0
#pragma bit xfRudderAOn   @ nOutputEnables.1
#pragma bit xfRudderBOn   @ nOutputEnables.2
// dummy is a gap since we can't use bit 3 of gpio as o/p
#pragma bit xfUsedAsInput @ nOutputEnables.3
#pragma bit xfElevatorAOn @ nOutputEnables.4
#pragma bit xfElevatorBOn @ nOutputEnables.5

// Bit settings in temporary output enables
// These must be in the same order as above!
#pragma bit fTempMotorOn     @ nTempOutputEnables.0
#pragma bit fTempRudderAOn   @ nTempOutputEnables.1
#pragma bit fTempRudderBOn   @ nTempOutputEnables.2
// dummy is a gap since we can't use bit 3 of gpio as o/p
#pragma bit fTempUsedAsInput @ nTempOutputEnables.3
#pragma bit fTempElevatorAOn @ nTempOutputEnables.4
#pragma bit fTempElevatorBOn @ nTempOutputEnables.5

/* Inputs */
// NB bit 3 must be an input
#pragma bit ip_rx @ GPIO.3  // bit stream from receiver

#endif

#pragma bit fInvertThrottle @ nTempOutputEnables.6
#pragma bit fFrameError     @ nTempOutputEnables.7


#ifdef INVERT
#define ip_active ip_rx
#define ip_inactive !ip_rx
#else
#define ip_active !ip_rx
#define ip_inactive ip_rx
#endif
// working variables
// The number of conecutive frame errors
unsigned long nErrorCount;

// General counter to measure lots of 'CYCLE_TIME'
unsigned char nTimer;

// Timer to measure gap
unsigned char nGapTimer;


// The PWM value to go to the corresponding channel
unsigned char nMotorCount;
unsigned char nRudderCount;
unsigned char nElevatorCount;

// Working variables so we don;t change the real ones while we are re calculating
unsigned char nMeasuredMotorCount;
unsigned char nMeasuredRudderCount;
unsigned char nMeasuredElevatorCount;

// Pervious values so we cane smooth out spikes
unsigned char nLastMotorCount;
unsigned char nLastRudderCount;
unsigned char nLastElevatorCount;


// pointers to where ch1,2,3 should be stored
// this will vary depending on the ransmitter type
unsigned char *pCh1Count;
unsigned char *pCh2Count;
unsigned char *pCh3Count;

// Counter used during initialsation to measure ch1,c2,c3
// before we assign them to nMotorCount, nRudderCount, nElevatorCount;
unsigned char nCh1Count;
unsigned char nCh2Count;
unsigned char nCh3Count;

// How far we are in the current PWM frame
unsigned char nFrameCount;



/***************************************************************/
/* void DoPWM()                                                */
/***************************************************************/

void DoPWM()
{
#ifdef PIC84
  // don't switch banks again
  #pragma update_RP 0
#endif

    if (nFrameCount == MAX_FRAME_COUNT)
    {
      nFrameCount = 0;

      if (fFrameError)
      {
        if (nErrorCount == 0)
        {
          // too many errors - stop everything!
          nOutputEnables = 0;

          #ifdef PIC84
          fMotorOn = 0;
          #endif
        }
        else
        {
          nErrorCount--;
        }
      }
      else
      {
        nErrorCount = ERROR_THRESHOLD;
      }


      // turn enabled inputs on
#ifdef PIC84
      PORTB = nOutputEnables;
      OP_MOTOR = fMotorOn;
#else
      GPIO = nOutputEnables;
#endif
    } // if (nFrameCount == MAX_FRAME_COUNT)
    else
    {
      nFrameCount ++;

      if (nFrameCount == nMotorCount)
        OP_MOTOR = FALSE;

      if (nFrameCount == nRudderCount)
      {
        OP_RUDDER_A = FALSE;
        OP_RUDDER_B = FALSE;
      }

      if (nFrameCount == nElevatorCount)
      {
        OP_ELEVATOR_A = FALSE;
        OP_ELEVATOR_B = FALSE;
      }

    } // if-ele (nFrameCount == MAX_FRAME_COUNT)

//  Add to trace cycle time in MPLAB
//    W=TMR0;
    while (MSB_TIMER_SET)
      ; // wait

    TMR0 = CYCLE_RELOAD;
} // DoPWM



// Routine to readd 3 channels from the incoming bit stream.
void ReadChannels()
{
startLookingAgain:

      // Initally look for a 3ms gap followed by a 300µS high period

      DoPWM();

      // Waiting for 0
      while (ip_inactive)
      {
        DoPWM();
      }

      nTimer = WAIT_3mS + 1; // since we --
      DoPWM();

      // Check is stays low for 3ms
      while (--nTimer != 0)
      {
        DoPWM();
        if (ip_inactive)
          goto HadError;
      }

      DoPWM();

      // Wait for high within the next 15ms
      // 3 channel tx 6ms of info 14ms of gap, we've allready waited 3ms
      nGapTimer = WAIT_17mS/2;  // Make count units of 70µS

      DoPWM();
      while (ip_active)
      {

        // This loop takes 70µS
        DoPWM();

        // Try to prevent 35µS jitter by dropping out here if the i/p has gone high
        if (ip_inactive)
          break;

        DoPWM();

        if (--nGapTimer == 0)
          goto HadError;

      }

      DoPWM(); // +1a

/*************************** The following pattern is repeated for each channel */

  // channel 1

      // The input has just gone high
      // Start measuring time from leading edge to leading edge
      // start at 2 since we just called DoPWM() 1 times (1a above)
      nTimer = 2+2; // add adjustment for the number of calls to DoPWM() without nTimer++

      // But we still want to check the input remains high for at least 300µS
      // So wait for input to go low while measuring time.
      while (ip_inactive)
      {
        DoPWM();
        nTimer++;
      }

      DoPWM(); // +1

      // The input has gone low, was it high long enough???
      if (nTimer < (WAIT_300uS + 3)) // We start nTimer at 3
      {
        // No, throw this frame away.
        goto HadError;
      }

      DoPWM(); // +2

      // Input is now low, wait for it to go high, measuring the time
      while (ip_active)
      {
        DoPWM();
        nTimer++;
      }

      DoPWM(); // +1a

      if (nTimer > MAX_ALLOWED_PULSE_WIDTH)
        goto HadError;

      DoPWM(); // +2a

      // Input has just gone high, store the time
      *pCh1Count = nTimer;

      DoPWM(); // +3a

/*************************** End of this pattern *************************/

    // channel 2

      // The input has just gone high
      // Start measuring time from leading edge to leading edge
      // start at 4 since we just called DoPWM() 3 times (1a, 2a, 3a above)
      nTimer = 4+2; // add adjustment for the number of calls to DoPWM() without nTimer++

      // But we still want to check the input remains high for at least 300µS
      // So wait for input to go low while measuring time.
      while (ip_inactive)
      {
        DoPWM();
        nTimer++;
      }

      DoPWM(); // +1

      // The input has gone low, was it high long enough???
      if (nTimer < (WAIT_300uS + 5)) // We start nTimer at 5
      {
        // No, throw this frame away.
        goto HadError;
      }

      DoPWM(); // +2

      // Input is now low, wait for it to go high, measuring the time
      while (ip_active)
      {
        DoPWM();
        nTimer++;
      }

      DoPWM(); // 1a

      if (nTimer > MAX_ALLOWED_PULSE_WIDTH)
        goto HadError;

      DoPWM(); // +2a


      // Input has just gone high, store the time
      *pCh2Count = nTimer;

      DoPWM(); // 3a

/*************************** End of pattern *************************/

    // channel 3

      // The input has just gone high
      // Start measuring time from leading edge to leading edge
      // start at 4 since we just called DoPWM() 3 times (1a, 2a, 3a above)
      nTimer = 4+2; // add adjustment for the number of calls to DoPWM() without nTimer++

      // But we still want to check the input remains high for at least 300µS
      // So wait for input to go low while measuring time.
      while (ip_inactive)
      {
        DoPWM();
        nTimer++;
      }

      DoPWM(); // +1

      // The input has gone low, was it high long enough???
      if (nTimer < (WAIT_300uS + 5)) // We start nTimer at 5
      {
        // No, throw this frame away.
        goto HadError;
      }

      DoPWM(); // +2

      // Input is now low, wait for it to go high, measuring the time
      while (ip_active)
      {
        DoPWM();
        nTimer++;
      }

      DoPWM();

      if (nTimer > MAX_ALLOWED_PULSE_WIDTH)
        goto HadError;

      DoPWM(); 

      // Input has just gone high, store the time
      *pCh3Count = nTimer;

      DoPWM();

/*************************** End of pattern *************************/

      // got to the end of a frame succefully, clear error count
      fFrameError = 0;

      return;

HadError:

      fFrameError = 1;
      goto startLookingAgain;

} // void ReadChannels()



void waitHalfSecond()
{
unsigned long nDelay;

// each loop takes 10µS
for (nDelay=0;nDelay < (500000/10);nDelay++);
}


/***************************************************************/
/* MAIN                                                        */
/***************************************************************/


void main( void)
{
#ifdef PIC84
  // switch banks during inittialisation
  #pragma update_RP 1

  PORTA = 0;
  PORTB = 0;

  TRISA = 0xF0;         /* bit 0..3 outputs */

  TRISB = 0xF0;          /* bit 0..3 i/p */

#else

  OSCCAL = W;
  GPIO = 0;
  TRIS = 0x08;        /* bit 3 i/p bits 0,1,2,4,5 o/p */

#endif


#ifdef PIC84

  OPTION = 0x0;         /* prescaler divide by 2 */
  PSA = 1;              /* prescale WDT */
//    RBPU_ = 1;

#else

  OPTION = 0x8;         /* prescaler divide by 2 - prescale WDT */

#endif


  TMR0 = CYCLE_RELOAD;

#ifdef PIC84
  // don't switch banks again
  #pragma update_RP 0
#endif

  nErrorCount = ERROR_THRESHOLD;
  nTimer = 0;

  nMotorCount = 0;
  nRudderCount = 0;
  nElevatorCount = 0;

  // Set the last throttle to Max this will cause a blip when we start up
  // Since the throttle will move back towards zero
  nLastMotorCount = MAX_ALLOWED_PULSE_WIDTH;

  nLastRudderCount = CENTER_STICK;
  nLastElevatorCount = CENTER_STICK;

  nFrameCount = 0;

  nTempOutputEnables = 0;
  nOutputEnables = 0;

// TEMP TO SPEED UP SIMULATION
#ifdef SIMULATE

  pCh1Count = &nMeasuredRudderCount;
  pCh2Count = &nMeasuredMotorCount;
  pCh3Count = &nMeasuredElevatorCount;

#else

  // wait for everything to settle!!!

  waitHalfSecond();
  waitHalfSecond();

  // Now work out what sort of transmiiter
  // by looking for a channel that stays low for 1/2 sec
  // call this channel the throttle channel

StartOver:

  nMeasuredMotorCount = MIN_THROTTLE_WIDTH+1;

  while ((nMeasuredMotorCount > MIN_THROTTLE_WIDTH) && (nMeasuredMotorCount < MAX_THROTTLE_WIDTH))
  {
    // Assign pointers to temporary variable
    pCh1Count = &nCh1Count;
    pCh2Count = &nCh2Count;
    pCh3Count = &nCh3Count;

    ReadChannels();

    if (nCh1Count <= MIN_THROTTLE_WIDTH || nCh1Count >= MAX_THROTTLE_WIDTH)
    {
      // throttle on ch 1 (JR)
      pCh1Count = &nMeasuredMotorCount;
      pCh2Count = &nMeasuredRudderCount;
      pCh3Count = &nMeasuredElevatorCount;
    }

    if (nCh2Count <= MIN_THROTTLE_WIDTH || nCh2Count >= MAX_THROTTLE_WIDTH)
    {
      // throttle on ch 2
      pCh1Count = &nMeasuredRudderCount;
      pCh2Count = &nMeasuredMotorCount;
      pCh3Count = &nMeasuredElevatorCount;
    }

    if (nCh3Count <= MIN_THROTTLE_WIDTH || nCh3Count >= MAX_THROTTLE_WIDTH)
    {
      // throttle on ch 3
      pCh1Count = &nMeasuredRudderCount;
      pCh2Count = &nMeasuredElevatorCount;
      pCh3Count = &nMeasuredMotorCount;
    }

    waitHalfSecond();
    waitHalfSecond();

    // ReadChannels will now set nMeasuredMotorCount since we altered the pointers
    // It should remain less than MIN_THROTTLE_WIDTH
    // for half a second
    ReadChannels();

  } //   while ((nMeasuredMotorCount > MIN_THROTTLE_WIDTH) && (nMeasuredMotorCount < MAX_THROTTLE_WIDTH))

#endif // SIMULATE

  if (nMeasuredMotorCount <= MIN_THROTTLE_WIDTH)
    fInvertThrottle = 0;
  else
    fInvertThrottle = 1;


  // Main loop

  while (1)
  {
    DoPWM();

    // Note we don't come back from read channels until we have a good frame
    ReadChannels();

    DoPWM();

// Now do all the calculations


// Rudder

    // We can't assign the output enables directly
    // or we will blip the o/p off every now and then
    fTempRudderAOn = FALSE;
    fTempRudderBOn = FALSE;
    DoPWM();

    // First apply some smoothing
    nMeasuredRudderCount += nLastRudderCount;
    DoPWM();
    nMeasuredRudderCount = nMeasuredRudderCount >> 1;
    DoPWM();
    nLastRudderCount = nMeasuredRudderCount;

    DoPWM();
    if (nMeasuredRudderCount < CENTER_STICK - 1)
    {
      DoPWM();
      fTempRudderAOn = TRUE;
      nMeasuredRudderCount = CENTER_STICK - nMeasuredRudderCount;
     }

    DoPWM();

    // Not if-else we want o/p off if i/p is center stick
    if (nMeasuredRudderCount > CENTER_STICK + 1)
    {
      DoPWM();
      fTempRudderBOn = TRUE;
      nMeasuredRudderCount = nMeasuredRudderCount - CENTER_STICK;
    }

    DoPWM();

    nRudderCount = nMeasuredRudderCount << 1;

    DoPWM();

// Throttle

    if (fInvertThrottle)
      nMeasuredMotorCount = (MIN_THROTTLE_WIDTH + MAX_MEASURED_WIDTH) - nMeasuredMotorCount;

    DoPWM();

    // First apply some smoothing
    nMeasuredMotorCount = nMeasuredMotorCount + nLastMotorCount;
    DoPWM();
    nMeasuredMotorCount = nMeasuredMotorCount >> 1;
    DoPWM();
    nLastMotorCount = nMeasuredMotorCount;

    DoPWM();
    if (nMeasuredMotorCount < MIN_THROTTLE_WIDTH)
    {
      DoPWM();
      fTempMotorOn = FALSE;
    }
    else
    {
      DoPWM();
      fTempMotorOn = TRUE;
      nMotorCount = nMeasuredMotorCount - MIN_PULSE_WIDTH;
    }

    DoPWM();

// Elevator

    fTempElevatorAOn = FALSE;
    fTempElevatorBOn = FALSE;
    DoPWM();

    // First apply some smoothing
    nMeasuredElevatorCount += nLastElevatorCount;
    DoPWM();
    nMeasuredElevatorCount = nMeasuredElevatorCount >> 1;
    DoPWM();
    nLastElevatorCount = nMeasuredElevatorCount;

    if (nMeasuredElevatorCount < CENTER_STICK - 1)
    {
      DoPWM();
      fTempElevatorAOn = TRUE;
      // < 23 --> 23 - time
      nMeasuredElevatorCount = CENTER_STICK - nMeasuredElevatorCount;
     }

    DoPWM();

    // Not if-else we want o/p off if i/p is center stick

    if (nMeasuredElevatorCount > CENTER_STICK + 1)
    {
      DoPWM();
      fTempElevatorBOn = TRUE;
      // > 23 --> time - 23
      nMeasuredElevatorCount = nMeasuredElevatorCount - CENTER_STICK;
    }

    DoPWM();

    nElevatorCount = nMeasuredElevatorCount << 1;

    DoPWM();

    // Now enable all those o/ps that are on from the temp copy
    nOutputEnables = nTempOutputEnables;

  } // while (1)

} // main