;***************************************************************************
;            6 Channel Pulse Position Modulation Decoder 
;***************************************************************************
;              Bruce Abbott   bhabbott@paradise.net.nz 
;
; for Microchip 12F629/75 (5 channels) or 16F630/76 (6 channels)
;
;============================= Description =================================
;
; PPM DECODER:
;
; Each frame consists of 4 to 6 channels (5 to 7 pulses), followed by a sync 
; gap. 
;
; The width of each channel is 1 to 2mS (1.5mS when sticks centered). The 
; sync gap is at least 2.5mS long, and frame length is approximately 20mS.
;
; The input signal should look something like this:- 
;
;            |<------------------ 1 frame (~20mS) -------------------->|
;            |< 1~2mS >|                                 
;             _	        _        _        _        _                    _
; ___________| |_______| |______| |______| |______| |__ // ____________| |____
;   sync gap     ch1       ch2      ch3      ch4       etc.  sync gap      ch1
;
; NOTE: Pulse polarity (shift) is assumed to be positive. If your Rx produces
;       negative pulses, then either change the polarity of instructions that
;       test the input signal (BTFSS<->BTFSC), or add an invertor to the input 
;       circuit!
;
; After receiving channel 6, channels 1, 3, and 5 are output simultaneously,
; followed by channels 2, 4 and 6. 
;
; FILTERING:
;
; Each channel is averaged with the value from the previous frame, reducing
; servo jitter on weak signals. If any channel in a frame is corrupted, the 
; whole frame is discarded and the last good frame is used instead.  
;
; FAILSAFE:
;
; On receiving a sufficient number of good frames we save it for failsafe. 
; Then, if the signal is corrupted for too long, we output the failsafe frame
; instead of the last good frame.  
;
; THROTTLE ARMING:
;
; When entering failsafe the throttle is cut, and it will not be restarted 
; until a good signal is detected AND the throttle is manually reset. This 
; should prevent the situation where motor-induced RF interference causes 
; the decoder to cycle in and out of failsafe repeatedly.  
;
; For this feature to work the throttle channel has to be determined. Futaba 
; and Hitec tranmitters assign the throttle to channel 3. JR and GWS use 
; channel 1. If channel 1 measures less than 1.3mS or more than 1.7mS at 
; startup then it is assigned to the throttle, otherwise channel 3 will be 
; assumed. 
;
; =============================================================================
;                             Summary of Changes
;
; 2004/6/21  V0.00 - modified code from rxdec6 v0.10  
; 2005/2/11  V0.01 - Relaxed input pulse width timing
; 2005/2/27  V0.02 - bugs fixed; 51X,42X not outputting all required channels.
;		   - bug fixed; failsafe throttle was not set.                           
;		   - PIC16F676 PPM_in assigned to RA5 (MCLR reserved for ICSP) 
;		   - Enabled Watchdog timer
; 2005/6/3   V0.03 - Negative 'Shift' assembly option. 
;                   (NOTE: only required if input pulses are negative-going!)
; 2006/4/15  V0.04 - bug fixed; only channels 1~3 were active if 4 or 5 channels 
;                    were detected.  		       
;                  - Reduced startup delay from 500mS to 250mS.  
;
; -----------------------------------------------------------------------------
;
#DEFINE ARM_THROTTLE	; enable if throttle arming control wanted.  	

#DEFINE	DETECT_JR	; enable if JR transmitter detection required.

;#DEFINE NEGATIVE_SHIFT	; enable if input pulses are negative going.
			; NOTE: normally only required if CHANGING the 
			; 'shift' of your receiver, eg. for using a 
			; 72MHz Futaba receiver with a JR transmitter.    


 ifdef NEGATIVE_SHIFT
#DEFINE	SKIP_PPM_HI	btfsc	GPIO,PPM_in
#DEFINE	SKIP_PPM_LO	btfss	GPIO,PPM_in
 else
#DEFINE	SKIP_PPM_HI	btfss	GPIO,PPM_in
#DEFINE	SKIP_PPM_LO	btfsc	GPIO,PPM_in
 endif
 
#DEFINE version  "0.04"

; Make sure that PROCESSOR and <include> file are compatible with your CPU.
; 12F629 uses 12F675 defines, 16F630 uses 16F676 definitions (just don't try 
; to use the A/D module...)

; Enable one of the DEFINEs below if your assembler requires it. 
; MPLab users should select the device from the "Configure" menu.

;#DEFINE __12F675	 
;#DEFINE __16F676

;#DEFINE OSCAL_NO 0x70	; set this if OSCCAL value was erased!

	ifdef	  __12F675
        PROCESSOR PIC12F675
        INCLUDE   <P12F675.inc>
	__CONFIG  _MCLRE_OFF&_CP_OFF&_WDT_ON&_BODEN_ON&_INTRC_OSC_NOCLKOUT
	endif
	ifdef	  __16F676
        PROCESSOR PIC16F676
        INCLUDE   <P16F676.inc>
	__CONFIG  _MCLRE_OFF&_CP_OFF&_WDT_ON&_INTRC_OSC_NOCLKOUT
	endif

        radix     dec

	errorlevel 0,-305,-302


; Bit definitions for the GPIO register and the TRIS register

	ifdef	__16F676

#DEFINE PPM_in	 5    ; pin 2   input pulse stream

#DEFINE CH_1     0    ; pin 10  Channel 1 output
#DEFINE CH_2	 1    ; pin 9   Channel 2 output
#DEFINE CH_3	 2    ; pin 8   Channel 3 output
#DEFINE CH_4	 3    ; pin 7   Channel 4 Output
#DEFINE CH_5	 4    ; pin 6   Channel 5 Output
#DEFINE CH_6	 5    ; pin 5   Channel 6 Output

#DEFINE GPIO   PORTA
#DEFINE TRISIO TRISA

	else	; 12F629/75

#DEFINE PPM_in	 3    ; pin 4   input pulse stream

#DEFINE CH_1     0    ; pin 7   Channel 1 output
#DEFINE CH_2	 1    ; pin 6   Channel 2 output
#DEFINE CH_3	 2    ; pin 5   Channel 3 output
#DEFINE CH_4	 4    ; pin 3   Channel 4 Output
#DEFINE CH_5	 5    ; pin 2   CHannel 5 Output

#DEFINE PORTC  GPIO   

	endif

#DEFINE TrisBits (1<<PPM_in)

; Bits to be set with the OPTION instruction
;   No wake up
;   No weak pullups
;   Timer 0 source internal
;   Which edge is don't care
;
;   Prescaler to Timer 0, divide by 256.
;
;
#DEFINE OptionBits B'11000111'

; =========================================================================
; Macro for generating short time delays
;
NO_OP           MACRO   count
NO_OP_COUNT     SET     count
                WHILE   NO_OP_COUNT>1
		goto	$+1		; 2 clocks
NO_OP_COUNT     SET     NO_OP_COUNT-2
                ENDW
		IF	NO_OP_COUNT
		nop			; 1 clock
		ENDIF
                ENDM

;===========================================================================
; Macro to create offsets for variables in RAM
;
		ifdef	__12C509
ByteAddr	SET	7
		else
ByteAddr	SET	32		; user RAM starts here
		endif

BYTE            MACRO     ByteName
ByteName        EQU       ByteAddr
ByteAddr	SET       ByteAddr+1
                ENDM


; ==========================================================================
;                 RAM Variable Definitions  
;
        BYTE	Flags		; various boolean flags            

        BYTE	PPMcount	; pulse length. 1~255 = 0.75~2.28mS  
	BYTE	Channels	; number of channels in current frame
	BYTE	NumChannels	; number of channels detected at startup

	BYTE	GoodFrames	; No. of good frames to go 
				; before accepting failsafe frame.
 
	BYTE	HoldFrames	; No. of bad frames to go 
				; before switching to failsafe.

	BYTE	ArmFrames	; No. of low throttle frames to go
				; before arming throttle.

	BYTE	Temp1		; general purpose storage
	BYTE	Temp2		;   ''       ''      ''

        BYTE	PPI_1		; channel 1 in        
        BYTE	PPI_2		; channel 2 in
	BYTE	PPI_3		; channel 3 in
	BYTE	PPI_4		; channel 4 in
	BYTE	PPI_5		; channel 5 in
	ifdef	CH_6
	BYTE	PPI_6		; channel 6 in
	endif
	BYTE	FLS_1		; channel 1 failsafe
	BYTE	FLS_2		; channel 2 failsafe		    
	BYTE	FLS_3		; channel 3 failsafe		    
	BYTE	FLS_4		; channel 4 failsafe		    
	BYTE	FLS_5		; channel 5 failsafe
	ifdef	CH_6
	BYTE	FLS_6		; channel 6 failsafe
	endif

	BYTE	PPA_1		; channel 1 average
	BYTE	PPA_2		; channel 2 average
	BYTE	PPA_3		; channel 3 average
	BYTE	PPA_4		; channel 4 average
	BYTE	PPA_5		; channel 5 average
	ifdef	CH_6
	BYTE	PPA_6		; channel 6 memory
	endif

	BYTE	PPM_1		; channel 1 memory
	BYTE	PPM_2		; channel 2 memory
	BYTE	PPM_3		; channel 3 memory
	BYTE	PPM_4		; channel 4 memory 
	BYTE	PPM_5		; channel 5 memory
	ifdef	CH_6
	BYTE	PPM_6		; channel 6 memory
	endif
	BYTE	PWM_1		; channel 1 out
	BYTE	PWM_2		; channel 2 out
	BYTE	PWM_3		; channel 3 out
	BYTE	PWM_4		; channel 4 out
	BYTE	PWM_5		; channel 5 out
	ifdef	CH_6
	BYTE	PWM_6		; channel 6 out
	endif


;-------------------------------------------------------------------------
;                   flag bit assignments 
;-------------------------------------------------------------------------
;
#DEFINE WATCH		0	; Watchdog timeout
#DEFINE GOT_FS		1	; failsafe frame has been captured
#DEFINE	BAD_FRAME	2	; error detected in frame
#DEFINE JR		3	; JR throttle detection option
#DEFINE	ARMED		4	; throttle armed



;-------------------------------------------------------------------------
;                         Constants
;-------------------------------------------------------------------------

; number of good frames required before accepting failsafe values.

#DEFINE GOODCOUNT 10

; number of bad frames acceptable before going to failsafe.

#DEFINE HOLDCOUNT 25 

; number of consecutive low throttle frames required before arming.

#DEFINE	ARMCOUNT 5


;****************************************************************************
;                                Code
;
	ORG	0
	goto	ColdStart

	ORG	8
	ifdef	__16F676
	dt	"-RXD676-"
	else	
	dt	"-RXD675-"
	endif
	dt	"--V"
	dt	version
	dt	"-"
	

;============================================================================

ColdStart:		
	bcf	Flags,WATCH
	btfss	STATUS,NOT_TO		; copy Watchdog timeout flag
	bsf	Flags,WATCH

; get oscillator calibration value and use it to fine-tune clock frequency.

	bsf	STATUS,RP0		 
	call	0x3ff			; get OSCCAL value
	movwf	OSCCAL			; set oscillator calibration 
        movlw	OptionBits 
        movwf	OPTION_REG                    	
	bcf	STATUS,RP0		
        clrwdt

; initialise I/O registers 

        clrf	GPIO			; all GPIO (PORTA) outputs low
	ifdef	__16F676
	clrf	PORTC			; all PORTC outputs low (12F630/76)
	endif
	bsf	STATUS,RP0		
        movlw	TrisBits
        movwf	TRISIO			; set GPIO (PORTA) directions
	ifdef	__16F676
	movlw	0
	movwf	TRISC			; set portc directions (12F630/76)
	endif
	ifdef	ANSEL
	clrf	ANSEL			; disable analog inputs (12F675/16F676)
	endif
	bcf	STATUS,RP0		
	movlw	b'00000111'		      
        movwf	CMCON			; Comparator off

; CPU specific stuff done, now we can start the main program!

        goto	Main		
							                               


;----------------------------------------------------------------------------
; GetPPM:             Get time to next PPM pulse	
;---------------------------------------------------------------------------- 
;
; input:	PPM signal has just gone high. 
; output:	PPMcount = Pulse Width * 6uS, next pulse has just started
; error:	PPMcount = XX and error code in W.
;
; Error Codes
;		0 = pulse too short, too long, or next pulse too soon 
;		1 = no next pulse (ie. no channel)
;		2 = good channel

PRECHARGE = ((750-12)/6)		; = 0.75mS 

GetPPM:		movlw	PRECHARGE	; preset count for signal high length
		movwf	PPMcount

		movlw	(30/6)
		movwf	Temp1
high_delay:	NO_OP	2
		decf	PPMcount	; wait 30uS to skip short glitches
		decfsz	Temp1
		goto	high_delay		

hiloop:		SKIP_PPM_HI		; signal gone low ?
		goto	pulselo
		nop			; 6uS per loop
		decfsz	PPMcount	; count down
		goto	hiloop
		retlw	0		; timed out, signal high

pulselo:	movlw	PRECHARGE-(180/6)	
		subwf	PPMcount,W	
		skpnc			; less than minimum pulse width ?
		retlw	0
		movlw   PRECHARGE-(690/6)
		subwf   PPMcount,W
		skpc			; greater than maximum pulse width ?
		retlw	0

		movlw	(30/6)
		movwf	Temp1
low_delay:	NO_OP	2
		decf	PPMcount	; wait 30uS to skip short glitches
		decfsz	Temp1
		goto	low_delay	

to750uS:	SKIP_PPM_LO		; signal should stay low until 0.75mS
		retlw	0
		nop			; 6uS per loop
		decfsz  PPMcount	; count down to zero @ 0.75mS
		goto    to750uS
		incf	PPMcount	; count up, start at 1

to2280uS:	SKIP_PPM_LO		; start of next channel pulse ?
		retlw	2		; return OK
		nop			; 6uS per loop
		incfsz  PPMcount	; count up to 256 @ 2.28mS
		goto	to2280uS
		retlw	1		; return error @ 2.28mS		
	

;-------------------------------------------------------------------------------
;                    Millisecond Delay Timer 
;-------------------------------------------------------------------------------
; Input: W = number of milliseconds to wait (max 256mS)
;

dx1k:		movwf	Temp1
_dx1k1:		movlw	(1000-5)/5
		movwf	Temp2
_dx1k2:		clrwdt			; avoid watchdog timeout
		nop
		decfsz	Temp2		; wait 1mS
		goto	_dx1k2
		decfsz	Temp1		
		goto	_dx1k1
		retlw	0


;----------------------------------------------------------------------------
;              	Begin Channels 1,3,5 Output Pulses
;----------------------------------------------------------------------------
Start135:	movlw	(1<<CH_1)|(1<<CH_3)|(1<<CH_5)
		movwf	PORTC		; start all outputs
		movlw	(750-9)/6
		movwf	Temp1
		NO_OP	3		; wait 750uS
		decfsz	Temp1
		goto	$-3	
		retlw	0

	ifdef	CH_6

;----------------------------------------------------------------------------
;              	Begin Channels 2,4,6 Output Pulses
;----------------------------------------------------------------------------
Start246:	movlw	(1<<CH_2)|(1<<CH_4)|(1<<CH_6)
		movwf	PORTC		; start all outputs
		movlw	(750-9)/6
		movwf	Temp1
		NO_OP	3		; wait 750uS
		decfsz	Temp1
		goto	$-3	
		retlw	0

	else

;----------------------------------------------------------------------------
;              	Begin Channels 2,4 Output Pulses
;----------------------------------------------------------------------------
Start24:	movlw	(1<<CH_2)|(1<<CH_4)
		movwf	PORTC		; start all outputs
		movlw	(750-9)/6
		movwf	Temp1
		NO_OP	3		; wait 750uS
		decfsz	Temp1
		goto	$-3	
		retlw	0

	endif

;----------------------------------------------------------------------------
;			Output Frame to Servos
;----------------------------------------------------------------------------
;
; There is not enough time left in the frame to output all the servo pulses
; sequentially. Therefore, channels 1, 3, and 5 are output simultaneously,
; followed by channels 2, 4, and 6.
;
; To produce 3 pulses simultaneously with maximum resolution, we convert the 
; PWM times to increments, eg. if the times for each channel were 10, 11, and 
; 13, their increments would be 10, 1, and 2.
; 

Output:		movf	PWM_1,w		; which channel will be first?		
		subwf	PWM_3,w		
		skpnz
		goto	Out15		; 1=3
		skpc			 
		goto    Out31		; 3<1
		movf	PWM_3,w
		subwf	PWM_5,w
		skpnz			
		goto	Out13X		; 1<3=5
		skpnc		
		goto	Out135		; 1<3<5
		movf	PWM_1,w
		subwf	PWM_5,w		
		skpnz	
		goto	Out1X3		; 1=5<3
		skpc			
		goto	Out513		; 5<1<3
		goto	Out153		; 1<5<3

Out15:		movf	PWM_1,w
		subwf	PWM_5,w
		skpnz
		goto	Out1XX		; 1=3=5
		skpc
		goto	Out51X		; 5<1=3
		goto	Out1X5		; 1=3<5

Out31:		movf	PWM_5,w		
		subwf	PWM_1,w
		skpnz
		goto	Out31X		; 3<1=5
		skpc
		goto	Out315		; 3<1<5
		movf	PWM_3,w
		subwf	PWM_5,w
		skpnz
		goto	Out3X1		; 3=5<1
		skpc
		goto	Out531		; 5<3<1
		goto	Out351		; 3<5<1


Out135:		movf	PWM_1,w
		subwf	PWM_3				
		addwf	PWM_3,w
		subwf	PWM_5

		call	Start135	; start ch1,ch3,ch5 servo pulses

		nop
		movlw	(1<<CH_3)|(1<<CH_5)
		nop
		decfsz	PWM_1		; wait 6uS * PWM_1
		goto	$-4
		movwf	PORTC		; end ch1 servo pulse  

		nop
		movlw	(1<<CH_5)
		nop
		decfsz	PWM_3		; wait 6uS * PWM_3		
		goto	$-4
		movwf	PORTC		; end ch3 servo pulse  		

		nop
		movlw	0
		nop
		decfsz	PWM_5		; wait 6uS * PWM_5		
		goto	$-4
		movwf	PORTC		; end ch5 servo pulse	
		goto	Out2
		
		
Out13X:		movf	PWM_1,w
		subwf	PWM_3

		call	Start135

		nop
		movlw	(1<<CH_3)|(1<<CH_5)
		nop
		decfsz	PWM_1		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	0
		nop
		decfsz	PWM_3				
		goto	$-4
		movwf	PORTC			
		goto	Out2
		
Out1X3:		movf	PWM_1,w
		subwf	PWM_3

		call	Start135

		nop
		movlw	(1<<CH_3)
		nop
		decfsz	PWM_1				
		goto	$-4
		movwf	PORTC		  		

		nop
		movlw	0
		nop
		decfsz	PWM_3				
		goto	$-4
		movwf	PORTC			
		goto	Out2

		


Out1X5:		movf	PWM_1,w
		subwf	PWM_5

		call	Start135

		nop
		movlw	(1<<CH_5)
		nop
		decfsz	PWM_1		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	0
		nop
		decfsz	PWM_5				
		goto	$-4
		movwf	PORTC			
		goto	Out2
		
Out51X:		movf	PWM_5,w
		subwf	PWM_1

		call	Start135

		nop
		movlw	(1<<CH_1)|(1<<CH_3)
		nop
		decfsz	PWM_5		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	0
		nop
		decfsz	PWM_1				
		goto	$-4
		movwf	PORTC			
		goto	Out2
		
Out1XX:		call	Start135

		nop
		movlw	0		
		nop
		decfsz	PWM_1				
		goto	$-4
		movwf	PORTC			
		goto	Out2
		
Out153:		movf	PWM_1,w
		subwf	PWM_5
		addwf	PWM_5,w
		subwf	PWM_3

		call	Start135

		nop
		movlw	(1<<CH_3)|(1<<CH_5)
		nop
		decfsz	PWM_1		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	(1<<CH_3)
		nop
		decfsz	PWM_5				
		goto	$-4
		movwf	PORTC		  		

		nop
		movlw	0
		nop
		decfsz	PWM_3				
		goto	$-4
		movwf	PORTC			
		goto	Out2


Out351:		movf	PWM_3,w
		subwf	PWM_5
		addwf	PWM_5,w
		subwf	PWM_1

		call	Start135

		nop
		movlw	(1<<CH_1)|(1<<CH_5)
		nop
		decfsz	PWM_3		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	(1<<CH_1)
		nop
		decfsz	PWM_5				
		goto	$-4
		movwf	PORTC		  		

		nop
		movlw	0
		nop
		decfsz	PWM_1				
		goto	$-4
		movwf	PORTC			
		goto	Out2
		
		
Out315:		movf	PWM_3,w
		subwf	PWM_1
		addwf	PWM_1,w
		subwf	PWM_5

		call	Start135

		nop
		movlw	(1<<CH_1)|(1<<CH_5)
		nop
		decfsz	PWM_3		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	(1<<CH_5)
		nop
		decfsz	PWM_1				
		goto	$-4
		movwf	PORTC		  		

		nop
		movlw	0
		nop
		decfsz	PWM_5				
		goto	$-4
		movwf	PORTC			
		goto	Out2
		
		
Out31X:		movf	PWM_3,w
		subwf	PWM_1

		call	Start135

		nop
		movlw	(1<<CH_1)|(1<<CH_5)
		nop
		decfsz	PWM_3		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	0
		nop
		decfsz	PWM_1				
		goto	$-4
		movwf	PORTC			
		goto	Out2
		
		

Out3X1:		movf	PWM_3,w
		subwf	PWM_1

		call	Start135

		nop
		movlw	(1<<CH_1)
		nop
		decfsz	PWM_3		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	0
		nop
		decfsz	PWM_1				
		goto	$-4
		movwf	PORTC			
		goto	Out2
		

Out531:		movf	PWM_5,w
		subwf	PWM_3
		addwf	PWM_3,w
		subwf	PWM_1

		call	Start135

		nop
		movlw	(1<<CH_1)|(1<<CH_3)
		nop
		decfsz	PWM_5		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	(1<<CH_1)
		nop
		decfsz	PWM_3				
		goto	$-4
		movwf	PORTC		  		

		nop
		movlw	0
		nop
		decfsz	PWM_1			
		goto	$-4
		movwf	PORTC			
		goto	Out2


Out513:		movf	PWM_5,w
		subwf	PWM_1
		addwf	PWM_1,w
		subwf	PWM_3

		call	Start135

		nop
		movlw	(1<<CH_1)|(1<<CH_3)
		nop
		decfsz	PWM_5		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	(1<<CH_3)
		nop
		decfsz	PWM_1				
		goto	$-4
		movwf	PORTC		  		

		nop
		movlw	0
		nop
		decfsz	PWM_3				
		goto	$-4
		movwf	PORTC			


Out2:
	ifdef	CH_6

		movf	PWM_2,w		
		subwf	PWM_4,w		
		skpnz
		goto	Out26		; 2=4
		skpc			 
		goto    Out42		; 4<2
		movf	PWM_4,w
		subwf	PWM_6,w
		skpnz			
		goto	Out24X		; 2<4=6
		skpnc		
		goto	Out246		; 2<4<6
		movf	PWM_2,w
		subwf	PWM_6,w		
		skpnz	
		goto	Out2X4		; 2=6<4
		skpc			
		goto	Out624		; 6<2<4
		goto	Out264		; 2<6<4

Out26:		movf	PWM_2,w
		subwf	PWM_6,w
		skpnz
		goto	Out2XX		; 2=4=6
		skpc
		goto	Out62X		; 6<2=4
		goto	Out2X6		; 2=4<6

Out42:		movf	PWM_6,w		
		subwf	PWM_2,w
		skpnz
		goto	Out42X		; 4<2=6
		skpc
		goto	Out426		; 4<2<6
		movf	PWM_4,w
		subwf	PWM_6,w
		skpnz
		goto	Out4X2		; 4=6<2
		skpc
		goto	Out642		; 6<4<2
		goto	Out462		; 4<6<2

Out246:	; ch2<ch4<ch6

		movf	PWM_2,w
		subwf	PWM_4
		addwf	PWM_4,w
		subwf	PWM_6

		call	Start246

		nop
		movlw	(1<<CH_4)|(1<<CH_6)
		nop
		decfsz	PWM_2		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	(1<<CH_6)
		nop
		decfsz	PWM_4				
		goto	$-4
		movwf	PORTC		  		

		nop
		movlw	0
		nop
		decfsz	PWM_6				
		goto	$-4
		movwf	PORTC			
		retlw	0
		
		
Out24X:	; ch2<ch4=ch6		

		movf	PWM_2,w
		subwf	PWM_4

		call	Start246

		nop
		movlw	(1<<CH_4)|(1<<CH_6)
		nop
		decfsz	PWM_2		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	0
		nop
		decfsz	PWM_4				
		goto	$-4
		movwf	PORTC			
		retlw	0
		
Out2X4:	; ch2=ch6<ch4

		movf	PWM_2,w
		subwf	PWM_4

		call	Start246

		nop
		movlw	(1<<CH_4)
		nop
		decfsz	PWM_2				
		goto	$-4
		movwf	PORTC		  		

		nop
		movlw	0
		nop
		decfsz	PWM_4				
		goto	$-4
		movwf	PORTC			
		retlw	0

		

Out2X6:	; ch2=ch4<ch6		

		movf	PWM_2,w
		subwf	PWM_6

		call	Start246

		nop
		movlw	(1<<CH_6)
		nop
		decfsz	PWM_2		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	0
		nop
		decfsz	PWM_6				
		goto	$-4
		movwf	PORTC			
		retlw	0
		


		
Out62X:	; ch6<ch2=ch4

		movf	PWM_6,w
		subwf	PWM_2

		call	Start246

		nop
		movlw	(1<<CH_2)|(1<<CH_4)
		nop
		decfsz	PWM_6		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	0
		nop
		decfsz	PWM_2				
		goto	$-4
		movwf	PORTC			
		retlw	0
		
		
		
Out2XX:	; ch2=ch4=ch6		

		call	Start246

		nop
		movlw	0
		nop
		decfsz	PWM_2				
		goto	$-4
		movwf	PORTC			
		retlw	0
		

		
Out264:	; ch2<ch6<ch4

		movf	PWM_2,w
		subwf	PWM_6
		addwf	PWM_6,w
		subwf	PWM_4

		call	Start246

		nop
		movlw	(1<<CH_4)|(1<<CH_6)
		nop
		decfsz	PWM_2		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	(1<<CH_4)
		nop
		decfsz	PWM_6				
		goto	$-4
		movwf	PORTC		  		

		nop
		movlw	0
		nop
		decfsz	PWM_4				
		goto	$-4
		movwf	PORTC			
		retlw	0
		

Out462:	; ch4<ch6<ch2

		movf	PWM_4,w
		subwf	PWM_6
		addwf	PWM_6,w
		subwf	PWM_2

		call	Start246

		nop
		movlw	(1<<CH_2)|(1<<CH_6)
		nop
		decfsz	PWM_4		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	(1<<CH_2)
		nop
		decfsz	PWM_6				
		goto	$-4
		movwf	PORTC		  		

		nop
		movlw	0
		nop
		decfsz	PWM_2				
		goto	$-4
		movwf	PORTC			
		retlw	0
		

		
Out426:	; ch4<ch2<ch6		

		movf	PWM_4,w
		subwf	PWM_2
		addwf	PWM_2,w
		subwf	PWM_6

		call	Start246

		nop
		movlw	(1<<CH_2)|(1<<CH_6)
		nop
		decfsz	PWM_4		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	(1<<CH_6)
		nop
		decfsz	PWM_2				
		goto	$-4
		movwf	PORTC		  		

		nop
		movlw	0
		nop
		decfsz	PWM_6				
		goto	$-4
		movwf	PORTC			
		retlw	0
		
		
Out42X:	; ch4<ch2=ch6

		movf	PWM_4,w
		subwf	PWM_2

		call	Start246

		nop
		movlw	(1<<CH_2)|(1<<CH_6)
		nop
		decfsz	PWM_4		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	0
		nop
		decfsz	PWM_2				
		goto	$-4
		movwf	PORTC			
		retlw	0
		

Out4X2:	; ch4=ch6<ch2

		movf	PWM_4,w
		subwf	PWM_2

		call	Start246

		nop
		movlw	(1<<CH_2)
		nop
		decfsz	PWM_4		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	0
		nop
		decfsz	PWM_2				
		goto	$-4
		movwf	PORTC			
		retlw	0
		

Out642:	; ch6<ch4<ch2

		movf	PWM_6,w
		subwf	PWM_4
		addwf	PWM_4,w
		subwf	PWM_2

		call	Start246

		nop
		movlw	(1<<CH_2)|(1<<CH_4)
		nop
		decfsz	PWM_6		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	(1<<CH_2)
		nop
		decfsz	PWM_4			
		goto	$-4
		movwf	PORTC		  		

		nop
		movlw	0
		nop
		decfsz	PWM_2				
		goto	$-4
		movwf	PORTC			
		retlw	0
		

Out624:		; ch6<ch2<ch4		

		movf	PWM_6,w
		subwf	PWM_2
		addwf	PWM_2,w
		subwf	PWM_4

		call	Start246

		nop
		movlw	(1<<CH_2)|(1<<CH_4)
		nop
		decfsz	PWM_6		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	(1<<CH_4)
		nop
		decfsz	PWM_2				
		goto	$-4
		movwf	PORTC		  		

		nop
		movlw	0
		nop
		decfsz	PWM_4				
		goto	$-4
		movwf	PORTC			

		retlw	0

	else
		
		movf	PWM_2,w		
		subwf	PWM_4,w
		skpnz
		goto	Out2X		
		skpc
		goto    Out42		

Out24:	; ch2<ch4

		movf	PWM_2,w
		subwf	PWM_4

		call	Start24

		nop
		movlw	(1<<CH_4)
		nop
		decfsz	PWM_2		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	0
		nop
		decfsz	PWM_4				
		goto	$-4
		movwf	PORTC		  		

		retlw	0
		
Out42:	; ch4<ch2

		movf	PWM_4,w
		subwf	PWM_2

		call	Start24

		nop
		movlw	(1<<CH_2)
		nop
		decfsz	PWM_4		
		goto	$-4
		movwf	PORTC		  

		nop
		movlw	0
		nop
		decfsz	PWM_2				
		goto	$-4
		movwf	PORTC		  		

		retlw	0

Out2X	; ch2=ch4

		call	Start24

		nop
		movlw	0
		nop
		decfsz	PWM_2		
		goto	$-4
		movwf	PORTC	

		retlw	0

	endif

;******************************************************************************
;				Main
;******************************************************************************		


Main:		btfsc	Flags,WATCH	; did the watchdog timeout ?
        	goto    Failsafe	; oops! try to keep going ...

		clrf	Flags		; clear all flags

		movlw	250		; wait 250mS for Rx to stabilise 
		call	dx1k		; (signal LED is on)

Start:		movlw	(1500-750)/6			
		movwf	PPM_1		
		movwf	PPM_2		; all channels at midpoint		
		movwf	PPM_3		
		movwf	PPM_4		           
		movwf	PPM_5		           
		ifdef	CH_6
		movwf	PPM_6
		endif
		
		movlw	GOODCOUNT	; set number of good frames required
		movwf	GoodFrames	; before saving failsafe values.

		movlw	HOLDCOUNT	; set number of bad frames allowed
		movwf	HoldFrames	; before going to failsafe.

		movlw	ARMCOUNT	; set number of low throttle frames
		movwf	ArmFrames	; before arming throttle

		goto	no_signal	


Failsafe:	movlw	1
		movwf	HoldFrames	; stay in failsafe until signal returns		

		btfss	Flags,GOT_FS	; do we have good failsafe values?
		goto	no_signal	

		movf	FLS_1,W
		movwf	PWM_1		
		movf	FLS_2,W		; get failsafe values
		movwf	PWM_2           
		movf	FLS_3,W
		movwf	PWM_3		
		movf	FLS_4,W
		movwf	PWM_4
		movf	FLS_5,W
		movwf	PWM_5
		ifdef	CH_6
		movf	FLS_6,W
		movwf	PWM_6
		endif

		call	Output		; output failsafe frame

		bcf	Flags,ARMED	; keep throttle OFF!
		movlw	ARMCOUNT
		movwf	ArmFrames	; recharge arming delay	

no_signal:	
		
wait_sync:	clrwdt			; we're still sane, no reset please!

		clrf	Temp1
		movlw	10		; set 'gap detect' timeout to 25.6mS
		movwf	Temp2
		
wait_gap:	SKIP_PPM_HI		; wait for a gap	]
		goto	time_gap	; 			]
		nop			;			]
		clrwdt			;			]
		nop			;			] 10uS per loop
		decf	Temp1		;			]
		skpnz			;			]
		decfsz	Temp2		; timed out ?		]
		goto	wait_gap	;			] 
		goto	badframe	; can't find sync gap!	 		
	
time_gap:	clrf	PPMcount	; reset gap timer 	

in_gap:		decf	Temp1		;			}
		skpnz			;			}
		decf	Temp2		; timed out ?		}
		skpnz			;			} 
		goto	badframe	; can't find sync gap!	} 10uS per loop
		SKIP_PPM_LO		; still in gap ?	}
		goto	wait_gap	; 			}
		incfsz	PPMcount	; gap > 2.56mS ?	}
		goto	in_gap		; no, continue timing	}

Get_Frame:	movlw	128-(23000/256)	; set 23mS timeout
		movwf	TMR0
wait_1st:	clrwdt
		btfsc	TMR0,7		; timer reached 128 ?
		goto	badframe	
		SKIP_PPM_HI		; wait for start of first channel
		goto	wait_1st
		clrf	Channels	; no channels received yet!
		NO_OP	4				
		call	GetPPM		; get first channel
		addlw   -2
		skpz
		goto	badframe
		movf	PPMcount,W
		movwf	PPI_1
		incf	Channels
		call	GetPPM		; get 2nd channel
		addlw   -2
		skpz
		goto	badframe
		movf	PPMcount,W
		movwf	PPI_2
		incf	Channels
		call	GetPPM		; get 3rd channel
		addlw   -2
		skpz    
		goto	badframe
		movf	PPMcount,W
		movwf	PPI_3
		incf	Channels
		call	GetPPM		; get 4th channel
		addlw   -2
		skpz
		goto	nochannel
		movf	PPMcount,W
		movwf	PPI_4
		incf	Channels
		call	GetPPM		; get 5th channel
		addlw	-2
		skpz
		goto	nochannel
		movf	PPMcount,W
		movwf	PPI_5
		incf	Channels
		ifdef	CH_6
		call	GetPPM		; get 6th channel
		addlw   -2
		skpz
		goto	nochannel
		movf	PPMcount,W
		movwf	PPI_6
		incf	Channels
		endif
		goto	update
		
nochannel:	addlw	1
		skpz			; channel timed out?
		goto	badframe
		btfss	Flags,GOT_FS	; got failsafe frame yet ?
		goto	update		; no, ignore missing channels		
		movf	Channels,w
		subwf	NumChannels	; got correct number of channels ?
		skpnz
		goto	update		; yes

badframe:	movlw	ARMCOUNT	; reset throttle arming delay
		movwf	ArmFrames
		decfsz	HoldFrames	; too many bad frames ?
		goto	hold
		goto	Failsafe

hold:		btfss	Flags,GOT_FS	; good frame available for hold ?
		goto	no_signal
		movf	PPA_1,w
		movwf	PWM_1
		movf	PPA_2,w
		movwf	PWM_2
		movf	PPA_3,w
		movwf	PWM_3		; get previous averaged outputs
		movf	PPA_4,w		
		movwf	PWM_4
		movf	PPA_5,w
		movwf	PWM_5
		ifdef	CH_6
		movf	PPA_6,w
		movwf	PWM_6
		endif
		call	Output		; create servo pulses
		goto	no_signal	
;
; Got a good frame. Average the pulse widths of this frame and the last frame. 
;
update:		movf	PPI_1,w
		addwf	PPM_1,w		; PPA = average(this+last)   
		movwf	PPA_1
		rrf	PPA_1
		movf	PPI_2,w
		addwf	PPM_2,w
		movwf	PPA_2
		rrf	PPA_2
		movf	PPI_3,w
		addwf	PPM_3,w
		movwf	PPA_3
		rrf	PPA_3
		movf	PPI_4,w
		addwf	PPM_4,w
		movwf	PPA_4
		rrf	PPA_4
		movf	PPI_5,w
		addwf	PPM_5,w
		movwf	PPA_5
		rrf	PPA_5
		ifdef	CH_6
		movf	PPI_6,w
		addwf	PPM_6,w
		movwf	PPA_6
		rrf	PPA_6
		endif

		movf	PPA_1,w
		movwf	PWM_1		; PWM = average
		movf	PPA_2,w
		movwf	PWM_2
		movf	PPA_3,w
		movwf	PWM_3
		movf	PPA_4,w
		movwf	PWM_4
		movf	PPA_5,w
		movwf	PWM_5
		ifdef	CH_6
		movf	PPA_6,w
		movwf	PWM_6
		endif

		btfss	Flags,GOT_FS	; failsafe frame captured ?
		goto	save_frame	; no, skip output

		ifdef	ARM_THROTTLE
		btfsc	Flags,ARMED	; throttle armed ?
		goto	do_output	; yes, do normal output
		movlw	(1300-750)/6	  
		btfsc	Flags,JR	
		subwf	PPM_1,w		; throttle < 1.3mS ?
		btfss	Flags,JR
		subwf	PWM_3,w
		skpc	
		goto	low_throttle	; yes	
		movlw	ARMCOUNT
		movwf	ArmFrames	; no, recharge arming delay
		goto	mod_throttle
low_throttle:	decfsz	ArmFrames	; got enough arming frames ?	
		goto	mod_throttle
		bsf	Flags,ARMED	; yes, arm the throttle!
		goto	save_frame
mod_throttle:	btfsc	Flags,JR
		goto	mod_jr
		movf	FLS_3,w
		movwf	PWM_3		; keep throttle off until armed
		goto	do_output
mod_jr:		movf	FLS_1,w
		movwf	PWM_1		; keep JR throttle off until armed
		endif

do_output:	call	Output		; output servo pulses

save_frame:	movf	PPI_1,W		
		movwf	PPM_1
		movf	PPI_2,W
		movwf	PPM_2
		movf	PPI_3,W
		movwf	PPM_3		; remember this frame 
		movf	PPI_4,W
		movwf	PPM_4
		movf	PPI_5,W
		movwf	PPM_5
		ifdef	CH_6
		movf	PPI_6,W
		movwf	PPM_6
		endif

		movlw	HOLDCOUNT	; recharge hold timeout
		movwf	HoldFrames

		btfsc	Flags,GOT_FS	; got failsafe frame ?
		goto	frame_done	; yes

		decfsz	GoodFrames	; got enough good frames ?
		goto	frame_done	; no

; get failsafe values

		movf	Channels,w	; remember number of channels 
		movwf	NumChannels
		movf	PPA_1,W
		movwf	FLS_1
		movf	PPA_2,W
		movwf	FLS_2
		movf	PPA_3,W		; copy averaged frame to failsafe	 
		movwf	FLS_3		
		movf	PPA_4,W
		movwf	FLS_4
		movf	PPA_5,W
		movwf	FLS_5
		ifdef	CH_6
		movf	PPA_6,W
		movwf	FLS_6
		endif
		bsf	Flags,GOT_FS	; failsafe frame captured	

		ifdef	ARM_THROTTLE
		ifdef	DETECT_JR
		movlw	(1300-750)/6
		subwf	FLS_1,w		; channel 1 < 1.3mS ?
		skpc
		goto    jr_detected
		movlw	(1700-750)/6
		subwf	FLS_1,w		; channel 1 >= 1.7mS ?
		skpc
		goto	futaba
		movlw	(1100-750)/6	; low throttle in failsafe!
		movwf	FLS_1
jr_detected:	bsf	Flags,JR	; JR throttle detected
		goto	arm
		endif	; DETECT_JR

futaba:		movlw	(1300-750)/6
		subwf	FLS_3,w		; channel 3 >= 1.3mS ?
		skpc
		goto	arm
		movlw	(1100-750)/6	; low throttle in failsafe!
		movwf	FLS_3

arm:		bsf	Flags,ARMED	; arm throttle now
		endif   ; ARM_THROTTLE

frame_done:	goto	wait_sync	; wait for next frame



	

;---------- Oscillator Calibration Subroutine  --------------

		org	0x3ff
		ifdef	OSCAL_NO
		retlw	OSCAL_NO
		endif	

		END