Implement ITM debug via ST-Link

- Enable Instrumentation Trace Macroblock
- Remove PB3 initialization conflict with ITM
- Add a bunch of debug prints

Core/Inc/main.h

@@ -24,6 +24,24 @@
 extern "C" {
 #endif
 
+#define DEBUG
+#ifdef DEBUG
+static inline void ITM_Print(const char *str) {
+    while (*str) {
+        uint32_t timeout = 1000;
+        while ((ITM->PORT[0].u32 == 0) && (timeout-- > 0));
+
+        if (timeout > 0) {
+            ITM->PORT[0].u8 = (uint8_t)(*str);
+        }
+        str++;
+    }
+}
+#define DEBUG_PRINT(x) ITM_Print(x)
+#else
+#define DEBUG_PRINT(x)
+#endif
+
 /* Includes ------------------------------------------------------------------*/
 #include "stm32f4xx_hal.h"
 #include "canlog.h"