diff --git a/include/hydrolink/protocol.h b/include/hydrolink/protocol.h
index 6859fca..a66fc2b 100644
--- a/include/hydrolink/protocol.h
+++ b/include/hydrolink/protocol.h
@@ -156,12 +156,16 @@ static inline uint16_t hydrolink_calculate_crc(const uint8_t *payload, uint8_t l
 }
 
 static inline void hydrolink_fill_header_and_crc(hydrolink_msg_t *msg, uint8_t msg_id, uint8_t payload_length, uint8_t dst_id, uint8_t src_id, uint8_t crc_extra) {
-  msg->id = msg_id;
-  msg->payload_length = payload_length;
-  msg->dst_id = dst_id;
-  msg->src_id = src_id;
   uint16_t crc;
   crc_xmodem_init(&crc);
+  msg->id = msg_id;
+  crc_xmodem_accumulate(msg->id, &crc);
+  msg->payload_length = payload_length;
+  crc_xmodem_accumulate(msg->payload_length, &crc);
+  msg->dst_id = dst_id;
+  crc_xmodem_accumulate(msg->dst_id, &crc);
+  msg->src_id = src_id;
+  crc_xmodem_accumulate(msg->src_id, &crc);
   for(int i = 0; i < msg->payload_length; ++i) {
     crc_xmodem_accumulate(msg->payload[i], &crc);
   }