PIC Microcontoller 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

CBLOCK	0x020			
	AD_NewValue:3
	NewSample:4	; copy of AD_Newvalue, used locally	
	OldFilter:4	; previous value of filtering ( NewFilter from last run )
	NewFilter:4	; the new filtered value
	FilterWeight:1	; how much weight that should be posed on old value
	FilterCounter:1 ; to increase step responce
	UpdateCounter:1 ; how often we want an global update
	AD_DataVal:3	; the global update location
	Bitvars: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 
    	MOVF   	INDF,W		; get lowest byte
	ADDWF	NewFilter+3,F	; add to filter sum lowest byte   
	
	DECF	FSR,F
	MOVF	INDF,W ; get next byte
	SKPNC			 ;  if overflow
	   INCFSZ	INDF,W ; increase source 
		ADDWF	NewFilter+2,F ; and add to dest.
	DECF	FSR,F
	MOVF	INDF,W ; get next byte
	SKPNC
	   INCFSZ	INDF,W
		ADDWF	NewFilter+1,F
	
	DECF	FSR,F
	MOVF	INDF,W ; get msb
	SKPNC
	   INCFSZ	INDF,W
		ADDWF	NewFilter,F
	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 )
	BCF	STATUS,C	; clear carry
	; divide by 2
	RRF	NewFilter,F
	RRF	NewFilter+1,F
	RRF	NewFilter+2,F
	RRF	NewFilter+3,F
	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 )
	BCF	STATUS,C	; clear carry
	; multiply old value with 2
	RLF	OldFilter+3,F
	RLF	OldFilter+2,F
	RLF	OldFilter+1,F
	RLF	OldFilter,F
	; multiply new value with 2
	BCF	STATUS,C	; clear carry
	RLF	NewSample+3,F
	RLF	NewSample+2,F
	RLF	NewSample+1,F
	RLF	NewSample,F
	ENDM





ORG	0x0000
	GOTO	INIT


; ** interupt routine for data collection
ORG	0x0004
INT
	; get data from AD
	CALL	GET_AD_DATA ; not included !

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

	RETFIE
	

; cold start vector
INIT
	; only 'dummy' code here
	BCF	_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 )
	; 
	MOVLW	FILTER_WEIGHT	
	MOVWF	FilterCounter
	CLRF	FilterWeight

MAIN_LOOP
	; wait for some data from AD or similar
	BTFSS	_AD_DataReady ; check if data available
	GOTO	MAIN_LOOP	; nope

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

	BCF	_AD_DataReady	; clear data ready flag

	GOTO	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
	MOVF	FilterCounter,F
	BTFSC	STATUS,Z
	GOTO	TWIST_GO

	DECFSZ	FilterCounter,F
	INCF	FilterWeight,F

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. )
	MOVF	NewFilter,W
	MOVWF	OldFilter+1
	MOVF	NewFilter+1,W
	MOVWF	OldFilter+2
	MOVF	NewFilter+2,W
	MOVWF	OldFilter+3

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


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


	MOVLW	NewSample+3	; get adress for new value
	MOVWF	FSR		; setup FSR
	MOVLW	OldFilter+3	; get adress for old value to W

	BTFSC	FilterWeight,0	; check if value that should be added is new or old
	MOVWF	FSR		; adress for old value already in W
	
	ADD_FSR_FILTER		; add it
	
	MUL_NEWOLD_BY2		; upshift old and new value, 10 instr.
	
	MOVLW	NewSample+3	; get adress for new value
	MOVWF	FSR		; setup FSR
	MOVLW	OldFilter+3	; get adress for old value to W

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

	MUL_NEWOLD_BY2		; upshift old and new value, 10 instr.

	MOVLW	NewSample+3	; get adress for new value
	MOVWF	FSR		; setup FSR
	MOVLW	OldFilter+3	; get adress for old value to W

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

	MUL_NEWOLD_BY2		; upshift old and new value, 10 instr.

	MOVLW	NewSample+3	; get adress for new value
	MOVWF	FSR		; setup FSR
	MOVLW	OldFilter+3	; get adress for old value to W

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

	MUL_NEWOLD_BY2		; upshift old and new value, 10 instr.
	

	MOVLW	NewSample+3	; get adress for new value
	MOVWF	FSR		; setup FSR
	MOVLW	OldFilter+3	; get adress for old value to W

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

	MUL_NEWOLD_BY2		; upshift old and new value, 10 instr.


	MOVLW	NewSample+3	; get adress for new value
	MOVWF	FSR		; setup FSR
	MOVLW	OldFilter+3	; get adress for old value to W

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

	MUL_NEWOLD_BY2		; upshift old and new value, 10 instr.
	
	MOVLW	NewSample+3	; get adress for new value
	MOVWF	FSR		; setup FSR
	MOVLW	OldFilter+3	; get adress for old value to W

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

	MUL_NEWOLD_BY2		; upshift old and new value, 10 instr.
	
	MOVLW	NewSample+3	; get adress for new value
	MOVWF	FSR		; setup FSR
	MOVLW	OldFilter+3	; get adress for old value to W

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


	; 235-268 instructions to get here

	; check for rounding
	BTFSS	NewFilter+3,7  ; test top bit of lowest byte
	GOTO	TWIST24_EXIT
	; add one to filter to have proper rounding
	MOVLW	0x01
	ADDWF	NewFilter+2,F
	SKPNC
	ADDWF	NewFilter+1,F
	SKPNC	
	ADDWF	NewFilter,F

TWIST24_EXIT
	; check for update
	DECFSZ	UpdateCounter,F
	RETURN	
	; update global filter
	MOVF	NewFilter+2,W
	MOVWF	AD_DataVal+2
	MOVF	NewFilter+1,W
	MOVWF	AD_DataVal+1
	MOVF	NewFilter,W
	MOVWF	AD_DataVal
	; set data ready flag
	BSF	_AD_DataReady
	; reinitialise update counter
	MOVLW	UPDATE_COUNT ; number of samples between global update	
	MOVWF	UpdateCounter

	RETURN

  .