nicoboy/wireclaw_esp32-S3
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Parse MAVLink frame boundaries before UDP send

Browse Source 
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>
This commit is contained in:
nicoboy
2026-06-21 16:32:15 +02:00
parent a6ba463c4c
commit e193a0b31c
1 changed files with 74 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

97
src/main.cpp
View File

@ -40,12 +40,15 @@ uint32_t bytesUartToUdp = 0;
uint32_t bytesUdpToUart = 0; uint32_t bytesUdpToUart = 0;
unsigned long lastStatsTime = 0; unsigned long lastStatsTime = 0;
// ── Buffer UART → UDP ──────────────────────────────────── // ── Parser MAVLink UART → UDP (variables) ─────────────────
#define UART_BUF_SIZE 64 #define MAV_BUF_SIZE 300
#define UART_FLUSH_MS 3 uint8_t mavBuf[MAV_BUF_SIZE];
uint8_t uartBuf[UART_BUF_SIZE]; int mavBufLen = 0;
int uartBufLen = 0; int mavExpectedLen = 0;
unsigned long uartLastByteTime = 0;
enum MavParseState { MAV_WAIT_STX, MAV_HEADER, MAV_PAYLOAD };
MavParseState mavState = MAV_WAIT_STX;
uint8_t mavVersion = 0;
// ── LED non bloquante ───────────────────────────────────── // ── LED non bloquante ─────────────────────────────────────
void updateLED() { void updateLED() {
@ -74,6 +77,68 @@ void setLedState(LedState s) {
ledStateStart = millis(); 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) ──────────────── // ── Reconnexion WiFi (mode STA uniquement) ────────────────
void checkWiFi() { void checkWiFi() {
#if !MODE_AP #if !MODE_AP
@ -142,23 +207,9 @@ void loop() {
if (!wifiConnected) return; if (!wifiConnected) return;
// UART → UDP (H743 → Jetson) — accumulation puis flush // UART → UDP (H743 → Jetson) — parser MAVLink frame-aware
while (MavSerial.available() && uartBufLen < UART_BUF_SIZE) { while (MavSerial.available()) {
uartBuf[uartBufLen++] = MavSerial.read(); mavParseByte(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;
} }
// UDP → UART (Jetson → H743) // UDP → UART (Jetson → H743)