diff --git a/modbus_rtu.c b/modbus_rtu.c
index 0ce8fc6..a82df0c 100644
--- a/modbus_rtu.c
+++ b/modbus_rtu.c
@@ -21,6 +21,7 @@
 
 */
 
+
 #include <stdio.h>
 #include <string.h>
 
@@ -109,6 +110,21 @@ static bool valid_crc (const char *buf, uint_fast16_t len)
 }
 */
 
+#if DEBUG
+static void log_modbus_frame(const char *label,
+                             const uint8_t *data,
+                             uint8_t len)
+{
+    char frame[3 * MODBUS_MAX_ADU_SIZE + 4];
+    char *ptr = frame;
+    ptr += sprintf(ptr, "%s:", label);
+    for(uint_fast8_t i = 0; i < len; i++) {
+        ptr += sprintf(ptr, " %02X", data[i]);
+    }
+    debug_printf("%s", frame);
+}
+#endif
+
 static void retry_exception (uint8_t code, void *context)
 {
     if(packet && packet->callbacks.retries) {
@@ -149,6 +165,9 @@ static void tx_message (volatile queue_entry_t *msg)
     rx_timeout = modbus.rx_timeout;
 
     stream.flush_rx_buffer();
+#if DEBUG
+    log_modbus_frame("TX", (const uint8_t *)msg->msg.adu, msg->msg.tx_length);
+#endif
     stream.write((char *)((queue_entry_t *)msg)->msg.adu, ((queue_entry_t *)msg)->msg.tx_length);
 }
 
@@ -240,6 +259,9 @@ static void modbus_poll (void *data)
                     }
                 }
 
+#if DEBUG
+                log_modbus_frame("RX", (const uint8_t *)((queue_entry_t *)packet)->msg.adu, rx_len);
+#endif
                 if((state = packet->async ? ModBus_Silent : ModBus_GotReply) == ModBus_Silent) {
                     if(packet->callbacks.on_rx_packet) {
                         packet->msg.rx_length = rx_len;
@@ -636,4 +658,4 @@ void modbus_rtu_init (int8_t stream, int8_t dir_aux)
         task_run_on_startup(report_warning, "Modbus failed to initialize!");
         system_raise_alarm(Alarm_SelftestFailed);
     }
-}
+}
\ No newline at end of file