Adding enableDebugging plus helper functions for print, println and printf

Author: PaulZC

src/ICM_20948.cpp

@@ -14,6 +14,61 @@ ICM_20948::ICM_20948()
 {
 }
 
+void ICM_20948::enableDebugging(Stream &debugPort)
+{
+  _debugSerial = &debugPort; //Grab which port the user wants us to use for debugging
+  _printDebug = true; //Should we print the commands we send? Good for debugging
+}
+void ICM_20948::disableDebugging(void)
+{
+  _printDebug = false; //Turn off extra print statements
+}
+
+// Debug Printing: based on gfvalvo's flash string helper code:
+// https://forum.arduino.cc/index.php?topic=533118.msg3634809#msg3634809
+
+void ICM_20948::debugPrint(const char *line)
+{
+  doDebugPrint([](const char *ptr) {return *ptr;}, line);
+}
+
+void ICM_20948::debugPrint(const __FlashStringHelper *line)
+{
+  doDebugPrint([](const char *ptr) {return (char) pgm_read_byte_near(ptr);},
+      (const char*) line);
+}
+
+void ICM_20948::debugPrintln(const char *line)
+{
+  doDebugPrint([](const char *ptr) {return *ptr;}, line, true);
+}
+
+void ICM_20948::debugPrintln(const __FlashStringHelper *line)
+{
+  doDebugPrint([](const char *ptr) {return (char) pgm_read_byte_near(ptr);},
+      (const char*) line, true);
+}
+
+void ICM_20948::doDebugPrint(char (*funct)(const char *), const char *string, bool newLine)
+{
+  if (_printDebug == false)
+    return; // Bail if debugging is not enabled
+
+  char ch;
+
+  while ((ch = funct(string++)))
+  {
+    _debugSerial->print(ch);
+  }
+
+  if (newLine)
+  {
+    _debugSerial->println();
+  }
+}
+
+
+
 ICM_20948_AGMT_t ICM_20948::getAGMT(void)
 {
     status = ICM_20948_get_agmt(&_device, &agmt);

src/ICM_20948.h

@@ -20,6 +20,9 @@ A C++ interface to the ICM-20948
 class ICM_20948
 {
 private:
+    Stream *_debugSerial;			//The stream to send debug messages to if enabled
+    boolean _printDebug = false;		//Flag to print the serial commands we are sending to the Serial port for debug
+
 protected:
     ICM_20948_Device_t _device;
 
@@ -31,6 +34,43 @@ class ICM_20948
 public:
     ICM_20948(); // Constructor
 
+    // Enable debug messages using the chosen Serial port (Stream)
+  	// Boards like the RedBoard Turbo use SerialUSB (not Serial).
+  	// But other boards like the SAMD51 Thing Plus use Serial (not SerialUSB).
+  	// These lines let the code compile cleanly on as many SAMD boards as possible.
+  	#if defined(ARDUINO_ARCH_SAMD)	// Is this a SAMD board?
+  	#if defined(USB_VID)						// Is the USB Vendor ID defined?
+  	#if (USB_VID == 0x1B4F)					// Is this a SparkFun board?
+  	#if !defined(ARDUINO_SAMD51_THING_PLUS) & !defined(ARDUINO_SAMD51_MICROMOD) // If it is not a SAMD51 Thing Plus or SAMD51 MicroMod
+  	void enableDebugging(Stream &debugPort = SerialUSB); //Given a port to print to, enable debug messages.
+  	#else
+  	void enableDebugging(Stream &debugPort = Serial); //Given a port to print to, enable debug messages.
+  	#endif
+  	#else
+  	void enableDebugging(Stream &debugPort = Serial); //Given a port to print to, enable debug messages.
+  	#endif
+  	#else
+  	void enableDebugging(Stream &debugPort = Serial); //Given a port to print to, enable debug messages.
+  	#endif
+  	#else
+  	void enableDebugging(Stream &debugPort = Serial); //Given a port to print to, enable debug messages.
+  	#endif
+
+  	void disableDebugging(void); //Turn off debug statements
+
+    // gfvalvo's flash string helper code: https://forum.arduino.cc/index.php?topic=533118.msg3634809#msg3634809
+    void debugPrint(const char *);
+    void debugPrint(const __FlashStringHelper *);
+    void debugPrintln(const char *);
+    void debugPrintln(const __FlashStringHelper *);
+    void doDebugPrint(char (*)(const char *), const char *, bool newLine = false);
+
+    // Debug printf - not glamorous but it works...
+    #define debugPrintf1( var ) {if (_printDebug == true) _debugSerial->printf( var );}
+    #define debugPrintf2( var1, var2 ) {if (_printDebug == true) _debugSerial->printf( var1, var2 );}
+    #define debugPrintf3( var1, var2, var3 ) {if (_printDebug == true) _debugSerial->printf( var1, var2, var3 );}
+    #define debugPrintf4( var1, var2, var3, var4 ) {if (_printDebug == true) _debugSerial->printf( var1, var2, var3, var4 );}
+
     ICM_20948_AGMT_t agmt;          // Acceleometer, Gyroscope, Magenetometer, and Temperature data
     ICM_20948_AGMT_t getAGMT(void); // Updates the agmt field in the object and also returns a copy directly