MAVLink Hub: Cross-Forwarding Between Connected Drones

frontend/dronebridge.js

@@ -20,6 +20,17 @@ function change_radio_dis_arm_visibility() {
 	}
 }
 
+function change_mav_blacklist_visibility() {
+	let mav_broadcast = document.getElementById("mav_broadcast");
+	let mav_broadcast_div = document.getElementById("mav_broadcast_div");
+	let mav_blacklist_div = document.getElementById("mav_blacklist_div");
+	if (mav_broadcast_div.style.display !== "none" && mav_broadcast.checked) {
+		mav_blacklist_div.style.display = "block";
+	} else {
+		mav_blacklist_div.style.display = "none";
+	}
+}
+
 function change_ap_ip_visibility() {
 	const esp32Mode = document.getElementById("esp32_mode").value;
 	const elements = {
@@ -31,8 +42,15 @@ function change_ap_ip_visibility() {
 		wifi_en_gn_div: document.getElementById("wifi_en_gn_div"),
 		static_ip_config_div: document.getElementById("static_ip_config_div"),
 		pass_div: document.getElementById("pass_div"),
+		mav_broadcast_div: document.getElementById("mav_broadcast_div"),
 	};
 
+	if (esp32Mode === "1" || esp32Mode === "3") {
+		elements.mav_broadcast_div.style.display = "block";
+	} else {
+		elements.mav_broadcast_div.style.display = "none";
+	}
+
 	if (esp32Mode === "2") {
 		elements.ap_ip_div.style.display = "none";
 		elements.ap_channel_div.style.display = "none";
@@ -69,6 +87,7 @@ function change_ap_ip_visibility() {
 	} else {
 		elements.wifi_ssid_div.style.visibility = "visible";
 	}
+	change_mav_blacklist_visibility();
 	change_radio_dis_arm_visibility();
 }
 

frontend/index.html

@@ -296,6 +296,20 @@ <h3 style="margin-top: 2rem">Serial</h3>
                     </div>
                 </div>
             </div>
+            <div id="mav_broadcast_div" class="row">
+                <div class="twelve columns">
+                    <div class="checkbox-wrapper-14">
+                        <input id="mav_broadcast" name="mav_broadcast" type="checkbox" class="switch" onchange="change_mav_blacklist_visibility()" checked>
+                        <label class="tooltip" for="mav_broadcast"><span class="tooltiptext">When enabled, the ESP32 acts as a MAVLink Hub, forwarding messages between all connected drones. Disable this to reduce network load if you have multiple drones and only need data in the GCS.</span>MAVLink Hub (Cross-Forwarding)</label>
+                    </div>
+                </div>
+            </div>
+            <div id="mav_blacklist_div" class="row" style="display: none;">
+                <div class="twelve columns">
+                    <label class="tooltip" for="mav_blacklist"><span class="tooltiptext">Comma separated list of MAVLink System IDs to exclude from forwarding. Heartbeats are always forwarded. e.g. 1,5,10</span>MAVLink Hub Blacklist (System IDs)</label>
+                    <input class="u-full-width" type="text" placeholder="1,5,10" id="mav_blacklist" name="mav_blacklist">
+                </div>
+            </div>
         </div>
     </form>
     <div class="row">

main/db_esp32_control.c

@@ -44,6 +44,7 @@
 #include "main.h"
 #include "db_serial.h"
 #include "db_esp_now.h"
+#include "db_mavlink_msgs.h"
 
 #define TAG "DB_CONTROL"
 
@@ -168,8 +169,68 @@ int db_open_int_telemetry_udp_socket() {
  * @param data Buffer with the data to send
  * @param data_length Length of the data in the buffer
  */
+/**
+ * Checks if a MAVLink system ID is in the blacklist
+ * @param system_id The system ID to check
+ * @return true if blacklisted, false otherwise
+ */
+bool is_system_id_blacklisted(uint8_t system_id) {
+    char *blacklist = DB_PARAM_MAV_BLACKLIST;
+    if (blacklist == NULL || blacklist[0] == '\0') return false;
+
+    char temp_blacklist[DB_PARAM_VALUE_MAXLEN];
+    strncpy(temp_blacklist, blacklist, sizeof(temp_blacklist));
+    temp_blacklist[sizeof(temp_blacklist)-1] = '\0';
+
+    char *token = strtok(temp_blacklist, ",");
+    while (token != NULL) {
+        if (atoi(token) == (int)system_id) return true;
+        token = strtok(NULL, ",");
+    }
+    return false;
+}
+
+/**
+ * Sends the data to all registered UDP clients.
+ * @param n_udp_conn_list The list of UDP clients
+ * @param data The data to be sent
+ * @param data_length Length of the data in the buffer
+ */
 void db_send_to_all_udp_clients(const uint8_t *data, uint data_length) {
+    // Simple MAVLink Sniffer to identify Heartbeats
+    bool is_heartbeat = false;
+    if (data_length >= 8) {
+        if (data[0] == 0xFE) { // MAVLink v1
+            if (data[5] == 0) is_heartbeat = true;
+        } else if (data[0] == 0xFD && data_length >= 10) { // MAVLink v2
+            if (data[7] == 0 && data[8] == 0 && data[9] == 0) is_heartbeat = true;
+        }
+    }
+
     for (int i = 0; i < udp_conn_list->size; i++) {  // send to all UDP clients
+        // If Hub mode is OFF and we are in AP mode:
+        // 1. Always allow Heartbeats
+        // 2. Always allow clients with no MAC (manually saved static IPs, e.g. a GCS configured via the UI)
+        //    since they were never registered via the WiFi STA event and start with is_gcs=false after reboot.
+        // 3. Only allow other telemetry to reach clients identified as GCS at runtime (SysID 255).
+        if (!is_heartbeat && !DB_PARAM_MAV_BROADCAST && (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_AP || DB_PARAM_RADIO_MODE == DB_WIFI_MODE_AP_LR)) {
+            bool has_mac = false;
+            for (int m = 0; m < 6; m++) {
+                if (udp_conn_list->db_udp_clients[i].mac[m] != 0) {
+                    has_mac = true;
+                    break;
+                }
+            }
+            if (has_mac && !udp_conn_list->db_udp_clients[i].is_gcs) {
+                continue; // Skip: WiFi STA that has not identified itself as a GCS
+            }
+        }
+
+        // Apply Blacklist: Skip if system ID is blacklisted and it's not a Heartbeat
+        if (!is_heartbeat && is_system_id_blacklisted(udp_conn_list->db_udp_clients[i].system_id)) {
+            continue;
+        }
+
         int sent = sendto(udp_conn_list->udp_socket, data, data_length, 0,
                           (struct sockaddr *) &udp_conn_list->db_udp_clients[i].udp_client,
                           sizeof(udp_conn_list->db_udp_clients[i].udp_client));
@@ -384,6 +445,12 @@ add_to_known_udp_clients(udp_conn_list_t *n_udp_conn_list, struct db_udp_client_
         if ((n_udp_conn_list->db_udp_clients[i].udp_client.sin_port == new_db_udp_client.udp_client.sin_port) &&
             (n_udp_conn_list->db_udp_clients[i].udp_client.sin_addr.s_addr ==
              new_db_udp_client.udp_client.sin_addr.s_addr)) {
+            // Update GCS status if the new packet identifies it as GCS
+            if (new_db_udp_client.is_gcs && !n_udp_conn_list->db_udp_clients[i].is_gcs) {
+                n_udp_conn_list->db_udp_clients[i].is_gcs = true;
+                ESP_LOGI(TAG, "Identified existing UDP client as GCS");
+            }
+            n_udp_conn_list->db_udp_clients[i].system_id = new_db_udp_client.system_id;
             return false; // client existing - do not add
         }
     }
@@ -394,7 +461,7 @@ add_to_known_udp_clients(udp_conn_list_t *n_udp_conn_list, struct db_udp_client_
     char ip_string[INET_ADDRSTRLEN];
     inet_ntop(AF_INET, &(new_db_udp_client.udp_client.sin_addr), ip_string, INET_ADDRSTRLEN);
     sprintf(ip_port_string, "%s:%d", ip_string, htons (new_db_udp_client.udp_client.sin_port));
-    ESP_LOGI(TAG, "Added %s to udp client distribution list - save to NVM: %i", ip_port_string, save_to_nvm);
+    ESP_LOGI(TAG, "Added %s to udp client distribution list (is_gcs: %i) - save to NVM: %i", ip_port_string, new_db_udp_client.is_gcs, save_to_nvm);
     // save to memory
     if (save_to_nvm) {
         save_udp_client_to_nvm(&new_db_udp_client, false);
@@ -502,7 +569,7 @@ _Noreturn void control_module_esp_now() {
             if (DB_PARAM_SERIAL_PROTO == DB_SERIAL_PROTOCOL_MAVLINK) {
                 // Parse, so we can listen in and react to certain messages - function will send parsed messages to serial link.
                 // We can not write to serial first since we might inject packets and do not know when to do so to not "destroy" an existing packet
-                db_parse_mavlink_from_radio(NULL, NULL, db_espnow_uart_evt.data, db_espnow_uart_evt.data_len);
+                db_parse_mavlink_from_radio(NULL, NULL, db_espnow_uart_evt.data, db_espnow_uart_evt.data_len, true);
             } else {
                 // no parsing with any other protocol - transparent here - just pass through
                 write_to_serial(db_espnow_uart_evt.data, db_espnow_uart_evt.data_len);
@@ -676,7 +743,7 @@ _Noreturn void control_module_udp_tcp() {
                     if (DB_PARAM_SERIAL_PROTO == DB_SERIAL_PROTOCOL_MAVLINK) {
                         // Parse, so we can listen in and react to certain messages - function will send parsed messages to serial link.
                         // We can not write to serial first since we might inject packets and do not know when to do so to not "destroy" an existign packet
-                        db_parse_mavlink_from_radio(connected_tcp_clients, udp_conn_list, tcp_client_buffer, recv_length);
+                        db_parse_mavlink_from_radio(connected_tcp_clients, udp_conn_list, tcp_client_buffer, recv_length, true);
                     } else {
                         // no parsing with any other protocol - transparent here
                         write_to_serial(tcp_client_buffer, recv_length);
@@ -701,23 +768,77 @@ _Noreturn void control_module_udp_tcp() {
                                        (struct sockaddr *) &new_db_udp_client.udp_client, &udp_socklen);
         if (recv_length > 0) {
             data_processed = true;
-            if (DB_PARAM_SERIAL_PROTO == DB_SERIAL_PROTOCOL_MAVLINK) {
-                // Parse, so we can listen in and react to certain messages - function will send parsed messages to serial link.
-                // We can not write to serial first since we might inject packets and do not know when to do so to not "destroy" an existing packet
-                db_parse_mavlink_from_radio(connected_tcp_clients, udp_conn_list, udp_buffer, recv_length);
-            } else {
-                // no parsing with any other protocol - transparent here
-                write_to_serial(udp_buffer, recv_length);
+
+            // Simple MAVLink Sniffer to identify GCS and Heartbeats
+            bool is_gcs_packet = false;
+            bool is_heartbeat = false;
+            uint8_t source_sys_id = 0;
+            if (recv_length >= 8) {
+                if (udp_buffer[0] == 0xFE) { // MAVLink v1
+                    source_sys_id = udp_buffer[3];
+                    if (source_sys_id == 255) is_gcs_packet = true;
+                    if (udp_buffer[5] == 0) is_heartbeat = true;
+                } else if (udp_buffer[0] == 0xFD && recv_length >= 10) { // MAVLink v2
+                    source_sys_id = udp_buffer[5];
+                    if (source_sys_id == 255) is_gcs_packet = true;
+                    if (udp_buffer[7] == 0 && udp_buffer[8] == 0 && udp_buffer[9] == 0) is_heartbeat = true;
+                }
             }
+            new_db_udp_client.system_id = source_sys_id;
+            new_db_udp_client.is_gcs = is_gcs_packet;
+
             // all devices that send us UDP data will be added to the list of UDP receivers
             // Allows to register new app on different port. Used e.g. for UDP conn setup in sta-mode.
-            // Devices/Ports added this way cannot be removed in sta-mode since UDP is connectionless, and we cannot
-            // determine if the client is still existing. This will blow up the list connected devices.
-            // In AP-Mode the devices can be removed based on the IP/MAC address
+            // Register client before forwarding so that target checks (is_gcs) work for responses
             add_to_known_udp_clients(udp_conn_list, new_db_udp_client, false);
+
+            if (DB_PARAM_SERIAL_PROTO == DB_SERIAL_PROTOCOL_MAVLINK) {
+                // Parse, so we can listen in and react to certain messages - function will send parsed messages to serial link if allowed.
+                // Packets from other drones are parsed (for ESP32 params) but not pushed to local FC UART if Hub is disabled.
+                // Apply blacklist to serial: if the local FC's SysID is blacklisted, only heartbeats reach it.
+                // GCS packets (SysID 255) always bypass the blacklist so commands always reach the FC.
+                bool should_forward_to_serial = (is_gcs_packet || DB_PARAM_MAV_BROADCAST || is_heartbeat)
+                                                && (is_heartbeat || is_gcs_packet || !is_system_id_blacklisted(db_get_mav_sys_id()));
+                db_parse_mavlink_from_radio(connected_tcp_clients, udp_conn_list, udp_buffer, recv_length, should_forward_to_serial);
+
+                // Forward radio data to all other network clients (MAVLink Router/Hub functionality)
+                // This ensures STAs can see each other and the GCS can see all STAs
+                // Uses Split-Horizon: Do not send back to source.
+                // Only applies to MAVLink mode since hub logic relies on MAVLink header sniffing.
+                if (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_AP || DB_PARAM_RADIO_MODE == DB_WIFI_MODE_AP_LR) {
+                    for (int i = 0; i < udp_conn_list->size; i++) {
+                        // Skip the source client
+                        if (udp_conn_list->db_udp_clients[i].udp_client.sin_addr.s_addr == new_db_udp_client.udp_client.sin_addr.s_addr &&
+                            udp_conn_list->db_udp_clients[i].udp_client.sin_port == new_db_udp_client.udp_client.sin_port) {
+                            continue;
+                        }
+
+                        // Forwarding Logic:
+                        // 1. Always forward Heartbeats (Msg ID 0) to everyone to keep links alive.
+                        // 2. Always forward everything from a GCS (SysID 255).
+                        // 3. If Hub mode is ON: Forward everything (unless blacklisted).
+                        // 4. If Hub mode is OFF: Only forward to identified GCS clients.
+                        if (is_heartbeat || is_gcs_packet || DB_PARAM_MAV_BROADCAST || udp_conn_list->db_udp_clients[i].is_gcs) {
+                            // Apply blacklist: skip blacklisted targets (heartbeats and GCS packets always pass through)
+                            if (!is_heartbeat && !is_gcs_packet && is_system_id_blacklisted(udp_conn_list->db_udp_clients[i].system_id)) {
+                                continue;
+                            }
+                            sendto(udp_conn_list->udp_socket, udp_buffer, recv_length, 0,
+                                   (struct sockaddr *)&udp_conn_list->db_udp_clients[i].udp_client,
+                                   sizeof(struct sockaddr_in));
+                        }
+                    }
+                }
+            } else {
+                // no parsing with any other protocol - only forward if it's from GCS or Hub is enabled
+                if (is_gcs_packet || DB_PARAM_MAV_BROADCAST || is_heartbeat) {
+                    write_to_serial(udp_buffer, recv_length);
+                }
+            }
         } else {
             // received nothing, keep on going
         }
+
         if (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_STA) {
             handle_internal_telemetry(db_internal_telem_udp_sock, udp_buffer, &udp_socklen,
                                       &new_db_udp_client.udp_client);
@@ -810,7 +931,7 @@ _Noreturn void control_module_ble() {
                 // Parse, so we can listen in and react to certain messages - function will send parsed messages to serial link.
                 // We cannot write to serial first since we might inject packets and do not know when to do so to not "destroy" an
                 // existing packet
-                db_parse_mavlink_from_radio(NULL, NULL, bleData.data, bleData.data_len);
+                db_parse_mavlink_from_radio(NULL, NULL, bleData.data, bleData.data_len, true);
             } else {
                 // no parsing with any other protocol - transparent here - just pass through
                 write_to_serial(bleData.data, bleData.data_len);

main/db_esp32_control.h

@@ -33,6 +33,8 @@
 struct db_udp_client_t {
     uint8_t mac[6];     // MAC address of connected client
     struct sockaddr_in udp_client;    // socket address (IP & PORT) of connected client
+    bool is_gcs;        // true if this client has sent a MAVLink packet from System ID 255 (GCS)
+    uint8_t system_id;  // The last seen MAVLink System ID of this client
 };
 
 typedef struct udp_conn_list_s {

main/db_parameters.c

@@ -46,7 +46,7 @@ uint8_t DB_RADIO_MODE_DESIGNATED = DB_WIFI_MODE_AP; // initially assign the same
 /* ---------- String based parameters - not available via MAVLink ---------- */
 
 db_parameter_t db_param_ssid, db_param_pass, db_param_wifi_ap_ip, db_param_wifi_sta_ip, db_param_wifi_sta_gw,
-    db_param_wifi_sta_netmask, db_param_udp_client_ip, db_param_wifi_hostname = {0};
+    db_param_wifi_sta_netmask, db_param_udp_client_ip, db_param_wifi_hostname, db_param_mav_blacklist = {0};
 
 /* ---------- From here with increasing param_index all parameters that are also available via MAVLink ---------- */
 
@@ -414,6 +414,28 @@ db_parameter_t db_param_rssi_dbm = {
         }
 };
 
+/**
+ * Enable/Disable cross-forwarding of MAVLink messages between clients (Hub mode)
+ */
+db_parameter_t db_param_mav_broadcast = {
+        .db_name = "mav_broadcast",
+        .type = UINT8,
+        .mav_t = {
+                .param_name = "SYS_MAV_HUB_EN",
+                .param_index = 20,
+                .param_type = MAV_PARAM_TYPE_UINT8,
+        },
+        .value = {
+                .db_param_u8 = {
+                        .value = true,
+                        .default_value = true,
+                        .min = false,
+                        .max = true,
+                }
+        }
+};
+
+
 /**
  * Array containing all references to the DB parameters assigned with db_param_init_parameters()
  */
@@ -492,6 +514,8 @@ void db_param_init_parameters() {
     db_param_udp_client_ip = db_param_init_str_param("udp_client_ip", "WIFI_UDP_IP", "", 0, IP4ADDR_STRLEN_MAX);
     // Specifies the hostname. Used in Wi-Fi ap & client mode.
     db_param_wifi_hostname = db_param_init_str_param("wifi_hostname", "WIFI_HOSTNAME", CONFIG_LWIP_LOCAL_HOSTNAME, 1, 32);
+    // MAVLink system ID blacklist for cross-forwarding (comma separated list of IDs)
+    db_param_mav_blacklist = db_param_init_str_param("mav_blacklist", "MAV_BLACKLIST", "", 0, DB_PARAM_VALUE_MAXLEN);
 
     db_parameter_t *db_params_l[] = {
             &db_param_ssid,
@@ -502,6 +526,7 @@ void db_param_init_parameters() {
             &db_param_wifi_sta_netmask,
             &db_param_udp_client_ip,
             &db_param_wifi_hostname,
+            &db_param_mav_blacklist,
             &db_param_radio_mode,
             &db_param_channel,
             &db_param_wifi_en_gn,
@@ -518,7 +543,8 @@ void db_param_init_parameters() {
             &db_param_ltm_per_packet,
             &db_param_dis_radio_armed,
             &db_param_udp_client_port,
-            &db_param_rssi_dbm
+            &db_param_rssi_dbm,
+            &db_param_mav_broadcast
     };
     memcpy(db_params, db_params_l, sizeof(db_params_l));
 }