;========================================================================
;                Motor PWM to Servo Pulse Converter 
;========================================================================
; 
; Replacement firmware for Xtreme Brushed to Brushless Convertor (EA-048)
;
;  0% and 100% PWM handled properly (original would stop sending pulses!) 
;  No filtering for faster response (suitable for bushless tail motor).
;  Stable servo pulse output using hardware pwm generator 
;
;                                              Bruce Abbott 2011-6-19
.NOLIST
.INCLUDE "m48def.inc"
.LIST
;
; Hardware:-
;
; Atmega48 with 16MHz ceramic resonator.
; LED on PC2 (pin 25), connected to GND via 1k resistor.
; Motor PWM input on PD6 (pin 10), low = motor on.
; Servo pulse output on PB1/OC1A (pin 13). 
;
; NOTE: Original servo pulse output was on pin 30 (PD0) but we 
;       need it to be on pin 13. Solder one end of a wire onto 
;       the test pad connected to pin 13, and the other end to 
;       the 1k resistor which is connected to pin 30.
;
;--------------------------------------------------------------------
;
;Changes:
; V0.1 2011-11-19 added FRAMETIME constant for changing ESC pulse rate
;
;--------------------------------------------------------------------
;                            Constants
;--------------------------------------------------------------------
.EQU	SERVOMIN  = 1100    ; minimum servo output pulse width (uS)
.EQU	SERVOMAX  = 1900    ; maximum servo output pulse width (uS)

.EQU	MAXPWM	  = 25000   ; 25mS = 40Hz slowest motor pwm frequency

.EQU	FRAMETIME = 20000-1 ; Servo pulse frame time (40000=20mS).  
			    ; Reduce value to increase frequency, eg.
			    ; 10000 = 200Hz (OK for Hobbywing ESCs) 

;--------------------------------------------------------------------
;                    CPU Register Assignments 
;--------------------------------------------------------------------
;
; NOTE: immediate addressing modes not permitted on R0-R15! 
; 
;             R0	; hardware multiply 
;             R1 	; hardware multiply   

.DEF	aa1 = R2	; 16 bit variable aa
.DEF	aa2 = R3

.DEF	bb1 = R4	; 16 bit variable bb
.DEF	bb2 = R5

.DEF	cc1 = R6
.DEF	cc2 = R7	; 32 bit variable cc
.DEF	cc3 = R8
.DEF	cc4 = R9

; motor PWM timers (16 bit)
.DEF	pwmon1 = R10	; motor pwm on time
.DEF	pwmon2 = R11

.DEF	pwmoff1 = R12	; motor pwm off time
.DEF	pwmoff2 = R13

; servo output pulse width (16 bit)
.DEF	servo1 = R14
.DEF	servo2 = R15

;general purpose registers (8/16 bit)
.DEF	tmp1 = R16
.DEF	tmp2 = R17

.DEF	tmp3 = R18
.DEF	tmp4 = R19

.DEF	tmp5 = R20
.DEF	tmp6 = R21

.DEF	tmp7 = R22
.DEF	tmp8 = R23

.DEF	tmp9 = R24
.DEF	tmp0 = R25

;Index Register X
;.DEF	XL   = R26
;.DEF	XH   = R27

;Index Register Y
;.DEF	YL   = R28
;.DEF	YH   = R29

;Index Register Z
;.DEF	ZL   = R30
;.DEF	ZH   = R31

;====================================================================
;                               CODE 
;====================================================================
;
;
; Power On Reset
	rjmp	main

; Interrupt Vectors	
;	(none)


; Main program starts here
.org	0x01a
main:
; stack pointer
	ldi	tmp1,LOW(RAMEND)
	out	SPL,tmp1
	ldi	tmp1,HIGH(RAMEND)
	out	SPH,tmp1

; i/o ports
	ldi	tmp1,0b11111111
	out	DDRB,tmp1	; set portb directions
	ldi	tmp1,0
	out	PORTB,tmp1	; all portb outputs low
	ldi	tmp1,0b11111111
	out	DDRC,tmp1	; set portc directions
	ldi	tmp1,0
	out	PORTC,tmp1	; all portc outputs low	 
	ldi	tmp1,0b10111110
	out	DDRD,tmp1	; set portd directions  
	ldi	tmp1,0
	out	PORTD,tmp1	; all portd outputs low

; timer1 set up for servo pulse output (1-2mS pulse repeated every 5-20mS)

	clr	XH		; point X register to extended I/O
	ldi	XL,TCCR1A
	ldi	tmp1,0b00000000	
	st	X,tmp1		; disable servo pulse PWM while setting up
	ldi	XL,TCCR1B
	ldi	tmp1,0b00011010 ; WGM13,WGM12 = MODE14. CS10 = 8x prescaler
	st	X,tmp1

	ldi	tmp1,LOW(FRAMETIME)  ; servo pulse PWM period (resolution = 0.5uS)
	ldi	tmp2,HIGH(FRAMETIME) 
	ldi	XL,ICR1H
	st	X,tmp2
	ldi	XL,ICR1L
	st	X,tmp1	

	ldi	tmp1,LOW(SERVOMIN*2)
	ldi	tmp2,HIGH(SERVOMIN*2) ; servo pulse width = low throttle
	ldi	XL,OCR1AH
	st	X,tmp2
	ldi	XL,OCR1AL	; load PWM output compare register
	st	X,tmp1

	ldi	XL,TCCR1A
	ldi	tmp1,0b10000010	; COM1A1,0 = OC1A active. WGM11 = MODE 14 
	st	X,tmp1		; start servo pulses

; ----------------------------- Main Loop -------------------------------
;
; 1. Measure motor PWM on and off times (low = on, high = off) 
; 2. Calculate PWM ratio and ESC ouput pulse width
; 3. Update servo pulse width generator (hardware PWM)
;
; NOTE1: The beginning of the 1st PWM cycle will be missed, so we 
;        discard it and measure the 2nd PWM cycle. 
;
; NOTE2: If the motor is fully on or off then there is no PWM, so 
;        the measurement process will time out. In this case we know 
;        that the throttle is fully on or off, so we can directly set 
;        the throttle pulse width (no calculations required).
;
mainloop:
	sbic	PIND,6		; motor pwm input low?	
	rjmp	pwmhigh		; no,

	clr	pwmon1		
	clr	pwmon2		; yes, clear motor pwm low count 
pwmlow:
	inc	pwmon1		
	brne	PC+2		; measure motor pwm low time
	inc	pwmon2		
	ldi	tmp1,LOW(MAXPWM)
	ldi	tmp2,HIGH(MAXPWM)
	sub	tmp2,pwmon2	; motor pwm stuck low ?
	brne	PC+4		
	sub	tmp1,pwmon1
	brne	PC+4
	rjmp	pwmlow_timeout
	nop
	nop
	nop
	nop
	nop
	sbis	PIND,6		; motor pwm still low?
	rjmp	pwmlow		; yes, continue counting

pwmhigh:
	clr	pwmoff1
	clr	pwmoff2		; clear pwm high count
pwmhigh1:
	inc	pwmoff1
	brne	PC+2		; measure motor pwm high time
	inc	pwmoff2
	ldi	tmp1,LOW(MAXPWM)
	ldi	tmp2,HIGH(MAXPWM)
	sub	tmp2,pwmoff2
	brne	PC+4
	sub	tmp1,pwmoff1
	brne	PC+4
	rjmp	pwmhigh_timeout	; motor pwm stuck high
	nop
	nop
	nop
	nop
	nop
	sbic	PIND,6		; still high?
	rjmp	pwmhigh1	; yes, continue counting

; measure PWM on/off times again (first measurement may be inaccurate)

	clr	pwmon1
	clr	pwmon2		; clear motor pwm low count
pwmlow2:
	inc	pwmon1		
	brne	PC+2		; measure motor pwm low time
	inc	pwmon2		
	ldi	tmp1,LOW(MAXPWM)
	ldi	tmp2,HIGH(MAXPWM)
	sub	tmp2,pwmon2	; motor pwm stuck low ?
	brne	PC+4		
	sub	tmp1,pwmon1
	brne	PC+4
	rjmp	pwmlow_timeout
	nop
	nop
	nop
	nop
	nop
	sbis	PIND,6		; motor pwm still low?
	rjmp	pwmlow2		; yes, continue counting

	clr	pwmoff1
	clr	pwmoff2		; clear high count
pwmhigh2:
	inc	pwmoff1
	brne	PC+2		; measure motor pwm high time
	inc	pwmoff2
	ldi	tmp1,LOW(MAXPWM)
	ldi	tmp2,HIGH(MAXPWM)
	sub	tmp2,pwmoff2
	brne	PC+4
	sub	tmp1,pwmoff1
	brne	PC+4
	rjmp	pwmhigh_timeout	; motor pwm stuck high
	nop
	nop
	nop
	nop
	nop
	sbic	PIND,6		; still high?
	rjmp	pwmhigh2	; yes, continue counting
;
; convert pwm on and off times to throttle output pulse width 
;
; motor pwm_cycle_time  = pwm_on_time + pwm_off_time 
; throttle proportion = pwm_on_time / pwm_cycle_time
; throttle level = throttle proportion * throttle servo pulse range
; servo pulse width = throttle level + minimum servo pulse width
;
; The equation:-
;
;   servo pulse width = pwmon/(pwmon+pwmoff)*(max-min)+min
;
; Rearranged to allow integer math:-
;
;   servo pulse width = ((pwmon*(max-min))/(pwmon+pwmoff))+min
;
	ldi	tmp1,LOW(SERVOMAX-SERVOMIN)
	ldi	tmp2,HIGH(SERVOMAX-SERVOMIN)
	movw	bb1,tmp1	; bb = max-min
	mov	aa1,pwmon1
	mov	aa2,pwmon2
	rcall	mulu16x16	; cc = pwmon*(max-min)
	mov	aa1,pwmon1
	mov	aa2,pwmon2	
	add	aa1,pwmoff1	; aa = pwmon+pwmoff
	adc	aa2,pwmoff2
	rcall	divu32x16	; cc = (pwmon*(max-min)/(pwmon+pwmoff)
	ldi	tmp1,LOW(SERVOMIN)
	ldi	tmp2,HIGH(SERVOMIN)
	add	cc1,tmp1
	adc	cc2,tmp2	; cc = ((pwmon*(max-min)/(pwmon+pwmoff)+min
	mov	servo1,cc1
	mov	servo2,cc2	; servo = cc

	sbi	PINC,2		; toggle LED on/off
	rjmp	updateservo

pwmlow_timeout:
	ldi	tmp1,LOW(SERVOMAX)
	ldi	tmp2,HIGH(SERVOMAX) ; 100% PWM = high throttle
	movw	servo1,tmp1
	sbi	PORTC,2		    ; LED on
	rjmp	updateservo

pwmhigh_timeout:
	ldi	tmp1,LOW(SERVOMIN)
	ldi	tmp2,HIGH(SERVOMIN) ; 0% PWM = low throttle
	movw	servo1,tmp1
	cbi	PORTC,2		    ; LED off

updateservo:
	movw	tmp1,servo1
	lsl	tmp1
	rol	tmp2		; tmp1:2 = servo*2
	ldi	XL,OCR1AH
	st	X,tmp2
	ldi	XL,OCR1AL	; update PWM output compare register
	st	X,tmp1

	rjmp	mainloop	; forever
   
;------------------------------------------------------------------------
;                 16 x 16 bit Hardware Multiply
;------------------------------------------------------------------------
; in: aa1:2 = 16 bit multiplier 
;     bb1:2 = 16 bit multiplicand
;out: cc1:4 = 32 bit result
;
;uses: r0,r1,temp1
;
mulu16x16:
	clr	tmp1		; holding register for carry
	mul	aa2,bb2		; Multiply 1H with 2H
	mov	cc3,R0		; copy to Result
	mov	cc4,R1
	mul	aa1,bb1		; Multiply 1L with 2L
	mov	cc1,R0		; copy to Result
	mov	cc2,R1
	mul	aa2,bb1		; Multiply 1H with 2L
	add	cc2,R0		; Add to Result
	adc	cc3,R1
	adc	cc4,tmp1	; add carry
	mul	aa1,bb2		; Multiply 1L with 2H
	add	cc2,R0		; Add to Result
	adc	cc3,R1
	adc	cc4,tmp1	; add carry
	ret

;----------------------------------------------------------------------
;                32 bit x 16 bit Unsigned Divide
;----------------------------------------------------------------------
;
; in:  cc1:4 = 32 bit dividend
;      aa1:2 = 16 bit divisor
;
; out: cc1:4 = cc / aa  (quotient)
;
; uses: tmp0-tmp6
;
divu32x16:
	clr  tmp0       ; tmp0 = 'zero' constant	
	ldi  tmp6,0x01  ; tmp6 = 'bit 0 set' constant

	clr  tmp1 
	clr  tmp2	; tmp1:4 = 32 bit temporary register
	clr  tmp3 
	clr  tmp4 

	ldi  tmp5,32	; tmp5 = loop count (32 bits)
divu_loop: 
	lsl  cc1 	; quotient * 2, and... 
	rol  cc2
	rol  cc3	
	rol  cc4 
	rol  tmp1 	; ...shift dividend into 32 bit register
	rol  tmp2
	rol  tmp3
	rol  tmp4 
	sub  tmp1,aa1	; try to subtract divisor from dividend
	sbc  tmp2,aa2 
	sbc  tmp3,tmp0 
	sbc  tmp4,tmp0 
	brcc divu_2	; positive remainder? 
	add  tmp1,aa1 
	adc  tmp2,aa2 
	adc  tmp3,tmp0 	; no, undo subtraction
	adc  tmp4,tmp0 
	rjmp divu_3 
divu_2: 
	or   cc1,tmp6 	; yes, set next bit in quotient
divu_3: 
	dec  tmp5 
	brne divu_loop 
	ret