Dither LO phase to reduce artifacts

src/main.c

@@ -83,6 +83,14 @@ static queue_t iq_queue;
 static uint8_t iq_queue_buffer[IQ_QUEUE_LEN][IQ_BLOCK_LEN];
 static size_t iq_queue_pos = 0;
 
+static uint32_t rnd = 0;
+
+inline static uint32_t rnd_next()
+{
+	rnd = rnd * 0x41c64e6d + 12345;
+	return rnd;
+}
+
 static void bias_set_delay(int delay)
 {
 	delay += 200;
@@ -236,25 +244,50 @@ static void adder_init()
 	pio_sm_set_enabled(pio1, 3, true);
 }
 
+#define STEP_BASE ((UINT_MAX + 1.0) / CLK_SYS_HZ)
+static uint32_t freq_step = 1;
+
 static void lo_generate(uint32_t *buf, double freq, uint32_t phase)
 {
-	static const double base = (UINT_MAX + 1.0) / CLK_SYS_HZ;
+	freq_step = STEP_BASE * freq;
 
-	uint32_t step = base * freq;
+	unsigned down = 4 + __builtin_clz(freq_step);
 
 	for (size_t i = 0; i < LO_WORDS; i++) {
-		unsigned bits = 0;
+		uint32_t bits = 0;
+		int shift = (rnd_next() >> down) - (rnd_next() >> down) + (rnd_next() >> down) -
+			    (rnd_next() >> down);
 
 		for (int j = 0; j < 32; j++) {
-			bits |= phase >> 31;
+			bits |= (phase + shift) >> 31;
 			bits <<= 1;
-			phase += step;
+			phase += freq_step;
 		}
 
 		buf[i] = bits;
 	}
 }
 
+static void lo_tweak(uint32_t *buf, uint32_t phase)
+{
+	static size_t i = 0;
+	uint32_t bits = 0;
+	unsigned down = 4 + __builtin_clz(freq_step);
+
+	phase += freq_step * i * 32;
+
+	for (int j = 0; j < 32; j++) {
+		int shift = (rnd_next() >> down) - (rnd_next() >> down) + (rnd_next() >> down) -
+			    (rnd_next() >> down);
+		bits |= (phase + shift) >> 31;
+		bits <<= 1;
+		phase += freq_step;
+	}
+
+	buf[i] = bits;
+	i = (i + 1) & (LO_WORDS - 1);
+}
+
 static void rx_lo_init(double req_freq, bool align)
 {
 	const double step_hz = (double)CLK_SYS_HZ / (4 << LO_BITS_DEPTH);
@@ -568,6 +601,9 @@ static void rf_rx(void)
 		if (queue_try_add(&iq_queue, &block)) {
 			iq_queue_pos = (iq_queue_pos + 1) & (IQ_QUEUE_LEN - 1);
 		}
+
+		lo_tweak(lo_cos, COS_PHASE);
+		lo_tweak(lo_sin, SIN_PHASE);
 	}
 }