diff --git a/Adafruit_MQTT.cpp b/Adafruit_MQTT.cpp
index 42fcf95..59fd9d9 100644
--- a/Adafruit_MQTT.cpp
+++ b/Adafruit_MQTT.cpp
@@ -303,7 +303,7 @@ bool Adafruit_MQTT::publish(const char *topic, const char *data, uint8_t qos) {
 
 bool Adafruit_MQTT::publish(const char *topic, uint8_t *data, uint16_t bLen, uint8_t qos) {
   // Construct and send publish packet.
-  uint16_t len = publishPacket(buffer, topic, data, bLen, qos);
+  uint16_t len = publishPacket(buffer, (uint16_t) sizeof(buffer), topic, data, bLen, qos);
   if (!sendPacket(buffer, len))
     return false;
 
@@ -637,20 +637,40 @@ uint8_t Adafruit_MQTT::connectPacket(uint8_t *packet) {
   return len;
 }
 
+uint16_t Adafruit_MQTT::packetAdditionalLen(uint16_t currLen)
+{
+  /* Increase length field based on current length */
+  if (currLen < 128) return 0;
+  if (currLen < 16384) return 1;
+  if (currLen < 2097151) return 2;
+  return 3;
+}
 
 // as per http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/os/mqtt-v3.1.1-os.html#_Toc398718040
-uint16_t Adafruit_MQTT::publishPacket(uint8_t *packet, const char *topic,
+uint16_t Adafruit_MQTT::publishPacket(uint8_t *packet, uint16_t maxPacketLen, const char *topic,
                                      uint8_t *data, uint16_t bLen, uint8_t qos) {
   uint8_t *p = packet;
-  uint16_t len=0;
+  uint16_t len=2;           // control + length
 
-  // calc length of non-header data
-  len += 2;               // two bytes to set the topic size
-  len += strlen(topic); // topic length
+  len += strlen(topic);
   if(qos > 0) { 
     len += 2; // qos packet id
   }
-  len += bLen; // payload length
+  // calculate additional bytes for length field (if any)
+  uint16_t additionalLen = packetAdditionalLen(len + bLen);
+
+  // payload length
+  if (len + bLen + 2 + additionalLen <= maxPacketLen) {
+    len += bLen + additionalLen;
+  } else {
+    // If we make it here, we got a pickle: the payload is not going
+    // to fit in the packet buffer. Instead of corrupting memory, let's
+    // do something less damaging by reducing the bLen to what we are
+    // able to accomodate. Alternatively, consider using a bigger
+    // maxPacketLen.
+    bLen = maxPacketLen - (len + 2 + packetAdditionalLen(maxPacketLen));
+    len = maxPacketLen - 4;
+  }
 
   // Now you can start generating the packet!
   p[0] = MQTT_CTRL_PUBLISH << 4 | qos << 1;
@@ -684,7 +704,8 @@ uint16_t Adafruit_MQTT::publishPacket(uint8_t *packet, const char *topic,
   memmove(p, data, bLen);
   p+= bLen;
   len = p - packet;
-  DEBUG_PRINTLN(F("MQTT publish packet:"));
+  DEBUG_PRINT(F("MQTT publish packet (length: "));
+  DEBUG_PRINT(len); DEBUG_PRINTLN(F("):"));
   DEBUG_PRINTBUFFER(buffer, len);
   return len;
 }
diff --git a/Adafruit_MQTT.h b/Adafruit_MQTT.h
index dc229df..6c1daec 100644
--- a/Adafruit_MQTT.h
+++ b/Adafruit_MQTT.h
@@ -243,11 +243,13 @@ class Adafruit_MQTT {
   // Functions to generate MQTT packets.
   uint8_t connectPacket(uint8_t *packet);
   uint8_t disconnectPacket(uint8_t *packet);
-  uint16_t publishPacket(uint8_t *packet, const char *topic, uint8_t *payload, uint16_t bLen, uint8_t qos);
+  uint16_t publishPacket(uint8_t *packet, uint16_t maxPacketLen,
+                         const char *topic, uint8_t *payload, uint16_t bLen, uint8_t qos);
   uint8_t subscribePacket(uint8_t *packet, const char *topic, uint8_t qos);
   uint8_t unsubscribePacket(uint8_t *packet, const char *topic);
   uint8_t pingPacket(uint8_t *packet);
   uint8_t pubackPacket(uint8_t *packet, uint16_t packetid);
+  static uint16_t packetAdditionalLen(uint16_t currLen);
 };