From 67b918731223ca43937d4d9247fc91d59ce29230 Mon Sep 17 00:00:00 2001
From: Vitalii Popov <symbx.live@gmail.com>
Date: Thu, 15 May 2025 03:08:50 +0300
Subject: [PATCH] add distance processing

---
 src/main.c | 94 ++++++++++++++++++++++++++++++++++++++++++++++++++----
 1 file changed, 87 insertions(+), 7 deletions(-)

diff --git a/src/main.c b/src/main.c
index 6b970b2..e1fd71c 100644
--- a/src/main.c
+++ b/src/main.c
@@ -10,6 +10,9 @@
 // UART buffer size
 #define BUFFER_LEN				48
 
+#define AMOUNT_DISTANCE_MEASURE 8
+#define ACTIVATION_DISTANCE     40
+
 // LED pin info
 #define LED_PIN_DIR				PORT_C.DDR.Pin3
 #define LED_PIN_CR1				PORT_C.CR1.Pin3
@@ -28,6 +31,11 @@
 #define TX_PIN_CR2				PORT_D.CR2.Pin5
 #define TX_PIN_OUT				PORT_D.ODR.Pin5
 
+#define PRESS_ACTIVATION_TIME   30 * 4
+#define SENSE_ACTIVATION_TIME   PRESS_ACTIVATION_TIME
+
+const char *RANGE = "Range ";
+
 // Time defines
 #define SHORT_PRESS_MS          25
 #define LONG_PRESS_MS           1000
@@ -48,8 +56,10 @@ volatile u08 tx_buffer[BUFFER_LEN];
 volatile u08 tx_pos = 0;
 volatile u08 tx_len = 0;
 
-volatile u08 state = 0;
+u08 state = 0;
 u08 led_state = 0;
+u16 last_distance[AMOUNT_DISTANCE_MEASURE] = {0};
+u08 distance_count = 0;
 
 /**
  * Hang processor with big delay and hope for watchdog to kick-in or directly
@@ -119,6 +129,9 @@ void send_some_command(void) {
 	start_cmd(3);
 }
 
+/**
+ * Initialization state process
+ */
 void initialize_state(void) {
 	switch (state) {
 		case 0:
@@ -169,6 +182,25 @@ void turn_off(void) {
 	LED_PIN_OUT = 0;
 }
 
+/**
+ * Calculate average distance from last N measurements
+ * Note: returns 0 if no measurements done
+ */
+u16 avg_distance(void) {
+	u16 sum = 0;
+	u08 cnt = 0;
+	for (u08 i = 0; i < AMOUNT_DISTANCE_MEASURE; i++) {
+		sum += last_distance[i];
+		if (last_distance[i] > 0) {
+			cnt++;
+		}
+	}
+	if (cnt > 0) {
+		return sum / cnt;
+	}
+	return 0;
+}
+
 /**
  * Tick event handler
  */
@@ -183,6 +215,12 @@ void tick_250ms(void) {
 	if (timer > 0) {
 		timer--;
 	}
+	if (timer == 0) {
+		u16 distance = avg_distance();
+		if (distance != 0 && distance < ACTIVATION_DISTANCE) {
+			timer = SENSE_ACTIVATION_TIME;
+		}
+	}
 	// If we still have time
 	// note: timer can be below zero to work without timer
 	if (timer != 0) {
@@ -199,11 +237,20 @@ void tick_250ms(void) {
  */
 int is_rx_finished(void) {
 	// head + len + cmd + ver + tail
-	if (rx_pos < 4 + 2 + 2 + 2 + 4) {
+	if (rx_pos < 6 || (rx_buffer[0] != RANGE[0] && rx_pos < 4 + 2 + 2 + 2 + 4)) {
 		return false;
 	}
 	// Check head, tail and just in case - length
-	return rx_buffer[0] == 0xFD &&
+	return 
+	      (rx_buffer[0] == RANGE[0] &&
+		   rx_buffer[1] == RANGE[1] &&
+		   rx_buffer[2] == RANGE[2] &&
+		   rx_buffer[3] == RANGE[3] &&
+		   rx_buffer[4] == RANGE[4] &&
+		   rx_buffer[5] == RANGE[5] &&
+		  (rx_buffer[rx_pos - 1] == '\r' || 
+		   rx_buffer[rx_pos - 1] == '\n')) ||
+	      (rx_buffer[0] == 0xFD &&
 	       rx_buffer[1] == 0xFC &&
 		   rx_buffer[2] == 0xFB &&
 		   rx_buffer[3] == 0xFA &&
@@ -211,7 +258,39 @@ int is_rx_finished(void) {
 		   rx_buffer[rx_pos - 2] == 0x02 &&
 		   rx_buffer[rx_pos - 3] == 0x03 &&
 		   rx_buffer[rx_pos - 4] == 0x04 &&
-		   (rx_buffer[4] | (rx_buffer[5] << 8)) == rx_pos - 12;
+		   (rx_buffer[4] | (rx_buffer[5] << 8)) == rx_pos - 12);
+}
+
+/**
+ * Add distance measurement to list
+ */
+void add_distance(u16 distance) {
+	last_distance[distance_count] = distance;
+	distance_count++;
+	if (distance_count >= AMOUNT_DISTANCE_MEASURE) {
+		distance_count = 0;
+	}
+}
+
+/**
+ * Process input from UART
+ */
+void process_rx(void) {
+	if (rx_buffer[0] == RANGE[0]) {
+        u16 distance = 0;
+		for (u08 i = 6; i <= rx_pos; i++) {
+			if (rx_buffer[i] >= '0' && rx_buffer[i] <= '9') {
+				distance = distance * 10 + (rx_buffer[i] - '0');
+			} else {
+				break;
+			}
+		}
+		if (distance != 0) {
+			add_distance(distance);
+		}
+	} else {
+		// TODO: Process command packet
+	}
 }
 
 interrupt(IRQ_UART1_RX_F, uart_recv) {
@@ -222,7 +301,7 @@ interrupt(IRQ_UART1_RX_F, uart_recv) {
 	}
 
 	if (rx_pos < 4) {
-		if (rx_pos == 0xFD - c) {
+		if (rx_pos == 0xFD - c || RANGE[rx_pos] == c) {
 			rx_buffer[rx_pos] = c;
 		} else {
 			// Something wrong - reset
@@ -232,7 +311,8 @@ interrupt(IRQ_UART1_RX_F, uart_recv) {
 	} else {
 		rx_buffer[rx_pos] = c;
 		if (is_rx_finished()) {
-			// TODO: Process
+			process_rx();
+			// Reset buffer
 			rx_pos = 0;
 			memset(rx_buffer, 0, BUFFER_LEN);
 			return;
@@ -267,7 +347,7 @@ interrupt(IRQ_EXTI_C, touch) {
 			timer = -1;
 		} else if (time > SHORT_PRESS_MS) {
 			// If it was short touch set timer for enough time
-			timer = 30 * 4; // 30 seconds * 4 (250ms tick)
+			timer = PRESS_ACTIVATION_TIME; // 30 seconds * 4 (250ms tick)
 		}
 	}
 }