81б. Алгоритм Гёрцеля: найти тон без БПФ

В разделе 81а сигнал фильтровали: пропустить одни частоты, погасить другие. В разделе 117 брали весь спектр сразу — быстрым преобразованием Фурье (БПФ). Часто нужно меньше: есть ли в сигнале тон 697 Гц, звенит ли зуммер на 2 кГц, пришла ли цифра тонового набора. Для нескольких частот БПФ считает сотни лишних бинов. Алгоритм Гёрцеля считает один бин — рекурсией второго порядка с одним умножением на отсчёт. Здесь — как он устроен, как выбрать длину блока и номер бина, сколько он стоит против БПФ, как не переполниться в q31 и как собрать из него детектор тонового набора (DTMF), который не срабатывает на музыку.

Опоры: своя модель на ПК на чистом Python (_verify/goertzel81c/goertzel81c.py) — алгоритм сверен с прямым расчётом ДПФ, детектор прогнан на тысячах блоков с шумом и расстройкой. Код на C собран armclang для STM32F407 и STM32G474 без предупреждений; тот же файл собран gcc на ПК и выдал те же мощности в q31, что и модель, бит в бит. Листинг внутреннего цикла — дизассемблер fromelf. На плате не запускалось, такты не мерились.

Как это называется в документации

Оригинал	Перевод
Goertzel algorithm	алгоритм Гёрцеля: один бин ДПФ рекурсией второго порядка
DFT bin	бин ДПФ: частота k·fs/N
frequency resolution	разрешение по частоте: ширина бина fs/N
spectral leakage	утечка: тон растекается по соседним бинам
Hamming window	окно Хэмминга
DTMF (dual-tone multi-frequency)	тоновый набор: цифра — два тона сразу
twist	перекос: разница уровней двух тонов цифры
second harmonic	вторая гармоника: удвоенная частота
guard bits, headroom	запас разрядов под рост числа

81б.1 Одна частота вместо спектра

ДПФ блока из N отсчётов в бине k — сумма X[k] = Σ x[n]·e^(−j2πkn/N). Гёрцель получает её модуль, не храня блок и не считая синусов на каждом отсчёте. На каждый отсчёт — одно звено второго порядка:

s[n] = x[n] + c·s[n−1] − s[n−2],     c = 2cos(2πk/N)

После последнего отсчёта блока из двух чисел состояния s1 = s[N−1] и s2 = s[N−2] получается мощность бина:

P = s1² + s2² − c·s1·s2 = |X[k]|²

Амплитуда тона, попавшего в бин, — 2√P / N. На отсчёт — одно умножение и два сложения; коэффициент c постоянный, считается один раз.

Модель на ПК сверила это с прямой суммой ДПФ на случайном блоке из 205 отсчётов: во всех бинах от 0 до 102 мощности совпали до 11 знаков. Номер бина не обязан быть целым: с k = 17,861 (точная частота 697 Гц) Гёрцель дал ту же мощность, что сумма ДПФ на этой частоте.

81б.2 N и k: разрешение и утечка

Ширина бина — fs / N, длина блока — N / fs. Пример — тоновый набор: цифра — один тон из нижней группы (697, 770, 852, 941 Гц) и один из верхней (1209, 1336, 1477, 1633 Гц). При fs = 8000 Гц и N = 205 блок длится 25,6 мс, бин — 39,02 Гц. Ближе всего друг к другу 697 и 770 Гц: 73 Гц, 1,87 бина — различимы.

Номер бина k = f·N / fs округляют до целого, и тон оказывается не в центре бина. Модель — наихудшая потеря амплитуды по 16 фазам тона, без окна:

Почему именно 205: модель перебрала все N от 100 до 250 и для каждого нашла наихудший сдвиг восьми тонов от центров бинов. Лучшее — N = 205, 0,269 бина; следом 113 (0,292) и 185 (0,297). Привычные для БПФ 256 отсчётов дают 0,360. Гёрцелю степень двойки не нужна — N выбирают под частоты.

Утечка — вторая сторона того же. Тон попадает не только в свой бин, но и в соседние. Без окна тон 770 Гц перевешивает соседа по группе всего на 16,2 дБ. Хуже с расстройкой: если частота ушла на 1,5 % (у 1633 Гц это 24,5 Гц, 0,63 бина), наихудшая потеря амплитуды — −11,7 дБ.

81б.3 Окно и цена против БПФ

Окно. Блок умножают на окно Хэмминга до звена — так же, как перед БПФ в разделе 117 окно Ханна. Бин становится шире и терпимее к расстройке: при ±1,5 % наихудшая потеря −4,4 дБ вместо −11,7, а перевес тона над соседом по группе — 27,3 дБ вместо 16,2. Цена — одно умножение на отсчёт, общее для всех частот, и таблица окна: 205 чисел q15 — 410 байт. Окно уменьшает и сам тон: усиление окна для тона — 0,538, и амплитуду до сравнения с порогом делят на него.

Умножения. Гёрцель на одну частоту — N умножений в цикле и 3 в конце (последний член удобно считать как s2·(s2 − c·s1)). Простое БПФ по основанию 2 — N/2·log2N комплексных умножений, каждое из них — четыре вещественных:

На частоту Гёрцель хранит три числа: s1, s2 и коэффициент — 12 байт. Блок отсчётов хранить не нужно: каждый отсчёт уходит в звенья сразу, хоть из прерывания АЦП. БПФ требует весь блок в памяти и таблицы поворотов — у CMSIS-DSP на 256 отсчётов это 2920 байт во флеш-памяти (раздел 117).

При одинаковом N = 256 Гёрцель на одну частоту — 259 умножений, и БПФ выгоднее начиная примерно с 16 частот (2·log2N). Для восьми тонов DTMF Гёрцель дешевле — и работает с N = 205, которое БПФ по основанию 2 не берёт.

81б.4 Фиксированная точка: запас на рост

Звено — резонатор: на своей частоте состояние растёт от отсчёта к отсчёту. Сколько — зависит от k. Граница роста за блок — сумма модулей импульсной характеристики звена; модель посчитала её для всех 16 бинов: наибольшая при k = 18 — 249,0 (почти 2⁸), наименьшая — 131,2. Синус полной шкалы точно на частоте бина разгоняет состояние до 195,6 — граница с запасом.

В коде вход q15 сдвигается в состояние int32 на 7 бит. Худший случай — произведение c·s1, где c почти 2: 32768·2^7·249·2 = 2 088 763 392 — меньше 2³¹ − 1 = 2 147 483 647. Со сдвигом 8 выходит 4 177 526 784 — не помещается. Это условие стоит в коде _Static_assert: сборка не пройдёт, если сдвиг увеличат.

Модель проверила полную шкалу на бине 18 с точной копией кода q31:

Точность. Тон с разной громкостью (697 и 941 Гц, по 8 фаз) сравнили с тем же расчётом в double на тех же отсчётах q15. Второй вариант — состояние в int16: чтобы рост в 249 раз поместился в 16 бит, вход приходится сдвигать вправо на 9, и от 16 бит отсчёта остаётся 7.

q31 с 64-битным произведением точен во всём диапазоне отсчёта q15; состояние q15 годится только для громких тонов. Float на Cortex-M4 — 24 бита мантиссы, и на слабом бине рядом с сильным тоном он уступает q31: на бине 20 при полной шкале на бине 18 (на 117,8 дБ ниже) float на ПК ошибся в 2,6 раза, q31 — на 0,4 %. Для порогов DTMF это неважно, для измерения слабой гармоники рядом с сильной — важно.

81б.5 Код на C и что делает компилятор

Блок в q31. Коэффициент хранится как cos·2³¹ (2cos в q31 не помещается), а произведение сдвигается на 30 вместо 31 — это и есть умножение на 2cos:

int64_t goertzel_q31(const int16_t *x, uint32_t n, int32_t cq)
{
    int32_t s1 = 0, s2 = 0;
    GZ_ONE_BY_ONE
    for (uint32_t i = 0; i < n; i++) {
        int32_t s0 = (int32_t)(((int64_t)cq * s1) >> 30) - s2 + x[i] * (1 << GZ_SHIFT);
        s2 = s1;
        s1 = s0;
    }
    int32_t t = (int32_t)(((int64_t)cq * s1) >> 30);       /* 2cos·s1 */
    return (int64_t)s1 * s1 + (int64_t)s2 * s2 - (int64_t)t * s2;
}

Мощность — в единицах 2⁴⁴ (два раза по 15 бит отсчёта и 7 бит сдвига): P = результат / 2⁴⁴. Рядом в файле — вариант во float, таблица из 16 номеров бинов и коэффициентов (тоны и вторые гармоники) и два _Static_assert на запас разрядов.

F407 и G474 — одно и то же ядро Cortex-M4 с FPU (в заголовках паков у обоих core_cm4.h и __FPU_PRESENT 1), и armclang с -O2 дал для них одинаковый код. Сам он разворачивает цикл по четыре отсчёта; GZ_ONE_BY_ONE — прагма, которая это запрещает, чтобы листинг показал один отсчёт. Внутренний цикл goertzel_q31 по fromelf:

цикл:   MOV    lr,r12
        SMULL  r4,r12,r12,r2
        LSRS   r4,r4,#30
        ORR    r12,r4,r12,LSL #2
        LDRSH  r4,[r0,#2]!
        SUB    r3,r12,r3
        ADD    r12,r3,r4,LSL #7
        SUBS   r1,#1
        MOV    r3,lr
        BNE    цикл

Десять команд на отсчёт. Полезных шесть: SMULL — 64-битное произведение cos·s1, LSRS и ORR — сдвиг на 30 из двух половин, LDRSH — отсчёт, SUB и ADD — вычесть s2 и прибавить отсчёт, сдвинутый на 7. Две MOV передвигают состояние, SUBS и BNE — счётчик. Во float — девять команд: VLDR, VMUL, VADD, VSUB и пять служебных. Конец блока компилятор сам переписал как s1² + s2·(s2 − c·s1). На 16 бинов и блок 205 отсчётов — 32 800 команд цикла за 25,6 мс.

Проверка на ПК: тот же файл, собранный gcc с -DGZ_HOST_TEST, прогнал 19 блоков — 16 цифр, слабый тон, полную шкалу и пилу — по 16 бинам. Все 304 мощности q31 совпали с моделью бит в бит.

81б.6 Детектор DTMF и ложные срабатывания

Восемь звеньев дают восемь амплитуд. Правила модели:

• в каждой группе берут самый сильный тон; оба выше порога 0,05 полной шкалы;
• тон перевешивает второй в своей группе не меньше чем на 10 дБ;
• перекос между тонами цифры — не больше 8 дБ;
• по выбору: вторая гармоника тона ниже него на 15 дБ, или два тона несут не меньше половины всей мощности блока.

Прогоны — по 256 цифр: точные частоты; частоты с отклонением до ±1,5 %; белый шум с отношением сигнал/шум 10 дБ и 0 дБ. Тоны по 0,3 полной шкалы, перекос до ±4 дБ, фазы случайные. Ложные срабатывания — пила от 60 до 500 Гц с шагом 1 Гц (441 сигнал) с гармониками до 3900 Гц: сигнал, богатый гармониками, как голос или музыка.

Что видно:

• Окно лечит расстройку: 256 цифр из 256 при ±1,5 % вместо 229. Но более широкий бин пропускает и больше чужих гармоник — 43 ложных вместо 12.
• Проверка второй гармоники обоих тонов режет верные цифры даже без шума: 240 из 256. Гармоники нижних тонов 1394, 1540 и 1704 Гц лежат в 1,77, 1,15 и 2,15 бина от верхних тонов 1336, 1477 и 1633 Гц — бин гармоники ловит утечку настоящего верхнего тона. Проверка только верхнего тона этим не страдает и снимает 43 ложных до 17.
• Доля мощности убирает ложные срабатывания совсем, но строже к шуму: на 0 дБ — 53 цифры из 256 против 180.
• Неверных цифр — ни одной во всех прогонах: детектор скорее промолчит, чем ошибётся.

Детектор на q31 с окном в q15 принял на 768 цифрах те же решения, что в double.

81б.7 Грабли

Тон есть, а детектор молчит. Частота ушла от центра бина. Без окна уход на 1,5 % стоит до 11,7 дБ, с окном Хэмминга — 4,4 дБ.

N = 256 «как у БПФ». Тоны DTMF ложатся до 0,36 бина от центров вместо 0,27 при N = 205. Гёрцелю степень двойки не нужна.

Порог по амплитуде «съехал» после окна. Окно уменьшает тон: усиление 0,538. Амплитуду делят на него или порог считают с окном.

Цифры набираются от музыки. Одни правила уровня и перевеса пропускают пилу: 43 ложных из 441 с окном. Нужна вторая гармоника или доля мощности.

Проверка гармоник режет верные цифры. Гармоника нижнего тона 1540 Гц — в 1,15 бина от верхнего 1477 Гц: её бин видит утечку верхнего тона. Гармонику проверяют у верхнего тона или берут долю мощности.

Мощность q31 — мусор на громком сигнале. Сдвиг входа больше 7: состояние растёт до 249 раз, и c·s1 выходит за int32 — ошибка 812 % при сдвиге 8. Запас ставят _Static_assert.

Слабые тоны пропадают в q15. Состояние int16 требует сдвига входа на 9: на −60 дБ ошибка мощности 20,9 дБ. Состояние — int32, произведение — 64 бита.

Мощность растёт от блока к блоку. s1 и s2 не обнулили перед новым блоком: звено продолжает прошлый блок.

81б.8 Итоги

• Гёрцель — один бин ДПФ: s[n] = x[n] + 2cos(2πk/N)·s[n−1] − s[n−2], в конце P = s1² + s2² − c·s1·s2. Сверено с суммой ДПФ до 11 знаков, k может быть дробным.
• DTMF при 8 кГц: N = 205, блок 25,6 мс, бин 39,02 Гц, тоны в бинах 18…42 не дальше 0,27 бина от центров — лучший N от 100 до 250.
• Окно Хэмминга: при расстройке ±1,5 % потеря 4,4 дБ вместо 11,7, перевес над соседом 27,3 дБ вместо 16,2.
• Цена: 8 тонов — 1664 умножения на блок и 96 байт на состояние и коэффициенты; БПФ на 256 — 4096 умножений и 2048 байт буферов. Равенство — около 16 частот.
• q31: рост до 249 раз — сдвиг входа 7 бит, со сдвигом 8 ошибка 812 %; состояние q15 теряет 20,9 дБ на тоне −60 дБ. Цикл — 10 команд на отсчёт (float — 9).
• Детектор: окно + доля мощности — 0 ложных на 441 пиле и 256 из 256 цифр при ±1,5 %; проверка гармоники нижнего тона режет верные цифры.

Полные исходники

goertzel81c.c — алгоритм Гёрцеля: блок в q31 и во float.

/* Раздел 81б. Алгоритм Гёрцеля: блок в q31 и во float.
 *
 * Для STM32F407 и STM32G474 (Cortex-M4F) собирается armclang без предупреждений;
 * с -DGZ_HOST_TEST — gcc на ПК: читает блоки отсчётов из stdin и печатает мощности,
 * проверка сверяет их с моделью goertzel81c.py. */
#include <stdint.h>

#define GZ_N      205u      /* отсчётов в блоке: 25,6 мс при 8 кГц */
#define GZ_BINS   16u       /* 8 тонов DTMF и 8 вторых гармоник */
#define GZ_SHIFT  7         /* вход q15 сдвигается в состояние на 7 бит */
#define GZ_GROWTH 250       /* рост состояния за блок, с запасом: модель даёт 249,0 при k = 18 */

/* цикл по отсчёту без разворота: так листинг показывает ровно один отсчёт;
 * -DGZ_UNROLL оставляет armclang его обычный разворот */
#if defined(__clang__) && !defined(GZ_UNROLL)
#define GZ_ONE_BY_ONE _Pragma("clang loop unroll(disable)")
#else
#define GZ_ONE_BY_ONE
#endif

/* 2cos·s1 должно поместиться в int32 — иначе переполнение знакового, неопределённое поведение */
_Static_assert((int64_t)32768 * (1 << GZ_SHIFT) * GZ_GROWTH * 2 <= INT32_MAX, "запас по переполнению");
_Static_assert((int64_t)32768 * (1 << (GZ_SHIFT + 1)) * GZ_GROWTH * 2 > INT32_MAX, "сдвиг 8 уже не помещается");

/* номера бинов k и cos(2πk/N)·2^31: тоны 697…1633 Гц, затем их вторые гармоники */
const uint8_t gz_k[GZ_BINS] = {18, 20, 22, 24, 31, 34, 38, 42, 36, 39, 44, 48, 62, 68, 76, 84};
const int32_t gz_cos_q31[GZ_BINS] = {
    1828878034, 1756489927, 1677503679, 1592215995, 1248909980, 1083228028, 848335877, 600708818,
    967599315, 787480650, 473275393, 213560429, -694828920, -1054685608, -1477235229, -1811414887,
};

int64_t goertzel_q31(const int16_t *x, uint32_t n, int32_t cq)
{
    int32_t s1 = 0, s2 = 0;
    GZ_ONE_BY_ONE
    for (uint32_t i = 0; i < n; i++) {
        int32_t s0 = (int32_t)(((int64_t)cq * s1) >> 30) - s2 + x[i] * (1 << GZ_SHIFT);
        s2 = s1;
        s1 = s0;
    }
    int32_t t = (int32_t)(((int64_t)cq * s1) >> 30);       /* 2cos·s1 */
    return (int64_t)s1 * s1 + (int64_t)s2 * s2 - (int64_t)t * s2;
}

float goertzel_f32(const float *x, uint32_t n, float c)
{
    float s1 = 0.0f, s2 = 0.0f;
    GZ_ONE_BY_ONE
    for (uint32_t i = 0; i < n; i++) {
        float s0 = x[i] + c * s1 - s2;
        s2 = s1;
        s1 = s0;
    }
    return s1 * s1 + s2 * s2 - c * s1 * s2;
}

#ifdef GZ_HOST_TEST
#include <math.h>
#include <stdio.h>

int main(void)
{
    int16_t q[GZ_N];
    float f[GZ_N];
    char name[32];
    int blocks = 0;
    while (scanf("%31s", name) == 1) {
        for (uint32_t i = 0; i < GZ_N; i++) {
            int v;
            if (scanf("%d", &v) != 1)
                return 1;
            q[i] = (int16_t)v;
            f[i] = (float)v / 32768.0f;
        }
        for (uint32_t b = 0; b < GZ_BINS; b++) {
            float c = (float)(2.0 * cos(2.0 * 3.14159265358979323846 * gz_k[b] / GZ_N));
            printf("%s %u q31 %lld f32 %.9e\n", name, (unsigned)gz_k[b], (long long)goertzel_q31(q, GZ_N, gz_cos_q31[b]),
                   (double)goertzel_f32(f, GZ_N, c));
        }
        blocks++;
    }
    printf("блоков %d\n", blocks);
    return 0;
}
#endif

Дальше: раздел 82 «Индикация на прототипе: ШИМ, RGB, сдвиговые регистры».