Brushless ESC motor driver

Home
Hardware
Software

Tempo fa ho trovato questo, e' il codice sorgente (in assembler) di un driver per motori brushless usato nei modellini radiocomandati, ad esempio di aerei. Sembrerebbe un driver abbastanza universale in grado di girare con piccole differenze su schede diverse, dalle piu' complesse e piene di LED, alle piu' semplici ed economiche.

Riusciro' a carpirne il segreto?

Prima di cominciare l'autopsia mi sembra d'obbligo ringraziare Bernhard Konze, bernhard.konze@versanet.de, per questa opera: non si direbbe ma e' veramente un lavoro immenso, concepito per girare su un sacco di schede, veramente un'opera degna di elogio. Spero di riuscire ad imparare qualcosa da questo lavoro mastodontico.

Per certi versi questo sembra il modo piu' complicato di scoprire come funziona un driver per motori brushless. Negli anni ho letto di tutto, ho tentato diversi esperimenti, ma tutto e' stato vano. Da ultimo ho comprato un driver per modellini e ho tentato di smontarlo per capire come funziona, anche questo tentativo non ha dato i risultati attesi. Adesso voglio vedere se partendo dal codice sorgente di un controller fatto e finito, sono capace di venire a capo del principio di funzionamento.

Prima di tutto un'occhiara al file readme.md ci dice subito che siamo nel posto giusto: questo coso e' un controller di tipo ESC, electronic speed control, basato su micro Atmel.

Dallo zip esce un sacco di roba, prima di perdersi conviene aprire il file: tgy.asm. Questo e' il vero codice sorgente, tutto il resto sono dei file specifici, scheda per scheda, che vengono inclusi nel codice principale, il loro scopo e' definire alcune costanti e macro specifiche per la singola scheda.

Dovendo aggredire un codice sconosciuto serve un punto di partenza, la scelta potrebbe pregiudicare i risultati successivi. Per non sapere ne leggere ne scrivere, io comincerei dalla tavola degli interrupt. Nel file tgy.asm si trova il seguente codice:

;-----bko-----------------------------------------------------------------
;**** **** **** **** ****
.cseg
.org 0
;**** **** **** **** ****
; ATmega8 interrupts

;.equ	INT0addr=$001	; External Interrupt0 Vector Address
;.equ	INT1addr=$002	; External Interrupt1 Vector Address
;.equ	OC2addr =$003	; Output Compare2 Interrupt Vector Address
;.equ	OVF2addr=$004	; Overflow2 Interrupt Vector Address
;.equ	ICP1addr=$005	; Input Capture1 Interrupt Vector Address
;.equ	OC1Aaddr=$006	; Output Compare1A Interrupt Vector Address
;.equ	OC1Baddr=$007	; Output Compare1B Interrupt Vector Address
;.equ	OVF1addr=$008	; Overflow1 Interrupt Vector Address
;.equ	OVF0addr=$009	; Overflow0 Interrupt Vector Address
;.equ	SPIaddr =$00a	; SPI Interrupt Vector Address
;.equ	URXCaddr=$00b	; USART Receive Complete Interrupt Vector Address
;.equ	UDREaddr=$00c	; USART Data Register Empty Interrupt Vector Address
;.equ	UTXCaddr=$00d	; USART Transmit Complete Interrupt Vector Address
;.equ	ADCCaddr=$00e	; ADC Interrupt Vector Address
;.equ	ERDYaddr=$00f	; EEPROM Interrupt Vector Address
;.equ	ACIaddr =$010	; Analog Comparator Interrupt Vector Address
;.equ	TWIaddr =$011	; Irq. vector address for Two-Wire Interface
;.equ	SPMaddr =$012	; SPM complete Interrupt Vector Address
;.equ	SPMRaddr =$012	; SPM complete Interrupt Vector Address

;-----bko-----------------------------------------------------------------
; Reset and interrupt jump table
; When multiple interrupts are pending, the vectors are executed from top
; (ext_int0) to bottom.
		rjmp reset	; reset
		rjmp rcp_int	; ext_int0
		reti		; ext_int1
		reti		; t2oc_int
		ijmp		; t2ovfl_int
		rjmp rcp_int	; icp1_int
		rjmp t1oca_int	; t1oca_int
		reti		; t1ocb_int
		rjmp t1ovfl_int	; t1ovfl_int
		reti		; t0ovfl_int
		reti		; spi_int
		rjmp urxc_int	; urxc
		reti		; udre
		reti		; utxc
		reti		; adc_int
		reti		; eep_int
		reti		; aci_int
		rjmp i2c_int	; twi_int
		reti		; spmc_int
      

La prima parte, onestamente, e' solo un blocco di commenti; la seconda parte invece e' la vera dichiarazione della tabella degli interrupt, possiamo metterla in bella in questo modo:

Per fortuna gli interrupt utilizzati non sono molti, questo dovrebbe semplificare le cose. L'interrupt Timer 2 overflow ci dara' del filo da torcere: usa l'istruzione ijmp che salta alla routine puntata da Z. Questo vuol dire che Z e' bloccato da questo uso particolare, ma significa anche che non sara' facile prevedere dove andra' a parare. Infine abbiamo la routine rcp_int che viene utilizzata due volte; non so, per il momento annotiamo la cosa.

Pensando che sia piu' facile, propongo innocentemente di partire dalla ricezione dati dalla seriale: dei 3 interrupt della USART, solo l'interrupt di ricezione completata e' stato utilizzato, ne deduco che questo coso riceva caratteri dalla seriale senza trasmettere niente indietro.

Vediamo quindi il codice alla label urxc_int.

;-----bko-----------------------------------------------------------------
urxc_int:
; This is Bernhard's serial protocol implementation in the UART
; version here: http://home.versanet.de/~b-konze/blc_6a/blc_6a.htm
; This seems to be implemented for a project described here:
; http://www.control.aau.dk/uav/reports/10gr833/10gr833_student_report.pdf
; The UART runs at 38400 baud, N81. Input is ignored until >= 0xf5
; is received, where we start counting to MOTOR_ID, at which
; the received byte is used as throttle input. 0 is neutral,
; >= 200 is FULL_POWER.
	.if USE_UART
		in	i_sreg, SREG
		in	i_temp1, UDR		; legge il char dalla seriale
		cpi	i_temp1, 0xf5		; e' 0xf5 ?
		breq	urxc_x3d_sync		; se si, salta
		sbrs	flags0, UART_SYNC	; skip se UART_SYNC=1
		rjmp	urxc_exit		; altrimenti esce
		brcc	urxc_unknown		; se C=0, salta
		lds	i_temp2, motor_count	; preleva il contatore
		dec	i_temp2			; scala il contatore
		brne	urxc_set_exit		; se non zero, salva il
						; contatore ed esce
		mov	rx_h, i_temp1		; salva il char ricevuto
		sbr	flags1, (1<<EVAL_RC)+(1<<UART_MODE)
urxc_unknown:	cbr	flags0, (1<<UART_SYNC)	; rimuove il flag UART_SYNC
		rjmp	urxc_exit		; esce
urxc_x3d_sync:	sbr	flags0, (1<<UART_SYNC)	; setta il flag UART_SYNC
		ldi	i_temp2, MOTOR_ID	; start counting down from MOTOR_ID
urxc_set_exit:	sts	motor_count, i_temp2	; motor_count <- MOTOR_ID
urxc_exit:	out	SREG, i_sreg		; recupera SREG
		reti				; termina
	.endif
;-----bko-----------------------------------------------------------------
      

Per quanto sembri piu' complicata di quanto mi sarei aspettato, con un po' di calma si puo' dipanare la matassa: in sostanza aspetta il char 0xf5, ignora tutto quello che arriva prima di questo byte; dopo di che carica un contatore, MOTOR_ID=1 quindi salta il byte immediatamente successivo il marker 0xf5 e salva il byte successivo in rx_h. A questo punto rimuove il flag UART_SYNC e setta i flag EVAL_RC e UART_MODE.

Da notare che questo codice e' modulare: perche' questa routine esista deve essere definito il simbolo USE_UART. In generale questo codice e' in grado di gestire input da diverse sorgenti, questo pezzo si occupa di ricevere la velocita' dalla seriale, ma potrebbe essere pilotato dal bus I2C o a impulsi.

Proviamo a seguire il simbolo USE_UART: compare diverse volte nel codice ed e' seguito dalle seguenti istruzioni:

		.if USE_UART
		sbrc	flags1, UART_MODE
		rjmp	evaluate_rc_uart
		.endif
      

La routine evaluate_rc_uart e' anch'essa vincolata dalla presenza del simbolo USE_UART. Facciamo finta che sia definito e seguiamo questa pista.

;-----bko-----------------------------------------------------------------
.if USE_UART
evaluate_rc_uart:
		mov	YH, rx_h		; Copy 8-bit input
		cbr	flags1, (1<<EVAL_RC)+(1<<REVERSE)
		ldi	YL, 0
		cpi	YH, 0
		breq	rc_duty_set		; Power off
	; Scale so that YH == 200 is MAX_POWER.
		movw	temp1, YL
		ldi2	temp3, temp4, 0x100 * (POWER_RANGE - MIN_DUTY) / 200
		rjmp	rc_do_scale		; The rest of the code is common
.endif
;-----bko-----------------------------------------------------------------
      

Troviamo il pezzo di codice qui sopra: mette in YH il byte ricevuto dalla seriale e azzera YL, poi fa un test, se Y e' nullo ha un'eccezione che lo porta a rc_duty_set. La sensazione che lascia e' che Y debba contenere la velocita' del motore, almeno in qualche istante, non sembra un registro dedicato.

Supponiamo che il clock della CPU sia a 16MHz, supponiamo (questo e' abbastanza vero) che il Timer 0 sia configurato con un clock a 2MHz. Timer 0 viene utilizzato per generare delle pause. Di fatto il suo interrupt di overflow non e' stato dichiarato, quindi arrivato alla fine del conteggio settera' il flag di overflow TOV0, che a sua volta andra' resettato a mano. Timer 0 e' un timer a 8 bit, contando da 0 alla frequenza di 2MHz, impiegherebbe 128us per generate un overflow.

Il seguente commento presente nel codice ci lascia intendere che ZH sia sempre nullo:

; ZL: Next PWM interrupt vector (low)
; ZH: Next PWM interrupt vector (high, stays at zero) -- used as "zero" register
      

Alla luce di queste considerazioni analizziamo il seguente pezzo di codice:

;-----bko-----------------------------------------------------------------
; Interrupts no longer need to be disabled to beep, but the PWM interrupt
; must be muted first
beep:		out	TCNT0, ZH		; azzera il contatore
beep1:		in	temp1, TCNT0		; preleva il contatore
		cpi	temp1, 2*CPU_MHZ	; CPU_MHZ=16
		brlo	beep1			; se minore, salta
						; questa attesa durera' 16us
		all_pFETs_off temp3		; spegne tutti i MOSFET
		all_nFETs_off temp3
		ldi	temp3, CPU_MHZ		; CPU_MHZ=16
beep2:		out	TCNT0, ZH		; azzera il contatore
		wdr				; watchdog reset
beep3:		in	temp1, TCNT0		; preleva il contatore
		cp	temp1, temp4		; TCNT0 < temp4
		brlo	beep3			; se minore, salta
						; deve contare fino a temp4
		dec	temp3			; per 16 volte
		brne	beep2
		dec	temp2			; scala temp2
		ret				; termina


wait240ms:	rcall	wait120ms
wait120ms:	rcall	wait60ms
wait60ms:	rcall	wait30ms
wait30ms:	ldi	temp2, 15		; carica temp2
wait1:		ldi	temp3, CPU_MHZ		; carica temp3
wait2:		out	TCNT0, ZH		; azzera il contatore di TIM 0
		ldi	temp1, (1<<TOV0)	; resetta il flag TOV0
		out	TIFR, temp1
		wdr				; watchdog reset
wait3:		in	temp1, TIFR		; in temp1 TIFR
		sbrs	temp1, TOV0		; skip se TOV0=1
		rjmp	wait3			; TOV0 si setta in 128us
		dec	temp3			; scala temp3
		brne	wait2			; se non zero, salta
		dec	temp2			; scala temp2
		brne	wait1			; se non zero, salta
wait_ret:	ret
      

Partiamo dalla routine wait30ms: il nocciolo azzera il contatore ed aspetta che generi overflow, questo avviene in 128us. Questo delay viene ripetuto per CPU_MHZ volte, questo simbolo dovrebbe valere 16, quindi 16x128us=2.048ms. Il tutto viene ripetuto per 15 volte (il caricamento di temp2, per un ritardo totale di 15x16x128us=30.72ms. Quindi le routine: wait240ms, wait120ms, wait60ms, wait30ms fanno veramente cio' che promettono (apporssimazioni a parte).

Passiamo quindi a beep, molto simile alla precedente: introduce un ritardo di 16us, poi spegne tutti i mosfet, poi aspetta temp4x8us, quindi decrementa temp2 e ritorna. Probabilmente la chiave sta in chi la chiama, infatti temp2 e temp4 devono essere caricati prima di chiamare.

La routine beep viene chiata da queste altre versioni di beep:

;-----bko-----------------------------------------------------------------
; beeper: timer0 is set to 1us/count
beep_f1:	ldi	temp2, 80		; ripete 80 volte
		ldi	temp4, 200		; delay = 8usx200 = 1.6ms
		RED_on				; accende i LED
		BLUE_on
beep_f1_on:	BpFET_on			; accende i MOSFET
		AnFET_on
		rcall	beep			; delay + spegne i MOSFET
		brne	beep_f1_on		; ripete temp2 volte
		BLUE_off			; spegne i LED
		RED_off
		ret

beep_f2:	ldi	temp2, 100
		ldi	temp4, 180		; delay = 8usx180 = 1.44ms
		GRN_on
		BLUE_on
beep_f2_on:	CpFET_on
		BnFET_on
		rcall	beep
		brne	beep_f2_on
		BLUE_off
		GRN_off
		ret

beep_f3:	ldi	temp2, 120
		ldi	temp4, 160		; delay = 8usx160 = 1.28ms
		BLUE_on
beep_f3_on:	ApFET_on
		CnFET_on
		rcall	beep
		brne	beep_f3_on
		BLUE_off
		ret

beep_f4:	ldi	temp2, 140
beep_f4_freq:	ldi	temp4, 140		; delay = 8usx140 = 1.12ms
beep_f4_fets:	RED_on
		GRN_on
		BLUE_on
beep_f4_on:	CpFET_on
		AnFET_on
		rcall	beep
		brne	beep_f4_on
		BLUE_off
		GRN_off
		RED_off
		ret
      

Secondo il mio modestissimo parere il commento sulla prima riga e' sbagliato: la frequenza di Timer 0 e' clk/8, con un clock di 16MHz la frequenza del timer diventa 2MHz, quindi ogni tic non vale 1us, ma 0.5us.

Al di la di questi dettagli sulla durata di un tic di Timer 0, il funzionamento sembra abbastanza chiaro: ciascuna accende una combinazione differente di LED e genera una frequenza diversa, la prima, beep_f1 dovrebbe generare un fischio a 625Hz, la seconda a 694Hz, la terza a 781Hz ed infine beep_f4 dovrebbe generare un fischio a 893Hz.

La cosa interessante e' che tutte quante tengono i MOSFET accesi solo per il ritardo iniziale di beep, ovvero per soli 16us, poi tutti i mosfet vengono spenti. Quello che invece non capisco e' perche' ciascun beep debba usare una coppia diversa di MOSFET.

Credo che non ci sia una vera ragione, e' solamente una strana forma di para condicio per MOSFET e avvolgimenti, probabilmente usare sempre gli stessi MOSFET faceva brutto! Le immagini di seguito mostrano le 4 configurazioni usate dai diversi 4 beep, in sostanza basta usare 2 transistor qualsiasi che non stiano sullo stesso ramo.

beep_f1

beep_f2

beep_f3

beep_f4

A questo punto divento curioso, capisco che puo' essere una mera perdita di tempo, una deviazione dalla retta via, ma da qualche parte bisogna pur cominciare: facciamo una prova di generazione dei beep.