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