'Goertzel self test
'Version 0.2 ozpropdev Nov 4th 2018

con

	xtal = 12_000_000
	dv = 12
	mlt = 80

	clk = 1 << 24 | (dv-1) << 18 | (mlt-1) << 8
	sys_clk = xtal / dv * mlt

	rx_pin = 63
	tx_pin = 62
	baudrate = 1_000_000 '115_200
	clocksperbit = float(sys_clk) / float(baudrate)
	nco = round(clocksperbit * 65536.0) & $FFFFFC00 
	tx = 62
	rx = 63

	adc_in = 8
	ref_dac_out = 12


	start = 100_000
	freq_step = 10_000

	ms = sys_clk / 1_000
	

'===========================================================================

dat		org


kx		hubset	##clk | %1111_01_00	'enable crystal+PLL, stay in 20MHz+ mode
ky		waitx	##20_000_000/100	'wait ~10ms for crystal+PLL to stabilize
kz		hubset	##clk | %1111_01_11	'now switch to PLL running at 80MHz

		wrpin	#%11111_0,#rx_pin
		wxpin	##nco | 7,#rx_pin
		dirh	#rx_pin

		wrpin	#%1_11110_0,#tx_pin
		wxpin	##nco | 7,#tx_pin
		dirh	#tx_pin

		mov	kz,#$1FF		'make 512-sample sin/cos table in lut
sincos		mov	kx,kz			'get angle into top 9 bits of x
		shl	kx,#32-9
		qrotate	#$7F,kx			'get sin/cos of (ro,theta)
		getqx	kx			'get cos
		getqy	ky			'get sin
		setbyte	kx,ky,#1			'get sin into byte1, cos in byte0
		setword	kx,kx,#1			'copy bytes 1,0 to 3,2
		xor	kx,##$8080		'make bytes 1,0 positive (not used, but could be)
		wrlut	kx,kz			'write sin:cos:psin:pcos into lut bottom bytes
		djnf	kz,#sincos		'make 512 samples

		wrpin	adcmode,#adc_in
		wrpin	dacmode,#ref_dac_out
		dirh	#ref_dac_out

wait4enter	testp	#rx_pin wc
	if_nc	jmp	#wait4enter
		rdpin	pa,#rx_pin
		shr	pa,#24
		cmp	pa,#13 wcz
	if_ne	jmp	#wait4enter

'===========================================================================
'===========================================================================

		mov	xfreq,##start
		call	#calc_nco
		
		setq	freq		'fill streamer command buffer
		xcont	streamer,#0
		setq	freq
		xcont	streamer,#0

		mov	ijmp1,#isr1
		setint1	#10		'streamer ready for another command


		getct	timer
		addct1	timer,delay

		mov	test_steps,#200

new_freq	call	#calc_nco
		mov	sample_count,#10
main
		getxacc	gtz_x		'dummy read/zero accumulators
		mov	gtz_y,0-0

		getct	st
		addct2	st,##10 * ms	'10mS sample
.loop		pollct2	wc
	if_nc	jmp	#.loop
		getxacc	gtz_x
		mov	gtz_y,0-0	'get goertzel accumulation

		qvector	gtz_x,gtz_y	'convert to rh0,theta
		getqx	rho		'get rho,theta results
		getqy	theta

		pollct1	wc
	if_nc	jmp	#main
		call	#update
		djnz	sample_count,#main

		callpa	#13,#send_char		'next freq.
		callpa	#10,#send_char
		add	xfreq,##freq_step
		djnz	test_steps,#new_freq

		jmp	#$			'all done



'reload streamer interrupt

isr1		setq	freq
		xcont	streamer,#0
		reti1


'===========================================================================

calc_nco	qmul	xfreq,##$8000_0000
		getqx	freq1
		getqy	freq1a
		setq	freq1a
		qdiv	freq1,##sys_clk
		getqx	freq
		ret

'===========================================================================

update		addct1	timer,delay
		mov	pb,xfreq
		call	#dec
		loc	ptra,#@msg1		'Hz
		call	#print

		loc	ptra,#@msg2		'x=
		call	#print
		mov	pb,gtz_x
		call	#hex8

		loc	ptra,#@msg3		'y=
		call	#print
		mov	pb,gtz_y
		call	#hex8

		loc	ptra,#@msg4		'rho=
		call	#print
		mov	pb,rho
		call	#hex8

		loc	ptra,#@msg5		'theta=
		call	#print
		mov	pb,theta
		call	#hex8

		callpa	#13,#send_char
		callpa	#10,#send_char
		ret	wcz


'===========================================================================

hex8		callpa	#"$",#send_char
		mov	digits,#8
.loop		getnib	pa,pb,#7
		cmp	pa,#9 wcz
	if_a	add	pa,#"A"-10
	if_be	add	pa,#"0"
		call	#send_char
		rol	pb,#4
		djnz	digits,#.loop
		callpa	#" ",#send_char
		ret	wcz

'===========================================================================

dec		mov	lzb,#0
		mov	dvx,##1_000_000_000
		mov	digits,#10
.loop		qdiv	pb,dvx
		getqx	pa wz
	if_nz	mov	lzb,#1
		add	pa,#"0"
		testb	lzb,#0 wc
	if_nc_and_z	mov	pa,#" "
		call	#send_char
		getqy	pb
		qdiv	dvx,#10
		getqx	dvx
		djnz	digits,#.loop
		callpa	#" ",send_char
		ret


'===========================================================================
send_char	rdpin	junk,#tx_pin wc
	if_c	jmp	#send_char
		wypin	pa,#tx_pin
		ret	wcz

print		rdbyte	pa,ptra++ wz
	if_z	ret	wcz
		call	#send_char
		jmp	#print


'===========================================================================

st		long	0
fn		long	0

lzb		long	0
sample_count	long	0
sample_size	long	0
set_freq	long	0
digits		long	0
dvx		long	0
junk		long	0

test_steps	long	0
xfreq		long	0
freq1		long	0
freq1a		long	0

delay		long	sys_clk / 5
timer		long	0

gtz_x		long	0
gtz_y		long	0
rho		long	0
theta		long	0

adcmode		long	%0000_0000_000_10001_10000000_01_00000_0
dacmode        long	%10100_00000000_01_00000_0
streamer	long	$0f00_0000 + 128  + (adc_in << 17) '32 cycles

freq		long	0	

		orgh	$400	
msg1		byte	"Hz ",0
msg2		byte	"X=",0
msg3		byte	"Y=",0
msg4		byte	"rho=",0
msg5		byte	"theta=",0