Доброго времени суток, уважаемые посетители Habr!
Данная статья будет короткой, но полезной.
В одной из предыдущих статей, уже описывал вычисление sin(x)/cos(x) с применением разложения в ряд Фурье с фиксированной точкой. Вычисление тригонометрических функций, в моем случае занимало порядка 125-130 тактов на пару (sin+cos) на процессорном ядре Cortex M7 (STM32H750). При этом, код компилировался для архитектуры Cortex M3. Точность sin/cos просчитанного таким образом составила менее 1.5LSB. Google посчитал ее как 1.2-1.3LSB, с минимальной дисперсией. Это уже дало динамический диапазон ~183dB. Для сравнения, полный динамический диапазон человеческого уха 120dB от болевого порога до шелеста листвы. А динамически диапазон звука который человек слышит одновременно порядка 40-60dB. Несколько позже поясню для чего приведено сравнение.
В общем и целом такого динамического диапазона и скорости уже достаточно чтобы производить операцию квадратурной свертки сигнала с частотой дискретизации до 450-500KHz. Кстати, на STM32F103C8T6, это заняло бы ~1.8uS на квадратурный отсчет. Т.е. с отключенными прерываниями процессор бы успел выполнить расчет одного бина честного преобразования Фурье в реальном времени. Это эквивалентно квадратурной демодуляцию на одной произвольной поднесущей до частоты 250КГц (хотя лучше брать Fsample/4 ), что позволяет работать с полосой до 125КГц, на простом контроллере в реальном времени.
Однако, этого мало для полноценной обработки сигналов. И тут я задумался. Как можно значительно повысить скорость работы и почти не потерять в точности? Первое что сделал,- разбил преобразование на блоки, фаза которых непрерывна. Это позволило работать с блоками отсчетов, которые, затем можно суммировать скользящим окном со сложностью O(1). Это привело к эффекту квадратурной демодуляции сигнала и без повышения сложности позволяло работать с малыми временными сдвигами. Фактически пришел к поблочной корреляция. Нечто вроде временного Rack-приема.
Такой алгоритм позволял ускорить обработку сигнала, и работать с разными временными сдвигами. В то же время алгоритм позволяет собрать энергию сигнала когерентно, для разных частот и при этом сохранить инвариантность по отношению к фазе. Речь идет о когерентном сложении произвольных частотно-маниупулированных сигналов с непрерывной фазой и разными временным сдвигами. Просто по окончании периода корреляции, вы будете знать вектор, на которой провернулся ваш сигнал на одной частоте относительно сигнала на другой частоте. Вам достаточно домножить вектор на матрицу поворота e^-jPhy, a затем просуммировать квадратурные составляющие векторов. И делать это можно блоками длинной кратной периоду колебания.
Однако, производительность системы оказалась недостаточной, для моих задач и оптимизация алгоритма привела к интересному решению. Это матрица поворота для вектора, с которым производится корреляция исходного сигнала. Т.е. в начале длинного блока, известна начальная фаза вектора с котором следует проводить корреляцию, а значит, можно просчитать начальные значения I/Q компонент этого вектора. Однако, если просчитывать пару (sin, cos) на каждый отсчет, это занимает много времени. В то же время, внутри блока, можно пойти по пути поворота вектора. Такой поворот имеет погрешность менее шести младших бит на на 128 отсчетов при расчете в целых числах. Это позволяет обрабатывать, например 24-битные аудио отсчеты, практически не внося в них дополнительного шума.
Но я планировал работать с отсчетами ADC/DAC процессора STM32F103RE, выровненными влево. Собственно так и родился следующий код:
/* ******************************************************************************************** * @file Modem.s dedicated to STM32Fxx device * @author Evgeny Sobolev * @version V1.0.0 * @date 2026-08-26*/.syntax unified.cpu cortex-m3.thumb.extern cosFixed.extern sinFixed.global FskModemModulateFreqInit.global FskModemModulate.global FskModemRemoveDc.global FskModemBlockCorrInit.global FskModemBlockCorr.global _gStm32DacDcOffset.global _gStm32AdcDcOffset//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 )//};.section .text.type FskModemModulateFreqInit, %function.align 4FskModemModulateFreqInit: // R0 <= struct pointer // R1 <= Phase increment per sample// R2 <= Amplitudepush {lr}// Amplitude str r2, [r0, #0x00]// Phasestr r1, [r0, #0x04] mov r2, r0mov r0, r1 bl cosFixed str r0, [r2, #0x08] mov r0, r1 bl sinFixed str r0, [r2, #0x0C]pop {lr}bx lr.section .text.type FskModemModulate, %function.align 4FskModemModulate: push {r4-r12, lr} // R0 <= pointer to FskModem_ModulateStruct// R1 <= Phase// R2 <= Buffer// R3 <= Sample countmov 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 <= Qmov r0, r1bl sinFixednegs r5, r0// R4 <= Imov r0, r1bl cosFixedmov r4, r0// R9 <= cos(phi)ldr r9, [r12, #0x08]// R10 <= sin(phi)ldr r10, [r12, #0x0C]// R11 <= -sin(phi)negs r11, r10ldr r1, [r12, #0]// R0 <= DacDcOffsetldr r0, =_gStm32DacDcOffsetldr r0, [r0]// R1 <= Amplitude// R2 <= Buffer// R3 <= Sample count// R4 <= I// R5 <= Q// R9 <= cos(phi)// R10 <= sin(phi)// R11 <= -sin(phi)// R0 <= DCoffsetFskModemModulateLoop:// Possible to use R6, R7, R8 as temp// R7 is scaled to Q2.30 value of orig smull r6, r7, r5, r1// Make from scaled -sin, sin value + DC-offset, Q1.15sub r6, r0, r7, asr #15// Store value of scaled sin(x)strh r6, [r2], #2// 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 FskModemModulateLooppop {r0}pop {r4-r12, lr} bx lr/* ******************************************************************************************** * @description Demodulator common ******************************************************************************************* */.section .text.type FskModemRemoveDc, %function.align 4FskModemRemoveDc:// R0 <= Curent DC offset exp-accumulator, returns the same// R1 <= Input buffer Q0.16 (16bit)// R2 <= Output buffer Q1.15 (16bit)// R3 <= Sample count Warning !!!: Shold be evenpush {r1-r7}// R6 <= ADC dc point offset ldr r6, =_gStm32AdcDcOffset ldr r6, [r6]FskDcCancelLoop:// Load next sampleldr r4, [r1], #4ubfx r5, r4, 16, 16ubfx r4, r4, 0, 16// Substract 16bit adc middle valuesub r4, r4, r6sub r5, r5, r6// Recalculate exponentional sum, i.e. replace (2*mid) by two REAL samplesasr r7, r0, #(15)sub r0, r0, r7add r0, r0, r4add r0, r0, r5// Get current mid valueasr r7, r7, #1sub r4, r4, r7sub r5, r5, r7bfi r4, r5, #16, #16stmia r2!,{r4}subs r3, r3, #2bne FskDcCancelLooppop {r1-r7}bx lr//struct FskModem_BlockCorrStruct {//uint32_t phase;// Current phase //uint32_tphaseIncrPerBlock;// Phase incremnet over block of samples//uint32_tsamplesPerBlock;// Sample count in each block//uint32_t rotCos;// cos(phi), i.e. rotation ( I )//uint32_t rotSin;// sin(phi), i.e. rotation (-Q )//};.section .text.type FskModemBlockCorrInit, %function.align 4FskModemBlockCorrInit:push { lr }// R0 <= Pointer to FskModem_BlockCorrStruct// R1 <= Initial phase value// R2 <= Phase increment// R3 <= Sub block size// Store initial phasestr r1, [r0, #0x00]// Calculate & store phase increment per blockmul r1, r2, r3str r1, [r0, #0x04]// Store samplesPerBlock (block size)str r3, [r0, #0x08]// R3 <= BlockCorrStruct*mov r3, r0// Calc and store cos rotation valuemov r0, r2// WARNING, If functions sinFixed,cosFixed you used, is not my. // I don't need to sore R0-R3bl cosFixedstr r0, [r3, #0x0C]// Calc and store sin rotation valuemov r0, r2bl sinFixedstr r0, [r3, #0x10]// Returnpop { lr }bx lr.section .text.type FskModemBlockCorr, %function.align 4FskModemBlockCorr: push {r4-r12, lr}// R0 <= Pointer to FskModem_BlockCorrStruct// R1 <= Output buffer pointer// R2 <= Input buffer pointer// R3 <= Block count// R12 <= Pointer to FskModem_BlockCorrStructmov r12, r0// R0,R4 <= Current phaseldr r0, [ r12, #0x00 ]mov r4, r0// Calculate currnet vector I valuebl cosFixed// R5 <= I, i.e cos(currentPhase)mov r6, r0// R0 <= Current phasemov r0, r4// Calculate currnent vector Q valuebl sinFixed// R7 <= Q, i.e sin(currentPhase)negs r7, r0// R8 <= Phase Increment per blockldr r8, [ r12, #4 ]// R0 <= R4 + R3*R8, i.e. phase + phase increment// Overflow of phase doesn't mettersmla r0, r3, r8, r4// Store new phasestr r0, [ r12, #0 ]// Load cos(phi), sin(phi)// R8 <= cos(phi) ( i.e. rotation I scale )ldr r8, [ r12, #0x0C ]// R9 <= sin(phi) ( i.e. rotation -Q scale )ldr r9, [ r12, #0x10 ]negs r10, r9// R1 <= Output Buffer pointer// R2 <= Input Buffer pointer// R3 <= Sample count// R4 <= I accumulator// R5 <= Q accumulator// R6 <= Current I// R7 <= Current Q// R8 <= I / rotation cos value scale// R9 <= Q / rotation sin value scale// R10 <= Q / rotation -sin value scale// R11 <= Temp / Block counter// R12 <= Struct pointer // R11 <= Block counter mov r11, r3FskModemBlockCorrLoopExt:// R3 <= Sample count per blockldr r3, [ r12, #8 ]// R4 <= 0, i.e. I accumulator resetmov r4, #0// R5 <= 0, i.e. Q accumulator resetmov r5, #0// Stack <= R1, i.e. Output Pointer// Stack <= R11, i.e. Block counterpush { r1, r11 }FskModemBlockCorrLoopInt:// Load next sample Q1.11zzzz - 16bit total?// but 12bit max in STM32F103 (Q.S.11zzzz)// R6 is I (Q1.31) - signed i.e Q.S.31 // R7 is Q (Q1.31) - signed i.e Q.S.31ldrsh r0, [r2], #2// R0 is Sample Q1.15 - signed// R0 <= Q1.31, i.e ( Q1.15 << 16)lsl r0, r0, #16// R11 <= R0, i.q. Q1.31 of ADC samplemov r11, r0// R0 <= Q2.30, i.e (Q1.31 * Q1.31)smull r0, r1, r6, r0// R4 <= R4 + R1, i.q Q17.15 + Q17.15add r4, r4, r1, asr #15// R0 <= Q2.30 i.e. (Q1.31 * Q1.16)smull r0, r1, r7, r11// R5 <= R5 + R1, i.q Q17.15 + Q17.15add r5, r5, r1, asr #15// Inline Bel202PhaseIncrement:// R6 <= I// R7 <= Q// R8 <= Cos Phi// R9 <= Sin Phi// R10 <= -Sin Phi// R0,R1,R11 as tempsmull r1, r0, r6, r8smlal r1, r0, r7, r10lsr r1, r1, #(32-1) orr r11, r1, r0, lsl #1smull r1, r0, r6, r9smlal r1, r0, r7, r8lsr r1, r1, #(32-1) orr r7, r1, r0, lsl #1mov r6, r11// End of Inline Bel202PhaseIncrement// So lets' calculate next value or exitsubs r3, r3, #1bne FskModemBlockCorrLoopInt// POP R1 <= Current output pointer// POP R11 <= Block counterpop { r1, r11 }// Store correlation resultsstr r4, [r1], #4str r5, [r1], #4subs r11, #1// So, loop again, if not all blocks are porecessedbne FskModemBlockCorrLoopExt pop {r4-r12, lr} bx lr.section .rodata.align 4_gStm32DacDcOffset:.word 32768_gStm32AdcDcOffset:.word 32768
Собственно, если код немного доработать и заменить скалярное произведение на комплексное, то его можно будет использовать для квадратурной демодуляции I/Q отсчетов. Например квадратурной пары с ADC микроконтроллеров.
Спасибо за внимание.
ссылка на оригинал статьи https://habr.com/ru/articles/1091826/