/*

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


*/


#include "..\compiler\cc5xfree\16f84a.h"
#include "..\compiler\cc5xfree\int16CXX.H"

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

/***********/
/* OPTIONS */
/***********/

#define GAIN 4              // for second set of o/p s

#undef  DO_PRINTS           // allow diagnostic printing
#undef  PRINT_INPUTS        // details of inputs
#undef  PRINT_DETAILS       // details of calculation as we go along

/* PWM outputs */

#pragma bit OP_PWMLeft1  @ PORTA.0
#pragma bit OP_PWMRight1 @ PORTA.1
#pragma bit OP_PWMLeft2  @ PORTA.2
#pragma bit OP_PWMRight2 @ PORTA.3
#pragma bit OP_RS232 	   @ PORTA.4

/* Inputs */

#pragma bit IP_WidthThrottle @ PORTB.2  // channel 2
#pragma bit IP_WidthRoll     @ PORTB.3  // channel 1


/*

Typical values:

Freq: 47.75 Hz (50?)

// NOTE my throttle is reversed to work with GWS spped controller!
Throttle:
	stick up trim up		    0xd1
	stick up trim center	  0xc3
	center stick 						0x91
	stick down trim center  0x71
	stick down trim right   0x61

Yaw:
	stick left trim left		0x62
	stick left trim center	0x71
	center stick 						0x9a
	stick right trim center 0xc3
	stick right trim right  0xd0

Pitch:
	stick up trim up		    0x64
	stick up trim center	  0x71
	center stick 						0x9a
	stick down trim center  0xc5
	stick down trim right   0xd2

Roll:
	stick left trim left		0x63
	stick left trim center	0x73
	center stick 						0x9b
	stick right trim center 0xc3
	stick right trim right  0xd2


i.e. limits

	one way			 0x71  113   -41  approx 1.1ms
	center stick 0x9a	 154     0  approx 1.5ms
	other way    0xc3  195	 +41  approx 1.9ms



*/

#define CENTER_STICK 0x9A // value read from receiver for center stick
#define MIN_READING  0x50 // A bit less than the mimimum you ever see

#define STICK_LEFT   0X71 // value read from receiver for full left stick
#define STICK_RIGHT  0XC3 // value read from receiver for full right stick

#define STICK_UP     0X71 // value read from receiver for full up stick
#define STICK_DOWN   0XC3 // value read from receiver for full down stick

#define THROTTLE_DOWN 0x71   // throttle stick all the way down
#define MIN_THROTTLE  0x73   // below this value all motors are off
#define MAX_THROTTLE  0xc3   // max ever reading from throttle
#define THROTTLE_OVERRANGE   0xe0   // more than throttle stick all the way up with trim on


// Count to detect a missing frame:
#define FRAME_MISSING_COUNT 12 // 255*10*12 = 30.6ms


#define FALSE 0
#define TRUE 1


/*
;
; The TMR0 interval (interval timer or irq interval) is controled by the
; constant tmrdelay. For an exact timer interval use the formula;
; 256 - (xtal freq / 4) * timer interval) + 3 = timer constant
;
; xtal = 20Mhz
; interval = 10µS
; 20/4 * 10 = 50


;tmrdelay   equ d'256'-d'50'+d'3'      ; 20 uSec TMR0 interval 10MHz
tmrdelay   equ d'256'-d'50'+d'2'      ; 10 uSec TMR0 interval 20MHz
                                      ; every 47 instructions !!!

*/

#define TMRDELAY 256-50+2

unsigned char	nWidth;								// Increments every 20µS
unsigned char	nMissingPulse;				// Decrements to zero every 5ms

unsigned char	nPWMFrameCount;

//unsigned char	nFlags;
//#pragma bit fNoPulse @ nFlags.1

unsigned char nPWMEnables;
#pragma bit OP_RS232_COPY @ nPWMEnables.4


unsigned char	IPWMLeft1;
unsigned char	IPWMRight1;
unsigned char	IPWMLeft2;
unsigned char	IPWMRight2;

unsigned char	PWMLeft1;
unsigned char	PWMRight1;
unsigned char	PWMLeft2;
unsigned char	PWMRight2;

signed long	nWorkingPWMLeft1;
signed long	nWorkingPWMRight1;
signed long	nWorkingPWMLeft2;
signed long	nWorkingPWMRight2;

signed long	nWidthThrottle;
signed char	nWidthRoll;



void DoPWM();


#pragma origin = 0x4


interrupt int_server( void)
{
	int_save_registers    // W, STATUS (and PCLATH)

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

    RTIF = 0;  /* reset flag */

		/* Increment free running count of 10µS */

		if (++nWidth == 0)
			if (nMissingPulse > 0)
				nMissingPulse --;

//#ifndef DO_PRINTS
    DoPWM();
//#endif

    TMR0 += TMRDELAY;
    }

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


//#ifndef DO_PRINTS

void DoPWM()
{

/*
 Wait for a frame start these occur every 2550µS
 i.e. every 255 times through
*/

	if (--nPWMFrameCount == 0)
	{
		/* refresh frame counter */
		nPWMFrameCount = 255;

		/*
		When the frame count reaches zero we turn on all PWM channels
		that have a count > 0 (i.e. have a bit set in nPWMEnables)
		*/

    /* Make copy of reqested PWMs */
		IPWMLeft1  = PWMLeft1;
		IPWMRight1 = PWMRight1;
		IPWMLeft2  = PWMLeft2;
		IPWMRight2 = PWMRight2;

		/* turn on all enabled channels */
		/* also maintain the RS232 out value in bit 4 */
		
		PORTA = (nPWMEnables & 0x1F);

	} // if (--nPWMFrameCount == 0)
	else
	{
		/* decrement counts for each PWM */
		/* and clear output if count reaches 0 */

		if (--IPWMLeft1 == 0)
			OP_PWMLeft1 = FALSE;

		if (--IPWMRight1 == 0)
			OP_PWMRight1 = FALSE;

		if (--IPWMLeft2 == 0)
			OP_PWMLeft2 = FALSE;

		if (--IPWMRight2 == 0)
			OP_PWMRight2 = FALSE;

	} // if-else (--nPWMFrameCount == 0)


} // void DoPWM()

//#endif // #ifndef DO_PRINTS


#ifdef DO_PRINTS

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

#define BIT_DELAY 21

#define delayOneBit() 				\
				while (nWidth < nTarget);	\
				nTarget += BIT_DELAY;						


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

	nTarget = BIT_DELAY;
	nWidth = 0;

	OP_RS232_COPY = 0;	/* startbit */
	OP_RS232 = 0;

  bitCount = 12;
  while (--bitCount > 0)
  {
 		delayOneBit();
		Carry = 1;  /* stopbit will rotate into ms bit*/
    dout = rr( dout);
		OP_RS232_COPY = Carry;
		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(',');
	TxByte(' ');
} // void PrintHex(char byte)

void Delay_1ms()
{
  nWidth = 0;
  // width goes up every 10µS
  while (nWidth < 100);	
}

void BigDelay()
{
long i;

// half a second
  for (i=0;i<500;i++)
    Delay_1ms();
}

#endif

/**************************/
/* Throttle - channel 2   */
/**************************/	
char GetThrottle()
{
	nWidth = 0;
	nMissingPulse = FRAME_MISSING_COUNT;  				// will reach zero after 30ms

  // first wait for zero
	while (IP_WidthThrottle == 1)
	{
		if (nMissingPulse == 0)
		  break;
	}

  // look for rising edge on input
	while (IP_WidthThrottle == 0)
	{
		if (nMissingPulse == 0)
		  break;
	}

	// look for falling edge on input
	nWidth = 0;
	while (IP_WidthThrottle == 1)
	{
		if (nMissingPulse == 0)
		  return 0;
	}

  return nWidth;

} // GetThrottle


/**************************/
/* Roll - Channel 1       */
/**************************/	
char GetRoll()
{
	nWidth = 0;
	nMissingPulse = FRAME_MISSING_COUNT;  				// will reach zero after 30ms

  // first wait for zero
	while (IP_WidthRoll == 1)
	{
		if (nMissingPulse == 0)
		  break;
	}

	nWidth = 0;
  // look for rising edge on input
	while (IP_WidthRoll == 0)
	{
		if (nMissingPulse == 0)
		  break;
	}

	// look for falling edge on input
	nWidth = 0;
	while (IP_WidthRoll == 1)
	{
		if (nMissingPulse == 0)
		  return 0;
	}

  return nWidth;

} // GetRoll



#ifdef DO_PRINTS

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

void PrintInputs()
{

#ifdef PRINT_INPUTS

	TxByte('T');
	PrintHex(nWidthThrottle);

	TxByte('R');
	PrintHex(nWidthRoll);

  crlf();

#endif

}


void PrintWorking(char W)
{

#ifdef PRINT_DETAILS

	TxByte(W);
	TxByte(' ');
	TxByte(' ');
	TxByte(' ');

	TxByte('l');
	PrintHex((char)nWorkingPWMLeft1;
	PrintHex((char)(nWorkingPWMLeft1>> 8));

	TxByte('r');
	PrintHex((char)nWorkingPWMRight1;
	PrintHex((char)(nWorkingPWMRight1>> 8));

	TxByte('L');
	PrintHex((char)nWorkingPWMLeft2;
	PrintHex((char)(nWorkingPWMLeft2>> 8));

	TxByte('R);
	PrintHex((char)nWorkingPWMRight2;
	PrintHex((char)(nWorkingPWMRight2>> 8));

	crlf();

#endif

}

#else // #if DO_PRINTS

#define PrintInputs()    // Nothing
#define PrintWorking(x)  // Nothing

#endif // #if-else DO_PRINTS

char clamp(long val)
{
		// Now clamp the values between 0 and 255
		if (val < 0)
			return 0;

		if (val > 255)
			return 255;

  return (char)val;
}



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


void main( void)
{

	PWMLeft1  = 0;
	PWMRight1 = 0;
	PWMLeft2  = 0;
	PWMRight2 = 0;

	nMissingPulse = 0;

	nWidthThrottle = 0;
	nWidthRoll = 0;

	nPWMFrameCount = 0;

	nPWMEnables = 0;

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

  TRISA =  0; 				/* all outputs */

	TRISB = 0xff; 			/* all inputs */


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

//    RBPU_ = 1;

  TMR0 = TMRDELAY;  
  RTIE = 1;   				/* enable TMR0 interrupt */
  GIE = 1;    				/* interrupts allowed */

#ifdef DO_PRINTS
	crlf();
  TxByte('H');
  TxByte('i');
	crlf();
#endif


  while (1)
	{

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

									 <- 1-2.5ms ->
                  --------------
ch 1 (roll)     --              -----------------------------------
                                -------
ch 2 (throttle) ----------------       ----------------------------
                                       -------
ch 3 (pitch)    -----------------------       ---------------------
                                              ------
ch 4 (yaw)      ------------------------------      ---------------

																								  	<-- >= 10ms -->


Scanning is performed across two frames:

                  --------------
ch 1 (roll)     --              -----------------------------------
                                       -------
																								
20ms later
                                -------
ch 2 (throttle) ----------------       ----------------------------

*/

		// channel 1
    nWidthRoll = GetRoll();

		// channel 2
 	  nWidthThrottle = GetThrottle();

		PrintInputs();

  	nPWMEnables = 0x0F;

		// Turn off all motors if throttle too low or too high
		if ((unsigned char)nWidthThrottle < MIN_THROTTLE)
			nPWMEnables = 0;

    // simular for other inputs
//		if ((unsigned char)nWidthThrottle > THROTTLE_OVERRANGE)
//			nPWMEnables = 0;

    if ( (unsigned char)nWidthRoll < MIN_READING)
			nPWMEnables = 0;


#ifdef DO_PRINTS

    if (nPWMEnables == 0)
    {
      TxByte('o');
      TxByte('f');
      TxByte('f');
    }
    else
    {
      TxByte('o');
      TxByte('n');
      TxByte(' ');
    }

#endif

/*********/
/* Roll  */
/*********/

		// make roll +/-
		nWidthRoll -= CENTER_STICK;

		// If roll is +ve (stick right) subtract from right
		nWorkingPWMRight1 = (signed long)0 - nWidthRoll;

		// and add to left
		nWorkingPWMLeft1 = nWidthRoll;

    {
    // Repeat for the second pair of o/ps but with extra gain

    signed long nTempRoll = nWidthRoll;
    nTempRoll *= GAIN;

		// If roll is +ve (stick right) subtract from right
		nWorkingPWMRight2 = (signed long)0 - nTempRoll;

		// and add to left
		nWorkingPWMLeft2 = nTempRoll;
    }

  	PrintWorking('r');


/************/
/* Throttle */
/************/

    // Add a mimimim value so that full up is 0xff
    // If we need to steer at max throttle then all we can do
    // is subtract
    nWidthThrottle += (0xff - MAX_THROTTLE);

    PrintInputs();

    // Add throttle to all channels
		nWorkingPWMLeft1  += nWidthThrottle;
		nWorkingPWMRight1 += nWidthThrottle;
		nWorkingPWMLeft2  += nWidthThrottle;
		nWorkingPWMRight2 += nWidthThrottle;

  	PrintWorking('T');


/**********/
/* Motors */
/**********/

		// Finally assign working to actual PWM values
		PWMLeft1  = clamp(nWorkingPWMLeft1);
		PWMRight1 = clamp(nWorkingPWMRight1);

		PWMLeft2  = clamp(nWorkingPWMLeft2);
		PWMRight2 = clamp(nWorkingPWMRight2);

#ifdef DO_PRINTS


	TxByte('L');
	PrintHex(PWMLeft1);

	TxByte('R');
	PrintHex(PWMRight1);

	TxByte('l');
	PrintHex(PWMLeft2);

	TxByte('r');
	PrintHex(PWMRight2);
  crlf();

  BigDelay();

#endif

	} // while (1)

} // main