;***************************************************************************
;      MAD3.asm   Decoder for Two Magnetic Actuators and Throttle
;***************************************************************************
;            Bruce Abbott   bhabbott@paradise.net.nz 
;
;      for Microchip PIC12C5xx/12F6xx running at @ 4MHz (INT RC Clock)
;
;===========================================================================
;                            Description
;
; Decodes PPM input into PWM outputs, giving proportional control of two 
; magnetic actuators and throttle.  
;
;
; ==========================================================================
;                          Summary of Changes
;
; 2004/2/2  V0.01  
;
; --------------------------------------------------------------------------
;

#DEFINE version  0
#DEFINE revision 1

;	 PROCESSOR PIC12C508
;        PROCESSOR PIC12F675
	
	ifdef	  __12C508
        INCLUDE   <P12C508.inc>
	__CONFIG  _MCLRE_OFF&_CP_OFF&_WDT_ON&_IntRC_OSC	
	else
        INCLUDE   <P12F675.inc>
	__CONFIG  _MCLRE_OFF&_CP_OFF&_WDT_ON&_BODEN_ON&_INTRC_OSC_NOCLKOUT
	endif

         radix     dec

	errorlevel 0,-305,-302


;#DEFINE OSCCAL_NO 0x70	; enable if OSCCAL value was erased!

; Bit definitions for the GPIO register and the TRIS register

#DEFINE MOTOR    0    ; pin 7   Motor PWM   
#DEFINE LEFT	 1    ; pin 6   Left PWM out
#DEFINE RIGHT	 2    ; pin 5   Right PWM out
#DEFINE PPM_in	 3    ; pin 4   input pulse stream
#DEFINE DOWN	 4    ; pin 3   Down PWM out 
#DEFINE UP	 5    ; pin 2   Up PWM out

#DEFINE TrisBits (1<<PPM_in)	; Port pins that are inputs

; 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 Watchdog, ~550mS timeout.
;
;
#DEFINE OptionBits B'11011101'

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

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

; ==========================================================================
;                 RAM Variable Definitions  
;

; PWM output values, loaded into PWM counters at start of each PWM cycle.

 	BYTE	Start_PWM	; GPIO output at start of PWM cycle 

	BYTE	Right_PWM	; PWM high time for right rudder
	BYTE	Left_PWM	; PWM high time for left rudder
	BYTE	Up_PWM		; PWM high time for up elevator
	BYTE	Down_PWM	; PWM high time for down elevator
	BYTE	Motor_PWM	; PWM high time for motor

; PWM counters 

	BYTE	pwm_period	; counts PWM period time 		

	BYTE	right_count	; counts PWM high time for right rudder
	BYTE	left_count	; counts PWM high time for left rudder
	BYTE	up_count	; counts PWM high time for up elevator
	BYTE	down_count	; counts PWM high time for down elevator
	BYTE	motor_count	; counts PWM high time for motor
	
; channel inputs

        BYTE	Rudder		; channel 1 (JR channel 2)      
        BYTE	Elevator	; channel 2 (JR channel 3)
	BYTE	Throttle	; channel 3 (JR channel 1)

; previous channel inputs
	
       	BYTE	PMM_1		; channel 1 memory        
        BYTE	PMM_2		; channel 2 memory
	BYTE	PMM_3		; channel 3 memory

; other variables

        BYTE	Flags		; various boolean flags            

	BYTE	GoodFrames	; No. of good frames that must be received
				; before starting output.
	BYTE	HoldFrames	; No. of bad frames to hold until failsafe

	BYTE	temp_1		; temporary storage 	
	BYTE	temp_2
        BYTE	temp_3		 


#DEFINE PWMSTEPS  16		; no. of steps in a PWM cycle


;
; flag values
;
#DEFINE TIMEOUT	0		; watchdog timed out
#DEFINE STARTED	1		; got enough good frames
#DEFINE	JR	2		; JR transmitter detected

;
; constants
;
#DEFINE GOODCOUNT  5	; good frames required before getting failsafe 

#DEFINE HOLDCOUNT 25 	; bad frames acceptable before going to failsafe 


;***************************************************************************
;                                Code
;***************************************************************************

	ORG	0
	goto	ColdStart

;-------------------------- version string ---------------------------------

	org	8		
	dt	"--MAG3--"
	dt	"--V"
	dw	version+"0"
	dt	"."
	dw	revision+"0"
	dt	"--"

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

;-----------------------------------------------------------------------
;                      Generate PWM output
;-----------------------------------------------------------------------
;
; Takes 20uS (including call) 
;
; Must be called every 26uS.   
;
;
;
DoPWM:		decfsz	pwm_period	;3    end of PWM cycle ?
		goto	pwm_step	;4(5) no, do the next step
		movf	Left_PWM,w	;5   yes, 
		movwf	left_count	;6
		movf	Right_PWM,w	;7
		movwf	right_count	;8
		movf	Up_PWM,w	;9  reload all counters
		movwf	up_count	;10
		movf	Down_PWM,w	;11
		movwf	down_count	;12
		movf	Motor_PWM,w	;13
		movwf	motor_count	;14
		movlw	PWMSTEPS	;15
		movwf	pwm_period	;16
		movf	Start_PWM,w	;17 restart active PWMs
		movwf	GPIO		;18
		retlw	0		;19,20
		
pwm_step:	movlw	0		;6  Assume all PWMs will be OFF
		decfsz	left_count	;7  Count down. If count<>0 then  
		iorlw	1<<LEFT		;8  PWM output is allowed ON.
		decfsz	right_count	;9 
		iorlw	1<<RIGHT	;10
		decfsz	up_count	;11
		iorlw	1<<UP		;12
		decfsz	down_count	;13
		iorlw	1<<DOWN		;14
		decfsz	motor_count	;15
		iorlw	1<<MOTOR	;16
		nop			;17   Turn off any PWMs that have
		andwf	GPIO		;18   just timed out. 
		retlw	0		;19,20

		


;----------------------------------------------------------------------------
;              Exponential Throttle translation table
;
; timing: call expo ;2
;         addwf pcl ;2  total = 6 
;         retlw     ;2

Expo:		addwf	PCL
		RETLW	0		;0
		RETLW	1		;1
		RETLW	3		;2
		RETLW	5		;3
		RETLW	7		;4
		RETLW	8		;5
		RETLW	9		;6
		RETLW	10		;7
		RETLW	11		;8
		RETLW	12		;9
		RETLW	12		;10
		RETLW	13		;11
		RETLW	13		;12
		RETLW	14		;13
		RETLW	14		;14
		RETLW	15		;15


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

ColdStart:		
	bcf	Flags,TIMEOUT
	btfss	STATUS,NOT_TO		; did Watchdog time out ? 
	bsf	Flags,TIMEOUT

	ifdef	__12C508
	ifdef	OSCCAL_NO		; get OSCAL value		
	movlw	OSCCAL_NO			
	endif
	movwf	OSCCAL			; set oscillator calibration 
	else 
	bsf	STATUS,RP0		; register bank 1 (12F629/75) 
	call	0x3ff			; get OSCCAL value
	movwf	OSCCAL			; set oscillator calibration 
        bcf	STATUS, RP0		; register bank 0 
        endif
                 
; Move prescaler from tmr0 to the watchdog, without causing accidental reset!

        clrwdt                    
        clrf	TMR0            
        movlw	OptionBits | 7  
        option                    	
        clrwdt
        movlw	OptionBits
        option
        clrwdt

; initialise I/O registers 

        clrf	GPIO			; all outputs low
	ifdef	__12C508
	movlw	TrisBits
	Tris	GPIO
	else
	bsf	STATUS,RP0		; register bank 1 
        movlw	TrisBits
        movwf	TRISIO			; set I/O pin directions
	ifdef	ANSEL
	clrf	ANSEL			; disable analog inputs (12F675)
	endif
	bcf	STATUS,RP0		; register bank 0
	movlw	b'00000111'		      
        movwf	CMCON			; Comparator off
	endif

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

       goto	Main		
							                               

;----------------------------------------------------------------------------
; GetPPM:             Get time to next PPM pulse	
;---------------------------------------------------------------------------- 
;
; input:	PPM pulse has just started 
; output:	PPMcount = Pulse Width * 26uS, next pulse has started
; error:	PPMcount = 0 
;
; starting 4 clocks after dopwm
; ending 3 clocks after dopwm
;
GetPPM:		movlw	1		;5 init PPMcount
		movwf	temp_1		;6
hiloop:		call	DoPWM		;			
		btfss	GPIO,PPM_in	;1 signal gone low ?
		goto	pulselo		;2(3)
		nop			;3
		incfsz	temp_1		;4 count up
		goto	hiloop		;5(6)
		nop			;6
		call	DoPWM
		nop			;1
		retlw	0		;2,3 timed out

loloop:		call	DoPWM
		btfsc	GPIO,PPM_in	;1 start of next channel pulse ?
		retlw	0		;2(3) return OK
		nop			;3
pulselo:	incfsz  temp_1		;4 count up to 256 @ 2.28mS
		goto	loloop		;5(6)
		nop			;6
		call	DoPWM
		nop			;1
		retlw	0		;2,3 timed out		
	

;-------------------------------------------------------------------------------
;                    Millisecond Delay Timer 
;-------------------------------------------------------------------------------
; Input: W = number of milliseconds to wait x 2 (max 512mS)
;
dx2k:		movwf	temp_1		
_dx2k1:		movlw	(2000-2)/10	
		movwf	temp_2		
_dx2k2:		clrwdt			; avoid watchdog timeout			
		NO_OP	6						
		decfsz	temp_2		
		goto	_dx2k2		
		decfsz	temp_1		
		goto	_dx2k1		
		retlw	0		


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


Main:		btfsc	Flags,TIMEOUT	; did the watchdog timeout ?
        	goto    wait_sync	; oops! Try to continue...

		movlw	250		; wait 500mS for Rx to stabilise 
		call	dx2k		; 

Start:		clrf	Flags		; clear all flags

		movlw	1500/26		; 1.5mS			
		movwf	PMM_1	
		movwf	PMM_2		; All channels centered
		movwf	PMM_3		

		movlw	GOODCOUNT	; set number of good frames required
		movwf	GoodFrames	; before starting up.

		movlw	HOLDCOUNT	; set number of bad frames allowed
		movwf	HoldFrames	; before stopping output 

		clrf	Left_PWM	; all PWM off
		clrf	Right_PWM
		clrf	Up_PWM
		clrf	Down_PWM
		clrf	Motor_PWM
		clrf	Start_PWM
		movlw	1
		movwf	pwm_period	; 1st DoPWM inits all counters	

wait_sync:	movlw	high(25000/52)	;4 
		movwf	temp_1		;5
		nop			;6
		call	DoPWM
		movlw	low(25000/52)	;1 set 'gap detect' timeout to 25mS
		movwf	temp_2		;2
		goto	wait_gap	;3,4

wait_gap1:	nop			;4			]
wait_gap:	nop			;5			]
wait_gap2:	clrwdt			;6 reset watchdog	]
		call	DoPWM		;			]
		NO_OP	3		;1~3			]
		btfss	GPIO,PPM_in	;4 is there a gap ?	] 52uS per loop
		goto	time_gap	;5(6) yes		]
		nop			;6			]
		call	DoPWM		;     no		]
		decfsz	temp_2		;1			]
		goto	wait_gap1	;2(3)			]
		decfsz	temp_1		;3 timed out ?		
		goto	wait_gap2	;4(5)			 
		goto	badframe	;5,6 can't find sync gap!	 		
	
time_gap:	call	DoPWM
		movlw	(2500/26)	;1 init gap length timer
		movwf	temp_3		;2
		NO_OP	3		;3~5
time_gap1:	nop			;6
		call	DoPWM
		decfsz	temp_2		;1			
		goto	time_gap2	;2(3)
		decfsz	temp_1		;3 timed out ?		
		goto	time_gap3	;4(5)			 
		goto	badframe	;5,6 
time_gap2:	NO_OP	2		;4,5
time_gap3:	nop			;6 
		call	DoPWM
		btfsc	GPIO,PPM_in	;1 still in gap ?	
		goto	wait_gap1	;2(3) no, wait for another gap 			
		decfsz	temp_3		;3 yes, is gap > 2.5mS ?	
		goto	time_gap1	;4(5) no, continue timing
		goto	wait_1st	;5,6 yes, sync gap detected

loop_1st:	clrwdt			;4 reset watchdog
		NO_OP	2		;5,6			
wait_1st:	call	DoPWM
		btfss	GPIO,PPM_in	;1 wait for start of first channel
		goto	loop_1st	;2(3)
		call	GetPPM		;3 get first channel
		movf	temp_1,w	;4
		movwf	Rudder		;5
		nop			;6
		call	DoPWM
		NO_OP	2		;1,2
		call	GetPPM		;3 get 2nd channel
		movf	temp_1,w	;4
		movwf	Elevator	;5
		nop			;6
		call	DoPWM
		NO_OP	2		;1,2
		call	GetPPM		;3 get 3rd channel
		movf	temp_1,w	;4
		movwf	Throttle	;5
		nop			;6
		call	DoPWM
		goto	check		;1,2
		

badframe:	call	DoPWM
		decfsz	HoldFrames	;1 too many bad frames ?
		goto	wait_sync	;2(3) no
Failsafe:	movlw	1		;
		movwf	HoldFrames	; Stay in failsafe  		
		clrf	Start_PWM	; 
		clrf	GPIO		; Stop all PWM now!
		goto	wait_sync	; 

;
; Error if channel width out of range.
;
check:
		movlw	750/26		;3  
		subwf	Rudder,w	;4
		skpc			;5  < 0.75mS ?
		goto	badframe	;6  too short
		call	DoPWM
		nop			;1
		movlw	750/26		;2
		subwf	Elevator,w	;3
		skpc			;4
		goto	badframe	;5(6)
		nop			;6
		call	DoPWM
		nop			;1
		movlw	750/26		;2
		subwf	Throttle,w	;3
		skpc			;4
		goto	badframe	;5(6)
		nop			;6
		call	DoPWM
		nop			;1
		movlw	2300/26		;2  
		subwf	Rudder,w	;3
		skpnc			;4  > 2.3mS ?
		goto	badframe	;5(6) too long
		nop			;6
		call	DoPWM
		nop			;1
		movlw	2300/26		;2
		subwf	Elevator,w	;3
		skpnc			;4
		goto	badframe	;5(6)
		nop			;6
		call	DoPWM
		nop			;1
		movlw	2300/26		;2
		subwf	Throttle,w	;3
		skpnc			;4
		goto	badframe	;5(6)
		nop			;6
		call	DoPWM
;
; output = average ( current , previous )
;
		movf	Rudder,w	;1
		movwf	temp_1		;2
		movf	PMM_1,w		;3
		addwf	Rudder		;4
		rrf	Rudder		;5
		nop			;6
		call	DoPWM
		movf	Elevator,w	;1
		movwf	temp_2		;2
		movf	PMM_2,w		;3
		addwf	Elevator	;4
		rrf	Elevator	;5
		nop			;6
		call	DoPWM
		movf	Throttle,w	;1
		movwf	temp_3		;2
		movf	PMM_3,w		;3
		addwf	Throttle	;4
		rrf	Throttle	;5
		nop			;6
		call	DoPWM

; remember the current frame. 

		movf	temp_1,W	;1 
		movwf	PMM_1		;2
		movf	temp_2,W	;3
		movwf	PMM_2		;4
		movf	temp_3,W	;5
		movwf	PMM_3		;6
		call	DoPWM
;
; We have a good frame!
;
		movlw	HOLDCOUNT	;1 reset failsafe timeout
		movwf	HoldFrames	;2
		NO_OP	4		;3~6
		call	DoPWM
		nop			;1
		btfsc	Flags,STARTED	;2 done startup yet ?  
		goto	update		;3(4) yes
		nop			;6
		call	DoPWM
		NO_OP	2		;1,2
;
; Don't start until several consecutive good frames have been detected.
;
		decfsz	GoodFrames	;3    got enough good frames ?
		goto	frame_done	;5(6) no, so don't update PWMs
		bsf	Flags,STARTED	;6   yes 
		call	DoPWM
;
; Detect channel allocations. JR and GWS transmit throttle on channel 1,
; others transmit throttle on channel 3. Channel 1 is assumed to be 
; throttle if less than 1.25mS at startup.
;
		movlw	1250/26		;1
		subwf	Rudder,w	;2 channel 1 < 1.25mS ?
		skpc			;3
		bsf	Flags,JR	;4 JR/GWS TX (throttle on channel 1)
;
;  If JR throttle detected, swap channels   
;
update:		btfsc	Flags,JR	;4    JR channel order ?
		goto	copyjr		;5(6) yes
		nop			;6    no
		call	DoPWM
		NO_OP	4		;1~4
		goto	limit		;5,6

copyjr:		call	DoPWM
		movf	Rudder,w	;1 
		movwf	temp_1		;2 temp_1 = CH1 (JR throttle)
		movf	Elevator,w	;3
		movwf	Rudder		;4 rudder = CH2 (JR rudder)
		movf	Throttle,w	;5 
		movwf	Elevator	;6 elevator = CH3 (JR elevator)
		call	DoPWM
		movf	temp_1,w	;1
		movwf	Throttle	;2 throttle = CH1 (JR throttle) 
		NO_OP	4		;3~6
;
; limit range to 1.1~1.9mS (rudder,elevator=0~31, throttle=0~15)  
;
limit:		call	DoPWM
		movlw	1100/26		;1
		subwf	Rudder		;2 
		skpc			;3
		clrf	Rudder		;4  1.1mS minimum
		NO_OP	2		;5,6
		call	DoPWM		
		movlw	1100/26		;1
		subwf	Elevator	;2 
		skpc			;3
		clrf	Elevator	;4  1.1mS minimum
		NO_OP	2		;5,6
		call	DoPWM
		movlw	1100/26		;1 
		subwf	Throttle	;2
		skpc			;3
		clrf	Throttle	;4 1.1mS minimum
		bcf	STATUS,C	;5
		rrf	Throttle	;6 throttle values = 0~15+
		call	DoPWM
		movlw	15		;1
		subwf	Throttle,w	;2
		movlw	15		;3
		skpnc			;4 
		movwf	Throttle	;5 throttle max. value = 15
		nop			;6
		call	DoPWM
		movf	Throttle,w	;1
		call	Expo		;2~7 apply exponential throttle curve
		movwf	Throttle	;8 
		call	DoPWM		;    this PWM is 2uS too late! 
		NO_OP	4		;3~6 but next PWM will be on time
;
; wait until the next PWM cycle starts. 
;
wait_pwm:	call	DoPWM
		decf	pwm_period,w	;1
		skpnz			;2
		goto	last_pwm	;3(4)
		nop			;4
		goto	wait_pwm	;5,6

last_pwm:	NO_OP	2		;5,6
		call	DoPWM		;
				
;
; Update PWM variables with new values, whilst completing the current PWM 
; cycle with old values. 
;
; split rudder and elevator into bi-directional PWM
;
;  1~15 = low      PWM = 15~1
;    16 = center   PWM = 0
; 17~31 = high     PWM = 1~15
; 
		movlw	16		;1
		subwf	Rudder,w	;2 right or left ?		
		skpc			;3
		goto	do_left		;4(5)

do_right:	movwf	Right_PWM	;5 right PWM = 0~15
		clrf	Left_PWM	;6 left PWM = 0
		call	DoPWM
		NO_OP	3		;1~3
		goto	do_elevator	;5,6

do_left:	clrf	Right_PWM	;6 right PWM = 0
		call	DoPWM
		movlw	16		;1
		movwf	Left_PWM	;2
		movf	Rudder,w	;3 left PWM = 16~1
		subwf	Left_PWM	;4		
		NO_OP	2		;5,6
do_elevator:	call	DoPWM
		movlw	16		;1		
		subwf	Elevator,w	;2  
		skpc			;3
		goto	do_down		;4(5)

do_up:		movwf	Up_PWM		;5 
		clrf	Down_PWM	;6 
		call	DoPWM
		nop			;1
		goto	do_throttle	;2,3

do_down:	clrf	Up_PWM		;6
		call	DoPWM
		movlw	16		;1
		movwf	Down_PWM	;2
		movf	Elevator,w	;3
		subwf	Down_PWM	;4
		NO_OP	2		;5,6
		call	DoPWM
		NO_OP	3		;1~3
;
; get throttle PWM
;
do_throttle:	movf	Throttle,w	;4
		movwf	Motor_PWM	;5
;
; Set starting state for each PWM output.
;
		clrf	Start_PWM	;6 Assume all PWM outputs are OFF 
		call	DoPWM
		tstf	Left_PWM	;1 PWM count = 0 ?
		skpz			;2
		bsf	Start_PWM,LEFT	;3 no, PWM output will be ON
		tstf	Right_PWM	;4
		skpz			;5
		bsf	Start_PWM,RIGHT	;6
		call	DoPWM	
		tstf	Down_PWM	;1
		skpz			;2
		bsf	Start_PWM,DOWN	;3
		tstf	Up_PWM		;4
		skpz			;5
		bsf	Start_PWM,UP	;6
		call	DoPWM
		tstf	Motor_PWM	;1
		skpz			;2
		bsf	Start_PWM,MOTOR	;3
		NO_OP	3		;4~6

frame_done:	call	DoPWM
		nop			;1
		goto	wait_sync	;2,3 wait for next frame




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

		ifdef	__12F675
		ifdef	OSCCAL_NO
		org	0x3ff
		retlw	OSCCAL_NO	
		endif
		endif

		END