diff --git a/lib/pbio/drv/usb/usb_stm32.c b/lib/pbio/drv/usb/usb_stm32.c index 225f893a0..be9e1b3a6 100644 --- a/lib/pbio/drv/usb/usb_stm32.c +++ b/lib/pbio/drv/usb/usb_stm32.c @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -284,6 +285,9 @@ PROCESS_THREAD(pbdrv_usb_process, ev, data) { static PBIO_ONESHOT(bcd_oneshot); static PBIO_ONESHOT(pwrdn_oneshot); static bool bcd_busy; + static struct etimer timer; + static uint32_t prev_status_flags = ~0; + static uint32_t new_status_flags; PROCESS_POLLHANDLER({ if (!bcd_busy && pbio_oneshot(!vbus_active, &no_vbus_oneshot)) { @@ -305,6 +309,8 @@ PROCESS_THREAD(pbdrv_usb_process, ev, data) { // Prepare to receive the first packet USBD_Pybricks_ReceivePacket(&husbd); + etimer_set(&timer, 500); + for (;;) { PROCESS_WAIT_EVENT(); @@ -340,11 +346,26 @@ PROCESS_THREAD(pbdrv_usb_process, ev, data) { USBD_Pybricks_ReceivePacket(&husbd); } - if (!usb_out_sz && usb_stdout_sz) { + if (usb_out_sz) { + continue; + } + + new_status_flags = pbsys_status_get_flags(); + + if (usb_stdout_sz) { memcpy(usb_out_buf, usb_stdout_buf, usb_stdout_sz); usb_out_sz = usb_stdout_sz; usb_stdout_sz = 0; + } else if ((new_status_flags != prev_status_flags) || etimer_expired(&timer)) { + pbio_uuid128_le_copy(usb_out_buf, pbio_pybricks_command_event_char_uuid); + usb_out_sz = UUID_SZ; + usb_out_sz += pbio_pybricks_event_status_report(&usb_out_buf[UUID_SZ], new_status_flags); + + etimer_restart(&timer); + prev_status_flags = new_status_flags; + } + if (usb_out_sz) { USBD_Pybricks_SetTxBuffer(&husbd, usb_out_buf, usb_out_sz); USBD_Pybricks_TransmitPacket(&husbd); }