; ************************* 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
.