add real formatted debug logging with timestamps

src/RedirectablePrint.cpp

@@ -1,6 +1,9 @@
 #include "RedirectablePrint.h"
 #include "configuration.h"
 #include <assert.h>
+#include <sys/time.h>
+#include <time.h>
+#include "concurrency/OSThread.h"
 
 /**
  * A printer that doesn't go anywhere
@@ -15,10 +18,82 @@ void RedirectablePrint::setDestination(Print *_dest)
 
 size_t RedirectablePrint::write(uint8_t c)
 {
+    // Always send the characters to our segger JTAG debugger
 #ifdef SEGGER_STDOUT_CH
     SEGGER_RTT_PutCharSkip(SEGGER_STDOUT_CH, c);
 #endif
 
     dest->write(c);
     return 1; // We always claim one was written, rather than trusting what the serial port said (which could be zero)
+}
+
+size_t RedirectablePrint::vprintf(const char *format, va_list arg)
+{
+    va_list copy;
+
+    va_copy(copy, arg);
+    int len = vsnprintf(printBuf, printBufLen, format, copy);
+    va_end(copy);
+    if (len < 0) {
+        va_end(arg);
+        return 0;
+    };
+    if (len >= printBufLen) {
+        delete[] printBuf;
+        printBufLen *= 2;
+        printBuf = new char[printBufLen];
+        len = vsnprintf(printBuf, printBufLen, format, arg);
+    }
+
+    len = Print::write(printBuf, len);
+    return len;
+}
+
+#define SEC_PER_DAY 86400
+#define SEC_PER_HOUR 3600
+#define SEC_PER_MIN 60
+
+size_t RedirectablePrint::logDebug(const char *format, ...)
+{
+    va_list arg;
+    va_start(arg, format);
+
+    // Cope with 0 len format strings, but look for new line terminator
+    bool hasNewline = *format && format[strlen(format) - 1] == '\n';
+
+    size_t r = 0;
+
+    // If we are the first message on a report, include the header
+    if (!isContinuationMessage) {
+        struct timeval tv;
+        if (!gettimeofday(&tv, NULL)) {
+            long hms = tv.tv_sec % SEC_PER_DAY;
+            //hms += tz.tz_dsttime * SEC_PER_HOUR;
+            //hms -= tz.tz_minuteswest * SEC_PER_MIN;
+            // mod `hms` to ensure in positive range of [0...SEC_PER_DAY)
+            hms = (hms + SEC_PER_DAY) % SEC_PER_DAY;
+
+            // Tear apart hms into h:m:s
+            int hour = hms / SEC_PER_HOUR;
+            int min = (hms % SEC_PER_HOUR) / SEC_PER_MIN;
+            int sec = (hms % SEC_PER_HOUR) % SEC_PER_MIN; // or hms % SEC_PER_MIN
+
+            r += printf("%02d:%02d:%02d ", hour, min, sec);
+        } else
+            r += printf("??:??:?? ");
+
+        auto thread = concurrency::OSThread::currentThread;
+        if(thread) {
+            print("[");
+            print(thread->ThreadName);
+            print("] ");
+        }
+    }
+
+    r += vprintf(format, arg);
+    va_end(arg);
+
+    isContinuationMessage = !hasNewline;
+
+    return r;
 }

src/RedirectablePrint.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <Print.h>
+#include <stdarg.h>
 
 /**
  * A Printable that can be switched to squirt its bytes to a different sink.
@@ -11,6 +12,13 @@ class RedirectablePrint : public Print
 {
     Print *dest;
 
+    /// We dynamically grow this scratch buffer if necessary
+    char *printBuf = new char[64];
+    size_t printBufLen = 64;
+
+    /// Used to allow multiple logDebug messages to appear on a single log line
+    bool isContinuationMessage = false;
+
   public:
     RedirectablePrint(Print *_dest) : dest(_dest) {}
 
@@ -20,6 +28,21 @@ class RedirectablePrint : public Print
     void setDestination(Print *dest);
 
     virtual size_t write(uint8_t c);
+
+    /**
+     * Debug logging print message
+     * 
+     * If the provide format string ends with a newline we assume it is the final print of a single
+     * log message.  Otherwise we assume more prints will come before the log message ends.  This
+     * allows you to call logDebug a few times to build up a single log message line if you wish.
+     * 
+     * FIXME, eventually add log levels (INFO, WARN, ERROR) and subsystems.  Move into 
+     * a different class.
+     */
+    size_t logDebug(const char * format, ...)  __attribute__ ((format (printf, 2, 3)));
+
+    /** like printf but va_list based */
+    size_t vprintf(const char *format, va_list arg);
 };
 
 class NoopPrint : public Print

src/concurrency/OSThread.cpp

@@ -14,6 +14,8 @@ bool OSThread::showRun = false;
 /// Show debugging info for threads we decide not to run;
 bool OSThread::showWaiting = false;
 
+const OSThread *OSThread::currentThread;
+
 ThreadController mainController, timerController;
 InterruptableDelay mainDelay;
 
@@ -68,12 +70,15 @@ bool OSThread::shouldRun(unsigned long time)
 
 void OSThread::run()
 {
+    currentThread = this;
     auto newDelay = runOnce();
 
     runned();
 
     if (newDelay >= 0)
         setInterval(newDelay);
+
+    currentThread = NULL;
 }
 
 } // namespace concurrency

src/concurrency/OSThread.h

@@ -42,6 +42,10 @@ class OSThread : public Thread
     static bool showWaiting;
 
   public:
+
+    /// For debug printing only (might be null)
+    static const OSThread *currentThread;
+
     OSThread(const char *name, uint32_t period = 0, ThreadController *controller = &mainController);
 
     virtual ~OSThread();

src/configuration.h

@@ -428,7 +428,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define DEBUG_MSG(...) SEGGER_RTT_printf(0, __VA_ARGS__)
 #else
 #ifdef DEBUG_PORT
-#define DEBUG_MSG(...) DEBUG_PORT.printf(__VA_ARGS__)
+#define DEBUG_MSG(...) DEBUG_PORT.logDebug(__VA_ARGS__)
 #else
 #define DEBUG_MSG(...)
 #endif