fft.asm

AVR单片机实现FFT

;-----------------------------------------------------------------------------;
; Audio Waveform Monitor R0.1                               Nov 11, 2004      ;
; (C)ChaN, 2004; http://elm-chan.org/                                         ;
;-----------------------------------------------------------------------------;
;
.include "m8def.inc"
.include "avr.inc"
.include "akiglcd.inc"

.equ	Sync	= 2		;Port D bit definitions

.equ	FFT_N	= 128	;Number of samples
.equ	LCD_H	= 16	;/

.def	_0	= r15		;Zero
.def	_Flags	= r25	;b0:In captureing
						;b1:In pause
						;b2:MOSI edge detector
						;b3:Low/High Freq
						;b4:ADC Divider (f黵 5,5kHz Samplerate)

;----------------------------------------------------------;
; Data memory area

.dseg
	.org	RAMTOP
CaptBuf:.byte	FFT_N*2		;Sampling buffer
BflyBuf:.byte	FFT_N*4		;Butterfly operation table, Wave form buffer
LvlBuf:	.byte	FFT_N/2		;Spectrum bar length, High Freq
LvlBuf2:.byte	FFT_N/2		;Spectrum bar length, Low Freq
LvlBuf3:.byte	FFT_N/2		;Spectrum bar length, Gesamt

AdPtr:	.byte	1		;Sampling pointer
LcdDiv:	.byte	1		;Divider for LCD drive


;----------------------------------------------------------;
; Program code area

.cseg
	rjmp	reset		; RESET
	rjmp	0		; INT0
	rjmp	0		; INT1
	rjmp	0		; TC2 COMP
	rjmp	0		; TC2 OVF
	rjmp	0		; TC1 CAPT
	rjmp	0		; TC1 COMPA
	rjmp	0		; TC1 COMPB
	rjmp	0		; TC1 OVF
	rjmp	0		; TC0 OVF
	rjmp	0		; SPI
	rjmp	0		; USART RXC
	rjmp	0		; USART UDRE
	rjmp	0		; USART TXC
;	rjmp	isr_adc		; ADC
;	rjmp	0		; EE_RDY
;	rjmp	0		; ANA_COMP
;	rjmp	0		; TWI
;	rjmp	0		; SPM_RDY


;----------------------------------------------------------;
; ADC interrupt (9.6ksps)


isr_adc:
	push	AL
	in	AL, SREG
	pushw	A
	pushw	Y
	sbrs	_Flags, 0	;Skip if in Idle
	rjmp	adc_skip	;/
	sbrs	_Flags, 3
	rjmp nodiv
	sbrs	_Flags, 4
	rjmp setd
	cbr 	_Flags, (1<<4)
	rjmp adc_skip
setd:
	sbr		_Flags, (1<<4)
nodiv:
	lds	YL, AdPtr	;Write pointer
	clr	YH		;
	addiw	Y, CaptBuf	;/
	inw	A, ADC		;Store A/D value
	stw	Y+, A		;/
	lds	AL, AdPtr	;Next pointer
	addi	AL, 2		;
	sts	AdPtr, AL	;/
	brne	PC+2		;Terminate if 128 samples captured.
	cbr	_Flags, bit0	;/
adc_skip:

	popw	Y
	popw	A
	out	SREG, AL
	pop	AL
	reti

;----------------------------------------------------------;
; Initialize peripherals (ATmega8 @ 18,432MHz)

reset:
	clr	_0			;Zero reg.
	ldiw	Z, RAMTOP		;Clear all variables
	ldiw	X, RAMEND-RAMTOP+1	;
	st	Z+, _0			;
	sbiw	XL, 1			;
	brne	PC-2			;/
	sbiw	ZL, 1			;SP
	outw	SP, Z			;/

	outi	PORTB, 0b00101000	;Port B
	outi	DDRB,  0b00010110

	outi	DDRC,  0

	outi	PORTD, 0b00000100	;Port D
	outi	DDRD,  0b00000100	;Port D

	; Start TC1 with 444kHz on OC1B pin (for MAX293 clock)
	ldiw	A, 36-1			;TOP value
	outw	ICR1, A			;/
	ldiw	A, 36/2			;half value
	outw	OCR1B, A		;/
	outi	TCCR1A, 0b00100010	;COM1B[1:0]=10, WGM1[1:0]=10
	outi	TCCR1B, 0b00011001	;WGM1[3:2]=11, CS1[2:0]=001

	; Start A/D with ch0, free running, 44ksps
	outi	ADMUX, 0b00000000	;MUX[3:0]=0000
	outi	ADCSR, 0b11101111 	;ADEN=1,ADSC=1,ADFR=1,ADIE=1,ADPS[2:0]=110

	outi 	ucsrb, 8
	outi 	ubrrh, 0
	outi 	ubrrl, 9

	in r16, WDTCR
  	ori r16, (1<<WDCE) | (1<<WDE)
  	out WDTCR, r16
  	ldi r16, (1<<WDE) | (1<<WDP2)
  	out WDTCR, r16

	clr	_Flags
	wdr
	sei

	sbr	_Flags, bit0	;Start wave form captureing


;----------------------------------------------------------;
; Main


main:
	sbrc	_Flags, 0	;Wait for end of wave captureing
	rjmp	main		;/
	
	sbrc _Flags, 3
	rjmp LoF
	outi	ADCSR, 0b11101000 + 7	;ADEN=1,ADSC=1,ADFR=1,ADIE=1,ADPS[2:0]=111, 11kHz Samplerate
	outi	ADMUX, 0b00000001	;MUX[3:0]=0001
	sbr _Flags, (1<<3)
	rjmp Hif
Lof:
	outi	ADCSR, 0b11101000 + 5	;ADEN=1,ADSC=1,ADFR=1,ADIE=1,ADPS[2:0]=101, 44khz Samplerate
	outi	ADMUX, 0b00000000	;MUX[3:0]=0000
	cbr _Flags, (1<<3)
Hif:
	 rcall	do_window	;Fill butterfly table			[340us]
   	 sbr	_Flags, bit0	;Restart captureing for next
	 rcall	do_fft		;Butterfly operations			[4.4ms]
	
	sbrc _Flags, 3	;Gesetzt: jetzt High Freq verarbeiten
	rjmp Lof2
	rcall	make_barsl	;Get scalar values, update spectrum	[3.3ms]
	rjmp Hif2
Lof2:
	rcall	make_barsh	;Get scalar values, update spectrum	[3.3ms]
	rcall	rfsh_bars	;Display spectrum bars into LCD		[660us]
 	wdr
Hif2:

hier:
	rjmp	main		; (approx. 70 cycles/sec)


;----------------------------------------------------------;
; Spectrum analyzer
;----------------------------------------------------------;
; 16bit fixed point FFT performance with megaAVR @16MHz
;
;  Points:  Input,    FFT, Output,   Total: Throughput
;   64pts:  .17ms,  1.9ms,  1.4ms,   3.5ms:   18.3kpps   (expected)
;  128pts:  .34ms,  4.4ms,  2.6ms,   7.3ms:   17.5kpps   (measured)
;  256pts:  .68ms, 10.1ms,  5.2ms,  16.0ms:   16.0kpps   (expected)
;  512pts:  1.4ms, 22.6ms, 10.4ms,  34.4ms:   14.8kpps   (expected)
;
; Input: Input waveform into butterfly table with applying window
; FFT: Execute butterfly operations
; Output: Descramble and output the spectrum as scalar values


do_window: ; Fill butterfly table with applying window function
	ldiw	X, CaptBuf	;Source
	ldiw	Y, BflyBuf	;Destination
	ldiw	Z, t_hamming128*2	;Window table
	ldi	CL, FFT_N
ms_l1:	lpmw	A, Z+		;Get a window attenuation ratio
	ldw	B, X+		;Get an input value
	subiw	B, 512		;unsigned -> signed
	FMULS16	A, B, T4, T2	;Apply window
	stw	Y+, T4		;Store real
	st	Y+, _0		;Clear image
	st	Y+, _0		;/
	dec	CL
	brne	ms_l1
	ret


do_fft:	; Execute butterfly operations (not optimized)
	ldi	XH, 1		;u
	ldi	EL, FFT_N/2	;l
df_l1:	ldiw	Z, BflyBuf	;Z = BflyBuf
	ldiw	Y, BflyBuf	;Y = BflyBuf + l * 4
	muli	EL, 4		;
	addw	Y, T0		;/
	mul	AL, XH		;T10L = u * 4
	mov	T10L, T0L	;/
	mov	XL, XH		;w = u
df_l2:	clr	T14L		;p=0
df_l3:	lddw	A, Z+0		;A = [Z+0] - [Y+0], [Z+0] += [Y+0]
	movw	CL, AL		;B = [Z+2] - [Y+2], [Z+2] += [Y+2]
	lddw	D, Y+0		;
	subw	A, D		;
	addw	C, D		;
	stw	Z+, C		;
	lddw	B, Z+0		;
	movw	CL, BL		;
	lddw	D, Y+2		;
	subw	B, D		;
	addw	C, D		;
	stw	Z+, C		;/
	movw	T0L, ZL
	movw	ZL, T14L	;C=cos(p), D=sin(p)
	addiw	Z, t_cos_sin128*2	;
	lpmw	C, Z+		;
	lpmw	D, Z+		;/
	movw	ZL, T0L
	FMULS16	A, C, T4, T2	;[Y+0] = A * C + B * D
	FMULS16	B, D, T8, T6	;[Y+2] = B * C - A * D
	addw	T2, T6		;
	adcw	T4, T8		;
	stw	Y+, T4		;
	FMULS16	B, C, T4, T2	;
	FMULS16	A, D, T8, T6	;
	subw	T2, T6		;
	sbcw	T4, T8		;
	stw	Y+, T4		;/
	add	T14L, T10L	;p += u
	rjne	df_l3
	muli	EL, 4		;Y += l * 4, Z += l * 4; (skip split segment)
	addw	Y, T0		;
	addw	Z, T0		;/
	dec	XL		;--w
	rjne	df_l2
	lsl	XH		;u *= 2
	lsr	EL		;l /= 2
	rjne	df_l1
	ret


make_barsl: ;Get scalar spectrum and update spectrum bar length
	ldiw	Z, t_desc128*2	;Descramble table
	ldiw	Y, LvlBuf	;Bar length buffer
	ldi	CH, FFT_N/2	;Number of elements
ub_l1:	lpmw	A, Z+		;Get a vector value (A = r, B = i)
	ldiw	X, BflyBuf	;
	addw	X, A		;
	ldw	A, X+		;
	ldw	B, X+		;/
	FMULS16	A, A, T4, T2	;A = sqrt(A * A + B * B); 0..32767 (scalar)
	FMULS16	B, B, T8, T6	;
	addw	T2, T6		;
	adcw	T4, T8		;
	SQRT32			;/
	movw	T2L, AL		; AL = sqrt(A); 0..181 (root compression)
	SQRT16			; / * remove this for linear scale
	ldi	AH, 100		;T0H = AL/3; (scaling)	(Orginal: 256/3)
	mul	AL, AH		;/
	ld	AH, Y		;Update bar length with peak holding
	subi	AH, 2		; <-peak decay rate
	brcs	PC+3		;
	cp	AH, T0H		;
	brcc	PC+2		;
	mov	AH, T0H		;
	subi AH, 1
	brcc notzero
	clr AH
notzero:
	st	Y+, AH		;/
	dec	CH
	rjne	ub_l1
	ret

make_barsh: ;Get scalar spectrum and update spectrum bar length
	ldiw	Z, t_desc128*2	;Descramble table
	ldiw	Y, LvlBuf2	;Bar length buffer
	ldi	CH, FFT_N/2	;Number of elements
ub_l1h:	lpmw	A, Z+		;Get a vector value (A = r, B = i)
	ldiw	X, BflyBuf	;
	addw	X, A		;
	ldw	A, X+		;
	ldw	B, X+		;/
	FMULS16	A, A, T4, T2	;A = sqrt(A * A + B * B); 0..32767 (scalar)
	FMULS16	B, B, T8, T6	;
	addw	T2, T6		;
	adcw	T4, T8		;
	SQRT32			;/
	movw	T2L, AL		; AL = sqrt(A); 0..181 (root compression)
	SQRT16			; / * remove this for linear scale
	ldi	AH, 100		;T0H = AL/3; (scaling)	(Orginal: 256/3)
	mul	AL, AH		;/
	ld	AH, Y		;Update bar length with peak holding
	subi	AH, 2		; <-peak decay rate
	brcs	PC+3		;
	cp	AH, T0H		;
	brcc	PC+2		;
	mov	AH, T0H		;
	subi AH, 1		;Unterste n Zeilen entfernen