Replace test heartbeat with transparent UART↔UDP MAVLink bridge
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 <noreply@anthropic.com>
This commit is contained in:
102
src/main.cpp
102
src/main.cpp
@ -3,18 +3,14 @@
|
||||
#include <WiFiUDP.h>
|
||||
#include <FastLED.h>
|
||||
#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);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user