Enhance MQTTMessageBuilder and MQTTBridge for improved packet handling and memory management

- Implemented size-adaptive JSON document allocation in MQTTMessageBuilder to reduce memory fragmentation.
- Increased buffer sizes for raw hex conversions to accommodate larger packets.
- Updated MQTTBridge to store raw radio data with each packet, improving data integrity during transmission.
- Enhanced packet queue management by clearing structures to prevent stale data and ensuring safe memory handling.
- Added debug logging for packet processing to aid in troubleshooting.

src/helpers/MQTTMessageBuilder.cpp

@@ -51,7 +51,15 @@ int MQTTMessageBuilder::buildPacketMessage(
   char* buffer,
   size_t buffer_size
 ) {
-  DynamicJsonDocument doc(1024);
+  // Size-adaptive JSON document: estimate needed size based on raw hex string length
+  // Base JSON overhead ~200 bytes, raw hex can be up to 510 chars (255 bytes packet)
+  // Use minimum needed to reduce memory fragmentation
+  size_t raw_len = raw ? strlen(raw) : 0;
+  size_t doc_size = 256 + raw_len + 128; // Base + raw + overhead, but cap at buffer_size
+  if (doc_size > buffer_size) doc_size = buffer_size;
+  if (doc_size < 512) doc_size = 512;   // Minimum for small packets
+  if (doc_size > 2048) doc_size = 2048; // Maximum cap
+  DynamicJsonDocument doc(doc_size);
   JsonObject root = doc.to<JsonObject>();
   
   root["origin"] = origin;
@@ -140,7 +148,8 @@ int MQTTMessageBuilder::buildPacketJSON(
   }
   
   // Convert packet to hex
-  char raw_hex[512];
+  // MAX_TRANS_UNIT is 255 bytes, hex = 510 chars, but allow for larger with headers
+  char raw_hex[1024];
   packetToHex(packet, raw_hex, sizeof(raw_hex));
   
   // Get packet characteristics
@@ -221,7 +230,8 @@ int MQTTMessageBuilder::buildPacketJSONFromRaw(
   }
   
   // Convert raw radio data to hex (this includes radio headers)
-  char raw_hex[512];
+  // MAX_TRANS_UNIT is 255 bytes, hex = 510 chars, but allow for larger with headers
+  char raw_hex[1024];
   bytesToHex(raw_data, raw_len, raw_hex, sizeof(raw_hex));
   
   // Get packet characteristics from the parsed packet
@@ -283,7 +293,8 @@ int MQTTMessageBuilder::buildRawJSON(
   }
   
   // Convert packet to hex
-  char raw_hex[512];
+  // MAX_TRANS_UNIT is 255, so max hex size is 510 chars + null = 511 bytes
+  char raw_hex[1024];
   packetToHex(packet, raw_hex, sizeof(raw_hex));
   
   return buildRawMessage(origin, origin_id, timestamp, raw_hex, buffer, buffer_size);
@@ -345,21 +356,15 @@ void MQTTMessageBuilder::bytesToHex(const uint8_t* data, size_t len, char* hex,
 }
 
 void MQTTMessageBuilder::packetToHex(mesh::Packet* packet, char* hex, size_t hex_size) {
-  // Convert entire packet to hex string
-  size_t total_len = packet->path_len + packet->payload_len + 2;
-  if (hex_size < total_len * 2 + 1) return;
+  // Serialize full on-air/wire format using Packet::writeTo()
+  // This includes header, transport codes (if present), path_len, path, and payload
+  uint8_t raw_buf[512];
+  uint8_t raw_len = packet->writeTo(raw_buf);
+  if (raw_len == 0 || raw_len > sizeof(raw_buf)) return;
   
-  size_t offset = 0;
+  // Check if hex buffer is large enough (2 hex chars per byte + null terminator)
+  if (hex_size < (size_t)raw_len * 2 + 1) return;
   
-  // Add path data
-  if (packet->path_len > 0) {
-    bytesToHex(packet->path, packet->path_len, hex + offset, hex_size - offset);
-    offset += packet->path_len * 2;
-  }
-  
-  // Add payload data
-  if (packet->payload_len > 0) {
-    bytesToHex(packet->payload, packet->payload_len, hex + offset, hex_size - offset);
-    offset += packet->payload_len * 2;
-  }
+  // Convert serialized packet to hex
+  bytesToHex(raw_buf, raw_len, hex, hex_size);
 }

src/helpers/bridges/MQTTBridge.cpp

@@ -82,6 +82,10 @@ MQTTBridge::MQTTBridge(NodePrefs *prefs, mesh::PacketManager *mgr, mesh::RTCCloc
   
   // Initialize packet queue
   memset(_packet_queue, 0, sizeof(_packet_queue));
+  // Initialize has_raw_data flags
+  for (int i = 0; i < MAX_QUEUE_SIZE; i++) {
+    _packet_queue[i].has_raw_data = false;
+  }
   
   // Set default broker configuration
   setBrokerDefaults();
@@ -235,6 +239,8 @@ void MQTTBridge::end() {
       _mgr->free(_packet_queue[index].packet);
       _packet_queue[index].packet = nullptr;
     }
+    // Clear the entire structure to free raw_data buffers
+    memset(&_packet_queue[index], 0, sizeof(QueuedPacket));
   }
   
   // Clear packet queue
@@ -242,6 +248,9 @@ void MQTTBridge::end() {
   _queue_head = 0;
   _queue_tail = 0;
   
+  // Clear all queue slots for safety
+  memset(_packet_queue, 0, sizeof(_packet_queue));
+  
   // Clean up timezone object to prevent memory leak
   if (_timezone) {
     delete _timezone;
@@ -367,7 +376,13 @@ void MQTTBridge::onPacketReceived(mesh::Packet *packet) {
     return;
   }
   
-  MQTT_DEBUG_PRINTLN("Packet received, queuing for transmission");
+  // Debug logging for packet types that might be getting filtered
+  uint8_t packet_type = packet->getPayloadType();
+  if (packet_type == 4 || packet_type == 9) {  // ADVERT or TRACE
+    MQTT_DEBUG_PRINTLN("Packet received: type=%d (ADVERT=%d, TRACE=%d), queuing for transmission", 
+                      packet_type, (packet_type == 4), (packet_type == 9));
+  }
+  
   // Queue packet for transmission
   queuePacket(packet, false);
 }
@@ -477,15 +492,20 @@ void MQTTBridge::processPacketQueue() {
   
   MQTT_DEBUG_PRINTLN("Processing packet queue - count: %d", _queue_count);
   
-  // Process up to 5 packets per loop to avoid blocking
+  // Process up to MAX_QUEUE_SIZE packets per loop to keep up with packet arrival rate
   int processed = 0;
-  while (_queue_count > 0 && processed < 5) {
+  int max_per_loop = MAX_QUEUE_SIZE; // Process all queued packets per loop
+  while (_queue_count > 0 && processed < max_per_loop) {
     QueuedPacket& queued = _packet_queue[_queue_head];
     
     MQTT_DEBUG_PRINTLN("Processing queued packet (is_tx: %s)", queued.is_tx ? "true" : "false");
     
-    // Publish packet
-    publishPacket(queued.packet, queued.is_tx);
+    // Publish packet (use stored raw data if available)
+    publishPacket(queued.packet, queued.is_tx, 
+                  queued.has_raw_data ? queued.raw_data : nullptr,
+                  queued.has_raw_data ? queued.raw_len : 0,
+                  queued.has_raw_data ? queued.snr : 0.0f,
+                  queued.has_raw_data ? queued.rssi : 0.0f);
     
     // Publish raw if enabled
     if (_raw_enabled) {
@@ -507,6 +527,7 @@ void MQTTBridge::processPacketQueue() {
 void MQTTBridge::publishStatus() {
   if (!isAnyBrokerConnected() || !_config_valid) return;
   
+  // Status messages are smaller, but use consistent buffer size
   char json_buffer[512];
   char origin_id[65];
   char timestamp[32];
@@ -569,28 +590,42 @@ void MQTTBridge::publishStatus() {
           }
 }
 
-void MQTTBridge::publishPacket(mesh::Packet* packet, bool is_tx) {
+void MQTTBridge::publishPacket(mesh::Packet* packet, bool is_tx, 
+                                const uint8_t* raw_data, int raw_len, 
+                                float snr, float rssi) {
   if (!packet) return;
   
-  char json_buffer[512];
+  // Size-adaptive buffer: estimate needed size based on packet size
+  // Most packets are <100 bytes (need ~400 byte JSON), large packets need ~1500 bytes
+  int packet_size = packet->getRawLength();
+  size_t json_buffer_size = (packet_size > 150) ? 2048 : 1024;
+  char json_buffer[2048]; // Always allocate max, but pass actual needed size to builders
   char origin_id[65];
   
   // Use actual device ID
   strncpy(origin_id, _device_id, sizeof(origin_id) - 1);
   origin_id[sizeof(origin_id) - 1] = '\0';
   
-  // Build packet message using raw radio data if available
+  // Build packet message using raw radio data if provided
+  // Use size-adaptive buffer size based on actual packet size
+  size_t buffer_size = (packet->getRawLength() > 150) ? 2048 : 1024;
   int len;
-  if (_last_raw_len > 0 && (millis() - _last_raw_timestamp) < 1000) {
-    // Use raw radio data (within 1 second of packet)
+  if (raw_data && raw_len > 0) {
+    // Use provided raw radio data
+    len = MQTTMessageBuilder::buildPacketJSONFromRaw(
+      raw_data, raw_len, packet, is_tx, _origin, origin_id, 
+      snr, rssi, _timezone, json_buffer, buffer_size
+    );
+  } else if (_last_raw_len > 0 && (millis() - _last_raw_timestamp) < 1000) {
+    // Fallback to global raw radio data (within 1 second of packet)
     len = MQTTMessageBuilder::buildPacketJSONFromRaw(
       _last_raw_data, _last_raw_len, packet, is_tx, _origin, origin_id, 
-      _last_snr, _last_rssi, _timezone, json_buffer, sizeof(json_buffer)
+      _last_snr, _last_rssi, _timezone, json_buffer, buffer_size
     );
   } else {
     // Fallback to reconstructed packet data
     len = MQTTMessageBuilder::buildPacketJSON(
-      packet, is_tx, _origin, origin_id, _timezone, json_buffer, sizeof(json_buffer)
+      packet, is_tx, _origin, origin_id, _timezone, json_buffer, buffer_size
     );
   }
   
@@ -616,13 +651,20 @@ void MQTTBridge::publishPacket(mesh::Packet* packet, bool is_tx) {
     char analyzer_topic[128];
     snprintf(analyzer_topic, sizeof(analyzer_topic), "meshcore/%s/%s/packets", _iata, _device_id);
     publishToAnalyzerServers(analyzer_topic, json_buffer, false);
+  } else {
+    // Debug: log when packet message building fails
+    uint8_t packet_type = packet->getPayloadType();
+    if (packet_type == 4 || packet_type == 9) {  // ADVERT or TRACE
+      MQTT_DEBUG_PRINTLN("Failed to build packet JSON for type=%d (len=%d), packet not published", packet_type, len);
+    }
   }
 }
 
 void MQTTBridge::publishRaw(mesh::Packet* packet) {
   if (!packet) return;
   
-  char json_buffer[512];
+  // Large packets need larger buffer for raw JSON too
+  char json_buffer[2048];
   char origin_id[65];
   
   // Use actual device ID
@@ -660,14 +702,36 @@ void MQTTBridge::publishRaw(mesh::Packet* packet) {
 
 void MQTTBridge::queuePacket(mesh::Packet* packet, bool is_tx) {
   if (_queue_count >= MAX_QUEUE_SIZE) {
-    // Queue full, remove oldest
+    // Queue full, remove oldest and free its memory
+    QueuedPacket& oldest = _packet_queue[_queue_head];
+    if (oldest.packet) {
+      MQTT_DEBUG_PRINTLN("Queue full, dropping oldest packet (queue size: %d)", _queue_count);
+      _mgr->free(oldest.packet);
+      oldest.packet = nullptr;
+    }
+    // dequeuePacket() will clear the structure
     dequeuePacket();
   }
   
   QueuedPacket& queued = _packet_queue[_queue_tail];
+  // Clear structure first to ensure clean state (removes any stale data)
+  memset(&queued, 0, sizeof(QueuedPacket));
+  
   queued.packet = packet;
   queued.timestamp = millis();
   queued.is_tx = is_tx;
+  queued.has_raw_data = false; // Default to false, set true if we have valid data
+  
+  // Capture current raw radio data if available (within 1 second window)
+  if (_last_raw_len > 0 && (millis() - _last_raw_timestamp) < 1000) {
+    if (_last_raw_len <= sizeof(queued.raw_data)) {
+      memcpy(queued.raw_data, _last_raw_data, _last_raw_len);
+      queued.raw_len = _last_raw_len;
+      queued.snr = _last_snr;
+      queued.rssi = _last_rssi;
+      queued.has_raw_data = true;
+    }
+  }
   
   _queue_tail = (_queue_tail + 1) % MAX_QUEUE_SIZE;
   _queue_count++;
@@ -676,6 +740,11 @@ void MQTTBridge::queuePacket(mesh::Packet* packet, bool is_tx) {
 void MQTTBridge::dequeuePacket() {
   if (_queue_count == 0) return;
   
+  // Clear the dequeued packet structure to free memory and prevent stale data
+  QueuedPacket& dequeued = _packet_queue[_queue_head];
+  memset(&dequeued, 0, sizeof(QueuedPacket));
+  dequeued.has_raw_data = false; // Explicitly set after memset
+  
   _queue_head = (_queue_head + 1) % MAX_QUEUE_SIZE;
   _queue_count--;
 }

src/helpers/bridges/MQTTBridge.h

@@ -84,6 +84,12 @@ private:
     mesh::Packet* packet;
     unsigned long timestamp;
     bool is_tx;
+    // Store raw radio data with each packet to avoid it being overwritten
+    uint8_t raw_data[256];
+    int raw_len;
+    float snr;
+    float rssi;
+    bool has_raw_data;
   };
   
   static const int MAX_QUEUE_SIZE = 10;
@@ -129,7 +135,9 @@ private:
   void connectToBrokers();
   void processPacketQueue();
   void publishStatus();
-  void publishPacket(mesh::Packet* packet, bool is_tx);
+  void publishPacket(mesh::Packet* packet, bool is_tx, 
+                     const uint8_t* raw_data = nullptr, int raw_len = 0, 
+                     float snr = 0.0f, float rssi = 0.0f);
   void publishRaw(mesh::Packet* packet);
   void queuePacket(mesh::Packet* packet, bool is_tx);
   void dequeuePacket();