From e9cc2d593319e8a6319fb8a89f171bbdcff53be1 Mon Sep 17 00:00:00 2001
From: Nate Karstens <nate.karstens@gmail.com>
Date: Sun, 24 Dec 2023 07:21:56 -0600
Subject: [PATCH] Periodically send status

Periodically sends status flags over the USB connection.

Signed-off-by: Nate Karstens <nate.karstens@gmail.com>
---
 lib/pbio/drv/usb/usb_stm32.c | 23 ++++++++++++++++++++++-
 1 file changed, 22 insertions(+), 1 deletion(-)

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 <usbd_pybricks.h>
 
 #include <pbdrv/usb.h>
+#include <pbio/protocol.h>
 #include <pbio/util.h>
 #include <pbsys/command.h>
 #include <pbsys/status.h>
@@ -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);
         }