From e090ef14bc8f04c3f73d7a423660db614829372c Mon Sep 17 00:00:00 2001 From: nicoboy Date: Sun, 21 Jun 2026 12:54:01 +0200 Subject: [PATCH] =?UTF-8?q?Replace=20test=20heartbeat=20with=20transparent?= =?UTF-8?q?=20UART=E2=86=94UDP=20MAVLink=20bridge?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit H743 FC is now wired to Serial2 (RX=GPIO18, TX=GPIO17, 57600 baud). ESP32 becomes a raw byte relay: UART→UDP and UDP→UART on port 14550. Removed fake heartbeat generator — real heartbeat comes from H743. Added byte counters with periodic stats log every 10s. Co-Authored-By: Claude Sonnet 4.6 --- src/main.cpp | 102 +++++++++++++++++++++------------------------------ 1 file changed, 42 insertions(+), 60 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 068e984..961ed3a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -3,18 +3,14 @@ #include #include #include "credentials.h" -#include "mavlink_heartbeat.h" // ── Mode : true = ESP32 crée son propre hotspot false = connexion au Jetson ── #define MODE_AP false // ── Ports ──────────────────────────────────────────────── -const uint16_t LOCAL_PORT = 14550; -const uint16_t JETSON_PORT_MAVSDK = 14551; -const uint16_t JETSON_PORT_PYMAV = 14552; -const uint16_t JETSON_PORT_HB = 14553; +const uint16_t UDP_PORT = 14550; -// ── UART vers Pi Zero ───────────────────────────────────── +// ── UART vers H743 (Matek FC) ──────────────────────────── #define UART_BAUD 57600 #define UART_RX_PIN 18 #define UART_TX_PIN 17 @@ -24,7 +20,7 @@ const uint16_t JETSON_PORT_HB = 14553; #define NUM_LEDS 1 CRGB leds[NUM_LEDS]; -typedef enum { LED_WAITING_WIFI, LED_HEARTBEAT, LED_IDLE } LedState; +typedef enum { LED_WAITING_WIFI, LED_ACTIVITY, LED_IDLE } LedState; LedState currentLedState = LED_WAITING_WIFI; unsigned long ledStateStart = 0; @@ -32,15 +28,18 @@ unsigned long ledStateStart = 0; WiFiUDP udp; HardwareSerial MavSerial(2); bool wifiConnected = false; -unsigned long lastHeartbeat = 0; -uint8_t mavSeq = 0; #if MODE_AP - const char* BROADCAST_IP = "192.168.4.255"; + const char* TARGET_IP = "192.168.4.255"; #else - const char* BROADCAST_IP = JETSON_IP; + const char* TARGET_IP = JETSON_IP; #endif +// ── Compteurs de bytes relayés ──────────────────────────── +uint32_t bytesUartToUdp = 0; +uint32_t bytesUdpToUart = 0; +unsigned long lastStatsTime = 0; + // ── LED non bloquante ───────────────────────────────────── void updateLED() { unsigned long elapsed = millis() - ledStateStart; @@ -48,8 +47,8 @@ void updateLED() { case LED_WAITING_WIFI: leds[0] = CRGB((uint8_t)(128 + 127 * sin(elapsed * 0.00628f)), 0, 0); break; - case LED_HEARTBEAT: - if (elapsed < 80) { + case LED_ACTIVITY: + if (elapsed < 50) { leds[0] = CRGB(0, 255, 0); } else { currentLedState = LED_IDLE; @@ -68,25 +67,6 @@ void setLedState(LedState s) { ledStateStart = millis(); } -// ── Heartbeat MAVLink ───────────────────────────────────── -void sendMavlinkHeartbeat() { - uint8_t buf[17]; - uint8_t len = mavlinkBuildHeartbeat(buf, mavSeq); - - udp.beginPacket(BROADCAST_IP, JETSON_PORT_HB); - udp.write(buf, len); - udp.endPacket(); - - setLedState(LED_HEARTBEAT); - - Serial.printf("[HB] seq=%d [", mavSeq - 1); - for (uint8_t i = 0; i < len; i++) { - Serial.printf("%02X", buf[i]); - if (i < len - 1) Serial.print(" "); - } - Serial.println("]"); -} - // ── Reconnexion WiFi (mode STA uniquement) ──────────────── void checkWiFi() { #if !MODE_AP @@ -94,7 +74,7 @@ void checkWiFi() { if (!wifiConnected) { wifiConnected = true; WiFi.setSleep(false); - udp.begin(LOCAL_PORT); + udp.begin(UDP_PORT); setLedState(LED_IDLE); Serial.print("WiFi connecte. IP: "); Serial.println(WiFi.localIP()); @@ -118,7 +98,7 @@ void setup() { FastLED.setBrightness(80); setLedState(LED_WAITING_WIFI); - Serial.println("\n=== WireClaw ESP32-S3 ==="); + Serial.println("\n=== WireClaw ESP32-S3 — MAVLink Bridge ==="); #if MODE_AP Serial.println("Mode: AP (hotspot autonome)"); #else @@ -126,14 +106,14 @@ void setup() { #endif MavSerial.begin(UART_BAUD, SERIAL_8N1, UART_RX_PIN, UART_TX_PIN); - Serial.printf("UART2: %d baud (RX=%d TX=%d)\n", UART_BAUD, UART_RX_PIN, UART_TX_PIN); + Serial.printf("UART: %d baud (RX=GPIO%d TX=GPIO%d)\n", UART_BAUD, UART_RX_PIN, UART_TX_PIN); #if MODE_AP WiFi.mode(WIFI_AP); WiFi.softAP(AP_SSID, AP_PASSWORD); delay(500); wifiConnected = true; - udp.begin(LOCAL_PORT); + udp.begin(UDP_PORT); setLedState(LED_IDLE); Serial.printf("Hotspot: %s\n", AP_SSID); Serial.printf("IP : %s\n", WiFi.softAPIP().toString().c_str()); @@ -144,9 +124,8 @@ void setup() { Serial.printf("Connexion a %s...\n", WIFI_SSID); #endif - Serial.printf("UDP ecoute: port %d\n", LOCAL_PORT); + Serial.printf("UDP port: %d -> %s:%d\n", UDP_PORT, TARGET_IP, UDP_PORT); delay(500); - if (wifiConnected) sendMavlinkHeartbeat(); } // ── Loop ────────────────────────────────────────────────── @@ -154,25 +133,9 @@ void loop() { updateLED(); checkWiFi(); - if (wifiConnected && millis() - lastHeartbeat > 1000) { - lastHeartbeat = millis(); - sendMavlinkHeartbeat(); - } - if (!wifiConnected) return; - // WiFi → UART (Jetson → Pi Zero) - int packetSize = udp.parsePacket(); - if (packetSize > 0) { - uint8_t buf[512]; - int len = udp.read(buf, sizeof(buf)); - if (len > 0) { - MavSerial.write(buf, len); - Serial.printf("[WiFi->UART] %d bytes\n", len); - } - } - - // UART → WiFi (Pi Zero → Jetson) + // UART → UDP (H743 → Jetson) if (MavSerial.available()) { uint8_t buf[512]; int len = 0; @@ -180,13 +143,32 @@ void loop() { buf[len++] = MavSerial.read(); } if (len > 0) { - udp.beginPacket(BROADCAST_IP, JETSON_PORT_MAVSDK); + udp.beginPacket(TARGET_IP, UDP_PORT); udp.write(buf, len); udp.endPacket(); - udp.beginPacket(BROADCAST_IP, JETSON_PORT_PYMAV); - udp.write(buf, len); - udp.endPacket(); - Serial.printf("[UART->WiFi] %d bytes\n", len); + bytesUartToUdp += len; + setLedState(LED_ACTIVITY); + Serial.printf("[UART->UDP] %d bytes\n", len); } } + + // UDP → UART (Jetson → H743) + int packetSize = udp.parsePacket(); + if (packetSize > 0) { + uint8_t buf[512]; + int len = udp.read(buf, sizeof(buf)); + if (len > 0) { + MavSerial.write(buf, len); + bytesUdpToUart += len; + setLedState(LED_ACTIVITY); + Serial.printf("[UDP->UART] %d bytes\n", len); + } + } + + // Stats toutes les 10 secondes + if (millis() - lastStatsTime > 10000) { + lastStatsTime = millis(); + Serial.printf("[STATS] UART->UDP: %lu bytes | UDP->UART: %lu bytes\n", + (unsigned long)bytesUartToUdp, (unsigned long)bytesUdpToUart); + } }