Update all files + calculator. Working with FM Stations

2026-06-15 07:19:25 +00:00 · 2024-06-25 02:39:10 -07:00
parent 25dc0234c0
commit 26d8facf2c
4 changed files with 650 additions and 47 deletions
@@ -0,0 +1,526 @@
+/**
+
+MIT-like-non-ai-license
+
+Copyright (c) 2024 Charles Lohr "CNLohr"
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the two following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+In addition the following restrictions apply:
+
+1. The Software and any modifications made to it may not be used for the
+purpose of training or improving machine learning algorithms, including but not
+limited to artificial intelligence, natural language processing, or data
+mining. This condition applies to any derivatives, modifications, or updates
+based on the Software code. Any usage of the Software in an AI-training dataset
+is considered a breach of this License.
+
+2. The Software may not be included in any dataset used for training or
+improving machine learning algorithms, including but not limited to artificial
+intelligence, natural language processing, or data mining.
+
+
+3. Any person or organization found to be in violation of these restrictions
+will be subject to legal action and may be held liable for any damages
+resulting from such use.
+
+If any term is unenforcable, other terms remain in-force.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+
+**/
+
+
+// NOT LORA!!!
+
+// Transmit a sweep on 97.7MHz
+//#define FM_TRANSMITTER_SWEEP
+
+// Nothing = Transmit a 315MHz signal.
+
+// XXX WARNING: Something is wrong with this - 
+//  The output isn't perfectly time aligned
+//  And as such there are extra images in weird places.
+//  TODO: Investigate the DMA+SPI Port jankyness.
+#include "ch32v003fun.h"
+#include <stdio.h>
+#include <string.h>
+
+#include "LoRa-SDR-Code.h"
+
+#ifdef LORAWAN
+#include "lorawan_simple.h"
+#endif
+
+#define DMA_SIZE_WORDS 128
+
+#define SENDBUFF_WORDS (DMA_SIZE_WORDS*2)
+uint8_t sendbuff[SENDBUFF_WORDS];
+
+// Bits are shifted out MSBit first, then to LSBit
+
+
+#define MAX_BYTES 25
+#define MAX_SYMBOLS (MAX_BYTES*2+16)
+
+// Our table is bespoke for the specific SF.
+#define CHIPSSPREAD CHIRPLENGTH_WORDS// QUARTER_CHIRP_LENGTH_WORDS (TODO: Use the quater value elsewhere in the code)
+#define MARK_FROM_SF0 (1<<SF_NUMBER) // SF7
+
+#define PREAMBLE_CHIRPS 10
+#define CODEWORD_LENGTH 2
+
+uint32_t quadsetcount;
+int16_t quadsets[MAX_SYMBOLS*4+PREAMBLE_CHIRPS*4+9+CODEWORD_LENGTH*4];
+int runningcount_bits = 0;
+volatile int fxcycle;
+volatile int quadsetplace = -1;
+volatile uint32_t temp;
+
+#if 0
+
+int16_t * AddChirp( int16_t * qso, int offset, int verneer )
+{
+	offset = offset * CHIPSSPREAD / (MARK_FROM_SF0);
+	offset += verneer;
+	*(qso++) = (CHIPSSPREAD * 0 / 4 + offset + CHIPSSPREAD ) % CHIPSSPREAD;
+	*(qso++) = (CHIPSSPREAD * 1 / 4 + offset + CHIPSSPREAD ) % CHIPSSPREAD;
+	*(qso++) = (CHIPSSPREAD * 2 / 4 + offset + CHIPSSPREAD ) % CHIPSSPREAD;
+	*(qso++) = (CHIPSSPREAD * 3 / 4 + offset + CHIPSSPREAD ) % CHIPSSPREAD;
+	return qso;
+}
+
+// This IRQ is called periodically to fill the output buffer that is shifted to SPI
+void DMA1_Channel3_IRQHandler( void ) __attribute__((interrupt)) __attribute__((section(".srodata")));
+void DMA1_Channel3_IRQHandler( void ) 
+{
+	//GPIOD->BSHR = 1;	 // Turn on GPIOD0 for profiling
+
+	// Backup flags.
+	volatile int intfr = DMA1->INTFR;
+	do
+	{
+		// Clear all possible flags.
+		DMA1->INTFCR = DMA1_IT_GL3;
+
+
+		//int place = DMA1_Channel3->CNTR;
+		// CNTR says that there are THIS MANY bytes left in the transfer.
+		// So a high CNTR value indicates a very early place in the buffer.
+
+		uint16_t * sb = 0;
+
+		temp++;
+
+		if( intfr & DMA1_IT_HT3 )
+		{
+			sb = sendbuff;
+		}
+		else if( intfr & DMA1_IT_TC3 )
+		{
+			sb = sendbuff + SENDBUFF_WORDS/2;
+		}
+
+		if( sb )
+		{
+			if( quadsetplace < 0 )
+			{
+				// Abort Send
+				memset( sb, 0, SENDBUFF_WORDS*2/2 );
+				DMA1_Channel3->CFGR &= ~DMA_CFGR1_EN;
+				goto complete;
+			}
+
+			fxcycle += DMA_SIZE_WORDS;
+			if( fxcycle == NUM_DMAS_PER_QUARTER_CHIRP*DMA_SIZE_WORDS )
+			{
+				fxcycle = 0;
+				// Advance to next quarter word.
+				quadsetplace++;
+
+				if( quadsetplace > quadsetcount )
+				{
+#ifdef TEST_TONE
+					quadsetplace = 0;
+#else
+					quadsetplace = -1;
+#endif
+					memset( sb, 0, SENDBUFF_WORDS*2/2 );
+					goto complete;
+				}
+			}
+
+			int symbol = quadsets[quadsetplace]; // Actually 0...CHIRPLENGTHWORDS
+
+			const uint16_t * tsb = 0;
+
+			// Select down- or up-chirp.
+			if( symbol < 0 )
+			{
+				int word = fxcycle - symbol - 1;
+				if( word >= CHIRPLENGTH_WORDS ) word -= CHIRPLENGTH_WORDS;
+				word++;
+				tsb = (&chirpbuff[word+REVERSE_START_OFFSET_BYTES/4]);
+			}
+			else
+			{
+				int word = fxcycle + symbol;
+				if( word >= CHIRPLENGTH_WORDS ) word -= CHIRPLENGTH_WORDS;
+				tsb = (&chirpbuff[word]);
+			}
+
+			// I tried using the DMA to do the copy - but that didn't work for some reason.
+			//while( DMA1_Channel2->CFGR & 1 );
+//			DMA1_Channel2->CNTR = DMA_SIZE_WORDS/2;
+//			DMA1_Channel2->MADDR = (uint32_t)tsb;
+//			DMA1_Channel2->PADDR = (uint32_t)sb;
+//			DMA1_Channel2->CFGR |= DMA_CFGR1_EN;
+
+			int cpy = DMA_SIZE_WORDS/2;
+#ifdef TEST_TONE
+	// Test tone
+	memset( sb, 0xaa, cpy*2 );
+#else
+#if DMA_SIZE_WORDS_DIVISIBLE_BY_FOUR == 0
+			#error need divisibiltiy by 2.
+#else
+			//while( cpy-- ) { *(sb++) = wordo; }
+			// Guarantee aligned access.
+			uint32_t * sbw = (uint32_t*)(((uint32_t)sb)&~3);
+			uint32_t * tsbw = (uint32_t*)(((uint32_t)tsb)&~3);
+			//while( cpy-- ) { *(sbw++) = *(tsbw++); }
+
+			// Align the data copy if needed
+			if( cpy & 1 )
+			{
+				*(sbw++) = *(tsbw++);
+			}
+			cpy /= 2; // Doubled up per loop
+			asm volatile("\
+1:\
+c.lw a3, 0(%[from])\n\
+c.lw a4, 4(%[from])\n\
+c.addi %[from], 8\n\
+c.addi %[cpy], -1\n\
+c.sw a3, 0(%[to])\n\
+c.sw a4, 4(%[to])\n\
+c.addi %[to], 8\n\
+c.bnez %[cpy], 1b\n\
+" : [cpy]"+r"(cpy), [from]"+r"(tsbw), [to]"+r"(sbw) : : "a3", "a4", "memory");
+#endif
+#endif
+		}
+
+complete:
+		intfr = DMA1->INTFR;
+	} while( intfr );
+
+	//GPIOD->BSHR = 1<<16; // Turn off GPIOD0 for profiling
+}
+#endif
+
+
+
+void LoopFunction()  __attribute__((section(".srodata")));
+void LoopFunction()
+{
+	uint8_t * start = (uint8_t*)DMA1_Channel2->MADDR;
+	uint8_t * end = (uint8_t*)((uint32_t)DMA1_Channel2->MADDR + SENDBUFF_WORDS);
+	uint8_t * here = start+ 8;
+	uint32_t targ = 2000;
+	uint32_t running = 0;
+	uint8_t * tail = end - DMA1_Channel2->CNTR;
+	uint32_t * cntr = DMA1_Channel2->CNTR;
+	uint32_t temp = 0;
+	uint32_t temp2 = 0;
+
+	asm volatile("\n\
+	li %[targ], 2000\n\
+genloop:\n\
+	lw %[temp], 0(%[cntr])\n\
+	sub %[tail], %[end], %[temp]\n\
+	beq %[here], %[tail], genloop\n\
+innerloop:\
+	li %[temp], 17\n\
+	blt %[running], %[targ], noskip\n\
+	li %[temp], 18\n\
+	sub %[running], %[running], %[targ]\n\
+noskip:\n\
+	sb %[temp], 0(%[here])\n\
+	addi %[here], %[here], 1\n\
+	bne %[here], %[end], skipreset\n\
+	add %[here], x0, %[start]\n\
+skipreset:\n\
+	bne %[here], %[tail], innerloop\n\
+	j genloop\n\
+" : [here]"+r"(here) :
+	[start]"r"(start),
+	[end]"r"(end),
+	[targ]"r"(targ),
+	[running]"r"(running),
+	[tail]"r"(tail),
+	[cntr]"r"(cntr),
+	[temp]"r"(temp),
+	[temp2]"r"(temp2) );
+
+/*
+	while(1)
+	{
+		int targ_f = 2000; //(frameno & 511)*9 + 1700;
+		int run_f = 0;
+		uint8_t * tail = end - DMA1_Channel2->CNTR;
+
+		while( here != tail )
+		{
+			int setf = 17;
+			if( run_f > targ_f )
+			{
+				setf = 18;
+				run_f -= targ_f;
+			}
+			run_f += setf*32;
+
+			*here = setf;
+
+			here++;
+		}
+*/
+/*
+		for( j = 0; j < sizeof( sendbuff ); j++ )
+		{
+			int setf = 10;
+			if( run_f > targ_f )
+			{
+				setf = 9;
+				run_f -= targ_f;
+			}
+			run_f += setf*32;
+			
+			sendbuff[j] = setf;
+		}
+*/
+}
+void LoopFunction2() __attribute__((aligned(256))) __attribute__((section(".srodata"))) __attribute__ ((noinline));
+
+__attribute__((section(".sdata"))) __attribute__((aligned(256))) const uint32_t tablef[] = {
+		0x08080808, 
+		0x08080809, 
+		0x08090809, 
+		0x09090908, 
+		0x09090909, 
+		0x0909090a, 
+		0x090a090a, 
+		0x0a0a0a09, 
+		0x0a0a0a0a,  // Below this line is unstable - i.e. sometimes there are missing DMA transfers.
+		0x0a0a0a0b, 
+		0x0a0b0a0b, 
+		0x0b0b0b0a, 
+		0x0b0b0b0b, 
+		0x0b0b0b0c, 
+		0x0b0c0b0c, 
+		0x0c0c0c0b, 
+		0x0c0c0c0c,
+		0x0c0c0c0d, 
+		0x0c0d0c0d, 
+		0x0d0d0d0c, 
+		0x0d0d0d0d,
+		0x0d0d0d0e, 
+		0x0d0e0d0e, 
+		0x0e0e0e0d,
+		0x0e0e0e0e, 
+		0x0e0e0e0f, 
+		0x0e0f0e0f, 
+		0x0f0f0f0e,
+		0x0f0f0f0f, 
+		0x0f0f0f10, 
+		0x0f100f10, 
+		0x1010100f,
+		0x10101010, 
+		0x10101011, 
+		0x10111011, 
+		0x11111110,
+	};
+
+void LoopFunction2()
+{
+
+	uint32_t * start = (uint8_t*)DMA1_Channel2->MADDR;
+	uint32_t * end = (uint8_t*)((uint32_t)DMA1_Channel2->MADDR + SENDBUFF_WORDS);
+	uint32_t * here = start;
+
+	int run_f = 0;
+
+	volatile uint32_t * cntrptr = &DMA1_Channel2->CNTR;
+
+	while(1)
+	{
+		//uint32_t * tail = 0xfffffffc & (uintptr_t)(((uint8_t*)end) - *cntrptr);
+		//if( tail == end ) tail--;
+		uint32_t * tail = ((SENDBUFF_WORDS-1) & (0xfffffffc)) & (uintptr_t)(((uint8_t*)start) + SENDBUFF_WORDS - *cntrptr);
+
+		while( here != tail )
+		{
+#ifdef FM_TRANSMITTER_SWEEP
+			// 97.7MHz FM Station
+			uint32_t cp = ((SysTick->CNT>>3)&0x3fff)+0x12900;
+#else
+			// 315MHz
+			uint32_t cp = 0x1bfc3;
+#endif
+			*(here++) = tablef[run_f>>12];
+			run_f &= (1<<12)-1;
+			run_f += cp;
+			if( here == end )
+				here = start;
+		}
+	}
+
+}
+
+
+int main()
+{
+	SystemInit();
+
+	funGpioInitAll();
+
+	// Set a wait state (1 = normal <= 48MHz)
+	FLASH->ACTLR = 1;
+
+// MCO for testing.
+//	funPinMode( PA8, GPIO_CFGLR_OUT_50Mhz_AF_PP );	RCC->CFGR0 |= RCC_CFGR0_MCO_PLL;
+
+//	printf( "Switching to HSE\n" );
+	Delay_Ms( 10 );
+
+	// Disable clock security system.
+	RCC->CTLR &= ~RCC_CSSON;
+
+	// Enable external crystal
+	RCC->CTLR |= RCC_HSEON;
+
+	// XXX NOTE: This is only used if you have a clock, not an oscillator.
+//	RCC->CTLR |= RCC_HSEBYP;
+
+	// Set System Clock Source to be 0.
+	RCC->CFGR0 = (RCC->CFGR0 & ~RCC_SW) | 0;
+
+	// Disable PLL
+	RCC->CTLR &= ~RCC_PLLON;
+
+	// Switch to HSE
+	RCC->CFGR0 |= RCC_PLLSRC;
+
+	// Enable PLL
+	RCC->CTLR |= RCC_PLLON;
+
+	// Wait for HSE to become ready.
+	while( !( RCC->CTLR & RCC_HSERDY) );
+	while( !( RCC->CTLR & RCC_PLLRDY) );
+
+	RCC->CFGR0 |= RCC_SW_1; //  Switch system clock to PLL
+//	printf( "HSE Switched\n" );
+	Delay_Ms( 10 );
+	RCC->CTLR &= ~RCC_HSION;
+	Delay_Ms( 10 );
+	//printf( "HSI Off [%08lx %08lx]\n", RCC->CTLR, RCC->CFGR0 ); HSI Off [03035180 0001000a]
+
+	RCC->APB2PCENR |= 	RCC_APB2Periph_GPIOD | RCC_APB2Periph_GPIOC |
+						RCC_APB2Periph_TIM1;
+
+	RCC->AHBPCENR |= RCC_AHBPeriph_DMA1;
+
+	funPinMode( PC3, GPIO_CFGLR_OUT_50Mhz_AF_PP ); // T1C3 on PC3
+
+	RCC->APB2PRSTR |= RCC_APB2Periph_TIM1;
+	RCC->APB2PRSTR &= ~RCC_APB2Periph_TIM1;
+
+
+	// Prescaler 
+	TIM1->PSC = 0x0000;
+	
+	// Auto Reload - sets period
+	TIM1->ATRLR = 20;
+	
+	// Reload immediately
+	TIM1->SWEVGR |= TIM_UG;
+	
+	// Enable CH1N output, positive pol
+	TIM1->CCER |= TIM_CC3E;
+	TIM1->CCER |= TIM_CC1E;
+	
+	// Compare 3 = for output
+	// Modes:
+	//  0, 1, 2: Nothing
+	//  3: Flip
+	//  4, 5: Nothing
+	//  6: "Fast PWM mode 1"
+	//  7: Flipping (Further out)
+#ifdef FM_TRANSMITTER_SWEEP
+	TIM1->CHCTLR2 =  TIM_OC3M_0 | TIM_OC3M_1 | TIM_OC3PE | TIM_OC3FE;
+#else
+	TIM1->CHCTLR2 =  TIM_OC3M_2 | TIM_OC3M_1 | TIM_OC3PE | TIM_OC3FE;
+#endif
+
+	// Compare 1 = for triggering
+	TIM1->CHCTLR1 = TIM_OC1M_2 | TIM_OC1M_1;
+	
+	// Set the Capture Compare Register value to 50% initially
+	TIM1->CH3CVR = 5;  // ACTUALLY Ignored typically it seems.
+	TIM1->CH1CVR = 0; // This triggers DMA.
+	
+	// Enable TIM1 outputs
+	TIM1->BDTR |= TIM_MOE;
+	
+	// Enable TIM1
+	TIM1->CTLR1 |= TIM_CEN;
+
+	TIM1->DMAINTENR = TIM_TDE | TIM_COMDE | TIM_CC1DE | TIM_UDE;
+
+
+	//DMA1_Channel3 is for SPI1TX
+	DMA1_Channel2->PADDR = (uint32_t)&TIM1->ATRLR;
+	DMA1_Channel2->MADDR = (uint32_t)sendbuff;
+	DMA1_Channel2->CNTR  = 0;// sizeof( bufferset )/2; // Number of unique copies.  (Don't start, yet!)
+	DMA1_Channel2->CFGR  =
+		DMA_M2M_Disable |		 
+		DMA_Priority_VeryHigh |
+		DMA_PeripheralDataSize_HalfWord |
+		DMA_MemoryDataSize_Byte |
+		DMA_MemoryInc_Enable |
+		DMA_Mode_Circular | // OR DMA_Mode_Circular or DMA_Mode_Normal
+		DMA_DIR_PeripheralDST |
+		0;
+		//DMA_IT_TC | DMA_IT_HT; // Transmission Complete + Half Empty Interrupts. 
+
+//	NVIC_EnableIRQ( DMA1_Channel3_IRQn );
+
+		int j;
+		for( j = 0; j < sizeof( sendbuff ); j++ )
+		{
+			sendbuff[j] = 12;
+		}
+
+
+	// Enter critical section.
+	DMA1_Channel2->MADDR = (uint32_t)sendbuff;
+	DMA1_Channel2->CNTR = SENDBUFF_WORDS; // Number of unique uint16_t entries.
+	DMA1_Channel2->CFGR |= DMA_CFGR1_EN;
+
+	LoopFunction2();
+}
+
@@ -64,13 +64,13 @@ SOFTWARE.

 #define SH1107_128x128

+#define PWM_OUTPUT
+#define ENABLE_OLED
 #include "ssd1306_i2c.h"
 #include "ssd1306.h"


-#define ADC_BUFFSIZE 512
-
-#define GOERTZEL_BUFFER 8192
+#define ADC_BUFFSIZE 1024

 volatile uint16_t adc_buffer[ADC_BUFFSIZE];

@@ -79,23 +79,51 @@ volatile uint16_t adc_buffer[ADC_BUFFSIZE];
 //const int32_t g_goertzel_coefficient = 870249096;
 //const int32_t g_goertzel_coefficient_s = 1963250500;

-/*
+#if 0
+#define PWM_PERIOD (30-1)
+#define GOERTZEL_BUFFER (752)
+const int32_t g_goertzel_omega_per_sample = 2485087396; // 0.368351 of whole per step / 27.031915MHz
+const int32_t g_goertzel_coefficient = -1453756170;
+const int32_t g_goertzel_coefficient_s = 1580594514;
+#endif
+
+#if 0
+#define PWM_PERIOD (30-1)
+#define GOERTZEL_BUFFER (420)
+const int32_t g_goertzel_omega_per_sample = 5509657063; // 0.816667 of whole per step / 0.880000MHz
+const int32_t g_goertzel_coefficient = 873460290;
+const int32_t g_goertzel_coefficient_s = -1961823932;
+#endif
+
+#if 0
 #define PWM_PERIOD (31-1)
-const int32_t g_goertzel_omega_per_sample = 1228662895; // 0.182118 of whole per step / 27.025000MHz
-const int32_t g_goertzel_coefficient = 888414806;
-const int32_t g_goertzel_coefficient_s = 1955097223;
-*/
+#define GOERTZEL_BUFFER (412)
+const int32_t g_goertzel_omega_per_sample = 1670254667; // 0.247573 of whole per step / 1.150016MHz
+const int32_t g_goertzel_coefficient = 32748822;
+const int32_t g_goertzel_coefficient_s = 2147233926;
+#endif

-#define PWM_PERIOD (28-1)
-const int32_t g_goertzel_omega_per_sample = 1154616630; // 0.171143 of whole per step / 0.880162MHz
-const int32_t g_goertzel_coefficient = 1021021706;
-const int32_t g_goertzel_coefficient_s = 1889232832;
+#if 0
+#define PWM_PERIOD (30-1)
+#define GOERTZEL_BUFFER (576)
+const int32_t g_goertzel_omega_per_sample = 1264972285; // 0.187500 of whole per step / 90.300000MHz
+const int32_t g_goertzel_coefficient = 821806413;
+const int32_t g_goertzel_coefficient_s = 1984016189;

+#endif

+#if 1
+#define PWM_PERIOD (30-1)
+#define GOERTZEL_BUFFER (396)
+const int32_t g_goertzel_omega_per_sample = 5485805733; // 0.813131 of whole per step / 90.303030MHz
+const int32_t g_goertzel_coefficient = 829669840;
+const int32_t g_goertzel_coefficient_s = -1980740764;

+#endif

+int intensity_max = 1;

-#define LOG_GOERTZEL_LIST 256
+#define LOG_GOERTZEL_LIST 512
 int32_t gertzellogs[LOG_GOERTZEL_LIST*2];
 int     gertzellogs_head;

@@ -141,7 +169,7 @@ void SetupADC()
 	while(ADC1->CTLR2 & ADC_CAL);

 	// ADC_SCAN: Allow scanning.
-	ADC1->CTLR1 = /*ADC_Pga_64 | */ADC_SCAN;
+	ADC1->CTLR1 = ADC_Pga_64 | ADC_SCAN;


 	// Turn on DMA
@@ -188,17 +216,20 @@ static void SetupTimer1()
 	TIM1->ATRLR = PWM_PERIOD;

 #ifdef PWM_OUTPUT
-	// PA10 = T1CH3.
-	GPIOA->CFGHR &= ~(0xf<<(4*2));
-	GPIOA->CFGHR |= (GPIO_Speed_10MHz | GPIO_CNF_OUT_PP_AF)<<(4*2);
+	// PA9 = T1CH2.
+	GPIOA->CFGHR &= ~(0xf<<(4*1));
+	GPIOA->CFGHR |= (GPIO_Speed_10MHz | GPIO_CNF_OUT_PP_AF)<<(4*1);

-	TIM1->CCER = TIM_CC3E | TIM_CC3P;
-	TIM1->CHCTLR2 = TIM_OC3M_2 | TIM_OC3M_1;
-	TIM1->CH3CVR = 5;  // Actual duty cycle (Off to begin with)
+	TIM1->CCER = TIM_CC2E | TIM_CC2P;
+	TIM1->CHCTLR1 |= TIM_OC2M_2 | TIM_OC2M_1 | TIM_OC2FE;
+	TIM1->CH2CVR = 5;  // Actual duty cycle (Off to begin with)
+
+	// Enable TIM1 outputs
+	TIM1->BDTR |= 0xc000;//TIM_MOE;
 #endif

-	TIM1->CCER = TIM_CC1E;
-	TIM1->CHCTLR1 = TIM_OC1M_2 | TIM_OC1M_1;
+	TIM1->CCER |= TIM_CC1E;
+	TIM1->CHCTLR1 |= TIM_OC1M_2 | TIM_OC1M_1;
 	TIM1->CH1CVR = 1;

 	// Setup TRGO to trigger for ADC (NOTE: Not on the 203! TIM1_TRGO is only connected to injection)
@@ -319,6 +350,29 @@ void DMA1_Channel1_IRQHandler( void )
 				gertzellogs[gertzellogs_head++] = g_goertzelp2_store;
 				gertzellogs_head = gertzellogs_head & ((LOG_GOERTZEL_LIST*2)-1);

+
+
+				#ifdef PWM_OUTPUT
+				int32_t zp = g_goertzelp_store;
+				int32_t zp2 = g_goertzelp2_store;
+				int32_t rr = (((int64_t)(g_goertzel_coefficient  ) * (int64_t)zp<<1)>>32) - (zp2);
+				int32_t ri = (((int64_t)(g_goertzel_coefficient_s) * (int64_t)zp<<1)>>32);
+
+				//rr>>=1;
+				//ri>>=1;
+				rr = rr * PWM_PERIOD / (intensity_max>>7);
+				ri = ri * PWM_PERIOD / (intensity_max>>7);
+				int s = rr * rr + ri * ri;
+				int intensity = 1<<( ( 32 - __builtin_clz(s) )/2);
+				intensity = (intensity + s/intensity)/2;
+				intensity = (intensity + s/intensity)/2;
+				
+				if( intensity >= PWM_PERIOD ) intensity = PWM_PERIOD-1;
+				if( intensity < 0 ) intensity = 0;
+
+				TIM1->CH2CVR = intensity;  // Actual duty cycle (Off to begin with)
+				#endif
+
 				g_goertzel_outs++;
 				goertzel = g_goertzel_omega_per_sample>>(29-16);
 				goertzelp = 0;
@@ -342,8 +396,6 @@ void DMA1_Channel1_IRQHandler( void )

 void InnerLoop()
 {
-	int intensity_max = 1;
-
 	while(1){

 		int k;
@@ -374,17 +426,18 @@ void InnerLoop()
 			int32_t rr = (((int64_t)(g_goertzel_coefficient  ) * (int64_t)zp<<1)>>32) - (zp2);
 			int32_t ri = (((int64_t)(g_goertzel_coefficient_s) * (int64_t)zp<<1)>>32);

-			rr>>=4;
-			ri>>=4;
+			//rr>>=1;
+			//ri>>=1;

 			int s = rr * rr + ri * ri;
 			int intensity = 1<<( ( 32 - __builtin_clz(s) )/2);
 			intensity = (intensity + s/intensity)/2;
 			intensity = (intensity + s/intensity)/2;
-			if( intensity > intensity_max ) intensity_max = intensity;
+//			if( intensity > intensity_max ) intensity_max = intensity;
+			intensity_max = intensity_max - (intensity_max>>8) + intensity;

-			rr = rr * 64 / intensity_max;
-			ri = ri * 64 / intensity_max;
+			rr = rr * 64 / (intensity_max>>7);
+			ri = ri * 64 / (intensity_max>>7);

 			rr += 64;
 			ri += 64;
@@ -394,19 +447,18 @@ void InnerLoop()
 			if( rr > 127 ) rr = 127;
 			if( ri > 127 ) ri = 127;
 			
+#ifdef ENABLE_OLED
 			ssd1306_drawPixel( rr, ri, 1 );
+#endif
 		}

-		intensity_max = intensity_max - (intensity_max>>4);
-
+#ifdef ENABLE_OLED
 		ssd1306_refresh();
 		ssd1306_setbuf(0);

 		int32_t rr = (((int64_t)(g_goertzel_coefficient  ) * (int64_t)g_goertzelp_store<<1)>>32) - (g_goertzelp2_store); \
 		int32_t ri = (((int64_t)(g_goertzel_coefficient_s) * (int64_t)g_goertzelp_store<<1)>>32); \

-		rr>>=4;
-		ri>>=4;
 		int s = rr * rr + ri * ri;
 		int x = 1<<( ( 32 - __builtin_clz(s) )/2);
 		x = (x + s/x)/2;
@@ -414,7 +466,11 @@ void InnerLoop()

 		char cts[32];
 		snprintf( cts, 32, "%6d", x );
+
 		ssd1306_drawstr( 0, 0, cts, 1 );
+#else
+		Delay_Ms(17);
+#endif
 		
 //		printf( "%6d %8d %8d - %8d %8d - %8d\n", g_goertzel_outs,g_goertzelp2_store, g_goertzelp_store, rr, ri, x );

@@ -461,7 +517,7 @@ int main()

 	//printf( "CTLR: %08x / CFGR0: %08x\n", (RCC->CTLR), (RCC->CFGR0) );
 	RCC->AHBPCENR |= 3; //DMA2EN | DMA1EN
-	RCC->APB2PCENR |= RCC_APB2Periph_TIM1 | RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2 | 0x07; // Enable all GPIO
+	RCC->APB2PCENR |= RCC_APB2Periph_TIM1 | RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2 | 0x07 | RCC_APB2Periph_GPIOA; // Enable all GPIO
 	RCC->APB1PCENR |= RCC_APB1Periph_TIM2;

 	SetupADC();
@@ -23,12 +23,13 @@ function DrawSpan( colspan, freq, target, docolor, extrastr = "" )
 	return ret;
 }

-function Goertz( n, mhz, fr )
+function Goertz( n, mhz, fr, brf )
 {
 	let omega = fr * 3.1415926535*2.0;
 	var textarea = document.getElementById("goertzeloutput");
 	textarea.value = 
 		"#define PWM_PERIOD ("+n+"-1)\n" +
+		"#define GOERTZEL_BUFFER ("+brf+")\n" +
 		"const int32_t g_goertzel_omega_per_sample = " + ( omega*2*(1<<29)).toFixed(0) + "; // " + ( omega / (3.1415926535*2.0)).toFixed(6) + " of whole per step / " + mhz.toFixed(6) + "MHz\n" +
 		"const int32_t g_goertzel_coefficient = " + (2 * Math.cos( omega ) * (1<<30)).toFixed(0) + ";\n" +
 		"const int32_t g_goertzel_coefficient_s = "+ (2 * Math.sin( omega ) * (1<<30)).toFixed(0) + ";\n";
@@ -46,6 +47,7 @@ function computeTable()
 	let target = Number(document.getElementById("targetmhz").value );
 	let quadrature = document.getElementById("QUADRATURE").checked;
 	let quanta = Math.round(Number(document.getElementById("quanta").value));
+	let quantasearch = Math.round(Number(document.getElementById("quantasearch").value));
 

 	const max_harmonics = 28|0;
@@ -77,7 +79,7 @@ function computeTable()
 			"<TABLE BORDER=1>" +
 			"<TR><TD>Goertzel</TD></TR>" +
 			"<TR><TD>Goertzel (Inverse)</TD></TR>" +
-			"</TABLE><TEXTAREA ROWS=5 COLS=120 ID=goertzeloutput></TEXTAREA>";
+			"</TABLE><TEXTAREA ROWS=6 COLS=120 ID=goertzeloutput></TEXTAREA>";
 	}

 	contents += "<TABLE BORDER=1>";
@@ -94,20 +96,38 @@ function computeTable()
 		let goertzelpointinv = 0;
 		let tgoertzelp = 0;
 		let tgoertzelpi = 0;
+		let quantaA = 0;
+		let quantaINV = 0;
 		for( let h = 0|min_harmonics; h <= max_harmonics; h++ )
 		{
 			let base = freq * h;
 			let next = freq * (h+1);
-			if( target <= next && target >= base )
-			{
-				var t;
-				goertzelpoint = ( target - base ) / ( next - base );
-				goertzelpoint = Math.round(quanta * goertzelpoint)/quanta;

-				tgoertzelp = h;
-				goertzelpointinv = (1.0 - ( target - base ) / ( next - base ));
-				goertzelpointinv = Math.round(quanta * goertzelpointinv)/quanta;
-				tgoertzelpi = h+1;
+			// Round quanta down to next order-of-4-even
+			for( let tquanta = (quanta&0xffffc) - (quantasearch&0xffffc); tquanta < (quanta&0xffffc) + (quantasearch&0xffffc); tquanta+=4 )
+			{
+				if( target <= next && target >= base )
+				{
+					var t;
+					let tgoertzelpoint = ( target - base ) / ( next - base );
+					tgoertzelpoint = Math.round(tquanta * tgoertzelpoint)/tquanta;
+					if( Math.abs( freq*(h+tgoertzelpoint) - target ) < Math.abs( freq*(h+goertzelpoint) - target ) )
+					{
+						goertzelpoint = tgoertzelpoint;
+						tgoertzelp = h;
+						quantaA = tquanta;
+					}
+
+					let tgoertzelpointinv = (1.0 - ( target - base ) / ( next - base ));
+					tgoertzelpointinv = Math.round(tquanta * tgoertzelpointinv)/tquanta;
+
+					if( Math.abs( freq*(h-tgoertzelpointinv+1) - target ) < Math.abs( freq*(h-goertzelpointinv+1) - target ) )
+					{
+						goertzelpointinv = tgoertzelpointinv;
+						tgoertzelpi = h;
+						quantaINV = tquanta;
+					}
+				}
 			}
 		}
 		contents += "</TR>";
@@ -135,7 +155,7 @@ function computeTable()
 					if( mode == 0 )
 					{
 						contents += "<TD COLSPAN=2>"
-						if( tgoertzelp == h ) contents += "<SPAN ONCLICK='Goertz(" + n + ", " + freq * (h+goertzelpoint) + ", " + (goertzelpoint) + ")'>↑" + (goertzelpoint).toFixed(6) + "</SPAN>";
+						if( tgoertzelp == h ) contents += "<SPAN ONCLICK='Goertz(" + n + ", " + freq * (h+goertzelpoint) + ", " + (goertzelpoint) + ", " + quantaA + ")'>↑" + (goertzelpoint).toFixed(6) + "</SPAN>";
 						contents += "</TD>";
 					}
 					else if( mode == 1 )
@@ -145,7 +165,7 @@ function computeTable()
 					else if( mode == 2 )
 					{
 						contents += "<TD COLSPAN=2>"
-						if( tgoertzelp == h-1 ) contents += "<SPAN ONCLICK='Goertz(" + n + ", " + freq * (h-goertzelpointinv) + ", " + goertzelpointinv + ")'>↓" + goertzelpointinv.toFixed(6) + "</SPAN>";
+						if( tgoertzelpi == h-1  ) contents += "<SPAN ONCLICK='Goertz(" + n + ", " + freq * (h-goertzelpointinv) + ", " + goertzelpointinv + ", " + quantaINV + ")'>↓" + goertzelpointinv.toFixed(6) + "</SPAN>";
 						contents += "</TD>";
 					}
 					else if( mode == 3 )
@@ -171,6 +191,7 @@ function computeTable()
 <TR><TD>Crystal MHz</TD><TD><INPUT ID=crystalmhz VALUE=144></TD></TR>
 <TR><TD>Target MHz</TD><TD><INPUT ID=targetmhz VALUE=27.019360></TD></TR>
 <TR><TD>Quanta</TD><TD><INPUT ID=quanta VALUE=1024> (Goertzel's Algorithm Only)</TD></TR>
+<TR><TD>Quanta Search Range</TD><TD><INPUT ID=quantasearch VALUE=64> (Goertzel's Algorithm Only)</TD></TR>
 <TR><TD>Table Type</TD><TD><INPUT TYPE=RADIO ID=QUADRATURE NAME=computetype checked>Quadrature</INPUT><INPUT TYPE=RADIO ID=GOERTZELS NAME=computetype>Goertzels</INPUT></TD></TR>
 <TR><TD COLSPAN=2><INPUT TYPE=SUBMIT VALUE="Compute" ONCLICK="computeTable()"></TD></TR>
 </TABLE>