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