SX Microcontroller DSP Math Method

24bit 256 step IIR filter by Tony Kübek

; ************************* TWIST24 ******************************
;
; Purpose of routine is to evaluate
;
; 	A*X0 + B*Y
; X1 =	---------
;   	  A + B
;
; Such as A+B = 2^n
;
; Simplified this formula will then become
;
; 	A*X0 + ~A*Y + Y
; X1 =  ---------------
;	     2^n
;
; Where:
; X1 - is new recursive lowpassed filter value
; X0 - is previous lowpassed filter value
; Y  - is new value
; A,B are weight factors, if A is large relative to B
; then more emphasis is placed on the average ( filtered ) value.
; If B is large then the latest sample are given more weight.
; ~A is A's bitwise complement
;
;
; X0,X1,Y are 24 bit variables
; A+B = 2^8 = 256 for this routine
;
; By Tony Kübek 2000-05-23, based on routine 'twist.asm' by
; Scott Dattalo ( BTW Thanks for sharing :-) )
;
; ****************************************************************
;

; NewSample = Y ( 4 byte ram, 3 byte data )
; OldFilter = X0 ( 4 byte ram, 3 byte data )
; NewFilter = X1 ( 4 byte ram, 3 byte data )
; FilterWeight = A ( 1 byte ram, how much weight that is placed on old filtered value )
;
; * variables not really needed but used in example *
; FilterCounter = 1 byte, counter to increase step responce.
; UpdateCounter = 1 byte, how many samples between 'global' updates
; AD_NewValue = 3 byte ram, last 24 bit reading from AD/or similar ( copied
; 		to NewSample, in case AD_NewValue should be used for other purposes )
; AD_DataVal = 3 byte ram, filtered value updated after UpdateCounter samples
; _AD_DataReady = 1 bit, set when AD_DataVal is updated
;


#define UPDATE_COUNT 	0x20 ; 32 samples between global updates
#define FILTER_WEIGHT 	0x80 ; filterweight, i.e. 'A' in example above

	org $10
	AD_NewValue ds 3
	NewSample ds 4	; copy of AD_Newvalue, used locally
	OldFilter ds 4	; previous value of filtering ( NewFilter from last run )
	NewFilter ds 4	; the new filtered value
	FilterWeight ds 1	; how much weight that should be posed on old value
	FilterCounter ds 1 ; to increase step responce
	UpdateCounter ds 1 ; how often we want an global update
	AD_DataVal ds 3	; the global update location
	Bitvars ds 1
	ENDC

#define _AD_DataReady	BitVars,0

;++++++++++++
;
; ADD_FSR_FILTER - Adds 32 bit value pointed to by FSR to NewFilter
; FSR must point to LEAST significant byte, FSR-3 is most significant
;

ADD_FSR_FILTER MACRO
	; 14-17 instructions

	; add value pointed to by FSR to filter
	mov	W, INDF	; get lowest byte
	add	NewFilter+3, W	; add to filter sum lowest byte

	dec	FSR
	mov	W, INDF	; get next byte
	snb	C	;  if overflow
	movsz	W, ++INDF	; increase source
	add	NewFilter+2, W	; and add to dest.
	dec	FSR
	mov	W, INDF	; get next byte
	snb	C
	movsz	W, ++INDF
	add	NewFilter+1, W

	dec	FSR
	mov	W, INDF	; get msb
	snb	C
	movsz	W, ++INDF
	add	NewFilter, W
	ENDM
;++++++++++++
;
; DIV_FILTER_BY2 - Divide NewFilter by 2
;
;
DIV_FILTER_BY2 MACRO
	; 5 instructions
	; right shift filter value by 1 ( i.e. divide by 2 )
	clrb	C	; clear carry
	; divide by 2
	rr	NewFilter
	rr	NewFilter+1
	rr	NewFilter+2
	rr	NewFilter+3
	ENDM
;++++++++++++
;
; MUL_NEWOLD_BY2 - Multiply OldValue and NewSample with 2
;
;
MUL_NEWOLD_BY2 MACRO
	; 10 instructions
	; right shift filter value by 1 ( i.e. divide by 2 )
	clrb	C	; clear carry
	; multiply old value with 2
	rl	OldFilter+3
	rl	OldFilter+2
	rl	OldFilter+1
	rl	OldFilter
	; multiply new value with 2
	clrb	C	; clear carry
	rl	NewSample+3
	rl	NewSample+2
	rl	NewSample+1
	rl	NewSample
	ENDM





	org	$0000
	jmp	INIT


; ** interupt routine for data collection
	org	$0004
INT
	; get data from AD
	call	GET_AD_DATA	; not included !

	; for each sample, copy to AD_NewValue
; and	call	filter once
	call	TWIST24

	reti


; cold start vector
INIT
	; only 'dummy' code here
	clrb	_AD_DataReady	; clear data ready flag


	; Note, this will inilialize the filter to gradually use
	; no filtering to 'full' filtering ( increase once for each sample )
	;
	mov	W, #FILTER_WEIGHT
	mov	FilterCounter, W
	clr	FilterWeight

MAIN_LOOP
	; wait for some data from AD or similar
	sb	_AD_DataReady	; check if data available
	jmp	MAIN_LOOP	; nope

	; filtered data available
	; do whatever needs to be done..

	clrb	_AD_DataReady	; clear data ready flag

	jmp	MAIN_LOOP





;++++++++++++
;
; TWIST24 - Variable 24 bit lowpass filter, caculates new lowpassed value in NewFilter,
; by weighing previous filtervalue and NewSample according to FilterWeight
; If FilterWeight is large more emphasis is placed on oldfiltered value
; Maximum value for FilterWeight is 255 ( i.e. 8 bit variable ).
; FilterWeight = 0 -> NewFilter = 0/256 of OldFilter + 256/256 of NewSample, i.e no filtering
; FilterWeight = 255 -> NewFilter = 255/256 of OldFilter + 1/256 of NewSample, i.e. full filtering
; NOTE: Previous filtered value should be kept in NewFilter as it is used for next pass.
;
TWIST24
	; about 252-285 instructions executed ( with global update and copying of new datavalue )
	; roufly 57 us at XTAL 20 Mhz

	; ! uses FSR !

	; Ramp function, uses an extra ram variable FilterCounter that
	; increases the FilterWeight until itself zero
	; Usage: Initialise to FilterWeight, then a speedier time to target will
	; be accomplished(step responce). During run, if for any reason an high ramp
	; is detected ( or loss of readings ) this could be re-initialised to any
	; value equal or less than FilterWeight to achive quicker step responce.
	; NOTE : Filterweight must then ALSO be initialised so that the following
	; is fulfilled: FilterCounter + FilterWeight = DesiredFilterWeight
	test	FilterCounter
	snb	Z
	jmp	TWIST_GO

	decsz	FilterCounter
	inc	FilterWeight

TWIST_GO
	; Copy previous filtered value ( note previous value is multiplied by 256
	; i.e. only copy top three bytes of source to lowest three bytes of dest. )
	mov	W, NewFilter
	mov	OldFilter+1, W
	mov	W, NewFilter+1
	mov	OldFilter+2, W
	mov	W, NewFilter+2
	mov	OldFilter+3, W

	; copy new value from AD to 'local' variable and add it it
	; to filter as start value
	mov	W, AD_NewValue	; get top byte of new reading
	mov	NewSample+1, W	; store in local variable
	mov	NewFilter+1, W	; also add this as start value to new filter
	mov	W, AD_NewValue+1	;
	mov	NewSample+2, W	;
	mov	NewFilter+2, W	;
	mov	W, AD_NewValue+2	;
	mov	NewSample+3, W	;
	mov	NewFilter+3, W	;
	clr	NewFilter	;


	clr	OldFilter	; clear top bytes ( we only have a 24 bit filter )
	clr	NewSample


	mov	W, #NewSample+3	; get adress for new value
	mov	FSR, W	; setup FSR
	mov	W, #OldFilter+3	; get adress for old value to W

	snb	FilterWeight.0	; check if value that should be added is new or old
	mov	FSR, W	; adress for old value already in W

	ADD_FSR_FILTER		; add it

	MUL_NEWOLD_BY2		; upshift old and new value, 10 instr.

	mov	W, #NewSample+3	; get adress for new value
	mov	FSR, W	; setup FSR
	mov	W, #OldFilter+3	; get adress for old value to W

	snb	FilterWeight.1	; check if value that should be added is new or old
	mov	FSR, W	; old value added to filter, adress in W
	ADD_FSR_FILTER		; add it

	MUL_NEWOLD_BY2		; upshift old and new value, 10 instr.

	mov	W, #NewSample+3	; get adress for new value
	mov	FSR, W	; setup FSR
	mov	W, #OldFilter+3	; get adress for old value to W

	snb	FilterWeight.2	; check if value that should be added is new or old
	mov	FSR, W	; old value added to filter, adress in W
	ADD_FSR_FILTER		; add it

	MUL_NEWOLD_BY2		; upshift old and new value, 10 instr.

	mov	W, #NewSample+3	; get adress for new value
	mov	FSR, W	; setup FSR
	mov	W, #OldFilter+3	; get adress for old value to W

	snb	FilterWeight.3	; check if value that should be added is new or old
	mov	FSR, W	; old value added to filter, adress in W
	ADD_FSR_FILTER		; add it

	MUL_NEWOLD_BY2		; upshift old and new value, 10 instr.


	mov	W, #NewSample+3	; get adress for new value
	mov	FSR, W	; setup FSR
	mov	W, #OldFilter+3	; get adress for old value to W

	snb	FilterWeight.4	; check if value that should be added is new or old
	mov	FSR, W	; old value added to filter, adress in W
	ADD_FSR_FILTER		; add it

	MUL_NEWOLD_BY2		; upshift old and new value, 10 instr.


	mov	W, #NewSample+3	; get adress for new value
	mov	FSR, W	; setup FSR
	mov	W, #OldFilter+3	; get adress for old value to W

	snb	FilterWeight.5	; check if value that should be added is new or old
	mov	FSR, W	; old value added to filter, adress in W
	ADD_FSR_FILTER		; add it

	MUL_NEWOLD_BY2		; upshift old and new value, 10 instr.

	mov	W, #NewSample+3	; get adress for new value
	mov	FSR, W	; setup FSR
	mov	W, #OldFilter+3	; get adress for old value to W

	snb	FilterWeight.6	; check if value that should be added is new or old
	mov	FSR, W	; old value added to filter, adress in W
	ADD_FSR_FILTER		; add it

	MUL_NEWOLD_BY2		; upshift old and new value, 10 instr.

	mov	W, #NewSample+3	; get adress for new value
	mov	FSR, W	; setup FSR
	mov	W, #OldFilter+3	; get adress for old value to W

	snb	FilterWeight.7	; check if value that should be added is new or old
	mov	FSR, W	; old value added to filter, adress in W
	ADD_FSR_FILTER		; add it


	; 235-268 instructions to get here

	; check for rounding
	sb	NewFilter+3.7	; test top bit of lowest byte
	jmp	TWIST24_EXIT
	; add one to filter to have proper rounding
	mov	W, #$01
	add	NewFilter+2, W
	snb	C
	add	NewFilter+1, W
	snb	C
	add	NewFilter, W

TWIST24_EXIT
	; check for update
	decsz	UpdateCounter
	ret
	; update global filter
	mov	W, NewFilter+2
	mov	AD_DataVal+2, W
	mov	W, NewFilter+1
	mov	AD_DataVal+1, W
	mov	W, NewFilter
	mov	AD_DataVal, W
	; set data ready flag
	setb	_AD_DataReady
	; reinitialise update counter
	mov	W, #UPDATE_COUNT	; number of samples between global update
	mov	UpdateCounter, W

	ret