From e193a0b31caac1968d8a7cc46d6f42b7131b9c39 Mon Sep 17 00:00:00 2001
From: nicoboy <contact@nicolasboyer.fr>
Date: Sun, 21 Jun 2026 16:32:15 +0200
Subject: [PATCH] Parse MAVLink frame boundaries before UDP send
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Replace fixed-size buffer flush with a state machine that detects
MAVLink v1 (0xFE) and v2 (0xFD) frame boundaries. Each complete
frame is sent as exactly one UDP packet — no more fragmentation.
Handles v2 signature extension (incompat flag bit 0).

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
---
 src/main.cpp | 97 +++++++++++++++++++++++++++++++++++++++-------------
 1 file changed, 74 insertions(+), 23 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index 4c3675e..6bd394b 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -40,12 +40,15 @@ uint32_t bytesUartToUdp = 0;
 uint32_t bytesUdpToUart = 0;
 unsigned long lastStatsTime = 0;
 
-// ── Buffer UART → UDP ────────────────────────────────────
-#define UART_BUF_SIZE   64
-#define UART_FLUSH_MS   3
-uint8_t uartBuf[UART_BUF_SIZE];
-int uartBufLen = 0;
-unsigned long uartLastByteTime = 0;
+// ── Parser MAVLink UART → UDP (variables) ─────────────────
+#define MAV_BUF_SIZE  300
+uint8_t mavBuf[MAV_BUF_SIZE];
+int mavBufLen = 0;
+int mavExpectedLen = 0;
+
+enum MavParseState { MAV_WAIT_STX, MAV_HEADER, MAV_PAYLOAD };
+MavParseState mavState = MAV_WAIT_STX;
+uint8_t mavVersion = 0;
 
 // ── LED non bloquante ─────────────────────────────────────
 void updateLED() {
@@ -74,6 +77,68 @@ void setLedState(LedState s) {
   ledStateStart = millis();
 }
 
+// ── Parser MAVLink UART → UDP (fonctions) ─────────────────
+void mavFlush() {
+  udp.beginPacket(TARGET_IP, UDP_PORT);
+  udp.write(mavBuf, mavBufLen);
+  udp.endPacket();
+  bytesUartToUdp += mavBufLen;
+  setLedState(LED_ACTIVITY);
+  Serial.printf("[UART->UDP] %d bytes\n", mavBufLen);
+  mavBufLen = 0;
+  mavState = MAV_WAIT_STX;
+}
+
+void mavReset() {
+  mavBufLen = 0;
+  mavState = MAV_WAIT_STX;
+}
+
+void mavParseByte(uint8_t b) {
+  switch (mavState) {
+
+    case MAV_WAIT_STX:
+      if (b == 0xFD) {
+        mavVersion = 2;
+      } else if (b == 0xFE) {
+        mavVersion = 1;
+      } else {
+        return;
+      }
+      mavBuf[0] = b;
+      mavBufLen = 1;
+      mavState = MAV_HEADER;
+      break;
+
+    case MAV_HEADER:
+      mavBuf[mavBufLen++] = b;
+      if (mavVersion == 2 && mavBufLen == 10) {
+        uint8_t payloadLen = mavBuf[1];
+        uint8_t incompatFlags = mavBuf[2];
+        mavExpectedLen = 10 + payloadLen + 2;
+        if (incompatFlags & 0x01) mavExpectedLen += 13;
+        mavState = MAV_PAYLOAD;
+      } else if (mavVersion == 1 && mavBufLen == 6) {
+        uint8_t payloadLen = mavBuf[1];
+        mavExpectedLen = 6 + payloadLen + 2;
+        mavState = MAV_PAYLOAD;
+      }
+      break;
+
+    case MAV_PAYLOAD:
+      mavBuf[mavBufLen++] = b;
+      if (mavBufLen >= mavExpectedLen) {
+        mavFlush();
+        return;
+      }
+      if (mavBufLen >= MAV_BUF_SIZE) {
+        Serial.printf("[MAV] buffer overflow (%d), resync\n", mavBufLen);
+        mavReset();
+      }
+      break;
+  }
+}
+
 // ── Reconnexion WiFi (mode STA uniquement) ────────────────
 void checkWiFi() {
 #if !MODE_AP
@@ -142,23 +207,9 @@ void loop() {
 
   if (!wifiConnected) return;
 
-  // UART → UDP (H743 → Jetson) — accumulation puis flush
-  while (MavSerial.available() && uartBufLen < UART_BUF_SIZE) {
-    uartBuf[uartBufLen++] = MavSerial.read();
-    uartLastByteTime = millis();
-  }
-
-  bool flushNow = uartBufLen > 0 &&
-    (uartBufLen >= UART_BUF_SIZE || millis() - uartLastByteTime >= UART_FLUSH_MS);
-
-  if (flushNow) {
-    udp.beginPacket(TARGET_IP, UDP_PORT);
-    udp.write(uartBuf, uartBufLen);
-    udp.endPacket();
-    bytesUartToUdp += uartBufLen;
-    setLedState(LED_ACTIVITY);
-    Serial.printf("[UART->UDP] %d bytes\n", uartBufLen);
-    uartBufLen = 0;
+  // UART → UDP (H743 → Jetson) — parser MAVLink frame-aware
+  while (MavSerial.available()) {
+    mavParseByte(MavSerial.read());
   }
 
   // UDP → UART (Jetson → H743)