/*
IZ9:  900mhz Plantraco compatible RX
Copyright David Lancaster 2007
All rights reserved
Commercial/for-profit use forbidden without permission
http://darkith.dyndns.org/~darkith/html/iz9.shtml


version 1.0 First release
*/


#define CP_off	|=128
#define CPD_off |=256
#pragma config CP_off
#pragma config CPD_off
#pragma config WDTE=on
//#pragma config FOSC=INT
#pragma config|=0b00000100	//INTOSC no clkout
#pragma config BODEN=on
#pragma config PWRTE=on

//PIN LOCATION SPECIFIC settings
//BE EXTREMELY CAREFUL WITH THESE!

//DIY-UHF RX4 16F630 RX pinouts:
//
//			   	---------
//		Vcc		|1    14| Gnd
//nSEL<--RA5	|2    13| RA0--> SDI
//nIRQ-->RA4 	|3    12| RA1--> SCK
//SDO -->RA3	|4    11| RA2--> ESC
//ELEB<--RC5 	|5    10| RC0--> RUDA
//ELEA<--RC4 	|6     9| RC1--> RUDB
//AILB<--RC3 	|7     8| RC2--> AILA
//		   		---------


//position of channel pins
#define	RUDPOSA		0
#define	RUDPOSB		1
#define	RUDNEGMASK	0b11111100
#define	AILPOSA		2
#define	AILPOSB		3
#define	AILNEGMASK	0b11110011
#define	ELEPOSA		4
#define	ELEPOSB		5
#define	ELENEGMASK	0b11001111

#define	ESCPIN		PORTA.2	//<-Note, PORTA, not PORTC

#define 	TRIS_PORTA	0b00011000			//PORTA all output, except RA3,RA4
#define		TRIS_PORTC	0b00000000			//PORTC all outputs

//pin connections to IA 4320 RX chip
#pragma bit		IA_SDI		@PORTA.0
#pragma bit		IA_SCK		@PORTA.1

#pragma bit		IA_SDO		@PORTA.3
#pragma bit		IA_nIRQ		@PORTA.4
#pragma bit		IA_nSEL		@PORTA.5


//IA4320 settings
//these require checking the datasheet, have fun
#define		IA_CONFIG900	0b.1001110000111011			//915 Mhz band, low battery detect on, no wake-up timer or osc in sleep
														//10 pf XTL capacitance, 134khz baseband bw, disable clockout
#define		IA_CONFIG868	0b.1001110000111011			//868 Mhz band, low battery detect on, no wake-up timer or osc in sleep
														//10 pf XTL capacitance, 134khz baseband bw, disable clockout
//#define		IA_FREQ		0b.1010011111010000			//915.00 Mhz
#define		IA_RX			0b.1100000001000000		 	//VDI=DQD, Max LNA gain, RX off
#define		IA_LBD			0b.1100001000000101			//LBD=2.7v due to 0.3v PCB loss under motor load. Batt will be at 3.0v, no clkout
#define		IA_AFC			0b.1100011011010111			//enable AFC, +15/-16 limited offset, fine mode
#define		IA_DATAFILT		0b.1100010001101100			//fast clock lock, Digital filtering, DQD=4
#define		IA_DATARATE		0b.1100100001010101			//set expected bit rate as ~4000 bps (4010)
#define		IA_FIFO			0b.1100111010000101			//FFIT @ 8bits, start fill on sync word, disable fill, enable fifo
#define		IA_FIFOFILL_EN	0b.0000000000000010			//used to enable FIFO fill start when or'ed with IA_FIFO
#define		IA_RX_EN		0b.0000000000000001			//used to enable RX when or'ed with IA_RX

//915mhz channels
#define		CHANNEL1		0xA255						//904.4775
#define		CHANNEL2		0xA855						//915.9975
#define		CHANNEL3		0xAE55						//927.5175
#define		CHANNEL4		0xA655						//912.1575
#define		CHANNEL5		0xAC55						//923.6775
#define		CHANNEL6		0xA455						//908.3175
//868mhz channels
#define		CHANNEL81		0xA655						//868.105
#define		CHANNEL82		0xA68D						//868.385
#define		CHANNEL83		0xA7BE						//869.910

#define		PWM_DELAY		255-30	//time between PWM runs 


bit framePart;
bit resetFifo;
bit LBD;
char frame1;
char frame2;

#define	LASTCHAN_EE_ADR	0x00	//last channel is stored stored at this eeprom address
char lastChan;		//channel index stored in EEPROM
char currentChan;	//index into current channel, starting at 0

char txState;	
#define TXSEARCHLAST 	0
#define	TXSEARCHUNARMED 1
#define	TXSEARCHANY		2
#define	TXBOUND			3

//timeout value, approx 200 ms
#define	TIMEOUTL	88;
#define	TIMEOUTH	159;	
char lostCNTR;
#define		LOSTVAL			3		//number of timeouts before considering ourselves really lost and take drastic action
									//this *might* prevent signal re-acquisition if we were only actually suffering from a short signal dropout
									//too slow and we'll probably crash anyway

bit validSignal;
char ail;
char ele;
char thr;
char rud;

char ailCntr;
char eleCntr;
char thrCntr;
char rudCntr;
char pwmCntr;


//function prototypes
void updateOut();
void sendSPI(uns16, char);
void resetTheFifo();

//#include "int16CXX.h" 
//#pragma origin 4 
void doPWM(void){ 
	if(!T0IF) return;
	
	//reset the timer and clear flag
	TMR0=PWM_DELAY;
	T0IF=0;
	
	if(validSignal){
		W=PORTC;
		if(--ailCntr==0){
			W=W&AILNEGMASK;
			}
		if(--eleCntr==0){
			W=W&ELENEGMASK;
			}
		if(--rudCntr==0){
			W=W&RUDNEGMASK;
			}
		PORTC=W;
		if(--thrCntr==0){
			ESCPIN=0;
			}
		if(--pwmCntr==0){
			updateOut();
			}
		}
	} 


void updateOut(){
	pwmCntr=31;
	char temp=0;
	
	ailCntr=ail;
	if(ailCntr.7) ailCntr=-ailCntr;
	ailCntr>>=1;	//divide by 2
	if(ailCntr){	//note that div/2 increases deadband to 2
		if(ail.7){temp.AILPOSA=1;}
		else {temp.AILPOSB=1;}
		}
	
	eleCntr=ele;
	if(eleCntr.7) eleCntr=-eleCntr;
	eleCntr>>=1;	//divide by 2
	if(eleCntr){	//note that div/2 increases deadband to 2
		if(ele.7){temp.ELEPOSA=1;}
		else {temp.ELEPOSB=1;}
		}
		
	rudCntr=rud;
	if(rudCntr.7) rudCntr=-rudCntr;
	rudCntr>>=1;	//divide by 2
	if(rudCntr){	//note that div/2 increases deadband to 2
		if(rud.7){temp.RUDPOSA=1;}
		else {temp.RUDPOSB=1;}
		}	
		
	thrCntr=-thr;
	if(thr.7==0) thrCntr=0;
	thrCntr>>=1;
	if(thrCntr) ESCPIN=1;
	else ESCPIN=0;
		
	PORTC=temp;		//copy pin values to output
		
}	

//watch that the following lookup tables don't end up spanning a page boundary
char ChanHighLUT(char chan){
	skip(chan);
	#asm
		retlw HIGH(CHANNEL1);
		retlw HIGH(CHANNEL2);
		retlw HIGH(CHANNEL3);
		retlw HIGH(CHANNEL4);
		retlw HIGH(CHANNEL5);
		retlw HIGH(CHANNEL6);
		retlw HIGH(CHANNEL81);
		retlw HIGH(CHANNEL82);
		retlw HIGH(CHANNEL83);
	#endasm
}
char ChanLowLUT(char chan){
	skip(chan);
	#asm
		retlw LOW(CHANNEL1);
		retlw LOW(CHANNEL2);
		retlw LOW(CHANNEL3);
		retlw LOW(CHANNEL4);
		retlw LOW(CHANNEL5);
		retlw LOW(CHANNEL6);
		retlw LOW(CHANNEL81);
		retlw LOW(CHANNEL82);
		retlw LOW(CHANNEL83);
	#endasm
}	

void bindTX(){	
	txState=TXBOUND;
	if(currentChan!=lastChan){
		//bound on a new channel, update eeprom
		EEADR=LASTCHAN_EE_ADR;
		EEDAT=currentChan;
		WREN=1;
		EECON2=0x55;
		EECON2=0xAA;
		WR=1;
		while(!EEIF){}	//wait for write to finish
		EEIF=0;			//clear interrupt flag to prevent spurious interrupts
		WREN=0;
		}
	//set PWM timer
	TMR0=PWM_DELAY;
	T0IF=0;
}	



void changeChan(char chan){
	sendSPI(IA_RX,16);	//disable the RX
	if(chan < 6){ //first 6 channels (0-5) are 900mhz
		sendSPI(IA_CONFIG900, 16);
		}
	else{				//last 3 channels are 868mhz
		sendSPI(IA_CONFIG868, 16);
		}
	uns16 chanVal;
	chanVal.high8=ChanHighLUT(chan);
	chanVal.low8=ChanLowLUT(chan);
	sendSPI(chanVal, 16);
	sendSPI(IA_RX|IA_RX_EN, 16);	//enable the RX
	resetTheFifo();
}	



void setupPIC(){
	CMCON=0x07;	//disable comparater
	#ifdef ANSEL
		ANSEL=0;	//disable A/D unit
	#endif
	//zero & configure ports
	PORTA=0;
	PORTC=0;
	TRISA=TRIS_PORTA;
	TRISC=TRIS_PORTC;
	IA_nSEL=1;	//turn off SPI
	
	OPTION=0b00001111;				//enable WPUs, TMR0 enable, prescaler to WDT, prescale 1:128 so we can finish startup without WDT reset
	//WPUA=GPWPU;
	
	//zero ram using builtin CC5X function
	clearRAM();
}

		
	void setupRadio(){
	
	sendSPI(IA_RX, 16);
	//sendSPI(IA_CONFIG900, 16);
	//sendSPI(CHANNEL1, 16);
	changeChan(currentChan);	//sets both config & freq settings
	sendSPI(IA_LBD, 16);
	sendSPI(IA_DATAFILT, 16);
	sendSPI(IA_DATARATE, 16);
	sendSPI(IA_AFC, 16);					
	sendSPI(IA_RX|IA_RX_EN, 16);
	sendSPI(IA_FIFO, 16);					//clear FIFO
	sendSPI(IA_FIFO|IA_FIFOFILL_EN, 16);	//enable FIFO
}	


/*  Everything below this line needs to call doPWM periodically */



void sendSPI(uns16 data, char bits){
	char counter=bits;
	uns16 val=data;
	doPWM();
	IA_SCK=0;
	IA_SDI=0;
	IA_nSEL=0;	//open SPI conversation
	while(counter){
		IA_SDI=val.15;
		val<<=1;
		IA_SCK=1;
		counter--;
		IA_SCK=0;
		doPWM();
	}
	IA_nSEL=1;	//close SPI conversation
	IA_SDI=!LBD;
}	

char get8SpiBits(){
	//fetch 8 bits using SPI, assumes SPI conversation is already open
	char counter=8;
	char val=0;

	doPWM();

	while(counter){
		IA_SCK=1;
		val<<=1;
		val.0=IA_SDO;
		counter--;
		IA_SCK=0;
		doPWM();
	}
	return val;
}

/*
void dumpFrame(){
	char counter=16;
	PORTC.1=1;
	while(counter){
		PORTC.0=frame1.7;
		frame1<<=1;
		frame1.0=frame2.7;
		frame2<<=1;
		counter--;
		}
	PORTC.1=0;
	PORTC.0=0;
}	

void debugFrame(){
	
	char origframe1=frame1;
	char counter=16;
	PORTA.2=1;	//set osc sync pulse
	while(counter){
		PORTC=0;
		switch(origframe1){
			case 0b.0000.0001:	//broadcast
				//if(frame2==0b.0000.0001)
					//PORTC=0b00000001;
				PORTC.0=frame1.7;
				break;
			case 0b.0000.0101:	//ail
				//PORTC=0b00000010;
				PORTC.1=frame1.7;
				//ail=frame2;
				break;
			case 0b.0000.0110:	//ele 
				//PORTC=0b00000100;
				PORTC.2=frame1.7;
				//ele=frame2;
				break;
			case 0b.0000.0011:	//rud
				//PORTC=0b00001000;
				PORTC.3=frame1.7;
				//rud=frame2;
				break;
			case 0b.0000.0010:	//thr
				//PORTC=0b00010000;
				PORTC.4=frame1.7;
				//thr=frame2;
				break;
			default:
				//PORTC=0b00100000;
				PORTC.5=frame1.7;
				break;
			}
		frame1<<=1;
		frame1.0=frame2.7;
		frame2<<=1;
		counter--;
		}	
	PORTA.2=0;
	PORTC=0;	
}
*/




void processFrame(){
	bit validFrame=0;
	doPWM();
	switch(frame1){
		case 0b.0000.0001:	//first half of broadcast
			if(frame2==0b.0000.0001){	//second half of broadcast
				if(txState!=TXBOUND){	//if not bound
					bindTX();	//mark as bound to TX
					validFrame=1;
					}
				}
			break;
		case 0b.0000.0101:	//ail
			validFrame=1;
			ail=frame2;
			break;
		case 0b.0000.0110:	//ele 
			validFrame=1;
			ele=frame2;
			break;
		case 0b.0000.0011:	//rud
			validFrame=1;
			rud=frame2;
			break;
		case 0b.0000.0010:	//thr
			validFrame=1;
			thr=frame2;
			break;
		default:
			//unknown frame type!
			break;
		}
	doPWM();
	if(validFrame){
		validFrame=0;
		validSignal=1;
		lostCNTR=LOSTVAL;
		if(txState==TXSEARCHANY) bindTX();	//accept any valid frame as a controlling tx in this state
		if(txState==TXBOUND){	//if we're bound, reset the loss-of-signal timer
			TMR1L=TIMEOUTL;
			TMR1H=TIMEOUTH;
			}
		}
	//debugFrame();
}		

void resetTheFifo(){
	sendSPI(IA_FIFO, 16);
	sendSPI(IA_FIFO|IA_FIFOFILL_EN, 16);
}

void handleInterrupt(){
	//handle interrupt signal from IA4320 chip
	IA_SCK=0;
	IA_SDI=0;	//start SPI with read command (start with 0 on first clk pulse)
	IA_nSEL=0;	//open SPI conversation
	doPWM();
	char data=get8SpiBits();
	if(data.6){ //FIFO overflow
		data=0;	//ignore any further flag
		framePart=0;
		resetTheFifo();	
		}
	LBD=data.4; //Low battery flag
	if(data.7){	//FFIT set, buffer has enough data
		data=get8SpiBits();			//don't care about this info...let it be overwritten
		data=get8SpiBits();			//grab the buffer data
		if(framePart==0){
			framePart=1;
			frame1=data;
			}
		else{
			frame2=data;
			framePart=0;
			resetTheFifo();	
			processFrame();
			}
		}
	doPWM();	
	IA_nSEL=1;	//close SPI conversation
	IA_SDI=!LBD;
}	

	

   #include "hexcodes.h"	
void main(){
	#asm
		DW __CALL(0x3FF)
	#endasm
    OSCCAL = W;	//calibrate the internal oscillator
	
	setupPIC();
	
	//let radio stabilize for 65ms after POR/BOR (min 50ms), skip for WDT reset (radio should have stablized during WDT count)
	if(TO){
		T1CON=0b00000001;	//start T1
		TMR1L=0;
		TMR1H=0;
		TMR1IF=0;
		while(!TMR1IF){
			}
	}
	
	//read last channel from EEPROM
	EEADR=LASTCHAN_EE_ADR;
	RD=1;
	lastChan=EEDAT;
	currentChan=lastChan;
	if(currentChan>8) currentChan=0;	//sanity check for 9 channels (0-8)

	//prime lostCNTR signal counter
	lostCNTR=LOSTVAL;

	setupRadio();
	
	if(POR_==0) {
		POR_=1;	//Flag power up reset so we can distinguish other reset types later
		BOR_=1;	//set flag so it can be cleared by a BOR
		}
	
	//if the reset was a result of a WDT reset, bind to last channel without searching
	if(TO==0) bindTX();
	
	//if the reset was a result of a BOR reset, bind to last channel without searching
	//PROBLEM: the cap takes forever to discharge, so even unplugging the batt for 30 sec looks like a BOR!
	//we can only enable this if we add a resistor to discharge the cap over a reasonable period of time
	//if(BOR_==0) bindTX();
	
	T1CON=0b00110001;	//start T1, prescaled 1:8
	TMR1L=TIMEOUTL;
	TMR1H=TIMEOUTH;
	TMR1IF=0;

/*
	#warning "***************** TEST DATA ENABLED ********************"
	ail=32;
	ele=10;
	thr=0xE0;
	rud=-16;
	validSignal=1;
	bindTX();
*/
	
	while(1){
		//clear WDT & set prescaler to 1:16.  18ms * 16 = ~144ms
		clrwdt();
		PS2=1;
		
		if(TMR1IF){		//either timeout finding tx at startup, or loss of signal during normal use
			TMR1L=TIMEOUTL;
			TMR1H=TIMEOUTH;
			TMR1IF=0;
			switch(txState){
				case TXSEARCHLAST:	//failed to find TX on last chan, start searching for unarmed only from first channel
					txState=TXSEARCHUNARMED;
					currentChan=0;
					changeChan(currentChan);
					break;
				case TXSEARCHUNARMED:	//increment through channels looking for unarmed TX
					if(currentChan==8) txState=TXSEARCHANY;	//check if we've exhausted the unarmed search and search for any tx
					//fall through to start search again
				case TXSEARCHANY:	//increment through channels looking for armed or unarmed TX (or just unarmed if fell through from above)
					currentChan++;
					if(currentChan>8) currentChan=0;	//loop back around
					changeChan(currentChan);
					break;
				case TXBOUND:	//timeout due to lack of signal
					ESCPIN=0;
					PORTC=0;
					validSignal=0;
					if(--lostCNTR==0){
						//really lost, time for desperate measures.  IA4320 chip may have browned out and reset.
						setupRadio();	//Try configuring it again 
						lostCNTR=50;	//set counter so subsequent radio resets happen only every 10 sec
						}
					break;
				}
			}
			
		doPWM();	
		
		if(IA_nIRQ==0) 
			handleInterrupt();
		}
}