From c1adb06029bc0294e732cb8335126a8f66648376 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jan=20Hamal=20Dvo=C5=99=C3=A1k?= <mordae@anilinux.org>
Date: Sun, 14 Jan 2024 21:32:53 +0100
Subject: [PATCH] Display angle
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Jan Hamal Dvořák <mordae@anilinux.org>
---
 src/main.c | 39 ++++++++++++++++++++++++++++++++++++---
 1 file changed, 36 insertions(+), 3 deletions(-)

diff --git a/src/main.c b/src/main.c
index 6fb36a6..a4226e3 100644
--- a/src/main.c
+++ b/src/main.c
@@ -36,7 +36,7 @@
 
 #define CLK_SYS_HZ (250 * MHZ)
 
-#define EXTRA_BITS 3
+#define EXTRA_BITS 1
 #define NUM_SAMPLES 32
 #define RSSI_ALPHA 1
 #define LPF_SAMPLES 8
@@ -63,6 +63,7 @@ static volatile struct status {
 	unsigned rssi_max;
 	unsigned sample_rate;
 	int frequency;
+	int angle;
 	int I, Q;
 } status;
 
@@ -280,6 +281,32 @@ static float lo_freq_init(float req_freq)
 	return freq;
 }
 
+inline static __unused int cheap_atan2(int y, int x)
+{
+	static const int deg[2][2] = { { 192, 0 }, { 128, 64 } };
+	return deg[y < 0][x < 0];
+}
+
+inline static __unused int imaskmod(int x, int mask)
+{
+	return (x < 0) ? -((-x) & mask) : x & mask;
+}
+
+inline static __unused int cheap_angle_diff(int angle1, int angle2)
+{
+	int diff = angle2 - angle1;
+
+	diff = imaskmod(diff, 255);
+
+	if (diff > 128)
+		return diff - 256;
+
+	if (diff < -128)
+		return diff + 256;
+
+	return diff;
+}
+
 inline static __unused unsigned popcount(unsigned v)
 {
 	v = v - ((v >> 1) & 0x55555555);
@@ -390,6 +417,8 @@ static void rf_rx(void)
 	int delta_watermark = 0;
 	unsigned prev_transfers = 0;
 
+	int angle = 0;
+
 	while (true) {
 		int I = 0, Q = 0;
 
@@ -562,6 +591,8 @@ static void rf_rx(void)
 
 		prevQ = Q;
 
+		angle = (angle * 7 + cheap_angle_diff(angle, cheap_atan2(I, Q))) / 8;
+
 		unsigned ssi = I * I + Q * Q;
 		const unsigned alpha = RSSI_ALPHA;
 
@@ -572,6 +603,7 @@ static void rf_rx(void)
 		status.rssi_raw = assi2 / 16;
 		status.frequency = frequency;
 		status.sample_rate = CLK_SYS_HZ / (delta_watermark * 32);
+		status.angle = angle;
 		status.I = I;
 		status.Q = Q;
 		status.mtime = time_us_32();
@@ -692,8 +724,9 @@ static void command(const char *cmd)
 
 			float rssi_rel = (float)st.rssi_raw / (float)st.rssi_max;
 
-			printf("%5.1f dB (%4u) [%5u %+6i] ", 10.0f * log10f(rssi_rel),
-			       (unsigned)sqrt(st.rssi_raw), st.sample_rate, st.frequency);
+			printf("%5.1f dB (%4u) [%5u %+6i] %+4.0f ", 10.0f * log10f(rssi_rel),
+			       (unsigned)sqrt(st.rssi_raw), st.sample_rate, st.frequency,
+			       360.0f * st.angle / 256.0f);
 
 			plot_IQ(st.I, st.Q);
 			puts("");