diff --git a/libraries/AP_HAL_ChibiOS/stdio.cpp b/libraries/AP_HAL_ChibiOS/stdio.cpp
index 959004a77d..85df8712bb 100644
--- a/libraries/AP_HAL_ChibiOS/stdio.cpp
+++ b/libraries/AP_HAL_ChibiOS/stdio.cpp
@@ -29,6 +29,9 @@
 #include <ctype.h>
 #include "hwdef/common/stdio.h"
 #include <AP_HAL/AP_HAL.h>
+#if HAL_USE_SERIAL_USB == TRUE
+#include <AP_HAL_ChibiOS/hwdef/common/usbcfg.h>
+#endif
 
 extern const AP_HAL::HAL& hal;
 
@@ -80,7 +83,9 @@ int __wrap_asprintf(char **strp, const char *fmt, ...)
 int __wrap_vprintf(const char *fmt, va_list arg)
 {
 #ifdef HAL_STDOUT_SERIAL
-  return chvprintf ((BaseSequentialStream*)&HAL_STDOUT_SERIAL, fmt, arg);
+  return chvprintf((BaseSequentialStream*)&HAL_STDOUT_SERIAL, fmt, arg);
+#elif HAL_USE_SERIAL_USB == TRUE
+  return chvprintf((BaseSequentialStream*)&SDU1, fmt, arg);
 #else
   (void)arg;
   return strlen(fmt);
diff --git a/libraries/AP_HAL_ChibiOS/system.cpp b/libraries/AP_HAL_ChibiOS/system.cpp
index 70b6b5b31d..3e98b6de49 100644
--- a/libraries/AP_HAL_ChibiOS/system.cpp
+++ b/libraries/AP_HAL_ChibiOS/system.cpp
@@ -203,7 +203,10 @@ void panic(const char *errormsg, ...)
     va_end(ap);
 
     hal.scheduler->delay_microseconds(10000);
-    while(1) {}
+    while (1) {
+        vprintf(errormsg, ap);
+        hal.scheduler->delay(500);
+    }
 }
 
 uint32_t micros()