; ************************* 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
file: /Techref/microchip/dsp/iir-24b-256s-tk.htm, 9KB, , updated: 2000/5/24 13:01, local time: 2024/12/13 03:10,
18.97.14.84:LOG IN
|
©2024 These pages are served without commercial sponsorship. (No popup ads, etc...).Bandwidth abuse increases hosting cost forcing sponsorship or shutdown. This server aggressively defends against automated copying for any reason including offline viewing, duplication, etc... Please respect this requirement and DO NOT RIP THIS SITE. Questions? <A HREF="http://www.ecomorder.com/Techref/microchip/dsp/iir-24b-256s-tk.htm"> PIC Microcontoller DSP Math Method - 24bit 256 step IIR filter by Tony Kübek</A> |
Did you find what you needed? |
Welcome to ecomorder.com! |
Welcome to www.ecomorder.com! |
.