diff --git a/IFW_MQTT_Server_Functions.ttcn b/IFW_MQTT_Server_Functions.ttcn
index 2e92e95..3f927c5 100644
--- a/IFW_MQTT_Server_Functions.ttcn
+++ b/IFW_MQTT_Server_Functions.ttcn
@@ -67,10 +67,58 @@
   {
     ctx := p_ctx;
   }
+  
+  function f_MQTT_getPacketId(in MQTT_v3_1_1_ReqResp p_msg)
+  return integer
+  {
+         if (ischosen(p_msg.publish))     { return p_msg.publish.packet_identifier }
+    else if (ischosen(p_msg.puback))      { return p_msg.puback.packet_identifier }
+    else if (ischosen(p_msg.pubrec))      { return p_msg.pubrec.packet_identifier }
+    else if (ischosen(p_msg.pubrel))      { return p_msg.pubrel.packet_identifier }
+    else if (ischosen(p_msg.pubcomp))     { return p_msg.pubcomp.packet_identifier }
+    else if (ischosen(p_msg.subscribe))   { return p_msg.subscribe.packet_identifier }
+    else if (ischosen(p_msg.suback))      { return p_msg.suback.packet_identifier }
+    else if (ischosen(p_msg.unsubscribe)) { return p_msg.unsubscribe.packet_identifier }
+    else if (ischosen(p_msg.unsuback))    { return p_msg.unsuback.packet_identifier }
+    else {
+      return -1;
+    }
+  }
+  
+  function f_MQTT_setPacketId(inout MQTT_v3_1_1_ReqResp p_msg, integer p_id)
+  return boolean
+  {
+         if (ischosen(p_msg.publish))     { p_msg.publish.packet_identifier := p_id; }
+    else if (ischosen(p_msg.puback))      { p_msg.puback.packet_identifier := p_id; }
+    else if (ischosen(p_msg.pubrec))      { p_msg.pubrec.packet_identifier := p_id; }
+    else if (ischosen(p_msg.pubrel))      { p_msg.pubrel.packet_identifier := p_id; }
+    else if (ischosen(p_msg.pubcomp))     { p_msg.pubcomp.packet_identifier := p_id; }
+    else if (ischosen(p_msg.subscribe))   { p_msg.subscribe.packet_identifier := p_id; }
+    else if (ischosen(p_msg.suback))      { p_msg.suback.packet_identifier := p_id; }
+    else if (ischosen(p_msg.unsubscribe)) { p_msg.unsubscribe.packet_identifier := p_id; }
+    else if (ischosen(p_msg.unsuback))    { p_msg.unsuback.packet_identifier := p_id; }
+    else { return false; }
+    
+    return true;
+  }
 	
-  function f_MQTT_Server_setMessageToSend(in MQTT_v3_1_1_ReqResp p_msg) runs on IFW_MQTT_Server_CT
+  function f_MQTT_Server_setMessageToSend(in MQTT_v3_1_1_ReqResp p_msg, boolean p_msgIdFromLastReceived := false) runs on IFW_MQTT_Server_CT
   {
     msgToSend := p_msg;
+
+    if (p_msgIdFromLastReceived)
+    {
+      var integer lastReceivedPacketId := f_MQTT_getPacketId(lastReceived);
+      
+      if (lastReceivedPacketId != -1)
+      {
+        if (not f_MQTT_setPacketId(msgToSend, lastReceivedPacketId))
+	{
+	  log("MQTT_Server: couldn't set packet id in msgToSend: ", msgToSend)
+	}
+      }
+      else { log("MQTT_Server: couldn't fetch packet id from lastReceived: ",lastReceived) };
+    }
   }	
 	
   function f_MQTT_Server_send() runs on IFW_MQTT_Server_CT
diff --git a/IFW_MQTT_Server_TestSteps.ttcn b/IFW_MQTT_Server_TestSteps.ttcn
index d2e97ec..0da01a0 100644
--- a/IFW_MQTT_Server_TestSteps.ttcn
+++ b/IFW_MQTT_Server_TestSteps.ttcn
@@ -75,14 +75,14 @@
     return true;
   }	
 	
-  function f_IFW_MqttServer_setMessageToSend(in integer p_serverIdx, MQTT_v3_1_1_ReqResp p_msg) runs on IFW_MAIN_CT
+  function f_IFW_MqttServer_setMessageToSend(in integer p_serverIdx, MQTT_v3_1_1_ReqResp p_msg, boolean p_msgIdFromLastReceived := false) runs on IFW_MAIN_CT
   return boolean
   {
     var IFW_MQTT_Server_CT v_server := mqttServers[p_serverIdx];	  
     if (v_server == null) { log("IFW: No MQTT server found"); return false; }	  
     f_isRunningGuard(v_server);
 
-    v_server.start(f_MQTT_Server_setMessageToSend(p_msg));
+    v_server.start(f_MQTT_Server_setMessageToSend(p_msg, p_msgIdFromLastReceived));
     v_server.done;
 
     return true;