 
     // No need to ping if we're sending real data.
     ping_last = currentMillis;
-    
-    size_t len = Payload_SIZE;
-    uint8_t sbuf[len];
-    rx_payload.serialize(sbuf);
-
-    // push payload data to all connected clients
-    for(i = 0; i < MAX_SRV_CLIENTS; i++){
-      if (serverClients[i] && serverClients[i].connected()) {
-        if (!serverClients[i].write('\t')) { // Payload start byte
-          serverClients[i].stop();
-        }
-        else {
-          serverClients[i].write(sbuf, len);
-          serverClients[i].write('\n');
-        }
-      }
-    }
+    broadcast_udp();
   } 
-  // Keep TCP clients connected with a ping.
+  // Send the data continually, as its UDP some may get missed.
   else if (currentMillis - ping_last >= ping_interval) {
     ping_last = currentMillis;
-
-    ping_payload.setMsgId(++ping_msg_id);
-    //ping_payload.setF(1234);
-    // Send a ping message.
-    size_t len = Payload_SIZE;
-    uint8_t sbuf[len];
-    ping_payload.serialize(sbuf);
-
-    // push payload data to all connected clients
-    for(i = 0; i < MAX_SRV_CLIENTS; i++){
-      if (serverClients[i] && serverClients[i].connected()) {
-        if (!serverClients[i].write('\t')) { // Payload start byte
-          serverClients[i].stop();
-          Serial.print("Stopped: ");
-          Serial.println(i);
-        }
-        else {
-          serverClients[i].write(sbuf, len);
-          serverClients[i].write('\n');
-        }
-      }
-    }
+    broadcast_udp();
   }
-
 
   // Update the display  
   if (currentMillis - previousMillis >= interval) {


#include "config.h"
#include "SSD1306.h" // alias for `#include "SSD1306Wire.h"`
#include "Payload.h"
#include <ESP8266WiFi.h>

//how many clients should be able to telnet to this ESP8266
#define MAX_SRV_CLIENTS 2


WiFiServer server(23);
WiFiClient serverClients[MAX_SRV_CLIENTS];

// Initialize the OLED display using Wire library
SSD1306  display(0x3c, 4, 5);

Payload rx_payload = Payload();
Payload ping_payload = Payload();
uint8_t input_string[Payload_SIZE];
uint8_t payload_state = 0;
uint8_t serial_byte_count = 0;

  display.init();
  display.flipScreenVertically();
  display.setFont(ArialMT_Plain_16);

  rx_payload.setMsgId(254);
  rx_payload.setA(1234);
