Take a look at Tracy Allens pages on math (and other stuff) routines for the BasicStamp, specifically this page when digital filtering is covered. I've adopted one of the routines for PBP. I believe, at one point, I stuck that routine to the output of the PID filter but I've never posted it and I don't think I've done/tested anything with it. I'll leave you with the filter itself for you to what you want with.
Code:
'****************************************************************
'* Name : LowPassFilter.pbp *
'* Author : Henrik Olsson *
'* Notice : Copyright (c) 2010 Henrik Olsson 2010 *
'* : All Rights Reserved *
'* Date : 2010-09-01 *
'* Version : 1.0 *
'* Notes : Adopted from Tracy Allens BS2Math pages. *
'* : *
'****************************************************************
'--Variables accessed from "outside"--
LowPassFilter_Constant VAR BYTE
LowPassFilter_Input VAR WORD
LowPassFilter_Output VAR WORD
'--Variable used internally
Filter_Sign VAR BIT
Filter_Value VAR WORD
Filter_Z VAR WORD
Filter_Y VAR WORD
'--User must set LowPassFilter_Constant to 0-9 and then GOSUB InitLowPassFilter before
'--calling the actual filter routine.
pid_LP_Filter_Init:
Select Case LowPassFilter_Constant
CASE 0
Filter_Value = 65535
Case 1
Filter_Value = 32768
Case 2
Filter_Value = 21846
Case 3
Filter_Value = 16384
Case 4
Filter_Value = 13108
Case 5
Filter_Value = 10923
Case 6
Filter_Value = 9363
Case 7
Filter_Value = 8192
CASE 8
Filter_Value = 7282
Case 9
Filter_Value = 6554
End Select
RETURN
LowPassFilter:
If LowPassFilter_Constant = 0 then
LowPassFilter_Output = LowPassFilter_Input
Goto LowPassFilterDone
ENDIF
'Max difference between current filter output and a new input to the filter
'is 2048 so it's recomended to all input values clamped to +/-1024 or unexpected
'results may occur,
'Complete filter executes in 174-177 cycles----
'Multiply input by 16 (shift left by 4) then subtract the current output (times 16) to get the difference. (47 cycles)
Filter_Z = LowPassFIlter_Input << 4
Filter_Z = Filter_Z - Filter_Y
'Get the sign of the difference then convert to ABSolute value (7 cycles).
Filter_Sign = Filter_Z.15 ' sign of the difference (4 cycles)
Filter_Z = abs Filter_Z ' magnitude of the difference (3 cycles)
'Add then divide. Basically this is the same as A=(A+1)/2, A=(A+2)/3, A=(A+3)/4 etc.
'All in all 52 cycles.
Filter_Z = Filter_Z + LowPassFilter_Constant '(52 cycles including ** operation on next line
Filter_Z = Filter_Z ** Filter_Value
'Now the value is scaled and needs to be added to or subtracted from the output. (8 or 10 cycles)
If Filter_Sign then
Filter_Y = Filter_Y - Filter_Z
ELSE
Filter_Y = Filter_Y + Filter_Z
ENDIF
Filter_Sign = Filter_Y.15 ' sign of total output (4 cycles)
'Get absoulte value and divide by 16 to scale down the value to the same scale
'as the original input value. Then re-apply the sign. (50 or 52 cycles)
LowPassFilter_Output = (ABS Filter_Y) >> 4
If Filter_Sign then LowPassFilter_Output = -LowPassFilter_Output '(4 or 7 cycles)
LowPassFilterDone:
RETURN
Bookmarks