modbus-debug

modbus_rtu.c

@@ -21,10 +21,30 @@
 
 */
 
+
 #include <stdio.h>
 #include <string.h>
 
 #include "hal.h"
+#if MODBUS_DEBUG
+#include "stream.h"
+static void log_modbus_frame(const char *label,
+                             const uint8_t *data,
+                             uint8_t len)
+{
+    char buf[64];
+    size_t n = (size_t)snprintf(buf, sizeof(buf), "%s:", label);
+    hal.stream.write(buf);
+
+    for(uint_fast8_t i = 0; i < len; i++) {
+        snprintf(buf, sizeof(buf), " %02X", data[i]);
+        hal.stream.write(buf);
+    }
+
+    hal.stream.write(ASCII_EOL);
+}
+#endif
+#include "report.h"
 #include "platform.h"
 #include "protocol.h"
 #include "settings.h"
@@ -152,6 +172,9 @@ static void tx_message (volatile queue_entry_t *msg)
     rx_timeout = modbus.rx_timeout;
 
     stream.flush_rx_buffer();
+#if MODBUS_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);
 }
 
@@ -243,6 +266,9 @@ static void modbus_poll (void *data)
                     }
                 }
 
+#if MODBUS_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;