Improve SNR, re-add frequency rounding

src/main.c

@@ -1,3 +1,4 @@
+#include <math.h>
 #include <pico/stdlib.h>
 #include <pico/stdio_usb.h>
 #include <pico/multicore.h>
@@ -21,7 +22,7 @@
 #include <stdlib.h>
 
 #define VREG_VOLTAGE VREG_VOLTAGE_1_20
-#define CLK_SYS_HZ (306 * MHZ)
+#define CLK_SYS_HZ (320 * MHZ)
 
 #define RX_PIN 10
 #define FB_PIN 11
@@ -35,7 +36,7 @@
 
 #define IQ_SAMPLES 32
 #define IQ_BLOCK_LEN (2 * IQ_SAMPLES)
-#define IQ_QUEUE_LEN 8
+#define IQ_QUEUE_LEN 16
 
 #define XOR_ADDR 0x1000
 #define LO_BITS_DEPTH 15
@@ -48,17 +49,17 @@
 static uint32_t lo_cos[LO_WORDS] __attribute__((__aligned__(1 << LO_BITS_DEPTH)));
 static uint32_t lo_sin[LO_WORDS] __attribute__((__aligned__(1 << LO_BITS_DEPTH)));
 
-#define DECIMATE 4
-
 #define INIT_SAMPLE_RATE 100000
 #define INIT_FREQ 94600000
 
-#define BASE_GAIN (1 << 15)
-#define ATTN_BITS 8
-
-static int sample_rate = INIT_SAMPLE_RATE;
-static int gain = BASE_GAIN / (CLK_SYS_HZ / INIT_SAMPLE_RATE / 2);
 static int frequency = INIT_FREQ;
+static int sample_rate = INIT_SAMPLE_RATE;
+
+#define BASE_GAIN (1 << 15)
+#define ATTN_BITS 10
+#define DECIMATE 4
+
+static int gain = BASE_GAIN / (CLK_SYS_HZ / INIT_SAMPLE_RATE);
 
 static queue_t iq_queue;
 static uint8_t iq_queue_buffer[IQ_QUEUE_LEN][IQ_BLOCK_LEN];
@@ -246,12 +247,11 @@ static void lo_generate_phase(uint32_t *buf, size_t len, uint32_t step, uint32_t
 	}
 }
 
-static void rx_lo_init(uint32_t freq)
+static void rx_lo_init(double freq)
 {
-	const uint32_t frac = (8 + 16llu * CLK_SYS_HZ / (8 << LO_BITS_DEPTH)) >> 4;
+	double frac = (double)CLK_SYS_HZ / (8 << LO_BITS_DEPTH);
-	uint32_t step = ((uint64_t)freq << 32) / (uint64_t)CLK_SYS_HZ;
+	freq = roundf(freq / frac) * frac;
-	step /= frac;
+	uint32_t step = freq * 4294967296.0 / CLK_SYS_HZ;
-	step *= frac;
 	lo_generate_phase(lo_cos, LO_WORDS, step, COS_PHASE);
 	lo_generate_phase(lo_sin, LO_WORDS, step, SIN_PHASE);
 }
@@ -434,7 +434,13 @@ inline static uint32_t pio_sm_get_blocking_unsafe(pio_hw_t *pio, int sm)
 	return pio->rxf[sm];
 }
 
-inline static int nextI()
+static int Ia1, Ia2, Ia3;
+static int Ib1, Ib2, Ib3;
+
+static int Qa1, Qa2, Qa3;
+static int Qb1, Qb2, Qb3;
+
+inline static void accI()
 {
 	static uint16_t py, px;
 
@@ -448,25 +454,14 @@ inline static int nextI()
 	py = y;
 	px = x;
 
-	int s = ((int)ny << 1) + (int)nx;
+	uint32_t s = (ny << 1) + nx;
-	int s0 = s;
 
-	static int a1, a2;
+	Ia1 += s;
-	s = s0 - a2;
+	Ia2 += Ia1;
-	a2 = a1;
+	Ia3 += Ia2;
-	a1 = s0;
-	s0 = s;
-
-	static int b1, b2;
-	s = s0 - b2;
-	b2 = b1;
-	b1 = s0;
-	s0 = s;
-
-	return s >> 2;
 }
 
-inline static int nextQ()
+inline static void accQ()
 {
 	static uint16_t py, px;
 
@@ -480,22 +475,43 @@ inline static int nextQ()
 	py = y;
 	px = x;
 
-	int s = ((int)ny << 1) + (int)nx;
+	uint32_t s = (ny << 1) + nx;
-	int s0 = s;
 
-	static int a1, a2;
+	Qa1 += s;
-	s = s0 - a2;
+	Qa2 += Qa1;
-	a2 = a1;
+	Qa3 += Qa2;
-	a1 = s0;
+}
-	s0 = s;
 
-	static int b1, b2;
+inline static int getI()
-	s = s0 - b2;
+{
-	b2 = b1;
+	uint32_t c1, c2, c3;
-	b1 = s0;
-	s0 = s;
 
-	return s >> 2;
+	c3 = Ia3 - Ib3;
+	Ib3 = Ia3;
+
+	c2 = c3 - Ib2;
+	Ib2 = c3;
+
+	c1 = c2 - Ib1;
+	Ib1 = c2;
+
+	return (int)c1;
+}
+
+inline static int getQ()
+{
+	uint32_t c1, c2, c3;
+
+	c3 = Qa3 - Qb3;
+	Qb3 = Qa3;
+
+	c2 = c3 - Qb2;
+	Qb2 = c3;
+
+	c1 = c2 - Qb1;
+	Qb1 = c2;
+
+	return (int)c1;
 }
 
 static void rf_rx(void)
@@ -511,28 +527,18 @@ static void rf_rx(void)
 		uint8_t *blockptr = block;
 
 		for (int i = 0; i < IQ_SAMPLES; i++) {
-			int I = 0, Q = 0;
+			for (int d = 0; d < DECIMATE; d++) {
+				accI();
+				accQ();
+			}
 
-			Q += nextQ();
+			int I = getI() * gain;
-			I += nextI();
+			int Q = getQ() * gain;
 
-			I -= nextQ();
-			Q += nextI();
-
-			Q -= nextQ();
-			I -= nextI();
-
-			I += nextQ();
-			Q -= nextI();
-
-			I *= gain;
 			I >>= ATTN_BITS;
-
 			*blockptr++ = I + 128;
 
-			Q *= gain;
 			Q >>= ATTN_BITS;
-
 			*blockptr++ = Q + 128;
 		}
 
@@ -550,13 +556,13 @@ static void run_command(uint8_t cmd, uint32_t arg)
 	if (0x01 == cmd) {
 		/* Tune to a new center frequency */
 		frequency = arg;
-		rx_lo_init(frequency + sample_rate);
+		rx_lo_init(frequency);
 	} else if (0x02 == cmd) {
 		/* Set the rate at which IQ sample pairs are sent */
 		sample_rate = arg;
-		gain = BASE_GAIN / (CLK_SYS_HZ / sample_rate / 2);
+		gain = BASE_GAIN / (CLK_SYS_HZ / sample_rate);
 		dma_timer_set_fraction(dma_t_samp, 1, CLK_SYS_HZ / (sample_rate * DECIMATE));
-		rx_lo_init(frequency + sample_rate);
+		rx_lo_init(frequency);
 	}
 }
 
@@ -647,7 +653,7 @@ int main()
 
 	queue_init(&iq_queue, sizeof(uint8_t *), IQ_QUEUE_LEN);
 
-	rx_lo_init(frequency + sample_rate);
+	rx_lo_init(frequency);
 
 	/* We need to have the sampling timer ready. */
 	dma_t_samp = dma_claim_unused_timer(true);