94 lines
2.7 KiB
Markdown
94 lines
2.7 KiB
Markdown
|
```
|
|||
|
|
FUNCTION "FC_LowPassFilter" : Void
|
|||
|
|
{ S7_Optimized_Access := 'TRUE' }
|
|||
|
|
VERSION : 0.1
|
|||
|
|
VAR_INPUT
|
|||
|
|
analog_input : Real; // Raw analog input signal
|
|||
|
|
alpha : Real := 0.8; // Smoothing factor (0.0 to 1.0)
|
|||
|
|
enable : Bool := TRUE; // Enable filter
|
|||
|
|
reset : Bool := FALSE; // Reset filter output to current input
|
|||
|
|
END_VAR
|
|||
|
|
|
|||
|
|
VAR_OUTPUT
|
|||
|
|
low_passed_output : Real; // Filtered output signal
|
|||
|
|
END_VAR
|
|||
|
|
|
|||
|
|
VAR_IN_OUT
|
|||
|
|
END_VAR
|
|||
|
|
|
|||
|
|
VAR
|
|||
|
|
prev_output : Real; // Previous filtered output (retains value)
|
|||
|
|
prev_input : Real; // Previous raw input
|
|||
|
|
initialized : Bool := FALSE; // First-run flag
|
|||
|
|
END_VAR
|
|||
|
|
|
|||
|
|
VAR_TEMP
|
|||
|
|
END_VAR
|
|||
|
|
|
|||
|
|
BEGIN
|
|||
|
|
// Clamp alpha to valid range [0.0, 1.0]
|
|||
|
|
IF #alpha < 0.0 THEN
|
|||
|
|
#alpha := 0.0;
|
|||
|
|
ELSIF #alpha > 1.0 THEN
|
|||
|
|
#alpha := 1.0;
|
|||
|
|
END_IF;
|
|||
|
|
|
|||
|
|
// Handle reset
|
|||
|
|
IF #reset THEN
|
|||
|
|
#low_passed_output := #analog_input;
|
|||
|
|
#prev_output := #analog_input;
|
|||
|
|
#prev_input := #analog_input;
|
|||
|
|
#initialized := TRUE;
|
|||
|
|
RETURN;
|
|||
|
|
END_IF;
|
|||
|
|
|
|||
|
|
// Skip if disabled
|
|||
|
|
IF NOT #enable THEN
|
|||
|
|
#low_passed_output := #analog_input;
|
|||
|
|
RETURN;
|
|||
|
|
END_IF;
|
|||
|
|
|
|||
|
|
// Initialize on first run to prevent startup transient
|
|||
|
|
IF NOT #initialized THEN
|
|||
|
|
#low_passed_output := #analog_input;
|
|||
|
|
#prev_output := #analog_input;
|
|||
|
|
#prev_input := #analog_input;
|
|||
|
|
#initialized := TRUE;
|
|||
|
|
RETURN;
|
|||
|
|
END_IF;
|
|||
|
|
|
|||
|
|
// First-order IIR low-pass filter: Y(k) = alpha * Y(k-1) + (1 - alpha) * X(k-1)
|
|||
|
|
#low_passed_output := (#alpha * #prev_output) + ((1.0 - #alpha) * #prev_input);
|
|||
|
|
|
|||
|
|
// Update previous values for next cycle
|
|||
|
|
#prev_output := #low_passed_output;
|
|||
|
|
#prev_input := #analog_input;
|
|||
|
|
|
|||
|
|
END_FUNCTION
|
|||
|
|
|
|||
|
|
```
|
|||
|
|
# Usage Example (in another block)
|
|||
|
|
```
|
|||
|
|
scl
|
|||
|
|
// Call the filter with alpha = 0.9 (heavy smoothing)
|
|||
|
|
"FC_LowPassFilter"(
|
|||
|
|
analog_input := "Raw_Pressure_Sensor",
|
|||
|
|
alpha := 0.9,
|
|||
|
|
enable := TRUE,
|
|||
|
|
reset := "Sensor_Fault",
|
|||
|
|
low_passed_output => "Filtered_Pressure"
|
|||
|
|
);
|
|||
|
|
```
|
|||
|
|
## Tuning Guidelines
|
|||
|
|
### Tuning Guidelines
|
|||
|
|
|
|||
|
|
| Alpha | Behavior | Use Case |
|
|||
|
|
|-------------|-----------------------------------|---------------------------------------|
|
|||
|
|
| 0.95 – 0.99 | Very heavy smoothing, slow response | Very noisy signals, slow processes |
|
|||
|
|
| 0.80 – 0.90 | Moderate smoothing | General purpose (default 0.8) |
|
|||
|
|
| 0.50 – 0.70 | Light smoothing, responsive | Faster processes, moderate noise |
|
|||
|
|
| 0.10 – 0.40 | Minimal smoothing, very responsive | Clean signals needing slight damping |
|
|||
|
|
|
|||
|
|
> **Tip:** For a given cutoff frequency $f_c$ and sample time $T_s$, you can calculate alpha as:
|
|||
|
|
> $$\alpha = e^{-2\pi \cdot f_c \cdot T_s}$$
|