feat: Add debug features

Author: epsilonrt

src/Modbus.cpp

@@ -12,7 +12,8 @@ Modbus::Modbus()  :
   _additional_data (0),
   _frame (nullptr),
   _len (0),
-  _reply (0) {
+  _reply (0),
+  _debug (nullptr) {
 }
 
 //-------------------------------------------------------------------------------
@@ -361,6 +362,22 @@ void Modbus::readRegisters (const byte fcode, const word startreg, const word nu
   _reply = MB_REPLY_NORMAL;
 }
 
+//-------------------------------------------------------------------------------
+// protected
+void Modbus::debugMessage (bool reply) {
+
+  if (_len && isDebug()) {
+    char str[3];  // buffer for sprintf
+    for (uint8_t i = 0; i < _len; i++) {
+
+      sprintf (str, "%02X", _frame[i]); // str stores the string representation of the byte, in hex form
+      _debug->write (!reply ? '[' : '<');
+      _debug->print (str);
+      _debug->write (!reply ? ']' : '>');
+    }
+    _debug->println ("");
+  }
+}
 
 #ifndef USE_HOLDING_REGISTERS_ONLY
 //-------------------------------------------------------------------------------

src/Modbus.h

@@ -73,9 +73,9 @@ class Modbus {
     void writeSingleRegister (const word reg, const word value);
     void writeMultipleRegisters (const byte *frame, const word startreg, const word numoutputs, const byte bytecount);
     void exceptionResponse (const byte fcode, const byte excode);
-    
+
     #ifndef USE_HOLDING_REGISTERS_ONLY
-    void readBits (const byte fcode,const word startreg, const word numregs);
+    void readBits (const byte fcode, const word startreg, const word numregs);
     void writeSingleCoil (const word reg, const word status);
     void writeMultipleCoils (const byte *frame, const word startreg, const word numoutputs, const byte bytecount);
     #endif
@@ -101,8 +101,10 @@ class Modbus {
     byte *_frame;
     byte  _len;
     byte  _reply;
+    Print *_debug;
     void receivePDU (byte *frame);
     void reportServerId();
+    void debugMessage (bool reply = false);
     #endif
 
   public:
@@ -180,6 +182,31 @@ class Modbus {
       return hreg (offset);
     }
 
+    /**
+       @brief Enable debug mode
+    */
+    inline void setDebug (Print &print) {
+      _debug = &print;
+      _debug->println ("Modbus: debug enabled....");
+    }
+
+    /**
+       @brief Returns true if debug mode is enabled
+    */
+    inline bool isDebug () {
+      return _debug != nullptr;
+    }
+
+    /**
+       @brief Print a debug message, only if debug mode is enabled
+       @param msg message to print
+    */
+    inline void debug (const char *msg) {
+      if (_debug) {
+        _debug->println (msg);
+      }
+    }
+
     #ifndef USE_HOLDING_REGISTERS_ONLY
 
     /**