From da73da4e38e62256aa68eb584c5594a157875bf1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jan=20Hamal=20Dvo=C5=99=C3=A1k?= <mordae@anilinux.org>
Date: Thu, 3 Jul 2025 21:45:25 +0200
Subject: [PATCH] Better filter, 16b samples

---
 src/main.c | 74 +++++++++++++++++++++++++++++++++++-------------------
 1 file changed, 48 insertions(+), 26 deletions(-)

diff --git a/src/main.c b/src/main.c
index 0ef69c2..221a1c4 100644
--- a/src/main.c
+++ b/src/main.c
@@ -141,8 +141,6 @@ static void init_bias()
 	gpio_set_drive_strength(FB_PIN, GPIO_DRIVE_STRENGTH_2MA);
 	gpio_set_slew_rate(FB_PIN, GPIO_SLEW_RATE_SLOW);
 
-	PIO->input_sync_bypass = 1u << RX_PIN;
-
 	const uint16_t insn[] = {
 		pio_encode_mov(pio_isr, pio_null),
 		pio_encode_in(pio_y, 4),
@@ -209,8 +207,8 @@ static void init_adder()
 		 * Should wrap here.
 		 * Jump to this portion must be inserted from the outside.
 		 */
-		pio_encode_in(pio_y, 32),
-		pio_encode_in(pio_x, 32),
+		pio_encode_in(pio_x, 16),
+		pio_encode_in(pio_y, 16),
 		pio_encode_out(pio_pc, 4),
 	};
 
@@ -438,42 +436,66 @@ inline static uint32_t pio_sm_get_blocking_unsafe(pio_hw_t *pio, int sm)
 
 inline static int nextI()
 {
-	static int prevI2, prevI1, prevI;
+	static uint16_t py, px;
 
-	int sI2 = pio_sm_get_blocking_unsafe(PIO, SM_COS);
+	uint32_t yx = pio_sm_get_blocking_unsafe(PIO, SM_COS);
+	uint16_t y = yx >> 16;
+	uint16_t x = yx;
 
-	int I2 = sI2 - prevI2;
-	prevI2 = sI2;
+	uint16_t ny = py - y;
+	uint16_t nx = px - x;
 
-	int sI1 = pio_sm_get_blocking_unsafe(PIO, SM_COS);
-	int I1 = sI1 - prevI1;
-	prevI1 = sI1;
+	py = y;
+	px = x;
 
-	int sI = I2 + I2 + I1;
-	int I = sI - prevI;
-	prevI = sI;
+	int s = ((int)ny << 1) + (int)nx;
+	int s0 = s;
 
-	return I;
+	static int a1, a2;
+	s = s0 - a2;
+	a2 = a1;
+	a1 = s0;
+	s0 = s;
+
+	static int b1, b2;
+	s = s0 - b2;
+	b2 = b1;
+	b1 = s0;
+	s0 = s;
+
+	return s >> 2;
 }
 
 inline static int nextQ()
 {
-	static int prevQ2, prevQ1, prevQ;
+	static uint16_t py, px;
 
-	int sQ2 = pio_sm_get_blocking_unsafe(PIO, SM_SIN);
+	uint32_t yx = pio_sm_get_blocking_unsafe(PIO, SM_SIN);
+	uint16_t y = yx >> 16;
+	uint16_t x = yx;
 
-	int Q2 = sQ2 - prevQ2;
-	prevQ2 = sQ2;
+	uint16_t ny = py - y;
+	uint16_t nx = px - x;
 
-	int sQ1 = pio_sm_get_blocking_unsafe(PIO, SM_SIN);
-	int Q1 = sQ1 - prevQ1;
-	prevQ1 = sQ1;
+	py = y;
+	px = x;
 
-	int sQ = Q2 + Q2 + Q1;
-	int Q = sQ - prevQ;
-	prevQ = sQ;
+	int s = ((int)ny << 1) + (int)nx;
+	int s0 = s;
 
-	return Q;
+	static int a1, a2;
+	s = s0 - a2;
+	a2 = a1;
+	a1 = s0;
+	s0 = s;
+
+	static int b1, b2;
+	s = s0 - b2;
+	b2 = b1;
+	b1 = s0;
+	s0 = s;
+
+	return s >> 2;
 }
 
 static void rf_rx(void)