diff --git a/src/Message.cpp b/src/Message.cpp
index 60259a597339b14b686352bd67f4eb1ff8f1eb0d..997b617ba4035f5e8af9e40ad27715df94551db9 100644
--- a/src/Message.cpp
+++ b/src/Message.cpp
@@ -20,7 +20,7 @@ void Message::appendBits(uint8_t numBits, uint16_t data) {
 			this->data[dataSize] |= static_cast<uint8_t>(data >> (numBits - bitsToAddNow));
 
 			// Remove used bits
-			data &= (1 << bitsToAddNow) - 1;
+			data &= (1 << (numBits - bitsToAddNow)) - 1;
 			numBits -= bitsToAddNow;
 
 			currentBit = 0;