DSP на STM32F4 Опции

[Atlantis-]

Здравствуйте!
Помогите разобраться с DSP на STM32F429. Никогда раньше этой темой не занимался, не понимаю в какую сторону начинать копать. Задача такая: мне нужно генерировать белый шум, фильтровать его разными фильтрами и выдавать на аудио ЦАП (48 кГц). Фильтры нужны режекторный второго порядка и полосовой, четвертого порядка, с БИХ. Фильтры я посчитал в матлабе, приготовился уже реализовывать в целочисленной арифметике. Но в этом процессоре есть блок вычислений с плавающей точкой. Я с этим никогда дела не имел. В референс мануале описание этого блока не нашел. Есть некая библиотека CMSIS DSP, в которой реализованы фильтры, но я так понял она мне не подойдет, поскольку там какая то странная структура реализованного IIR фильтра и работает он с какими то блоками данных. А мне надо просто сгенерировать Х, посчитать Y, запомнить Х-1 и Y-1 для следующего отчета.
Скажите, какое преимущество мне даст работа с float и вообще, могу ли я так просто, без всяких библиотек взять и расписать свои фильтры во float?

[adnega]

Скажите, какое преимущество мне даст работа с float и вообще,

Сам Матлаб может дать вам характеристики фильтра с учетом квантования.
Целочисленная порой сильно уступает ПТ даже одинарной точности.

могу ли я так просто, без всяких библиотек взять и расписать свои фильтры во float?

Я вообще написал фильтр на asm

CODE

//---------------------------------------------
// fpu_iir
//---------------------------------------------
.thumb
.thumb_func
.global fpu_iir
.type fpu_iir, %function
fpu_iir:
.cfi_startproc
cbnz r1, fpu_iir_load
bx lr
fpu_iir_load:
push {r0-r4, lr}
VPUSH {s0-s7}
// load f
VLDR.F32 s1, [r3, #0] // G
VLDR.F32 s2, [r3, #4] // a1
VLDR.F32 s3, [r3, #8] // a2
VLDR.F32 s4, [r3, #12] // b
VLDR.F32 s5, [r3, #16] // tn-1
VLDR.F32 s6, [r3, #20] // tn-2
fpu_iir_loop:
// load
ldr r4, [r0], #4
// s0 <- x
VMOV.F32 s0, r4
// calc tn
VMUL.F32 s7, s1, s0 // G * x
VFMS.F32 s7, s2, s5 // -a1 * tn-1
VFMS.F32 s7, s3, s6 // -a2 * tn-2
// calc y
VADD.F32 s0, s7, s6 // tn-2
VFMA.F32 s0, s4, s5 // b * tn-1
// save t
VMOV.F32 s6, s5 // tn-2
VMOV.F32 s5, s7 // tn-1
// y -> r0
VMOV.F32 r4, s0
// store
str r4, [r2], #4
subs r1, #1
bne fpu_iir_loop
VSTR.F32 s5, [r3, #16] // tn-1
VSTR.F32 s6, [r3, #20] // tn-2
VPOP {s0-s7}
pop {r0-r4, pc}
.size fpu_iir, .-fpu_iir
.cfi_endproc

[Atlantis-]

Я вообще написал фильтр на asm

На asm то понятно, компилятор никуда не денется. А на Си если писать, для операций с float надо что-то настраивать, подключать? Или просто задать коэффициенты float a,b; float x,y; и пошел их перемножать, складывать, вычитать, а компилятор сам подставит нужные команды, для работы с плавающей точкой? Под компилятором я подразумеваю Keil.

[aaarrr]

А на Си если писать, для операций с float надо что-то настраивать, подключать?

Ничего не надо настраивать, только ядро правильное указать.

[Atlantis-]

Ничего не надо настраивать, только ядро правильное указать.

Да, я попробовал перемножить два int-a и два float-a, посмотрел дизассемблер, там отличие только в том, что в первом случае команда MULS, а во втором VMUL.F32
Судя из статьи, VMUL.F32 выполняется за 1 такт, как и обычное умножение. Круто!
Тогда у меня еще один вопрос, на входе у меня генератор белого шума, который выдает int, а на выходе, для ЦАП мне надо 24-битное число. Как в первом случае быстро преобразовать int (сдвинутый на 6 разрядов, чтобы на входе тоже было 24 бита) во float, а во втором случае float в int?

И я правильно понимаю, что используя float вместо int я нисколько не потеряю в скорости выполнения программы? Даже выиграю, с учетом того, что не надо будет масштабировать результат?

[aaarrr]

Как в первом случае быстро преобразовать int (сдвинутый на 6 разрядов, чтобы на входе тоже было 24 бита) во float, а во втором случае float в int?

Поделить/перемножить с требуемым масштабным коэффициентом.

[adnega]

Как в первом случае быстро преобразовать int (сдвинутый на 6 разрядов, чтобы на входе тоже было 24 бита) во float, а во втором случае float в int?

Сдвиг на 6 разрядов можно заменить на умножение на константу 2^6.
Эту константу можно внедрить в коэффициенты фильтра.
Есть команды МК для преобразований int во float и обратно.

[Atlantis-]

Есть команды МК для преобразований int во float и обратно.

какие?
где можно про них подробней почитать?

Сдвиг на 6 разрядов можно заменить на умножение на константу 2^6

То есть, сдвиг на 6 разрядов выполняется за 6 тактов, а умножение на константу - за один, хитро.
Но деление на константу выполняется за 14 тактов

Еще у меня в фильтре получается очень большое соотношение частоты дискретизации и частоты среза. Чтобы правильно посчитать фильтр надо иметь высокую точность. Как прикинуть, хватит ли мне float-а, зная порядок коэффициентов фильтра?

[DASM]

какие?
Еще у меня в фильтре получается очень большое соотношение частоты дискретизации и частоты среза. Чтобы правильно посчитать фильтр надо иметь высокую точность. Как прикинуть, хватит ли мне float-а, зная порядок коэффициентов фильтра?

Насколько мой склероз помнит - тут нужно ресэмплинг делать и использовать CIC фильтры, длиннющие FIR/IIR в данном случае совсем не гуд.

[Atlantis-]

Насколько мой склероз помнит - тут нужно ресэмплинг делать и использовать CIC фильтры, длиннющие FIR/IIR в данном случае совсем не гуд.

Как это обойти - другой вопрос. В матлабе я рассчитал коэффициенты длиной 13 бит Quantized Numerator. Одновременно там рассчитываются некие идеальные коэффициенты Reference numerator, которые получаются длиннее. Так вот у меня вопрос, как прикинуть, нельзя ли эти Reference numerator записать в виде float, хватит ли точности float?

[DASM]

Так у флоат 23 бита мантиссы, как оно может быть менее точным, чем 13 бит?

[Atlantis-]

Так у флоат 24 бита мантиссы, как оно может быть менее точным, чем 13 бит?

Чтобы оперировать 13-битными числами мне пришлось считать фильтр на пониженной частоте квантования, а потом ее повышать. А для float я хочу сразу делать фильтр на высокой частоте квантования.

[DASM]

Понятно. Математик из меня никакой, может просто промоделируете в Матлабе или SystemVue это?

[Atlantis-]

Понятно. Математик из меня никакой, может просто промоделируете в Матлабе или SystemVue это?

Я не знаю как заставить считать матлаб во float. Свои фильтры, целочисленные, я в маткад моделировал. Но там все понятно, раз коэффициенты 13 бит, такая точность и будет. А как там точность float задать я не знаю. Ну запишу я в свою модель идеальные коэффициенты из матлаба, он их посчитает как то, может с двойной точностью. А контроллер то будет с одинарной точностью считать.

[DASM]

Не знаю что и сказать, SystemVue позволяет считать и флоат и дабл и фиксед с любой точностью коэфф. и кол-ва бит. Матлаб, уверен, тоже.

Эскизы прикрепленных изображений

 

[Atlantis-]

Не знаю что и сказать, SystemVue позволяет считать и флоат и дабл и фиксед с любой точностью коэфф. и кол-ва бит. Матлаб, уверен, тоже.

Да, я нашел где в матлабе посчитать коэффициенты с Single-precision floating-point
Хорошо, теперь, если я буду подавать на вход фильтра с такими коэффициентами случайный 24-битный сигнал, у меня на выходе будет всегда получаться 24-битное число и никакие переполнения невозможны? Я просто никогда не работал с плавающей точкой, у целых чисел понятно когда появится переполнение, а тут - не очень.

[DASM]

Да, я нашел где в матлабе посчитать коэффициенты с Single-precision floating-point
Хорошо, теперь, если я буду подавать на вход фильтра с такими коэффициентами случайный 24-битный сигнал, у меня на выходе будет всегда получаться 24-битное число и никакие переполнения невозможны? Я просто никогда не работал с плавающей точкой, у целых чисел понятно когда появится переполнение, а тут - не очень.

Если переполнения нет в таком же фильтре fixed, то откуда оно появится в значительно более точном и на много порядков с большим ДД float?? Чисто общие соображения.

[adnega]

Еще у меня в фильтре получается очень большое соотношение частоты дискретизации и частоты среза.

Это плохо. Лучше перенести фильтрацию на низкую частоту дискретизации.

[sigmaN]

Что нужно знать про арифметику с плавающей запятой
https://habrahabr.ru/post/112953/

Особо полезно прочесть вот эту часть
4. Подводные камни в арифметике с плавающей запятой

[Atlantis-]

Что нужно знать про арифметику с плавающей запятой
https://habrahabr.ru/post/112953/

да, я читал эту статью

Особо полезно прочесть вот эту часть
4. Подводные камни в арифметике с плавающей запятой

а в чем польза? какой вывод можно сделать из этого пункта? не считать фильтры во float-ах?

[adnega]

а в чем польза? какой вывод можно сделать из этого пункта? не считать фильтры во float-ах?

Считать, но правильно.
Зачастую фильтрация - это сложение с накоплением произведений пар чисел.
Нужно все перемножить. Полученные значения расставить по возрастанию.
Суммировать от меньшего к большему. Так ошибка будет минимальна.

[khach]

Начинать писание своего фильтра надо с того что скачать CMSIS-DSP последней версии, выбрать подходящий фильтр и скомпилить его. Потом изучить результат компиляции в асме, проверить работоспособность и ресурсы которые фильтр жрет и только тогда заниматься его оптимизацией. Там много неявных граблей связанных с превыборкой инструкций процессора из флеша или рама, компилятор о них знает лучше, чем неподготовленный армовский ассемблерописатель.

[Atlantis-]

Считать, но правильно.
Зачастую фильтрация - это сложение с накоплением произведений пар чисел.
Нужно все перемножить. Полученные значения расставить по возрастанию.
Суммировать от меньшего к большему. Так ошибка будет минимальна.

Не понял, как суммировать от меньшего к большему? Коэффициенты фильтра у меня отличаются в два раза, а входной сигнал - рандомный. Знак тоже не угадаешь. Или что мне сначала сравнивать произведения между собой, а потом складывать?

[sigmaN]

Начинать писание своего фильтра надо с того что скачать CMSIS-DSP последней версии, выбрать подходящий фильтр и скомпилить его.

Скорее всего там даже будет использоваться "железная" реализация умножения с накоплением, которая вроде как есть в F4
http://www.compel.ru/lib/ne/2012/6/3-bogat...adre-cortex-m4/
Так что фильтры должны довольно шустро считаться )

[Atlantis-]

Начинать писание своего фильтра надо с того что скачать CMSIS-DSP последней версии, выбрать подходящий фильтр и скомпилить его.

В этой библиотеке реализован только решетчатый фильтр IIR, а мне надо обычный.

[sigmaN]

https://www.keil.com/pack/doc/CMSIS/DSP/htm...scade_d_f1.html Можно реализовтаь фильтр любого порядка.
Если нужен первый то делаете b2=0 and a2=0. Впрочем там всё хорошо описано по ссылке

[Atlantis-]

https://www.keil.com/pack/doc/CMSIS/DSP/htm...scade_d_f1.html Можно реализовтаь фильтр любого порядка.
Если нужен первый то делаете b2=0 and a2=0. Впрочем там всё хорошо описано по ссылке

Спасибо, это я как то пропустил...
Изучаю, не понял одно требование

Pay careful attention to the sign of the feedback coefficients. Some design tools use the difference equation
y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] - a1 * y[n-1] - a2 * y[n-2]
In this case the feedback coefficients a1 and a2 must be negated when used with the CMSIS DSP Library.

Что то странное, у меня только а1 отрицательный, а2 - положительный.
Или это правильно переводится как "а1 и а2 должны нейтрализовываться" и у меня все правильно?

Сообщение отредактировал Atlantis- - Aug 27 2016, 16:33

[sigmaN]

Читаем описание функци, реализует она следующее:

Algorithm
Each Biquad stage implements a second order filter using the difference equation:
y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]

Ниже заостряется внимание на том, что некоторые программы для моделирования фильтров подразумевают, что уравнение будет другим

Some design tools use the difference equation
y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] - a1 * y[n-1] - a2 * y[n-2]

Видите разницу? в первом случае +a1 +a2 а во втором -a1 -a2

Соответственно вам подсказывают, что если вы пользуетесь как раз таким софтом, то коэффициенты a1 и a2 must be negated.
Если по русски то перед применением коэффициенты a1 и a2 умножьте на -1, таким образом изменив их знак )

[Atlantis-]

Аааа, вон оно как...понял, спасибо большое!
Пока сам написал реализацию пары уравнений работающих с float, проверил анализатором спектра - похоже на правду. Смущает несколько моментов:
1) результат вычислений получается float, для вывода на ЦАП, естественно, преобразую его к int - теряется дробная часть, это нормально?
2) Отрицательные значения. Я подаю на вход фильтра белый шум, 24-битный. Соответственно, после фильтра у меня тоже должно получаться 24-битный результат. Но! Отрицательные значения в процессоре - это инвертированное положительное число + 1. А для ЦАП отрицательное значение получается простой инверсией положительного числа. Я отлавливаю в итоговом результате, если 23-й бит = 1, то я из всего числа вычитаю единицу. Это правильно?
3) Мне коэффициенты надо с компьютера передавать, а они у меня float теперь. Тут мне посоветовали, домножать, потом делить. Но как мне передать например число 0.99577337503433228 ? Чтобы целое получить, надо домножить на 10^17 и что потом с ним делать? Оно же даже в int не влезет.

[Atlantis-]

Порылся в интернете, на последний вопрос вроде нашел ответ.
Сделал так, результат сходится

Код

float float_value=0.6543278;
char float_bytes[sizeof(float)];
float *ptr;
float new_value;

//разбиение на байты
float_bytes[0] = *((char *)&float_value + 0);//0x07
float_bytes[1] = *((char *)&float_value + 1);//0x82
float_bytes[2] = *((char *)&float_value + 2);//0x27
float_bytes[3] = *((char *)&float_value + 3);//0x3F
//http://floatingpoint.ru/online/dec2float.php - проверка

//собрали из байт float
ptr = (float*)float_bytes;
new_value = *ptr;

[sigmaN]

1) результат вычислений получается float, для вывода на ЦАП, естественно, преобразую его к int - теряется дробная часть, это нормально?

Корректнее наверно тут использвоать округление вместо простого отбрасывания дробной части http://www.cplusplus.com/reference/cmath/ceil/

2) Отрицательные значения. Я подаю на вход фильтра белый шум, 24-битный. Соответственно, после фильтра у меня тоже должно получаться 24-битный результат. Но! Отрицательные значения в процессоре - это инвертированное положительное число + 1. А для ЦАП отрицательное значение получается простой инверсией положительного числа.

Что это за ЦАП то такой у вас? Прям с двухполярным питанием и честной выдачей наружу напряжения ниже нуля?
Очень сомневаюсь. Регистр ЦАПа скорее всего беззнаковый от 0 до скольки там разрадность позволяет.
А потому за 0 надобно принять половинку разрядности ЦАПа. Если он например 16ти разрядный, то за 0 надо принять половинку 2^16 / 2 = 32768
Таким образом получившиеся после фильтра циферки перед передачей в ЦАП надо приготовить.
У вас будет еще одна задачка если в процессе цифровой обработки получились космические цифры, которые в диапазон ЦАПа(-32768 до +32767) не влазят.
Что с этим делать это решать вам(зависит от задачи).
Например можно организовать нормализацию https://ru.wikipedia.org/wiki/%D0%9D%D0%BE%...%83%D0%BA%D0%B0 или компрессию динамического диапазона https://ru.wikipedia.org/wiki/%D0%9A%D0%BE%...%B0%D0%BB%D0%B0

Хорошей идеей было бы на выходе тракте цифровой обработки сигнала сделать так, чтобы данные были представлены числами от -1 до 1. Т.е. нормализовать их. Тогда вы будете знать, что 1 это максимальный уровень, а -1 минимальный. "Вписать" такой сигнал в нужный диапазон значений можно будет простым умножением на константу.

В общем потом, когда ваши данные так или иначе укладываются в диапазон ЦАПа(-32768 до +32767) применяете тот самый виртуальный 0.
DAC_REGISTER = 32768 + filter_data[i];
И не надо заморачиваться как там это будет посчитано и как в системе хранится отрицательное значение. Тестировать там какие-то биты в данном случае - дичайший говнокод.

3) Мне коэффициенты надо с компьютера передавать, а они у меня float теперь.

Учитывая, что большинство машин сейчас уважают IEEE 754 стандарт ваш хак с вычленением байт конечно будет работать, но назвать этот подход правильным нельзя.
Если делать правильно и портабельно, то в том числе передают даже в виде строки. sprintf() на стороне передатчика, sscanf() на стороне приемника
Либо определяйтесь, что между компьютером и устройством коэффициенты передаются в формате с фиксированной точкой, чтобы всё было чётко определено и задокументировано.
Еше стоит учесть, что бывают машины с разным порядком байт https://ru.wikipedia.org/wiki/%D0%9F%D0%BE%...%82%D0%BE%D0%B2

[Atlantis-]

Корректнее наверно тут использвоать округление вместо простого отбрасывания дробной части http://www.cplusplus.com/reference/cmath/ceil/

Нашел в интернете округление - просто прибавление 0.5 к float. Жалко с отрицательными не прокатит, там надо вычитать, снова if надо городить

Что это за ЦАП то такой у вас? Прям с двухполярным питанием и честной выдачей наружу напряжения ниже нуля?
Очень сомневаюсь. Регистр ЦАПа скорее всего беззнаковый от 0 до скольки там разрадность позволяет.
А потому за 0 надобно принять половинку разрядности ЦАПа. Если он например 16ти разрядный, то за 0 надо принять половинку 2^16 / 2 = 32768
Таким образом получившиеся после фильтра циферки перед передачей в ЦАП надо приготовить.
У вас будет еще одна задачка если в процессе цифровой обработки получились космические цифры, которые в диапазон ЦАПа(-32768 до +32767) не влазят.
Что с этим делать это решать вам(зависит от задачи).
Например можно организовать нормализацию https://ru.wikipedia.org/wiki/%D0%9D%D0%BE%...%83%D0%BA%D0%B0 или компрессию динамического диапазона https://ru.wikipedia.org/wiki/%D0%9A%D0%BE%...%B0%D0%BB%D0%B0

Хорошей идеей было бы на выходе тракте цифровой обработки сигнала сделать так, чтобы данные были представлены числами от -1 до 1. Т.е. нормализовать их. Тогда вы будете знать, что 1 это максимальный уровень, а -1 минимальный. "Вписать" такой сигнал в нужный диапазон значений можно будет простым умножением на константу.

В общем потом, когда ваши данные так или иначе укладываются в диапазон ЦАПа(-32768 до +32767) применяете тот самый виртуальный 0.
DAC_REGISTER = 32768 + filter_data[i];
И не надо заморачиваться как там это будет посчитано и как в системе хранится отрицательное значение. Тестировать там какие-то биты в данном случае - дичайший говнокод.

ЦАП - CS4344, пробовал на AD1852. Ни в том ни в другом даташите не нашел, как же представлять отрицательные числа. Но это как то должно наверно быть обозначено?
Для AD1852 написал тестовую синусоиду, сначала я не парился, думал, что максимальное отрицательное значение - это 0х000000. Проверил - какая то фигулька получилась, а не синусоида. Потом вот просто начал инвертировать положительные значения - получилась синусоида, размах вроде как и должен быть. А для CS4344 я просто подал тот же массив - получилась синусоида, ну и хорошо.
А вообще мне интересно, как обозначается в доке, каким способом кодируется отрицательные значения в ЦАП?

Учитывая, что большинство машин сейчас уважают IEEE 754 стандарт ваш хак с вычленением байт конечно будет работать, но назвать этот подход правильным нельзя.
Если делать правильно и портабельно, то в том числе передают даже в виде строки. sprintf() на стороне передатчика, sscanf() на стороне приемника
Либо определяйтесь, что между компьютером и устройством коэффициенты передаются в формате с фиксированной точкой, чтобы всё было чётко определено и задокументировано.
Еше стоит учесть, что бывают машины с разным порядком байт https://ru.wikipedia.org/wiki/%D0%9F%D0%BE%...%82%D0%BE%D0%B2

Да мне главное правильно собрать float из байтов на микроконтроллере. А разобрать его на компе - чисто для проверки. Хотя зачем, я могу даже сразу вручную разобрать все нужные float-ы на байты, на этом сайте http://floatingpoint.ru/online/dec2float.php
Хранить и передавать в виде байт. Тем более коэффициенты у меня все уже вычислены. Наверно так будет наиболее надежно))

[sigmaN]

Нашел в интернете округление - просто прибавление 0.5 к float. Жалко с отрицательными не прокатит, там надо вычитать, снова if надо городить

Именно так.

А вообще мне интересно, как обозначается в доке, каким способом кодируется отрицательные значения в ЦАП?

Ну какие могут быть отрицательные значения при однополярном питании?
В принципе я вам направление для мысли дал. Разбирайтесь....

[ig_z]

Ну какие могут быть отрицательные значения при однополярном питании?

TC использует аудио цап ацп. Сейчас они все двуполярные по коду и однополярные по питанию. В ацп даже цифровой фильтр на несколько гц встраивают, чтобы фильтровать смещение нуля (аналоговое, как я понимаю).

[sigmaN]

Не знал, не знал. Ну тогда даташит на ЦАП в помощь ТСу ))))

[Atlantis-]

Вот я смотрю даташиты на оба ЦАП и не нахожу даже упоминания про то какие данные отправлять и как кодируется отрицательный диапазон...

[Atlantis-]

Нашел ответ на свой вопрос

The AD1852 accepts serial audio data
in MSB first, twos-complement format.

[sigmaN]

Ну вот, значит отрицательные числа представлены так-же как и в процессоре
осталось только разобраться с MSB first...
B в MSB имеется ввиду Bit или Byte?
если Byte то получается Big Endian, а проц у вас Little endian.... https://ru.wikipedia.org/wiki/%D0%9F%D0%BE%...%82%D0%BE%D0%B2
Может быть поэтому во время эксперимента у вас не получалась синусоида....

[ig_z]

если Byte то получается Big Endian, а проц у вас Little endian....

Это забота аппаратного аудио интерфейса в процессоре, если ТС его использует. Более вероятна проблема выбора 32 бит данных через 16 бит порты в некоторых "оптимизированных" платформах

[Atlantis-]

Сделал полосовые фильтры, проверил - все нормально. А с режекторным какая то засада, не понимаю, где может быть ошибка?
Делаю вот так:

Код

        struct {
        
        float a0;
        float a1;
        float a2;
        float b1;
        float b2;
        int k;
        int k1;
        int k2;    
            
        float X[4];
        float Y[4];
            
        int Y1;
            
    } NotchedFilter;

в прерывании I2S

Код

//выдаю данные
hsai_BlockA1.Instance->DR = noise.NotchedFilter.Y1;
hsai_BlockA1.Instance->DR = noise.NotchedFilter.Y1;
//считаю следующий отсчет
//                    noise.NotchedFilter.Y[noise.NotchedFilter.k] = noise.NotchedFilter.a2*noise.NotchedFilter.X[noise.NotchedFilter.k2] +
//                    noise.NotchedFilter.a1*noise.NotchedFilter.X[noise.NotchedFilter.k1] + noise.NotchedFilter.a0*noise.NotchedFilter.X[noise.NotchedFilter.k] -
//                    noise.NotchedFilter.b1*noise.NotchedFilter.Y[noise.NotchedFilter.k1] - noise.NotchedFilter.b2*noise.NotchedFilter.Y[noise.NotchedFilter.k2];
                    
                    noise.NotchedFilter.Y[noise.NotchedFilter.k] = noise.NotchedFilter.a2*(noise.NotchedFilter.X[noise.NotchedFilter.k2] + noise.NotchedFilter.X[noise.NotchedFilter.k]) +
                    noise.NotchedFilter.a1*(noise.NotchedFilter.X[noise.NotchedFilter.k1] - noise.NotchedFilter.Y[noise.NotchedFilter.k1]) -
                    noise.NotchedFilter.b2*noise.NotchedFilter.Y[noise.NotchedFilter.k2];
                    noise.NotchedFilter.Y1 = noise.NotchedFilter.Y[noise.NotchedFilter.k];//оптимизировал а0=а2, а1=b1
                    
                    noise.NotchedFilter.k = (noise.NotchedFilter.k + 1)&BUFFMASK;
                    noise.NotchedFilter.k1 = (noise.NotchedFilter.k - 1)&BUFFMASK;//посчитали индекс Xk-1
                    noise.NotchedFilter.k2 = (noise.NotchedFilter.k - 2)&BUFFMASK;//посчитали индекс Xk-2

что тут может быть не так? проверил под отладкой - k меняется как и должно, сам посчитал один отсчет - сошлось с показанием процессора.

[Atlantis-]

Как то так получается, что при подаче на вход режекторного фильтра 24-битного числа, на выходе получается зашкал. Все нормально фильтрует, если подавать на вход 22-битное число или сдвигать на 2 результат.
Может кто-нибудь подсказать, как такое может получиться? В матлабе у фильтра никакого усиления нет.