Когерентная демодуляция CPFSK на микроконтроллерах семейства ARM Cotex M (STM32F103 — STM32H750)

[ValeriyS]

Спасибо за статью, тема интересная. Сам когда-то с Bell 202 возился, так что полез в код, ну и… есть пара моментов, напишу как есть.

Сначала думал, что мне показалось. В FskModemBlockCorr Q инициализируется как -sin(phase) (там negs), а крутится потом вектор на +phi. Получается, внутри вызова опорник идет в сторону theta0 - nphi, а в структуру при этом сохраняется theta0 + Nphi. На стыке вызовов скачок на 2Nphi. Я сначала погонял на 1200 Гц и ничего не увидел, ну оно и понятно: при 9600 это 8 отсчетов на бит, ровно период, всё кратно 2pi и баг прячется. А на 2200 опорник прыгает на 120 градусов на каждом вызове. Хотя если у вас блоки всегда кратны периоду, как вы пишете, то вы этого и не видели, наверное. Но 2200 Гц на 1200 бод в целое число периодов не укладывается.

С модулятором, кстати, ровно то же самое, я это уже потом сообразил. Чередовал 1200/2200 по 8 отсчетов, и на стыке символов скачок 60864 единицы, а у непрерывного синуса такой амплитуды шаг больше 43210 не бывает в принципе. То есть это уже не совсем CP FSK выходит :) Лечится просто: убрать negs, а в модуляторе add вместо sub.

Еще одно, это я уже в эмуляторе поймал (unicorn, гонял ваш asm как есть, sin/cos подставлял идеальные). Вектор стартует с полной шкалы Q1.31, запаса ноль. Когда он проходит через ось, ошибка округления иногда выталкивает компоненту за 2^31, она заворачивается, и опорник переворачивается на 180 градусов до следующего пересечения. На 2200/9600 у меня это на 12-м отсчете случилось. Стартовать с половины шкалы, Q2.30, сдвигать на 14 вместо 15, и всё.

А, ну и мелочь: после bl cosFixed/sinFixed используются r1-r3 и r12, а по AAPCS их никто сохранять не обязан. С вашими sin/cos работает, с чужими уже нет. Тем более r4-r11 и так на стеке, можно держать там.

Так, теперь собственно про “можно ли лучше”. Я долго думал, а зачем вообще крутить вектор на каждом отсчете? Для Bell 202 тоны соизмеримы с частотой дискретизации, значит опорный сигнал периодичный. При 9600 у 1200 Гц период 8 отсчетов, у 2200 Гц 48. Кладем один период в табличку {cos, -sin} в Q1.15 и ходим по ней по кругу. Указатель в таблице и есть фаза. Ни sin/cos в рантайме, ни дрейфа, ни переполнения, и непрерывность между вызовами точная, просто по построению. (Если период на длину блока не делится, берем НОК, P = lcm(fs/gcd(fs,f), L), там всё равно получается немного.)

И раз уж Q1.15 хватает за глаза (АЦП-то 12 бит, 183 дБ там, уж простите, не нужны), можно делать MUL 32x32->32. На M3 это 1 такт, а не SMULL/SMLAL по 3-7. По таблицам из TRM у меня вышло где-то 11-15 тактов на отсчет против ~34-50 у вас. На железе не мерил, это оценка, но разница заметная. Внутренний цикл примерно такой (ИИ помогает, да):

Ассемблер (Cortex-M3), полностью совпадает с проверенной версией:

FskTblCorr_Run:
    push    {r4-r10, lr}
    cmp     r3, #0
    beq     9f
    ldr     lr,  [r0, #0]       @ tbl
    ldr     r12, [r0, #8]       @ pos (phase)
    ldr     r10, [r0, #12]      @ L
    lsl     r10, r10, #2
1:  movs    r4, #0              @ I
    movs    r5, #0              @ Q
    add     r9, r12, r10        @ end of block in table
2:  ldrsh   r6, [r2], #2        @ x
    ldrsh   r7, [r12], #2       @ cos
    ldrsh   r8, [r12], #2       @ -sin
    mul     r7, r6, r7
    mul     r8, r6, r8
    add     r4, r4, r7, asr #15
    add     r5, r5, r8, asr #15
    cmp     r12, r9
    bne     2b
    stmia   r1!, {r4, r5}
    ldr     r7, [r0, #4]        @ tblEnd
    cmp     r12, r7
    it      eq
    moveq   r12, lr             @ wrap
    subs    r3, r3, #1
    bne     1b
    str     r12, [r0, #8]
9:  pop     {r4-r10, pc}

Что забавно, на C это пишется в три строчки, и clang с -O2 собирает практически те же инструкции, еще и разворачивает на 4. Так что asm тут, может, и не нужен вовсе. Точность сравнивал с float корреляцией: на блоке из 8 отсчетов расхождение меньше одного LSB 12-битного АЦП.

while (blocks--) {
    int32_t accI = 0, accQ = 0;
    uint32_t m = L;
    while (m--) {
        int32_t x = *in++;
        accI += (x * p[0]) >> 15;
        accQ += (x * p[1]) >> 15;
        p += 2;
    }
    *out++ = accI;
    *out++ = accQ;
    if (p == s->tblEnd)
        p = s->tbl;
}

Хотя да, бывает, что период получается огромный. 2200 на 44100, например, там 14 тысяч пар. Тогда таблица на один блок с нулевой фазой, а результат блока доворачивается один раз: Y_k = e^(-j*theta_k) * C_k. Это по сути ваша же идея с e^-jPhy, только на уровне блока, а не каждого отсчета.

Для модулятора вообще проще DDS: общий аккумулятор фазы на оба тона плюс синус из таблицы, и непрерывность получается сама собой.

И вот еще, наверное, стоило с этого начать. Если цель именно когерентность, то реальный выигрыш от нее будет в многосимвольном детекторе, Витерби по решетке CPFSK. А ваши поблочные комплексные корреляции как раз готовые метрики для него. Было бы интересно почитать продолжение в эту сторону.

[EvgenySbl]

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

По остальным вопросам:

1. На самом деле для ADC с разрядностью 12 бит, действительно такая точность не требуется. Но это если Вы не планируете работать с очень длинными выборками и подмешивать шум на границе 1-бита для того чтобы достать сигнал с суббитовой точностью, это буде эффективно. Коррелятор это позволяет с учетом масштаба.

2. Если планируете работать с длинным выборками, и проводить корреляцию с произвольной частотой сигнала, то отсчеты Вам все равно аппроксимировать при табличном методе или корректировать в случае Cordic-поворота. Для этого нужен квадратный корень, или "мои" sinFixed/cosFixed.

3. Не всегда таблица работает эффективнее чем прямой расчет. Все дело в том, что расчет выполняется в регистра и практически не трогает шину SRAM с которой в это время работает DMA. Есть еще один хороший вопрос. Что лучше с учетом Latency флеш-памяти: взять отсчет из Flash или перемножить и сложить регистры?

4. Но а теперь о точности. Вообще у меня в планах было обрабатывать не совсем сигнал. Я задумался над сжатием, похожим на MP3, но с учетом фазы. И здесь, блочная корреляция на периоде синусойды может быть произведена с произвольной начальной фазой кратной длинне блока интегрирования. Собственно, по этому вектор там 32-битный.


Ниже, код который я еще на проверял, но Вам будет понятен концепт.

/*
  ********************************************************************************************
  * @file      .cpp
  * @source    It is like streaming FFT
  * @author    Evgeny Sobolev
  * @version   V1.0.0
  * @date      2026-08-25
  *
  * @description Streaming quadrature FFT   
  * @description Solution to make High Quality vector music audio codec   
  ********************************************************************************************
 */


#include <stdint.h>
#include <stddef.h>
#include <SincCosFixed.h>
#include <FskBlockCorr.h>


template< int32_t StepsPerBlock = 16 >
struct Freq {
    Phase               phase;
    CorrResult          IQ[StepsPerBlock];
};

template< uint32_t FrequencesPerBlock = 256, int32_t StepsPerBlock = 16 >
struct FskBlockContext {
    DcOffset                blockDcOffset;
    ScaleFactor             blockScaleFactor;
    Freq<StepsPerBlock>     freq[FrequencesPerBlock];
    EnergyResult            power[StepsPerBlock];
    FskBlockCorrContext     freqContext[FrequencesPerBlock];
};

// Вспомогательный fixed-point умножитель (аналог smull с нормализацией)
inline int32_t mul_q31( int32_t a, int32_t b ) {
    return static_cast<int32_t>( (static_cast<int64_t>(a) * static_cast<int64_t>(b) ) / ( static_cast<int64_t>(1) << 31 ) );
}

template< uint32_t BlockSize = 1024, uint32_t FrequencyCount = 256, int32_t StepsPerBlock = 16 >
void FskCorr( FskBlockContext< FrequencyCount, StepsPerBlock >* prevContext, 
              FskBlockContext< FrequencyCount, StepsPerBlock >* context, 
              const int32_t inSamples[BlockSize],
              CorrResult[FrequencyCount][StepsPerBlock] outSamples,
              uint32_t sampleCount ) {

    static constexpr uint32_t PowerOfTwoDcExpFilter = 18;
    
    int32_t noDcSamples[BlockSize];
    int32_t scaledSamples[BlockSize];

    // Let's calculate DC offset and remove it using exponentional filter
    context->blockDcOffset = FskRemoveDc<PowerOfTwoDcExpFilter>( prevContext->blockDcOffset, inSamples, noDcSamples, BlockSize );

    // Rescale value's to get very high dynamic range, more then 24bits
    // It's like per block AGC, before frequency analyse is process
    ScaleFactor scaleFactor = FskMakeFixedPointQ1_31( noDcSamples, scaledSamples, BlockSize );
    context->blockScaleFactor = scaleFactor;

    FskBlockContext<FrequencyCount, StepsPerBlock>* energyCalcContext = context->freq[0];
    BlockEnergyCalc( energyCalcContext, noDcSamples, &( context->power[0] ), StepsPerBlock );

    // Make frequency analyze
    for ( uint32_t freqIndex = 0; freqIndex < FreqCount; freqIndex++ ) {

        // Let's make correlation, using current frequency
        FskBlockContext<FrequencyCount, StepsPerBlock>* pFreq = context->freq[freqIndex];
        FskBlockCorrContext pFreqContext = context->freqContext[freqIndex];

        // Make CPFSK correlation
        pFreq->phase = FskBlockCorr( pFreqContext, scaledSamples, pFreq->IQ, StepsPerBlock );

        // Rescale samples to there original values, but
        // Now I am using values after CPFSK correlation
        // It's FPU emulaton, using very high dynamic range, more then ...
        for( uint32_t stepIndex = 0; stepIndex < StepsPerBlock; stepIndex++ ) {
            CorrResult* pIQ = &(pFreq->IQ[stepIndex] );
            pIQ->I = mul_q31( pIQ->I, scaleFactor );
            pIQ->Q = mul_q31( pIQ->Q, scaleFactor );
        }

        // So. Now I have partly corrlated steps
        // Each block had StepsPerBlock steps
        // Each step is partly correlated
        // What can I do?
        // I can sum it using different period length
        // I can sum it using different phase offset

        // Not it is O(StepsPerBlock*StepsPerBlock), but it's possible to make it O(1)
        // TO DO: 
        // Make it as O(1) algorithm
        for( uint32_t stepOffsetIndex = 0; stepOffsetIndex < StepsPerBlock; stepOffsetIndex++ ) {
            int64_t sumI = 0;
            int64_t sumQ = 0;
            for( uint32_t stepIndex = 0; stepIndex < StepsPerBlock; stepIndex++ ) {
                CorrResult*     pIQ = nullptr;           
                const uint32_t  index  = stepOffsetIndex + stepIndex;
                if ( index < StepsPerBlock ) {
                    FskBlockCorrContext pFreqContext = prevContext->freqContext[freqIndex];
                    pIQ = &(pFreq->IQ[index]);
                } else {
                    FskBlockCorrContext pFreqContext = context->freqContext[freqIndex];
                    pIQ = &(pFreq->IQ[index - StepsPerBlock]);
                }
                sumI += pIQ->I;
                sumQ += pIQ->Q;
            }

            // So, let's calulate output values
            const int32_t Isample = static_cast<int32_t>( sumI / StepsPerBlock );
            const int32_t Qsample = static_cast<int32_t>( sumQ / StepsPerBlock );

            // So get output
            CorrResult* pOutIQ = &(outSamples[freqIndex][stepOffsetIndex]);

            // Story output values
            pOutIQ->I = Isample;
            pOutIQ->Q = Qsample;
        }

    }

}

template<uint8_t ExpDcLength_Log2 = 18>
DcOffset FskRemoveDc( const DcOffset curExpDcOffset, const int32_t* inSamples, int32_t* outSamples, uint32_t sampleCount ) {
    DcOffset expDcOffset = curExpDcOffset;
    for ( uint32_t sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++ ) {
        const int32_t sample = inSamples[sampleIndex];
        const int32_t expSampleToSubstruct = expDcOffset / ( static_cast<int32_t>(1) << ExpDcLength_Log2 );
        const int32_t outSample = sample - expSampleToSubstruct;
        expDcOffset += outSample;
        outSamples[sampleIndex] = outSample;
    }
    return expDcOffset;
}

 ScaleFactor FskMakeFixedPointQ1_31( const int32_t* inSamples, int32_t* outSamples, const uint32_t sampleCount ) {
    static constexpr const int32_t minAmpLimit = 128;

    // Calculate absolute maximum value
    int32_t maxSampleValue = 0;
    for ( uint32_t sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++ ) {     
        const int32_t sample = inSamples[sampleIndex];
        const int32_t absSampleValue =   ( sample < 0 ) ? -sample : sample;
        maxSampleValue = ( absSampleValue > maxSampleValue ) ? absSampleValue :  maxSampleValue;
    }

    // Limit dynamic range
    if ( maxSampleValue < minAmpLimit ) maxSampleValue = minAmpLimit;

    // Calculate scale factor
    const int32_t scaleMutiplyer = ( static_cast<int64_t>(1) << 31 ) / maxSampleValue;

    // Scale values to maximum
    for ( uint32_t sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++ ) {
        const int32_t sample = inSamples[sampleIndex];
        int32_t scaledSample = mul_q31( sample, scaleMutiplyer );
        outSamples[sampleIndex] = scaledSample;
    }

    // Return dynamic range scale factor
    return maxSampleValue;
}

void FskBlockCorrInit( FskBlockCorrContext* const context, uint32_t targetFreq, uint32_t sampleRate, uint32_t samplesPerBlock, Phase currentPhase = 0 ) {
    // Store values
    context->_currentPhase = currentPhase;
    context->_samplesPerBlock = samplesPerBlock;

    // Calculate NCO step
    uint64_t calcStep = (static_cast<uint64_t>(targetFreq) << 32) / sampleRate;
    context->_phaseStep = static_cast<uint32_t>(calcStep);

    // Rotation matrix
    context->_rotCos = cosFixed(context->_phaseStep);
    context->_rotSin = sinFixed(context->_phaseStep);
    context->_rotNegSin = -context->_rotSin;

    // Reverse multiply value
    context->_blockRevMulValue = static_cast<int32_t>( (static_cast<uint64_t>(1) << 31) / samplesPerBlock );
}

void BlockEnergyCalc( FskBlockCorrContext* const context, const int32_t* inSamples, EnergyResult* outResult, uint32_t blockCount ) {
    const uint32_t samplesPerBlock = context->_samplesPerBlock;

    for ( uint32_t blockIndex = 0; blockIndex < blockCount; blockIndex++ ) {
        uint64_t energySum = 0;
        // Make block correlation
        for( uint32_t sampleIndex = 0; sampleIndex < samplesPerBlock; sampleIndex++ ) {
            const int32_t sample = static_cast<int32_t>(*inSamples++);
            energySum += sample * sample;
        }
        outResult->energy = energySum;
        outResult++;
    }

}

// Block correlation of samples
Phase FskBlockCorr( FskBlockCorrContext* const context, const int32_t* inSamples, CorrResult* outResult, uint32_t blockCount ) {
    const uint32_t samplesPerBlock = context->_samplesPerBlock;
    const int32_t rotCos = context->_rotCos;
    const int32_t rotSin = context->_rotSin;
    const int32_t rotNegSin = context->_rotNegSin;
    const uint32_t currentPhase = context->_currentPhase;

    int32_t vecI =  cosFixed( currentPhase );
    int32_t vecQ = -sinFixed( currentPhase );

    for ( uint32_t blockIndex = 0; blockIndex < blockCount; blockIndex++ ) {
        // Correlation accumulators
        int64_t accumI = 0;
        int64_t accumQ = 0;
        // Make block correlation
        for( uint32_t sampleIndex = 0; sampleIndex < samplesPerBlock; sampleIndex++ ) {
            int32_t sampleQ31 = static_cast<int32_t>(*inSamples++);
            // Make sum of samples         
            accumI += mul_q31( vecI, sampleQ31 );
            accumQ += mul_q31(vecQ,  sampleQ31 );
            // Rotate vector
            const int32_t vecI_prev  = vecI;
            vecI = mul_q31( vecI, rotCos ) + mul_q31( vecQ, rotSin );
            vecQ = mul_q31( vecI_prev, rotNegSin ) + mul_q31( vecQ, rotCos );
        }
        // Store block correlation results
        const int32_t revMulValue = context->_blockRevMulValue;
        outResult->I = static_cast<int32_t>( accumI * revMulValue / ( static_cast<int64_t>(1) << 31 ) );
        outResult->Q = static_cast<int32_t>( accumQ * revMulValue / ( static_cast<int64_t>(1) << 31 ) );
        outResult++;
    }

    const uint32_t totalSampleCount = context->_samplesPerBlock * blockCount;
    const uint32_t totalPhaseIncrement = context->_phaseStep * totalSampleCount;
    context->_currentPhase += totalPhaseIncrement;

    return context->_currentPhase;
}

Поясню.
На первом этапе происходит масштабирование отсчетов произвольной частоты, на блочных интервалах заданной длинны. Фактически, это блочное АРУ.
Затем происходит поблочное FFT, и масштабирование коэффициентов. Затем происходит перемножение коэффициентов для масштабирования результатов поблочной корреляции. Т.е. я использую максимальный динамический диапазон. А затем, эти отсчеты будут складываться скользящим окном.

Но в моем случае, могу применить один трюк. Модификация длинны периода накопления. Это фактически частотно-временная косынка, где результаты преобразования могут быть динамически усреднены на N-периодов и к тому де с произвольной начальной фазой.