;*****************************************************************************
;    Pulse width modulator initialization routine.
;          pulse period = 20ms;  initial pulse length = 1.5ms
;          (values shown are for controlling model plane servos)
;    Inputs: none
;    Returned values: none
;    Uses registers r0, r1
;
;    Calling method:   BL   pwm_init
;                                                   LDA   rev 1.0  12/01/10
;*****************************************************************************
pwm_init		STMFD	sp!,{r14}		; save link register on the stack

		        LDR     r0,=PWM_PR
				LDR     r1,=2			; prescaler = 2 divides PCLK by 3
                STR     r1,[r0]         ; load prescaler
                                        ; PCLK rate is 1 MHZ (1 uSEC period)
                LDR     r0,=PWM_MCR
				LDR     r1,=0x02        ; bit 1 is set, others are zero
				STR     r1,[r0]         ; load PWM_MCR: PWM_TC resets when PWM_TC=PWM_MR0

                LDR     r0,=PWM_MR0
				LDR     r1,=20000       ; count 20,000 1uSEC intervals = 20 milli secs
				STR     r1,[r0]         ; load match register 0. sets pulse period

				LDR     r0,=PWM_MR4
                LDR     r1,=1500		; initial pulse width = 1.5 milli secs
				STR     r1,[r0]         ; load match register 4
				LDR     r0,=PWM_MR6
				LDR     r1,=1500		; initial pulse width
				STR     r1,[r0]			; load match register 6

	            LDR     r0,=PWM_LER
				LDR     r1,=0x51
				STR     r1,[r0]         ; Enable latch registers 0, 4, & 6

                LDR     r0,=PWM_PCR
				LDR     r1,=0x5000      ; bits 12 & 14 set
				STR     r1,[r0]         ; load PWM_CR: enables PWM4 and PWM6 outputs

                LDR     r0,=PWM_TCR
				LDR     r1,=0x09        ; bits 3,1,0 set
				STR     r1,[r0]         ; load PWM_TCR enable PWM counter & prescaler

                LDR     r0,=PINSEL0
                LDR     r1,=0xA0000     ; bits 19 & 17 set.  selects output PWM 6 & 4
				STR     r1,[r0]		    ; Enable PWM6 & PWM4 outputs

end_pwm_init	LDMFD	sp!,{pc}		; Return from subroutine  pwm_init


;*****************************************************************************
;   Pulse width modulator update routine
;	Inputs:  r0 = command
;                   1 = update PWM channel 4
;                   2 = update PWM channel 6
;                   3 = update both channel 4 and 6
;            r1 = new pulse width (# of microseconds) for PWM channel 4
;            r2 = new pulse width (# of microseconds) for PWM channel 6
;   Returned values: none
;   Uses registers r0, r1, r2, r3
;
;   Calling method:  first put values in r0, r1, r2 as appropriate
;                    BL pwm_update
;                                                   LDA    rev 2.0  12/01/11
;*****************************************************************************

pwm_update		STMFD	sp!,{r14}		; r0 specifies channels to update 

load_pwm4		CMP		r0,#1			; 1 = load PWM chan 4
				BNE		load_pwm6
				LDR     r3,=PWM_MR4
				STR     r1,[r3]         ; load match register 4

load_pwm6		CMP		r0,#2			; 2 = load PWM chan 6
				BNE		load_both
				LDR     r3,=PWM_MR6
				STR     r2,[r3]			; load match register 6

load_both		CMP		r0,#3
				BNE		pwm_reg_update
				LDR		r3,=PWM_MR4
				STR		r1,[r3]
				LDR		r3,=PWM_MR6
				STR		r2,[r3]

pwm_reg_update	CMP		r0,#1			; check for just pwm 4 update
				MOVEQ	r1,#0x10
				CMP		r0,#2			; check for just pwm 6 update
				MOVEQ	r1,#0x40
				CMP		r0,#3			; check for pwm 4 & 6 update
				MOVEQ	r1,#0x50

				LDR		r3,=PWM_LER
				STR		r1,[r3]			; enable latch registers

end_pwm_update	LDMFD	sp!,{pc}		; Return from subroutine  pwm_update