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

Доброго времени суток, уважаемые посетители 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 4
FskModemModulateFreqInit:
    // R0 <= struct pointer
    // R1 <= Phase increment per sample
	// R2 <= Amplitude
	push {lr}
	// Amplitude
    str r2, [r0, #0x00]
	// Phase
	str r1, [r0, #0x04]
    mov r2, r0
	mov 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 4
FskModemModulate:
    push {r4-r12, lr} 
	// R0 <= pointer to FskModem_ModulateStruct
	// R1 <= Phase
	// R2 <= Buffer
	// R3 <= Sample count
	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
	mov r0, r1
	bl sinFixed
	negs r5, r0
	// R4 <= I
	mov r0, r1
	bl cosFixed
	mov r4, r0
			
	// R9 <= cos(phi)
	ldr  r9,  [r12, #0x08]
	// R10 <= sin(phi)
	ldr  r10, [r12, #0x0C]
	// R11 <= -sin(phi)
	negs r11, r10

	ldr r1, [r12, #0]
	
	// R0 <= DacDcOffset
	ldr r0, =_gStm32DacDcOffset
	ldr r0, [r0]
	

	// R1   <=  Amplitude
	// R2   <=  Buffer
	// R3   <=  Sample count
	// R4   <=  I
	// R5   <=  Q
	// R9   <=  cos(phi)
	// R10  <=  sin(phi)
	// R11  <=  -sin(phi)
	// R0   <= DCoffset
FskModemModulateLoop:
	// 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.15
	sub 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 - 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 FskModemModulateLoop
	
	pop {r0}
	pop {r4-r12, lr} 
  	bx lr


/*
  ********************************************************************************************
  * @description  Demodulator common
  *******************************************************************************************
 */
.section .text
.type FskModemRemoveDc, %function
.align 4
FskModemRemoveDc:
	// 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 even
	push {r1-r7}
	// R6 <= ADC dc point offset
  	ldr r6, =_gStm32AdcDcOffset
  	ldr r6, [r6]
FskDcCancelLoop:	
	// Load next sample
	ldr r4, [r1], #4
	ubfx r5, r4, 16, 16
	ubfx r4, r4, 0, 16
	// Substract 16bit adc middle value
	sub   r4, r4, r6
	sub   r5, r5, r6
	// Recalculate exponentional sum, i.e. replace (2*mid) by two REAL samples
	asr r7, r0, #(15)
	sub r0, r0, r7
	add r0, r0, r4
	add r0, r0, r5
	// Get current mid value
	asr r7, r7, #1
	sub r4, r4, r7	
	sub r5, r5, r7
	bfi  r4, r5, #16, #16
	stmia r2!,{r4}
	subs r3, r3, #2	
	bne FskDcCancelLoop
	pop {r1-r7}
	bx lr


//struct FskModem_BlockCorrStruct {
//	uint32_t 	phase;				// Current phase 
//	uint32_t	phaseIncrPerBlock;	// Phase incremnet over block of samples
//	uint32_t	samplesPerBlock;	// 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 4
FskModemBlockCorrInit:
	push { lr }
	
	// R0 <= Pointer to FskModem_BlockCorrStruct	
	// R1 <= Initial phase value
	// R2 <= Phase increment
	// R3 <= Sub block size
	
	// Store initial phase
	str r1, [r0, #0x00]
	
	// Calculate & store phase increment per block
	mul r1, r2, r3
	str r1, [r0, #0x04]
	
	// Store samplesPerBlock (block size)
	str r3, [r0, #0x08]
		
	// R3 <= BlockCorrStruct*
	mov r3, r0
		
	// Calc and store cos rotation value
	mov r0, r2
	
	// WARNING, If functions sinFixed,cosFixed you used, is not my.  
	// I don't need to sore R0-R3
	bl cosFixed
	str r0, [r3, #0x0C]
	
	// Calc and store sin rotation value
	mov r0, r2
	bl sinFixed
	str r0, [r3, #0x10]
	
	// Return
	pop { lr }
	bx lr



.section .text
.type FskModemBlockCorr, %function
.align 4
FskModemBlockCorr:
    push {r4-r12, lr}
	// R0 <= Pointer to FskModem_BlockCorrStruct
	// R1 <= Output buffer pointer
	// R2 <= Input buffer pointer
	// R3 <= Block count

	// R12 <= Pointer to FskModem_BlockCorrStruct
	mov r12, r0

	// R0,R4 <= Current phase
	ldr r0, [ r12, #0x00 ]	
	mov r4, r0
	// Calculate currnet vector I value
	bl cosFixed
	// R5 <= I, i.e cos(currentPhase)
	mov r6, r0
	// R0 <= Current phase
	mov r0, r4
	
	// Calculate currnent vector Q value
	bl sinFixed
	// R7 <= Q, i.e sin(currentPhase)
	negs r7, r0
	
	// R8 <= Phase Increment per block
	ldr r8, [ r12, #4 ]
	
	// R0 <= R4 + R3*R8, i.e. phase + phase increment
	// Overflow of phase doesn't metters
	mla r0, r3, r8, r4
	
	// Store new phase
	str 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, r3
FskModemBlockCorrLoopExt:
	// R3 <= Sample count per block
	ldr r3, [ r12, #8 ]	
	// R4 <= 0, i.e. I accumulator reset
	mov r4, #0
	// R5 <= 0, i.e. Q accumulator reset
	mov r5, #0
	// Stack <= R1,  i.e. Output Pointer
	// Stack <= R11, i.e. Block counter
	push { 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.31
	ldrsh 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 sample
	mov r11, r0	

	// R0 <= Q2.30, i.e (Q1.31 * Q1.31)
	smull r0, r1, r6, r0
	// R4 <= R4 + R1, i.q Q17.15 + Q17.15
	add 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.15
	add r5, r5, r1, asr #15
	
	// Inline Bel202PhaseIncrement:
	// R6  <=  I
	// R7  <=  Q
	// R8  <=  Cos Phi
	// R9  <=  Sin Phi
	// R10 <= -Sin Phi
	// R0,R1,R11 as temp
	smull r1, r0, r6, r8
	smlal r1, r0, r7, r10
	lsr r1, r1, #(32-1)
    orr r11, r1, r0, lsl #1
	
	smull r1, r0, r6, r9
	smlal r1, r0, r7, r8
	lsr r1, r1, #(32-1)
    orr r7, r1, r0, lsl #1
	mov r6, r11
	// End of Inline Bel202PhaseIncrement
	
	// So lets' calculate next value or exit
	subs r3, r3, #1
	bne FskModemBlockCorrLoopInt
	
	// POP  R1  <= Current output pointer
	// POP  R11 <= Block counter
	pop { r1, r11 }

	// Store correlation results
	str r4, [r1], #4
	str r5, [r1], #4
	
	subs r11, #1
	// So, loop again, if not all blocks are porecessed
	bne FskModemBlockCorrLoopExt

  	pop {r4-r12, lr}
  	bx lr


.section .rodata
.align 4
_gStm32DacDcOffset:
	.word 32768
_gStm32AdcDcOffset:
	.word 32768

Собственно, если код немного доработать и заменить скалярное произведение на комплексное, то его можно будет использовать для квадратурной демодуляции I/Q отсчетов. Например квадратурной пары с ADC микроконтроллеров.