diff --git a/FlashMQTests/tst_maintests.cpp b/FlashMQTests/tst_maintests.cpp
index ca7b8e6..6b1374f 100644
--- a/FlashMQTests/tst_maintests.cpp
+++ b/FlashMQTests/tst_maintests.cpp
@@ -81,6 +81,8 @@ private slots:
     void test_validUtf8();
     void test_validUtf8Sse();
 
+    void testPacketInt16Parse();
+
 };
 
 MainTests::MainTests()
@@ -755,6 +757,21 @@ void MainTests::test_validUtf8Sse()
     QVERIFY(!data.isValidUtf8(e));
 }
 
+void MainTests::testPacketInt16Parse()
+{
+    std::vector<uint64_t> tests {128, 300, 64, 65550, 32000};
+
+    for (const uint16_t id : tests)
+    {
+        Publish pub("hallo", "content", 1);
+        MqttPacket packet(pub);
+        packet.setPacketId(id);
+        packet.pos -= 2;
+        uint16_t idParsed = packet.readTwoBytesToUInt16();
+        QVERIFY(id == idParsed);
+    }
+}
+
 QTEST_GUILESS_MAIN(MainTests)
 
 #include "tst_maintests.moc"
diff --git a/mqttpacket.cpp b/mqttpacket.cpp
index 16c806d..8b96993 100644
--- a/mqttpacket.cpp
+++ b/mqttpacket.cpp
@@ -143,6 +143,7 @@ MqttPacket::MqttPacket(const PubAck &pubAck) :
     writeByte(2); // length is always 2.
     char topicLenMSB = (pubAck.packet_id & 0xFF00) >> 8;
     char topicLenLSB = (pubAck.packet_id & 0x00FF);
+    packet_id_pos = pos;
     writeByte(topicLenMSB);
     writeByte(topicLenLSB);
 }
@@ -641,7 +642,9 @@ uint16_t MqttPacket::readTwoBytesToUInt16()
     if (pos + 2 > bites.size())
         throw ProtocolError("Invalid packet: header specifies invalid length.");
 
-    uint16_t i = bites[pos] << 8 | bites[pos+1];
+    uint8_t a = bites[pos];
+    uint8_t b = bites[pos+1];
+    uint16_t i = a << 8 | b;
     pos += 2;
     return i;
 }
diff --git a/mqttpacket.h b/mqttpacket.h
index 0a55042..7c98298 100644
--- a/mqttpacket.h
+++ b/mqttpacket.h
@@ -43,6 +43,10 @@ public:
 
 class MqttPacket
 {
+#ifdef TESTING
+    friend class MainTests;
+#endif
+
     std::string topic;
     std::vector<std::string> *subtopics; // comes from local thread storage. See std::vector<std::string> *ThreadData::splitTopic(std::string &topic)
     std::vector<char> bites;