FbFilterM.SCL

FUNCTION_BLOCK "FbFilterM"
TITLE = 'Фильтр медианный.'

{ S7_read_back := 'true' ;
  S7_blockview := 'big'  }

AUTHOR  : VA
FAMILY  : LIB_PLC
VERSION : '1.1'

VAR_INPUT // Входные переменные, сохраняемые.
In    :REAL := 0.0  ; // Входной сигнал до фильтрации.
Size  :INT  := 16   ; // Размер массива для сортировки (Количество выборок).
Ts_in :DINT := 1000 ; // Время между выборками и сортировками для входа In [мс].
Ts_ms :DINT := 100  ; // Шаг дискретизации по времени [мс].
Reset :BOOL := FALSE; // Сброс при перезагрузке.
END_VAR

VAR_OUTPUT // Выходные переменные, сохраняемые.
Out :REAL := 0.0; // Выходной сигнал (середина отсортированного массива).
END_VAR

CONST // Константы.
NUMBER := 64; // Размер массива.
END_CONST

VAR // Внутренние переменные, сохраняемые.
TimerTs     :DINT := 0; // Таймер для ресемплинга [мс].
Size_prev   :INT  := 0; // Предыдущее значение соответствующего входного параметра.
Old_i       :INT  := 0; // Адрес самого старого элемента в массиве.
InputArray  :ARRAY [0..NUMBER] OF REAL; // Массив входных значений для сортировки.
TimeArray   :ARRAY [0..NUMBER] OF INT ; // Массив времени жизни элемента 0-самый молодой 1-старше 2- еще старше.
END_VAR

VAR_TEMP // Временные переменные, не сохраняемые.
FlagRunCode :BOOL; // Флаг выполнить программу с шагом дискретизации Ts_in.
Adr_max     :INT ; // Максимальное значение адреса массива.
i           :INT ; // Счетчик цикла.
j           :INT ; // Счетчик цикла.
TempValue   :REAL; // Временная переменная для сортировки значений.
TempTime    :INT ; // Временная переменная для сортировки времени жизни элемента.
END_VAR

BEGIN

// Фильтр медианный.
//       DbFilterM
//    +-------------+
//    |  FbFilterM  |
// ->-|In        Out|->-
//   -|Size         |
//   -|Ts_in        |
//   -|Ts_ms        |
//   -|Reset        |
//    +-------------+

// Фильтр медианный.
// Хорошо фильтрует случайные импульсные выбросы.
// Разрабатывался для фильтрации сигналов с расходомеров воды.
// Как не в себя жрет память и процессор.
// В 2025 году фильтр проверен на практике все отлично работает на электромагитных расходомерах жидкости.
// На самом электромагнитном расходомере необходимо выключить или уменьшить значение встроенной постоянной времени фильтра.
// 
// Суть алгоритма.
// Берем массив измеренных значений InputArray собранный с входа In и обновляем его каждые Ts_in миллисекунд.
// Самое старое значение массива выкидываем и записываем на его место новое значение с входа In.
// Чтобы понимать какое значение новое, а какое старое делаем дополнительный массив возраста значений InputArray.
// После записи нового значения сортируем массив InputArray пузырьком по возрастанию, чем больше индекс массива, тем больше там лежит значение.
// Выход фильта Out есть середина этого массива, собственно поэтому он медианный.
// 
// Планируется вызывать этот фильтр в основном цикле OB1 main().
// 
// Для настройки фильтра есть два параметра первый размер массива.
// Второй параметр изменение (увеличение) шага дискретизации фильтра относительно времени скана контроллера.
// То есть выборки фильтра должны происходить реже чем вызывается сам фильтр (Ts_in > Ts_ms).

// ****************************************************************************
// Так как массив считается с 0 то максимальный адрес на 1 меньше размера.
Adr_max := Size - 1;
// Защита от выхода массива за границы.
IF (Size >= NUMBER) THEN
Adr_max := NUMBER - 2;
END_IF;
// Менее 5и элементов не вижу смысла сортировать.
IF (Size <= 5) THEN
Adr_max := 4;
END_IF;

//****************************************************************************
// Ресэмплинг выключен если Ts_ms >= Ts_in
IF (Ts_ms >= Ts_in) THEN
TimerTs := 0;
FlagRunCode := TRUE;
ELSE
  // Ресэмплинг для возможности изменения времени между выборками.
  IF (TimerTs >= Ts_in) THEN
  TimerTs := 0;
  FlagRunCode := TRUE;
  ELSE
  TimerTs := TimerTs + Ts_ms;
  FlagRunCode := FALSE;
  END_IF;
END_IF;

//****************************************************************************
// Инициализация массива.
// При перезапуске ПЛК.
// При изменении настроек фильтра.
IF (Reset OR (Size <> Size_prev) ) THEN
  // Цикл от 0 до Adr_max включительно с шагом +1.
  FOR i := 0 TO Adr_max BY 1 DO
    // Заполняем весь массив значений входным сигналом.
    InputArray[i] := In;
    // Заполняем массив времени жизни элемента.
    TimeArray[i] := i;
  END_FOR;
// После инициализации самый старый элемент вверху массива.
Old_i := Adr_max;
// Массив после инициализации при In = 0.0, Adr_max = 4.
//  InputArray TimeArray  i
// +----------+---------+---+
// |    0.0   |    4    | 4 | <- Old_i <- Adr_max
// |    0.0   |    3    | 3 |
// |    0.0   |    2    | 2 |
// |    0.0   |    1    | 1 |
// |    0.0   |    0    | 0 |
// +----------+---------+---+
// Направление работы цикла от 0 до 4.
END_IF;

//----------------------------------------------------------------------------
// Код выполняемый с шагом дискретизации Ts_in.
IF (FlagRunCode AND (Size <> 0)) THEN

//----------------------------------------------------------------------------

  FOR i := 0 TO Adr_max BY 1 DO
    // Обновляем массив возраста элементов.
    IF (TimeArray[i] = Adr_max) THEN
      // Самый старый элемент становится самым новым.
      TimeArray[i] := 0;
    ELSE
      // Все остальные стареют на +1.
      TimeArray[i] := TimeArray[i] + 1;
      // Ищем самый старый элемент массива.
      IF (TimeArray[i] = Adr_max) THEN
        // Запоминаем самый старый элемент массива
        // В него потом запишем новое значение входа
        Old_i := i;
      END_IF;
    END_IF;
  END_FOR;

//----------------------------------------------------------------------------

// Записываем новое значение входа в самый старый элемент массива.
InputArray[Old_i] := In;

//----------------------------------------------------------------------------

// Сортировка по возрастанию массива значений InputArray.
// Массив до сортировки 
//  InputArray TimeArray  i
// +----------+---------+---+
// |    1.0   |    0    | 0 | 
// |    4.0   |    1    | 1 | 
// |    3.0   |    2    | 2 | 
// |    6.0   |    3    | 3 | 
// |    5.0   |    4    | 4 | <- Adr_max <- Old_i
// +----------+---------+---+
// Массив после сортировки 
//  InputArray TimeArray  i
// +----------+---------+---+
// |    1.0   |    0    | 0 | 
// |    3.0   |    2    | 1 | 
// |    4.0   |    1    | 2 | 
// |    5.0   |    4    | 3 | <- Old_i
// |    6.0   |    3    | 4 | <- Adr_max
// +----------+---------+---+
// Пузырьковая сортировка.
// Думаю при диком желании это можно свернуть до одного цикла while тк массив каждый раз оставляем отсортированным.
// Так же для ускорения можно заменить вход / выход и массив значений с 32х битной плавающей точки на 16и битное целочисленное значение.
// https://ru.wikibooks.org/wiki/%D0%AF%D0%B7%D1%8B%D0%BA_%D0%A1%D0%B8_%D0%B2_%D0%BF%D1%80%D0%B8%D0%BC%D0%B5%D1%80%D0%B0%D1%85/%D0%A1%D0%BE%D1%80%D1%82%D0%B8%D1%80%D0%BE%D0%B2%D0%BA%D0%B0
// У сименса for не такой как на си пришлось переписать на while.
// Ну и индексы массива у сименса на единицу разнятся с си.
  i := 0;
  WHILE (i < Adr_max) DO
    j := 0;
    WHILE (j < Adr_max - i) DO
      IF (InputArray[j] > InputArray[j + 1]) THEN
        // Если они идут в неправильном порядке, то меняем их местами.
        TempValue := InputArray[j];
        InputArray[j] := InputArray[j + 1] ;
        InputArray[j + 1] := TempValue;
        // Сортируем второй массив устаревания в соответствии с первым массивом значений.
        TempTime := TimeArray[j];
        TimeArray[j] := TimeArray[j + 1] ;
        TimeArray[j + 1] := TempTime;
      END_IF;
    j := j + 1;
    END_WHILE;
  i := i + 1;
  END_WHILE;

END_IF; // IF (FlagRunCode) THEN
//----------------------------------------------------------------------------

//  InputArray TimeArray  i
// +----------+---------+---+
// |    1.0   |    0    | 0 | 
// |    3.0   |    2    | 1 | 
// |    4.0   |    1    | 2 | <- Out = [Adr_max / 2]
// |    5.0   |    4    | 3 | 
// |    6.0   |    3    | 4 | <- Adr_max
// +----------+---------+---+

// Выход это середина отсортированного массива.
IF (Size <> 0) THEN 
Out := InputArray[Adr_max / 2];
ELSE
Out := In; // Если Size = 0 фильтр выключен.
END_IF;

//****************************************************************************

// Запомнить предыдущее значение настроек фильтра.
Size_prev  := Size ; // Предыдущее значение соответствующего входного параметра.

//****************************************************************************

// @COPYLEFT ALL WRONGS RESERVED :)
// Author: VA
// Contacts: DIY.PLC.314@gmail.com
// Date start LIB_PLC: 2014
// License: GNU GPL-2.0-or-later
// https://www.gnu.org/licenses/old-licenses/gpl-2.0.html
// https://www.youtube.com/watch?v=n1F_MfLRlX0
// https://www.youtube.com/@DIY_PLC
// https://github.com/DIYPLC/LIB_PLC
// https://oshwlab.com/diy.plc.314/PLC_HW1_SW1
// https://3dtoday.ru/3d-models/mechanical-parts/body/korpus-na-din-reiku
// https://t.me/DIY_PLC
// https://drive.google.com/drive/folders/16GDX0i07rwDrl_oPzL-lQAqMiAz3ouJV?usp=sharing

END_FUNCTION_BLOCK