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

Radio elevon mixer

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

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

$History: mixer.c $
 * 
 * *****************  Version 3  *****************
 * User: Andy Birkett Date: 30/04/01   Time: 20:05
 * Updated in $/mixer
 * Swap airelon / elevator
 * 
 * *****************  Version 2  *****************
 * User: Andy Birkett Date: 2/03/01    Time: 20:31
 * Updated in $/mixer
 * ASM comments removed
 * Servo B reversed
 * Correct values for constants
 * 
 * *****************  Version 14  *****************
 * User: Andy Birkett Date: 26/02/01   Time: 20:16
 * Updated in $/ffweb
 * Change order of scanning
$Log: /mixer/mixer.c $
 * 
 * 3     30/04/01 20:05 Andy Birkett
 * Swap airelon / elevator
 * 
 * 2     2/03/01 20:31 Andy Birkett
 * ASM comments removed
 * Servo B reversed
 * Correct values for constants
 * 


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




*/

//#define DO_PRINTS
//#define TEST
#define INVERT_B

#include "..\compiler\16f84.h"
#include "..\compiler\int16CXX.H"

#define CP_off  |= 0x3FF8
#pragma config PWRTE=on, WDTE=off, FOSC=HS, CP_off

/* PWM outputs */

#pragma bit OP_PWMA     @ PORTA.0 // left
#pragma bit OP_PWMB     @ PORTA.1 // right

#pragma bit OP_RS232    @ PORTA.4

/* Inputs */
#pragma bit IP_Airelon @ PORTB.0  // channel 1
#pragma bit IP_Elevator  @ PORTB.1  // channel 3


/*

Units are µS.

Values from receiver:

full right  2119 full down 2104   - i.e. with full trim
right       1983      down 1980 
center      1547           1547 to 1550
left        1149        up 1134
full left   1001   full up 1001   - i.e. with full trim
 
With damping turned on:
 
right 1733  down 1700
left  1360  up   1423
 

Output without damping:
 
stick     up    down  center left  right
servo a   1340  1762  1547   1343  1760
servo b   1293  1715  1500   1703  1286
 
*/
 
#define OUTPUT_PULSE_CORRECTION 50

#define CENTER_STICK 1547   // value read from receiver for center stick
#define FULL_SCALE   (CENTER_STICK * 2)

#define MIN_READING  900    // A bit less than the mimimum you ever see
#define MAX_READING  2200   // A bit more than the maximum you ever see

#define STICK_LEFT   1149   // value read from receiver for full left stick
#define STICK_RIGHT  1983   // value read from receiver for full right stick

#define STICK_UP     1134   // value read from receiver for full up stick
#define STICK_DOWN   1980   // value read from receiver for full down stick

#define FALSE 0
#define TRUE 1




// unsigned char  nFlags;
// #pragma bit fMissingPulse @ nFlags.0  // Set if no pulse in ??µS


unsigned char nWidthHi;             // Increments every 256 clock (256µS)

signed long nWidthElevator;
signed long nWidthAirelon;
signed long nWidthA;
signed long nWidthB;




#pragma origin = 0x4


/* Time between free-running ints @ 4MHz = 256µS */

interrupt int_server( void)
{
// In theory this routine never alters any registers that matter
//  int_save_registers    // W, STATUS (and PCLATH)

  if ( RTIF)  
  {
     /* TMR0 overflow interrupt */

     RTIF = 0;  /* reset flag */

     /* Increment free running count of 256µS */
     nWidthHi++;
  }

   /*
     NOTE: GIE is AUTOMATICALLY cleared on interrupt entry and set
           to 1 on exit (by RETFIE). Setting GIE to 1 inside the
           interrupt service routine will cause nested interrupts
           if an interrupt is pending. Too deep nesting may crash
           the program !
   */

//  int_restore_registers // W, STATUS (and PCLATH)
}




#ifdef DO_PRINTS

/* wait 208 lots of 1µS = 208µS approx 1/10 of a byte at 4800*/

#define BIT_DELAY 208


void TxByte( char dout)
/* sends one byte */
{
char bitCount;

  TMR0 = 0;           // clear bit timer  

  OP_RS232 = 0;

  bitCount = 12;
  while (--bitCount > 0)
  {
    while (TMR0 < BIT_DELAY)
      ; // wait

    TMR0 = 0;         // re-start timer to time next bit

    Carry = 1;        // stop bit will rotate into ms bit
    dout = rr( dout);
    OP_RS232 = Carry;
  }

} // void TxByte( char dout)


void crlf() 
{
  TxByte('\r');
  TxByte('\n');
}

char HexNibble(char W)
{
  W = W & 0x0F;
  skip(W);
  #pragma return[]="0123456789ABCDEF"
}


void PrintHex(char byte)
{
  W = swap(byte);
  W = HexNibble(W);
  TxByte(W);
  W = HexNibble(byte);
  TxByte(W);
  TxByte('H');
  TxByte(',');
} // void PrintHex(char byte)


#endif // DO_PRINTS


/*
; Reading a 16 bit timer *
; The pic16c84 only has an 8 bit timer. To maintain a 16 bit timer the program must manage *
; the upper 8 bits in software and increment the high byte in the timer overflow interrupt *
; service routine. This works well. Reading the timer is the problem because two bytes must *
; be read while the timer continues to run. If a read process is begun just before timer *
; overflow then the high byte will be incremented in the middle of the process and the result *
; will be in error by 256. Even if the interrupts are disabled the same problem will occurr *
; because of timer rollover without high byte updating. *
; *
; The solution is to stop the timer which can be done for three instruction cycles by writing *
; to the timer register. OR’ing the timer with zero accomplishes this and the counter is *
; stopped long enough to read the 16 bit value. *
; *
;**********************************************************************************************
*/


/************************/
/* Elevator - Channel 1 */
/************************/  
void GetElevator()
{
char nTemp;

  // clear high part of count - can't increment since interrupts are disabled
  nWidthHi = 0;

  // make sure i/p is 0
  while (IP_Elevator == 1)
  {
  }

  // look for NON-ZERO on input
  while (IP_Elevator == 0)
  {
  }

  // look for falling edge on input and measure time
  TMR0 = 0;           /* clear timer          */
  RTIF = 0;           /* reset overflow flag  */
  GIE = 1;            /* interrupts enabled   */

  while (IP_Elevator == 1)
  {
  }

  W=0;
  TMR0=TMR0 | W;                  // write to TMR0 without changing it
  nWidthElevator.low8 = TMR0;     // then read TMR0 - the clock is now
                                  //   stopped for 3 instruction cycles
  W=nWidthHi;                     // so quickly grab the high timer count

  nWidthElevator.high8 = W;

  while (GIE == 1)
    GIE = 0;            /* interrupts disabled */

} // GetElevator

/***********************/
/* Airelon - Channel 3 */
/***********************/ 
void GetAirelon()
{
  // clear high part of count - can't increment since interrupts are disabled
  nWidthHi = 0;

  // make sure i/p is 0
  while (IP_Airelon == 1)
  {
  }

  // look for NON-ZERO on input
  while (IP_Airelon == 0)
  {
  }

  // look for falling edge on input and measure time
  TMR0 = 0;           /* clear timer          */
  RTIF = 0;           /* reset overflow flag  */
  GIE = 1;            /* interrupts enabled   */

  while (IP_Airelon == 1)
  {
  }

  W=0;
  TMR0=TMR0 | W;                  // write to TMR0 without changing it
  nWidthAirelon.low8 = TMR0;     // then read TMR0 - the clock is now
                                  //   stopped for 3 instruction cycles
  W=nWidthHi;                     // so quickly grab the high timer count

  nWidthAirelon.high8 = W;

  while (GIE == 1)
    GIE = 0;            /* interrupts disabled */

} // GetAirelon


#ifdef DO_PRINTS

/***************************************************************/
/* Prining                                                     */
/***************************************************************/

void PrintRaw()
{
  TxByte('E');
  TxByte(':');
  PrintHex(nWidthElevator.high8);
  PrintHex(nWidthElevator.low8);

  TxByte('A');
  TxByte(':');
  PrintHex(nWidthAirelon.high8);
  PrintHex(nWidthAirelon.low8);
  TxByte(' ');
  TxByte(' ');
} // void PrintRaw()


void PrintCalculated()
{

  TxByte('a');
  TxByte(':');
  PrintHex(nWidthA.high8);
  PrintHex(nWidthA.low8);

  TxByte('b');
  TxByte(':');
  PrintHex(nWidthB.high8);
  PrintHex(nWidthB.low8);
  crlf();
} // void PrintCalculated()

#else // DO_PRINTS

#define PrintRaw() // nothing
#define PrintCalculated() // nothing
#define crlf() // nothing

#endif // DO_PRINTS

void DoDelay(signed long nWidth)
{
  // take into account the length of this routine etc.
  nWidth -= OUTPUT_PULSE_CORRECTION;
  
  nWidth = -nWidth;         // make the width negative so we can count up to 0
 
  nWidthHi = nWidth.high8;  // high part will be incremented by ISR
  
  TMR0 = 0;                 // Set timer to zero so it can't expire before we are ready
  RTIF = 0;                 // clear overflow flag
  GIE = 1;                  // and enable interrupts

  TMR0 = nWidth.low8;       // now set low part of timer to get the fractional part

  while (nWidthHi != 0)     // and wait for the high part to wrap to 0
    ; // wait

  while (GIE == 1)          // finally turn interrupts off
    GIE = 0; 
} // void DoDelay(signed long nWidth)



void  DoOutputs()
{
OP_PWMA = 1;
DoDelay(nWidthA);
OP_PWMA = 0;

OP_PWMB = 1;
DoDelay(nWidthB);
OP_PWMB = 0;
} // void  DoOutputs()



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


void main( void)
{

  nWidthElevator = 0;
  nWidthAirelon = 0;

  PORTA = 0;
  OP_RS232 = 1;       /* inverse of startbit */

  TRISA =  0;         /* all outputs */

  TRISB = 0xff;       /* all inputs */


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

//    RBPU_ = 1;

  RTIE = 1;           /* enable TMR0 interrupt */
  GIE = 0;            /* interrupts initially disabled */



/***************************/
#ifdef TEST
nWidthA = 1500;
nWidthB = 2000;
DoOutputs();

nWidthA = 1000;
nWidthB = 2500;
DoOutputs();
#endif

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


#ifdef DO_PRINTS
#ifndef TEST
  TxByte('>');
  crlf();
#endif
#endif


  while (1)
  {

/*
                <-------------- 20.9 ms -------------------------->               

                  <- 1-2.5ms ->
                  --------------
ch 1 (airelon)  --              -----------------------------------
                                -------
ch 2 (throttle) ----------------       ----------------------------
                                       -------
ch 3 (elevator) -----------------------       ---------------------
                                              ------
ch 4 (rudder)   ------------------------------      ---------------

                                                    <-- >= 10ms -->
*/


/******************/
/* Read inputs   */
/******************/

  GetAirelon();
  GetElevator();

  PrintRaw();


/******************/
/* Calculate O/Ps */
/******************/

/*
A = (x+y)/2
B = (x-y)/2
*/

  nWidthA = nWidthElevator + nWidthAirelon;
  nWidthA = nWidthA >> 1;  // div 2

  nWidthB = nWidthElevator - nWidthAirelon;
  nWidthB = nWidthB >> 1;  // div 2

#ifdef INVERT_B
// invert width B
// nWidthB = FULL_SCALE - (nWidthB + CENTER_STICK);
  nWidthB = CENTER_STICK - nWidthB;
#else
  nWidthB += CENTER_STICK;
#endif

  PrintCalculated();

/* clamp between min/max */
/*
if (nWidthA < MIN_READING)
  nWidthA = MIN_READING;

if (nWidthA > MAX_READING)
  nWidthA = MAX_READING;

if (nWidthB < MIN_READING)
  nWidthB = MIN_READING;

if (nWidthA > MAX_READING)
  nWidthB = MAX_READING;
*/


  DoOutputs();

  } // while (1)

} // main