diff --git a/src/RedirectablePrint.cpp b/src/RedirectablePrint.cpp index 0205aa66..541b55c7 100644 --- a/src/RedirectablePrint.cpp +++ b/src/RedirectablePrint.cpp @@ -1,6 +1,9 @@ #include "RedirectablePrint.h" #include "configuration.h" #include +#include +#include +#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; } \ No newline at end of file diff --git a/src/RedirectablePrint.h b/src/RedirectablePrint.h index 2d525118..ca208612 100644 --- a/src/RedirectablePrint.h +++ b/src/RedirectablePrint.h @@ -1,6 +1,7 @@ #pragma once #include +#include /** * 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 diff --git a/src/concurrency/OSThread.cpp b/src/concurrency/OSThread.cpp index 916e68e1..58a0d59c 100644 --- a/src/concurrency/OSThread.cpp +++ b/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 diff --git a/src/concurrency/OSThread.h b/src/concurrency/OSThread.h index dc600387..3be149d5 100644 --- a/src/concurrency/OSThread.h +++ b/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(); diff --git a/src/configuration.h b/src/configuration.h index 28eea052..6fa07ce6 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -428,7 +428,7 @@ along with this program. If not, see . #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