Добрый день, уважаемые коллеги!
Я инженер, и моя работа так или иначе была связана с проектированием, производством и поставками оборудования, работой с микросхемами систем связи. В наши задачи входила работа с протокольным уровнем систем связи, проектирование устройств, стабилизация их параметров, подготовка к производству. Это мир большого телекома. На работе мы пользовались весьма серьезными приборами вроде 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 // Amplitude uint32_t phaseIncrement // Phase increment uint32_t rotCos; // cos(phi), i.e. rotation ( I ) uint32_t rotSin; // sin(phi), i.e. rotation ( Q ) }
amplitude - это значение размаха колебания или по другому длина вектора, которая будет сформирована на выходе функции.
phaseIncrement— это тот самый инкремент фазы, или угол на который мы будем поворачивать вектор
rotCos – это коэффициент угла поворота
rotSin – это коэффициент угла поворота
На самом деле мы поступим более хитро и коэффициент поворота у нас будет всегда немного меньше единицы, чтобы не произошло переполнение в целочисленных операциях. В то же время это дает возможность создать функцию экспоненциального затухания вектора на границе символа. Т.е. если длина вектора sqrt( rotCos^2 + rotSin^2 ), меньше единицы, то исходный опорный вектор будет не только поворачиваться но и затухать. Это позволяет синтезировать такие сигналы.
Вообще говоря, код инициализации не обязательно было писать на ассемблере, но мне так было проще, потому что не хотел держать отдельный файл с функцией, впрочем можно было сделать inline в Header-файле, но мне так проще.
.section .text .type FskModemModulateFreqInit, %function .align 4 FskModemModulateFreqInit: // R0 <= struct pointer // R1 <= Phase increment per sample // R2 <= Amplitude // R3 <= Exp factor push {r4,lr} // R4 <= struct pointer mov r4, r0 // Store amplitude str r2, [r4, #0x00] // Store phase increment per sample str r1, [r4, #0x04] // R0 <= PhaseIncrement mov r0, r1 // R2 <= PhaseIncrement mov r2, r0 // R1 <= Radius, I mean scaled rotation radius mov r1, r3 cmp r1, #0 itt eq movweq r1, #0xFFFE movteq r1, #0x7FFF // Make rotation I component bl rCosFixed // Store rotCos str r0, [r4, #0x08] // R0 <= phaseIncrement mov r0, r2 // Make rotation Q component bl rSinFixed // Store rotSin str r0, [r4, #0x0C] pop {r4,lr} bx lr
Итак. Структура описана, структура заполнена. Так давайте же синтезируем сигнал. Тут все просто и сложно одновременно. Просто, потому что алгоритм сводится к получению начального значения вектора, и последующему его вращению.
.section .text .type FskModemModulateAdd, %function .align 4 FskModemModulateAdd: push {r4-r11, lr} push {r12} // R0 <= pointer to FskModem_ModulateStruct // R1 <= Phase // R2 <= Buffer // R3 <= Sample count // R12 <= struct pointer mov r12, r0 // R5 <= Phase Incremnet ldr r5, [r12, #4] // R0 <= Phase + PhaseIncrement * SampleCount mla r0, r5, r3, r1 // Update current phase on stack push {r0} // Calculate current vector value // R5 <= Q /*************************************/ /****** Calculate vector value *******/ /*************************************/ // Save current phase rempory // R5 <= R1 ( Phase ) mov r5, r1 // R0 <= Phase mov r0, r1 // R1 <= Amplitude // ldr r1, [r12, #0] bl cosFixed // R4 <= cos(Phase) mov r4, r0 // R0 <= Phase mov r0, r5 bl 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 <= Amplitude ldr 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.15 asr r8, r7, #15 /* Calculate and store Q */ smull r6, r7, r5, r1 // Make from scaled sin, sin value + DC-offset, Q0.16/Q1.15 asr r6, r7, #15 /* DSP appand block */ // Load current sample value of ( cos(x)/sin(x) ) ldr r0, [r2] // Unpack value sbfx r7, r0, #16, #16 sbfx r0, r0, #0, #16 // Append value add r6, r6, r7 add r8, r8, r0 // Pack again bfi r6, r8, #16, #16 // Store value str 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 - Temp smull r6, r7, r4, r9 smlal r6, r7, r5, r11 // R7 <= I (Q1.31) lsl r7, r7, #1 orr r7, r7, r6, lsr #31 smull r6, r8, r4, r10 smlal r6, r8, r5, r9 // R5 <= Q (Q1.31) lsl r5, r8, #1 orr r5, r5, r6, lsr #31 // R4 <= I (update, Q1.31) mov r4, r7 subs r3, #1 bne FskModemModulateAddLoop pop {r0} pop {r12} pop {r4-r11, lr} bx lr
Однако, код содержит некоторые трюки, связанные с масштабированием коэффициентов, что приводит к повышенной точности вычисления. Несмотря на то, что выходные отсчеты представляют из себя 16-битные пары, математика поворота вектора 32-битная. Т.е. Вектор изначально вычисляется в 32-битах, а затем масштабируется так чтобы получить исходную амплитуду. Но и здесь, тоже есть трюк. Поскольку код позволяет задать экспоненциальное затухание отсчетов, это решает вопрос затухания амплитуды. Оно получается естественным, экспоненциальным. Кстати, код можно упростить и работать с полноразмерными 32-битными парами отсчетов, из которых при длине блока 128 байт можно эффективно использовать больше 24 бит, что позволяет работать с музыкальными фрагментами, например, осуществляя поиск транзиентов или фундаментальных частот и их гармоник, или синтезируя их.
Другой трюк, который использован в коде, это сложение отсчетов. Для чего он нужен? Все дело в том что модуляция не обязана быть просто N-CPFSK, она может быть и OFDM например. И для этих целей, код производит добавление отсчетов к текущим значениям, а не перезапись буфера. Это позволяет заполнить буфер значением смещения, а затем добавлять в него синусоиды, в том числе экспоненциально-затухающие.
А потом, я подумал. Если у меня добавлено затухание сигнала в конце, почему бы не добавить его раскачку вначале... Так появился следующий код.
.section .text .type FskModemModulateRev, %function .align 4 FskModemModulateRev: push {r4-r11, lr} push {r12} // R0 <= pointer to FskModem_ModulateStruct // R1 <= Phase // R2 <= Buffer // R3 <= Sample count // R12 <= struct pointer mov r12, r0 // R5 <= Phase Incremnet ldr r5, [r12, #4] // R0 <= Phase + PhaseIncrement * SampleCount mla r0, r5, r3, r1 // Update current phase on stack push {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 <= Phase mov r0, r5 bl 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 <= Amplitude ldr r1, [r12, #0] // R0 <= DacDcOffset ldr r0, =_gStm32DacDcOffset ldr r0, [r0] // R3 <= R3*4 add 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 <= DCoffset FskModemModulateRevLoop: // 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.15 add 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.15 add r6, r0, r7, asr #15 // Store both cos(x)/sin(x) bfi r6, r8, #16, #16 stmdb r2!, {r6} //sub r2, r2, #4 //str r6, [r2] // Update phase use // R4, R5 - I/Q, R9,R10 - angles // R6, R7 - Temp smull r6, r7, r4, r9 smlal r6, r7, r5, r10 // R7 <= I (Q1.31) lsl r7, r7, #1 orr r7, r7, r6, lsr #31 smull r6, r8, r4, r11 smlal r6, r8, r5, r9 // R5 <= Q (Q1.31) lsl r5, r8, #1 orr r5, r5, r6, lsr #31 // R4 <= I (update, Q1.31) mov r4, r7 subs r3, #1 bne FskModemModulateRevLoop pop {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;
В чем преимущество такого подхода? Это позволяет формировать точные отсчеты с непрерывной фазой на высокой частоте дискретизации и минимизировать нагрузку на процессор. Где можно применить этот код?
FSK / CPFSK модуляторы.
OFDM - модуляторы. Код прибавляет значение к паре отсчетов, можно добавить больше одной частоты и получить OFDM. Эффективен, если частот немного.
Синтезаторы частот музыкальных инструментов или голоса. Например в кодеках вроде Codec2 для формирования фундаментальной частоты и ее гармоник.
В качестве опорного сигнала для коррелятора аудио-кодека. Но об этом в других статьях.
Спасибо за внимание!