diff --git a/src/PubSubClient.cpp b/src/PubSubClient.cpp
index ad5826f..6f7e64a 100755
--- a/src/PubSubClient.cpp
+++ b/src/PubSubClient.cpp
@@ -393,10 +393,14 @@ boolean PubSubClient::publish(const char* topic, const uint8_t* payload, unsigne
         // Leave room in the buffer for header and variable length field
         uint16_t length = MQTT_MAX_HEADER_SIZE;
         length = writeString(topic,buffer,length);
+
+        // Add payload
         uint16_t i;
         for (i=0;i<plength;i++) {
             buffer[length++] = payload[i];
         }
+
+        // Write the header 
         uint8_t header = MQTTPUBLISH;
         if (retained) {
             header |= 1;