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