Fix pio uart integration with neopix pio

1 files changed, 12 insertions, 10 deletions

diff --git a/src/util.c b/src/util.c
index b4a7fba..187c477 100644
--- a/src/util.c
+++ b/src/util.c
@@ -12,20 +12,24 @@ int loglevel = LOG_DEBUG;
 
 static void
 __attribute__((format(printf, 1, 0)))
-panic_task(const char *fmtstr, va_list ap)
+panic_task(const char *fmtstr, va_list ap, uint32_t sleep_ms)
 {
+	static uint32_t start_ms = 0;
 	va_list cpy;
-	char c;
 
 	if (!tud_cdc_available())
 		return;
 
-	if (!tud_cdc_read(&c, 1))
+	if (!start_ms) start_ms = board_millis();
+
+	if (board_millis() < start_ms + sleep_ms)
 		return;
 
 	va_copy(cpy, ap);
 	vprintf(fmtstr, cpy);
 	printf("\n\r");
+
+	start_ms += sleep_ms;
 }
 
 static void
@@ -36,12 +40,12 @@ blink_task(struct neopix *pix, uint32_t blink_ms)
 
 	if (!start_ms) start_ms = board_millis();
 
-	if (board_millis() - start_ms < blink_ms)
+	if (board_millis() < start_ms + blink_ms)
 		return;
 
 	neopix_put(pix, neopix_u32rgb(255 * led_state, 0, 0));
-
 	led_state ^= true;
+
 	start_ms += blink_ms;
 }
 
@@ -71,13 +75,11 @@ blink_panic(const char *fmtstr, ...)
 
 	va_start(ap, fmtstr);
 
-	if (!onboard_led.init)
-		neopix_init(&onboard_led, pio0, 0, ONBOARD_LED_PIN);
-
 	while (1) {
 		tud_task();
-		panic_task(fmtstr, ap);
-		blink_task(&onboard_led, 200);
+		panic_task(fmtstr, ap, 1000);
+		if (onboard_led.init)
+			blink_task(&onboard_led, 200);
 	}
 
 	va_end(ap);