Модуляция CPFSK и не только в квадратурах на микропроцессорах STM32

Добрый день, уважаемые коллеги!

Я инженер, и моя работа так или иначе была связана с проектированием, производством и поставками оборудования, работой с микросхемами систем связи. В наши задачи входила работа с протокольным уровнем систем связи, проектирование устройств, стабилизация их параметров, подготовка к производству. Это мир большого телекома. На работе мы пользовались весьма серьезными приборами вроде CMW-270, FSL-6, FSQ-8, векторными анализаторами и гигагерцовыми осциллографами Agilent. А дома у меня маленький мирок в котором приборы значительно проще. И процессоры проще. Но интерес тот же. Вот и попробуем сделать кое-что интересное из этой сферы.

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

Итак. Нам предстоит работать с модуляцией Continuous-Phase Frequency-Shift Keying (CPFSK). Эта модуляция является разновидностью частотной модуляции при которой не происходит разрыва фазы. Давайте разработаем эффективный модулятор на микроконтроллере STM32.

В результате, получим функции на Assembler Corex M3, позволяющие относительно быстро синтезировать гладкие сигналы с непрерывной фазой в квадратурах.

Данную задачу можно решить решить прямо, просто интегрируя значение текущей фазы и производить расчет новых значений sin(x)/cos(x) для сигнала. Однако, даже в случае применения оптимизированных мной функций rSinFixed, rCosFixed на высоких частотах дискретизации это приведет к значительной нагрузке на процессор. Табличные способы генерации отсчетов подходят, но не всегда. Все дело в том что начальная фаза сигнала не определена, а в процессе вычисления требуется гладко склеить предыдущий символ со следующим. Т.е. нам нужна непрерывность фазы. Как мы будем решать данный вопрос, оптимально?

Вообще говоря, существует методы синтеза синусоидальных колебаний, которые базируются на повороте вектора на комплексной плоскости. Смысл этих методов заключается в том, что сигналы sin(x) и cos(x) рассматриваются как проекция вектора на комплексную плоскость. При этом проекция на ось X, есть значение I, а проекция на ось Y есть значение Q. В математике, координаты такого вектора записываются в формате I – j*Q. В случае если длина вектора равна единице, то проекция I соответствует значению cos(alpha), а значение Q соответствует sin(alpha). Данный вектор, можно повернуть на определенный угол, используя матрицу поворота. Для этого достаточно провести следующую операцию Inew = I*cos(alpha) – Q*sin(alpha), Qnew=Q*cos(alpha) + I*sin(alpha). Поворачивая этот вектор с нужным шагом угла можно получить требуемую частоту отсчетов.

Однако, метод таит в себе погрешности и требует периодической корректировки. Все дело в том что математические операции не обладают бесконечной точностью и длинна единичного вектора начинает колебаться. А ошибка дискретного угла alpha, приводит к накоплению погрешности в фазе колебания. Как мы будем решать этот вопрос? Для решения данного вопроса, будет проводиться периодическая коррекция вектора с применением ранее реализованных мной функций rSinFixed(apha) и rCosFixed(alpha). Это позволит генерировать длительный тон, на теряя фазы и амплитуды колебания.

Поскольку, работа предполагается блоками, потребуется где-то хранить структуру содержащую промежуточные данные. Рассмотрим ее подробнее.

struct FskModem_ModulateStruct {uint32_t amplitude // Amplitudeuint32_t phaseIncrement // Phase incrementuint32_t rotCos; // cos(phi), i.e. rotation ( I )uint32_t rotSin; // sin(phi), i.e. rotation ( Q )}

1. amplitude — это значение размаха колебанияили по другому длина вектора, котораябудет сформирована на выходе функции.

2. phaseIncrement— это тот самый инкремент фазы, илиугол на который мы будем поворачиватьвектор

3. rotCos– этокоэффициент угла поворота

4. rotSin– этокоэффициент угла поворота

Насамом деле мы поступим более хитро икоэффициент поворота у нас будет всегданемного меньше единицы, чтобы непроизошло переполнение в целочисленныхоперациях. В то же время это даетвозможность создать функциюэкспоненциального затухания векторана границе символа. Т.е. если длинавектора sqrt( rotCos^2 +rotSin^2 ), меньшеединицы, то исходный опорный векторбудет не только поворачиваться но изатухать. Это позволяет синтезировать такие сигналы.

Вообще говоря, код инициализации не обязательно было писать на ассемблере, но мне так было проще, потому что не хотел держать отдельный файл с функцией, впрочем можно было сделать inline в Header-файле, но мне так проще.

.section .text.type FskModemModulateFreqInit, %function.align 4FskModemModulateFreqInit:// R0 <= struct pointer// R1 <= Phase increment per sample// R2 <= Amplitude// R3 <= Exp factorpush {r4,lr}// R4 <= struct pointermov r4, r0// Store amplitudestr r2, [r4, #0x00]// Store phase increment per samplestr r1, [r4, #0x04]// R0 <= PhaseIncrementmov r0, r1// R2 <= PhaseIncrementmov r2, r0// R1 <= Radius, I mean scaled rotation radiusmov r1, r3cmp r1, #0itt eqmovweq r1, #0xFFFEmovteq r1, #0x7FFF// Make rotation I componentbl rCosFixed// Store rotCosstr r0, [r4, #0x08]// R0 <= phaseIncrementmov r0, r2// Make rotation Q componentbl rSinFixed// Store rotSinstr r0, [r4, #0x0C]pop {r4,lr}bx lr

Итак. Структура описана, структура заполнена. Так давайте же синтезируем сигнал. Тут все просто и сложно одновременно. Просто, потому что алгоритм сводится к получению начального значения вектора, и последующему его вращению.

.section .text.type FskModemModulateAdd, %function.align 4FskModemModulateAdd:push {r4-r11, lr}push {r12}// R0 <= pointer to FskModem_ModulateStruct// R1 <= Phase// R2 <= Buffer// R3 <= Sample count// R12 <= struct pointermov r12, r0// R5 <= Phase Incremnetldr r5, [r12, #4]// R0 <= Phase + PhaseIncrement * SampleCountmla r0, r5, r3, r1// Update current phase on stackpush {r0}// Calculate current vector value// R5 <= Q/*************************************//****** Calculate vector value *******//*************************************/// Save current phase rempory// R5 <= R1 ( Phase )mov r5, r1// R0 <= Phasemov r0, r1// R1 <= Amplitude// ldr r1, [r12, #0]bl cosFixed// R4 <= cos(Phase)mov r4, r0// R0 <= Phasemov r0, r5bl sinFixed// R5 <= sin(Phase)mov r5, r0/************************************//**** Get rotation coefficients ****//************************************/// R9 <= cos(phi)ldr r9, [r12, #0x08]// R10 <= sin(phi)ldr r10, [r12, #0x0C]// R11 <= -sin(phi)negs r11, r10// R1 <= Amplitudeldr r1, [r12, #0]// R1 <= Amplitude// R2 <= Buffer// R3 <= Sample count// R4 <= I// R5 <= Q// R9 <= cos(phi)// R10 <= sin(phi)// R11 <= -sin(phi)FskModemModulateAddLoop:// Possible to use R0, R6, R7, R8 as temp// R7 is scaled to Q2.30 value of orig/* Calculate and store I */smull r6, r7, r4, r1// Make from scaled cos, cos value + DC-offset, Q0.16/Q1.15asr r8, r7, #15/* Calculate and store Q */smull r6, r7, r5, r1// Make from scaled sin, sin value + DC-offset, Q0.16/Q1.15asr r6, r7, #15/* DSP appand block */// Load current sample value of ( cos(x)/sin(x) )ldr r0, [r2]// Unpack valuesbfx r7, r0, #16, #16sbfx r0, r0, #0, #16// Append valueadd r6, r6, r7add r8, r8, r0// Pack againbfi r6, r8, #16, #16// Store valuestr r6, [r2], #4// So, it is possible to use, but Cortex M3// qadd16, i.e.// ldr r0, [r2]// bfi r6, r8, #16, #16// qadd16 r6, r0, r6// str r6, [r2], #4// Update phase use// R4, R5 - I/Q, R9,R10 - angles// R6, R7 - Tempsmull r6, r7, r4, r9smlal r6, r7, r5, r11// R7 <= I (Q1.31)lsl r7, r7, #1orr r7, r7, r6, lsr #31smull r6, r8, r4, r10smlal r6, r8, r5, r9// R5 <= Q (Q1.31)lsl r5, r8, #1orr r5, r5, r6, lsr #31// R4 <= I (update, Q1.31)mov r4, r7subs r3, #1bne FskModemModulateAddLooppop {r0}pop {r12}pop {r4-r11, lr}bx lr

Однако, код содержит некоторые трюки, связанные с масштабированием коэффициентов, что приводит к повышенной точности вычисления. Несмотря на то, что выходные отсчеты представляют из себя 16-битные пары, математика поворота вектора 32-битная. Т.е. Вектор изначально вычисляется в 32-битах, а затем масштабируется так чтобы получить исходную амплитуду. Но и здесь, тоже есть трюк. Поскольку код позволяет задать экспоненциальное затухание отсчетов, это решает вопрос затухания амплитуды. Оно получается естественным, экспоненциальным. Кстати, код можно упростить и работать с полноразмерными 32-битными парами отсчетов, из которых при длине блока 128 байт можно эффективно использовать больше 24 бит, что позволяет работать с музыкальными фрагментами, например, осуществляя поиск транзиентов или фундаментальных частот и их гармоник, или синтезируя их.

Другой трюк, который использован в коде, это сложение отсчетов. Для чего он нужен? Все дело в том что модуляция не обязана быть просто N-CPFSK, она может быть и OFDM например. И для этих целей, код производит добавление отсчетов к текущим значениям, а не перезапись буфера. Это позволяет заполнить буфер значением смещения, а затем добавлять в него синусоиды, в том числе экспоненциально-затухающие.

А потом, я подумал. Если у меня добавлено затухание сигнала в конце, почему бы не добавить его раскачку вначале… Так появился следующий код.

.section .text.type FskModemModulateRev, %function.align 4FskModemModulateRev:push {r4-r11, lr}push {r12}// R0 <= pointer to FskModem_ModulateStruct// R1 <= Phase// R2 <= Buffer// R3 <= Sample count// R12 <= struct pointermov r12, r0// R5 <= Phase Incremnetldr r5, [r12, #4]// R0 <= Phase + PhaseIncrement * SampleCountmla r0, r5, r3, r1// Update current phase on stackpush {r0}// Calculate current vector value// R5 <= Q/*************************************//****** Calculate vector value *******//*************************************/// Save current phase tempory// R0 is Updated Phase// R5 <= R0 ( Phase )mov r5, r0// R1 <= Amplitude// ldr r1, [r12, #0]bl cosFixed// R4 <= cos(Phase)mov r4, r0// R0 <= Phasemov r0, r5bl sinFixed// R5 <= sin(Phase)mov r5, r0/************************************//**** Get rotation coefficients ****//************************************/// R9 <= cos(phi)ldr r9, [r12, #0x08]// R10 <= sin(phi)ldr r10, [r12, #0x0C]// R11 <= -sin(phi)negs r11, r10// R1 <= Amplitudeldr r1, [r12, #0]// R0 <= DacDcOffsetldr r0, =_gStm32DacDcOffsetldr r0, [r0]// R3 <= R3*4add r2, r2, r3, lsl #2// R2 <= end of buffer pointer// R1 <= Amplitude// R2 <= Buffer// R3 <= Sample count// R4 <= I// R5 <= Q// R9 <= cos(phi)// R10 <= sin(phi)// R11 <= -sin(phi)// R0 <= DCoffsetFskModemModulateRevLoop:// Possible to use R6, R7, R8 as temp// R7 is scaled to Q2.30 value of orig/* Calculate and store I */smull r6, r7, r4, r1// Make from scaled cos, cos value + DC-offset, Q0.16/Q1.15add r8, r0, r7, asr #15/* Calculate and store Q */smull r6, r7, r5, r1// Make from scaled sin, sin value + DC-offset, Q0.16/Q1.15add r6, r0, r7, asr #15// Store both cos(x)/sin(x)bfi r6, r8, #16, #16stmdb r2!, {r6}//sub r2, r2, #4//str r6, [r2]// Update phase use// R4, R5 - I/Q, R9,R10 - angles// R6, R7 - Tempsmull r6, r7, r4, r9smlal r6, r7, r5, r10// R7 <= I (Q1.31)lsl r7, r7, #1orr r7, r7, r6, lsr #31smull r6, r8, r4, r11smlal r6, r8, r5, r9// R5 <= Q (Q1.31)lsl r5, r8, #1orr r5, r5, r6, lsr #31// R4 <= I (update, Q1.31)mov r4, r7subs r3, #1bne FskModemModulateRevLooppop {r0}pop {r12}pop {r4-r11, lr}bx lr

Код позволяет сформировать сигналы формы, которая приведена на обложке статьи. Просто нужно уметь им пользоваться. Упрощенно, это может выглядеть так (псевдокод).

static constexpr const uint32_t SampleCount = 320;static constexpr const int32_t amplitude = 1200; //1850;static constexpr const uint32_t sampleRate = 128;static constexpr const uint32_t frequency = 10;static constexpr const uint32_t frequency1 = 2;static constexpr const uint32_t samplesPerPeriod = sampleRate / frequency;static constexpr const uint32_t samples1PerPeriod = sampleRate / frequency1;static constexpr const uint32_t phaseIncrementPerSample = 0xFFFFFFFF / samplesPerPeriod;static constexpr const uint32_t phase1IncrementPerSample = 0xFFFFFFFF / samples1PerPeriod;static constexpr const uint32_t expScale = 0x7FF00000;static constexpr const uint32_t expScaleSide = 0x70000000;sampleCount = SampleCount / 8;FskModemModulateFreqInit( modulator, phaseIncrementPerSample, (amplitude << 16), expScaleSide );phase = FskModemModulateRevAdd( modulator, phase, &samples[ sampleOffset ], sampleCount );sampleOffset += sampleCount;sampleCount = SampleCount / 4;FskModemModulateFreqInit( modulator, phaseIncrementPerSample, (amplitude << 16), 0 );phase = FskModemModulateAdd( modulator, phase, &samples[ sampleOffset ], sampleCount );sampleOffset += sampleCount;sampleCount = SampleCount / 16;FskModemModulateFreqInit( modulator, ( static_cast<uint32_t>( (static_cast<uint64_t>(3) * phaseIncrementPerSample) + phase1IncrementPerSample) / 4 ), (amplitude << 16), expScale );phase = FskModemModulateAdd( modulator, phase, &samples[ sampleOffset ], sampleCount );sampleOffset += sampleCount;sampleCount = SampleCount / 16;FskModemModulateFreqInit( modulator, ( static_cast<uint32_t>( (static_cast<uint64_t>(3) * phase1IncrementPerSample) + phaseIncrementPerSample) / 4 ), (amplitude << 16), expScale );phase = FskModemModulateRevAdd( modulator, phase, &samples[ sampleOffset ], sampleCount );sampleOffset += sampleCount;sampleCount = SampleCount / 4;FskModemModulateFreqInit( modulator, phase1IncrementPerSample, (amplitude << 16), 0 );phase = FskModemModulateAdd( modulator, phase, &samples[ sampleOffset ], sampleCount );sampleOffset += sampleCount;sampleCount = SampleCount / 8;FskModemModulateFreqInit( modulator, phase1IncrementPerSample, (amplitude << 16), expScaleSide );phase = FskModemModulateAdd( modulator, phase, &samples[ sampleOffset ], sampleCount );sampleOffset += sampleCount;

В чем преимущество такого подхода? Это позволяет формировать точные отсчеты с непрерывной фазой на высокой частоте дискретизации и минимизировать нагрузку на процессор. Где можно применить этот код?

1. FSK / CPFSK модуляторы.

2. OFDM — модуляторы. Код прибавляет значение к паре отсчетов, можно добавить больше одной частоты и получить OFDM. Эффективен, если частот немного.

3. Синтезаторы частот музыкальных инструментов или голоса. Например в кодеках вроде Codec2 для формирования фундаментальной частоты и ее гармоник.

4. В качестве опорного сигнала для коррелятора аудио-кодека. Но об этом в других статьях.

Спасибо за внимание!