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