filterconv.asm

Low-pass signal filtering by convolution

; Program to filter signal
; Written in Pinnacle
; Input data rate 200Hz, 3bytes
; Sinc filter Fc=8Hz, kernel=199, 2bytes
; Output data rate 20Hz, 3bytes from 20 MULT-ADD units

	SSW EQU   2FH 
	test = 2fh.4 
        signIn = 2FH.3
    	signM = 2FH.2
 	signF = 2FH.1
 	sign = 2Fh.0
 	 				
	in = 30H ;,31, 32
	count1 = 33h
	count2 = 34h
	count3 = 35h
	summ = 36h;37;38;39;3A;3B
	
	f1 = 4eh
	f2 = 4fh
	
	C1 = 53h	;Filter counters
	C2 = 54h
	C3 = 55h
	C4 = 56h
	C5 = 57h
	C6 = 58h
	C7 = 59h
	C8 = 5ah
	C9 = 5bh
	C10 = 5ch	
	C11 = 5dh	;Filter counters
	C12 = 5eh
	C13 = 5fh
	C14 = 60h
	C15 = 61h
	C16 = 62h
	C17 = 63h
	C18 = 64h
	C19 = 65h
	C20 = 66h
	
	SUM = 7dh ; 7e 7f
	SUM1	EQU   80H ;81, 82, 83, 84, 85
	SUM2	EQU   86H ;87, 88, 89, 8a, 8b
	SUM3	EQU   8cH ;8d, 8e, 8f, 90, 91
	SUM4	EQU   92H ;93, 94, 95, 96, 97
	SUM5	EQU   98H ;99, 9a, 9b, 9c, 9d
	SUM6	EQU   9eH ;9f, a0, a1, a2, a3
	SUM7	EQU  0a4H ;a5, a6, a7, a8, a9
	SUM8	EQU  0aaH ;ab, ac, ad, ae, af
	SUM9	EQU  0b0H ;b1, b2, b3, b4, b5
	SUM10	EQU  0b6H ;b7, b8, b9, ba, bb
	SUM11	EQU  0bch ;bd, be, bf, c0, c1
	SUM12	EQU  0c2H ;c3, c4, c5, c6, c7
	SUM13	EQU  0c8H ;c9, ca, cb, cc, cd
	SUM14	EQU  0ceH ;cf, d0, d1, d2, d3
	SUM15	EQU  0d4H ;d5, d6, d7, d8, d9
	SUM16	EQU  0daH ;db, dc, dd, de, df
	SUM17	EQU  0e0H ;e1, e2, e3, e4, e5
	SUM18	EQU  0e6H ;e7, e8, e9, ea, eb
	SUM19	EQU  0ecH ;ed, ee, ef, f0, f1
	SUM20	EQU  0f2H ;f3, f4, f5, f6, f7
	
	
	M = 199		;Filter lengh


  ORG 0030H
  
  
main:	mov SUM+1, #0
	mov SUM, #0
MA:			
	mov IE, #0
	mov P2, #0FFh
	mov P0, #0FFh
	mov P1, #07Fh	;Clr ADC
	MOV P3, #0FFH
	
      DJNZ SUM+1,MA
      DJNZ SUM,MA
      
      mov R0, #0
ma1:  
      mov @R0, #0	;Clear IRAM
      djnz R0, ma1
      
      mov SP, #07h
      

wait:	
  	jb P1.0, wait	;wait for data ready
	CALL XXXX	;mythical subroutine to get input data  (in, in+1, in+2)
  	acall Conv
  	sjmp wait
  	

wait1:	
	jb P1.1, wait1  ;wait for data out sincronisation
	call YYYY	;mythical subroutine to send filtered data out (SUM, SUM+1, SUM+2)		

			;prepare new Convolution
rr0:	mov A, C1
	jnz rr1
	mov R1, #SUM1
	acall Shift	;shift right SUM1
	mov R1, #SUM1+1
	acall Move	;mov to SUM
	mov R1, #SUM1
	acall Clear
	mov C1, #M
	ajmp rr21
	
rr1:
	mov A, C2
	jnz rr2
	mov R1, #SUM2
	acall Shift	;shift right SUM2
	mov R1, #SUM2+1
	acall Move	;mov to SUM
	mov R1, #SUM2
	acall Clear
	mov C2, #M
	ajmp rr21
	
rr2:
	mov A, C3
	jnz rr3
	mov R1, #SUM3
	acall Shift	;shift right SUM3
	mov R1, #SUM3+1
	acall Move	;mov to SUM
	mov R1, #SUM3
	acall Clear
	mov C3, #M
	ajmp rr21
	
rr3:
	mov A, C4
	jnz rr4
	mov R1, #SUM4
	acall Shift	;shift right SUM4
	mov R1, #SUM4+1
	acall Move	;mov to SUM
	mov R1, #SUM4
	acall Clear
	mov C4, #M
	ajmp rr21
	
rr4:
	mov A, C5
	jnz rr5
	mov R1, #SUM5
	acall Shift	;shift right SUM5
	mov R1, #SUM5+1
	acall Move	;mov to SUM
	mov R1, #SUM5
	acall Clear
	mov C5, #M
	ajmp rr21
	
rr5:
	mov A, C6
	jnz rr6
	mov R1, #SUM6
	acall Shift	;shift right SUM6
	mov R1, #SUM6+1
	acall Move	;mov to SUM
	mov R1, #SUM6
	acall Clear
	mov C6, #M
	ajmp rr21
	
rr6:
	mov A, C7
	jnz rr7
	mov R1, #SUM7
	acall Shift	;shift right SUM7
	mov R1, #SUM7+1
	acall Move	;mov to SUM
	mov R1, #SUM7
	acall Clear
	mov C7, #M
	ajmp rr21
	
rr7:
	mov A, C8
	jnz rr8
	mov R1, #SUM8
	acall Shift	;shift right SUM8
	mov R1, #SUM8+1
	acall Move	;mov to SUM
	mov R1, #SUM8
	acall Clear
	mov C8, #M
	ajmp rr21
	
rr8:
	mov A, C9
	jnz rr9
	mov R1, #SUM9
	acall Shift	;shift right SUM9
	mov R1, #SUM9+1
	acall Move	;mov to SUM
	mov R1, #SUM9
	acall Clear
	mov C9, #M
	ajmp rr21
	
rr9:
	mov A, C10
	jnz rr10
	mov R1, #SUM10
	acall Shift	;shift right SUM10
	mov R1, #SUM10+1
	acall Move	;mov to SUM
	mov R1, #SUM10
	acall Clear
	mov C10, #M
	ajmp rr21
	
rr10:
	mov A, C11
	jnz rr11
	mov R1, #SUM11
	acall Shift	;shift right SUM11
	mov R1, #SUM11+1
	acall Move	;mov to SUM
	mov R1, #SUM11
	acall Clear
	mov C11, #M
	ajmp rr21
	
rr11:
	mov A, C12
	jnz rr12
	mov R1, #SUM12
	acall Shift	;shift right SUM12
	mov R1, #SUM12+1
	acall Move	;mov to SUM
	mov R1, #SUM12
	acall Clear
	mov C12, #M
	ajmp rr21
	
rr12:
	mov A, C13
	jnz rr13
	mov R1, #SUM13
	acall Shift	;shift right SUM13
	mov R1, #SUM13+1
	acall Move	;mov to SUM
	mov R1, #SUM13
	acall Clear
	mov C13, #M
	ajmp rr21
	
rr13:
	mov A, C14
	jnz rr14
	mov R1, #SUM14
	acall Shift	;shift right SUM14
	mov R1, #SUM14+1
	acall Move	;mov to SUM
	mov R1, #SUM14
	acall Clear
	mov C14, #M
	ajmp rr21
	
rr14:
	mov A, C15
	jnz rr15
	mov R1, #SUM15
	acall Shift	;shift right SUM15
	mov R1, #SUM15+1
	acall Move	;mov to SUM
	mov R1, #SUM15
	acall Clear
	mov C15, #M
	ajmp rr21
	
rr15:
	mov A, C16
	jnz rr16
	mov R1, #SUM16
	acall Shift	;shift right SUM16
	mov R1, #SUM16+1
	acall Move	;mov to SUM
	mov R1, #SUM16
	acall Clear
	mov C16, #M
	ajmp rr21
	
rr16:
	mov A, C17
	jnz rr17
	mov R1, #SUM17
	acall Shift	;shift right SUM17
	mov R1, #SUM17+1
	acall Move	;mov to SUM
	mov R1, #SUM17
	acall Clear
	mov C17, #M
	ajmp rr21
	
rr17:
	mov A, C18
	jnz rr18
	mov R1, #SUM18
	acall Shift	;shift right SUM18
	mov R1, #SUM18+1
	acall Move	;mov to SUM
	mov R1, #SUM18
	acall Clear
	mov C18, #M
	ajmp rr21
	
rr18:
	mov A, C19
	jnz rr19
	mov R1, #SUM19
	acall Shift	;shift right SUM19
	mov R1, #SUM19+1
	acall Move	;mov to SUM
	mov R1, #SUM19
	acall Clear
	mov C19, #M
	ajmp rr21
	
rr19:
	mov A, C20
	jnz rr21
	mov R1, #SUM20
	acall Shift	;shift right SUM20
	mov R1, #SUM20+1
	acall Move	;mov to SUM
	mov R1, #SUM20
	acall Clear
	mov C20, #M


rr21:	jmp wait1
	
Shift:	mov count2, #2
	mov B, R1
sh2:	mov count1, #4
sh1:	mov A, @R1	;SUMn
	rrc A
	mov @R1, A
	inc R1
	djnz count1, sh1
	mov R1, B
	djnz count2, sh2
	ret

Move:	
	mov A, @R1
	mov SUM, A
	inc R1
	mov A, @R1
	mov SUM+1, A
	inc R1
	mov A, @R1
	mov SUM+2, A
	ret

Clear:
	mov count1, #6
cc1:	mov @R1, #0
	inc R1
	djnz count1, cc1
	ret	

Conv:	mov A, in
	mov C, Acc.7
	mov signIn, C	;store Data sign
	jnC c0
	clr C		;make positive
	mov A, in+2
	subb A, #1
	cpl A
	mov in+2, A
	mov A, in+1
	subb A, #0
	cpl A
	mov in+1, A
	mov A, in
	subb A, #0
	cpl A
	mov in, A

c0:		;SUM1
	mov A, C1
	jz c1
	mov R0, #SUM1+5
	acall MultAdd
	dec C1
c1:
		;SUM2
	mov A, C2
	jz c2
	mov R0, #SUM2+5
	acall MultAdd
	dec C2
c2:
		;SUM3
	mov A, C3
	jz c3
	mov R0, #SUM3+5
	acall MultAdd
	dec C3
c3:
		;SUM4
	mov A, C4
	jz c4
	mov R0, #SUM4+5
	acall MultAdd
	dec C4
c4:
		;SUM5
	mov A, C5
	jz c5
	mov R0, #SUM5+5
	acall MultAdd
	dec C5
c5:
		;SUM6
	mov A, C6
	jz c6
	mov R0, #SUM6+5
	acall MultAdd
	dec C6
c6:
		;SUM7
	mov A, C7
	jz c7
	mov R0, #SUM7+5
	acall MultAdd
	dec C7
c7:
		;SUM8
	mov A, C8
	jz c8
	mov R0, #SUM8+5
	acall MultAdd
	dec C8
c8:
		;SUM9
	mov A, C9
	jz c9
	mov R0, #SUM9+5
	acall MultAdd
	dec C9
c9:
		;SUM10
	mov A, C10
	jz c10
	mov R0, #SUM10+5
	acall MultAdd
	dec C10
c10:
		;SUM11
	mov A, C11
	jz c11
	mov R0, #SUM11+5
	acall MultAdd
	dec C11
c11:
		;SUM12
	mov A, C12
	jz c12
	mov R0, #SUM12+5
	acall MultAdd
	dec C12
c12:
		;SUM13
	mov A, C13
	jz c13
	mov R0, #SUM13+5
	acall MultAdd
	dec C13
c13:
		;SUM14
	mov A, C14
	jz c14
	mov R0, #SUM14+5
	acall MultAdd
	dec C14
c14:
		;SUM15
	mov A, C15
	jz c15
	mov R0, #SUM15+5
	acall MultAdd
	dec C15
c15:
		;SUM16
	mov A, C16
	jz c16
	mov R0, #SUM16+5
	acall MultAdd
	dec C16
c16:
		;SUM17
	mov A, C17
	jz c17
	mov R0, #SUM17+5
	acall MultAdd
	dec C17
c17:
		;SUM18
	mov A, C18
	jz c18
	mov R0, #SUM18+5
	acall MultAdd
	dec C18
c18:
		;SUM19
	mov A, C19
	jz c19
	mov R0, #SUM19+5
	acall MultAdd
	dec C19
c19:
		;SUM20
	mov A, C20
	jz c20
	mov R0, #SUM20+5
	acall MultAdd
	dec C20
c20:
	ret	

MultAdd:
	mov DPTR, #filter	;get filter
	clr C
	rlc A
	mov DPL, A
	jnC m1
	inc DPH
m1:	mov A, #1
	movc A, @A+DPTR
	mov f1, A
	mov C, Acc.7
	mov A, #2
	movc A, @A+DPTR
	mov f2, A
	
  	mov signF, C	;store Filter sign
  	jnC g1
  	mov A, f2	;make positive
	clr C
	subb A, #1
	cpl A
	mov f2, A
	mov A, f1
	subb A, #0
	cpl A
	mov f1, A

g1:		mov C, signF	;xrl signF, signIn	Product sign
		anl C, /signIn
		mov sign, C
		mov C, signIn
		anl C, /signF
		orl C, sign
		mov sign, C

  	mov A, in+2	;multiply		
  	mov B,f2
  	mul AB
  	mov summ+5, A
  	mov summ+4, B
  		mov A, in+1
  		mov B,f2
  		mul AB
  		add A, summ+4
  		mov summ+4, A
  		mov A, B
  		addc A, #0
  		mov summ+3, A
  			mov A, in
	  		mov B,f2
	  		mul AB
	  		add A, summ+3
	  		mov summ+3, A
	  		mov A, B
	  		addc A, #0
	  		mov summ+2, A
	  		
				mov A, in+2
				mov B,f1
			  	mul AB
			  	add A, summ+4
			  	mov summ+4, A
			  	mov A, B
			  	addc A, summ+3
			  	mov summ+3, A
			  	mov A, summ+2
			  	addc A, #0
			  	mov summ+2, A
			  	mov A, #0
			  	addc A, #0
			  	mov summ+1, A 
			  		mov A, in+1
			  		mov B,f1
			  		mul AB
			  		add A, summ+3
			  		mov summ+3, A
			  		mov A, B
			  		addc A, summ+2
			  		mov summ+2, A
			  		mov A, summ+1
			  		addc A, #0
			  		mov summ+1, A
			  			mov A, in
				  		mov B,f1
				  		mul AB
				  		add A, summ+2
				  		mov summ+2, A
				  		mov A, B
						addc A, summ+1
				  		mov summ+1, A
				  		mov summ, #0
					jnb sign, s1	;restore sign
					mov A, summ+5
					cpl A
					add A, #1
					mov summ+5, A
					mov A, summ+4
					cpl A
					addc A, #0
					mov summ+4, A
					mov A, summ+3
					cpl A
					addc A, #0
					mov summ+3, A
					mov A, summ+2
					cpl A
					addc A, #0
					mov summ+2, A
					mov A, summ+1
					cpl A
					addc A, #0
					mov summ+1, A
					mov A, summ
					cpl A
					addc A, #0
					mov summ, A
				s1:				;ADD
					mov A, @R0
					add A, summ+5
					mov @R0, A
					dec R0
					mov A, @R0
					addc A, summ+4
					mov @R0, A
					dec R0
					mov A, @R0
					addc A, summ+3
					mov @R0, A
					dec R0
					mov A, @R0
					addc A, summ+2
					mov @R0, A
					dec R0
					mov A, @R0
					addc A, summ+1
					mov @R0, A
					dec R0
					mov A, @R0
					addc A, summ
					mov @R0, A
ret
			;filter 200Hz
	
filter: DB 00h
	DW 00h,0fff0h,0ffdfh,0ffd0h,0ffc3h,0ffb9h,0ffb2h,0ffb0h,0ffb3h,0ffbbh,0ffc9h,0ffdbh,0fff3h,0eh,02ch,04bh,068h,083h
	DW 097h,0a4h,0a6h,09dh,088h,065h,037h,00h,0ffc1h,0ff7eh,0ff3ch,0ff00h,0feceh,0feach,0fe9eh,0fea7h,0fecah,0ff06h
	DW 0ff5bh,0ffc5h,03eh,0c1h,0145h,01c1h,022bh,0279h,02a4h,02a4h,0276h,0218h,018bh,0d6h,00h,0ff15h,0fe23h,0fd3ah
	DW 0fc6bh,0fbc7h,0fb5dh,0fb3ah,0fb66h,0fbe7h,0fcbbh,0fddbh,0ff3dh,0cch,0275h,041bh,05a2h,06eeh,07e2h,0865h,0863h
	DW 07d0h,06a6h,04ebh,02abh,00h,0fd0ah,0f9f3h,0f6eah,0f424h,0f1d7h,0f037h,0ef75h,0efbch,0f12ch,0f3d9h,0f7cbh,0fcf9h
	DW 034ch,0aa1h,012c3h,01b72h,02466h,02d50h,035dfh,03dc2h,044adh,04a5ch,04e96h,05130h,05211h,05130h,04e96h,04a5ch
	DW 044adh,03dc2h,035dfh,02d50h,02466h,01b72h,012c3h,0aa1h,034ch,0fcf9h,0f7cbh,0f3d9h,0f12ch,0efbch,0ef75h,0f037h
	DW 0f1d7h,0f424h,0f6eah,0f9f3h,0fd0ah,00h,02abh,04ebh,06a6h,07d0h,0863h,0865h,07e2h,06eeh,05a2h,041bh,0275h,0cch
	DW 0ff3dh,0fddbh,0fcbbh,0fbe7h,0fb66h,0fb3ah,0fb5dh,0fbc7h,0fc6bh,0fd3ah,0fe23h,0ff15h,00h,0d6h,018bh,0218h,0276h
	DW 02a4h,02a4h,0279h,022bh,01c1h,0145h,0c1h,03eh,0ffc5h,0ff5bh,0ff06h,0fecah,0fea7h,0fe9eh,0feach,0feceh,0ff00h
	DW 0ff3ch,0ff7eh,0ffc1h,00h,037h,065h,088h,09dh,0a6h,0a4h,097h,083h,068h,04bh,02ch,0eh,0fff3h,0ffdbh,0ffc9h,0ffbbh
	DW 0ffb3h,0ffb0h,0ffb2h,0ffb9h,0ffc3h,0ffd0h,0ffdfh,0fff0h,00h
end