diff --git a/lib/include/mqtt_config.h b/lib/include/mqtt_config.h
index d9b64c7..4932297 100644
--- a/lib/include/mqtt_config.h
+++ b/lib/include/mqtt_config.h
@@ -15,6 +15,8 @@
 #define MQTT_RECON_DEFAULT_MS       (10*1000)
 #define MQTT_POLL_READ_TIMEOUT_MS   (1000)
 
+#define MQTT_MSG_ID_INCREMENTAL     CONFIG_MQTT_MSG_ID_INCREMENTAL
+
 #if CONFIG_MQTT_BUFFER_SIZE
 #define MQTT_BUFFER_SIZE_BYTE       CONFIG_MQTT_BUFFER_SIZE
 #else
diff --git a/lib/include/mqtt_msg.h b/lib/include/mqtt_msg.h
index 78d6e40..a1dc612 100644
--- a/lib/include/mqtt_msg.h
+++ b/lib/include/mqtt_msg.h
@@ -70,8 +70,9 @@ typedef struct mqtt_message {
 
 typedef struct mqtt_connection {
     mqtt_message_t message;
-
-    uint16_t message_id;
+#if MQTT_MSG_ID_INCREMENTAL
+    uint16_t last_message_id;   /*!< last used id if incremental message id configured */
+#endif
     uint8_t *buffer;
     size_t buffer_length;
 
@@ -83,7 +84,7 @@ typedef struct mqtt_connect_info {
     char *password;
     char *will_topic;
     char *will_message;
-    int keepalive;          // keepalive=0 -> keepalive is disabled
+    int keepalive;          /*!< keepalive=0 -> keepalive is disabled */
     int will_length;
     int will_qos;
     int will_retain;
diff --git a/lib/mqtt_msg.c b/lib/mqtt_msg.c
index fcfe538..c884577 100644
--- a/lib/mqtt_msg.c
+++ b/lib/mqtt_msg.c
@@ -64,7 +64,11 @@ static uint16_t append_message_id(mqtt_connection_t *connection, uint16_t messag
     // If message_id is zero then we should assign one, otherwise
     // we'll use the one supplied by the caller
     while (message_id == 0) {
+#if MQTT_MSG_ID_INCREMENTAL
+        message_id = ++connection->last_message_id;
+#else
         message_id = platform_random(65535);
+#endif
     }
 
     if (connection->message.length + 2 > connection->buffer_length) {