pbio/sys/telemetry: Add module.

Adds a first version for hub telemetry. We start off with just motor angles, generalizing the motor angles emitted via a socket in the virtual hub.

Author: laurensvalk

bricks/_common/sources.mk

@@ -251,6 +251,7 @@ PBIO_SRC_C = $(addprefix lib/pbio/,\
 	sys/status.c \
 	sys/storage_settings.c \
 	sys/storage.c \
+	sys/telemetry.c \
 	)
 
 # LEGO specification library

lib/pbio/drv/bluetooth/bluetooth_simulation.c

@@ -10,6 +10,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
+#include <arpa/inet.h>
 
 #include "bluetooth.h"
 #include <pbdrv/bluetooth.h>
@@ -50,17 +51,31 @@ bool pbdrv_bluetooth_is_connected(pbdrv_bluetooth_connection_t connection) {
     return false;
 }
 
+// Socket used to send data to Python animation.
+static int data_socket = -1;
+static struct sockaddr_in serv_addr;
 
 pbio_error_t pbdrv_bluetooth_send_pybricks_value_notification(pbio_os_state_t *state, const uint8_t *data, uint16_t size) {
     PBIO_OS_ASYNC_BEGIN(state);
 
-    // Only care about stdout for now.
-    if (size < 1 || data[0] != PBIO_PYBRICKS_EVENT_WRITE_STDOUT) {
+    // Stdout also goes to native stdout.
+    if (size > 1 && data[0] == PBIO_PYBRICKS_EVENT_WRITE_STDOUT) {
+        int ret = write(STDOUT_FILENO, data + 1, size - 1);
+        (void)ret;
+    }
+
+    // No output configured, so done.
+    if (data_socket < 0) {
         return PBIO_SUCCESS;
     }
 
-    int ret = write(STDOUT_FILENO, data + 1, size - 1);
-    (void)ret;
+    // Send the value notification via socket.
+    ssize_t sent = sendto(data_socket, data, size, 0, (struct sockaddr *)&serv_addr, sizeof(serv_addr));
+    if (sent < 0) {
+        printf("send() failed");
+        close(data_socket);
+        data_socket = -1;
+    }
 
     PBIO_OS_ASYNC_END(PBIO_SUCCESS);
 }
@@ -180,6 +195,24 @@ void pbdrv_bluetooth_init_hci(void) {
     bluetooth_thread_err = PBIO_ERROR_AGAIN;
     bluetooth_thread_state = 0;
     pbio_os_process_start(&pbdrv_bluetooth_simulation_process, pbdrv_bluetooth_simulation_process_thread, NULL);
+
+    // Configure socket if specified.
+    if (getenv("PBIO_TEST_CONNECT_SOCKET")) {
+        data_socket = socket(AF_INET, SOCK_DGRAM, 0);
+        if (data_socket < 0) {
+            printf("socket() failed");
+            return;
+        }
+        serv_addr.sin_family = AF_INET;
+        serv_addr.sin_port = htons(5002);
+        if (inet_pton(AF_INET, "127.0.0.1", &serv_addr.sin_addr) <= 0) {
+            printf("inet_pton() failed");
+            close(data_socket);
+            data_socket = -1;
+            return;
+        }
+    }
+
 }
 
 #endif // PBDRV_CONFIG_BLUETOOTH_SIMULATION

lib/pbio/drv/motor_driver/motor_driver_virtual_simulation.c

@@ -5,15 +5,6 @@
 
 #if PBDRV_CONFIG_MOTOR_DRIVER_VIRTUAL_SIMULATION
 
-#include <arpa/inet.h>
-#include <stdint.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <sys/wait.h>
-#include <unistd.h>
-#include <unistd.h>
-
 #include <contiki.h>
 
 #include <pbdrv/clock.h>
@@ -185,27 +176,6 @@ pbio_error_t pbdrv_motor_driver_set_duty_cycle(pbdrv_motor_driver_dev_t *driver,
     return PBIO_SUCCESS;
 }
 
-static int data_socket = -1;
-static struct sockaddr_in serv_addr;
-
-static void animation_socket_connect(void) {
-
-    data_socket = socket(AF_INET, SOCK_DGRAM, 0);
-    if (data_socket < 0) {
-        perror("socket() failed");
-        return;
-    }
-
-    serv_addr.sin_family = AF_INET;
-    serv_addr.sin_port = htons(5002);
-    if (inet_pton(AF_INET, "127.0.0.1", &serv_addr.sin_addr) <= 0) {
-        perror("inet_pton() failed");
-        close(data_socket);
-        data_socket = -1;
-        return;
-    }
-}
-
 PROCESS(pbdrv_motor_driver_virtual_simulation_process, "pbdrv_motor_driver_virtual_simulation");
 
 PROCESS_THREAD(pbdrv_motor_driver_virtual_simulation_process, ev, data) {
@@ -223,25 +193,6 @@ PROCESS_THREAD(pbdrv_motor_driver_virtual_simulation_process, ev, data) {
     for (;;) {
         PROCESS_WAIT_EVENT_UNTIL(ev == PROCESS_EVENT_TIMER && etimer_expired(&tick_timer));
 
-        // If data parser pipe is connected, output the motor angles.
-        if (data_socket >= 0 && timer_expired(&frame_timer)) {
-            timer_reset(&frame_timer);
-            uint8_t buf[sizeof(uint32_t) * PBDRV_CONFIG_MOTOR_DRIVER_NUM_DEV + 1] = { 3 };
-            // Output motor angles on one line.
-            for (dev_index = 0; dev_index < PBDRV_CONFIG_MOTOR_DRIVER_NUM_DEV; dev_index++) {
-                driver = &motor_driver_devs[dev_index];
-                pbio_set_uint32_le(&buf[dev_index * sizeof(uint32_t) + 1], (uint32_t)(driver->angle / 1000));
-            }
-
-            // Send the constructed message
-            ssize_t sent = sendto(data_socket, buf, sizeof(buf), 0, (struct sockaddr *)&serv_addr, sizeof(serv_addr));
-            if (sent < 0) {
-                perror("send() failed");
-                close(data_socket);
-                data_socket = -1;
-            }
-        }
-
         for (dev_index = 0; dev_index < PBDRV_CONFIG_MOTOR_DRIVER_NUM_DEV; dev_index++) {
             driver = &motor_driver_devs[dev_index];
 
@@ -317,11 +268,6 @@ void pbdrv_counter_init(void) {
 void pbdrv_motor_driver_init(void) {
     simulation_init();
 
-    // Skip if no data parser is given.
-    if (getenv("PBIO_TEST_CONNECT_SOCKET")) {
-        animation_socket_connect();
-    }
-
     if (simulation_enabled) {
         process_start(&pbdrv_motor_driver_virtual_simulation_process);
     }

lib/pbio/include/pbio/protocol.h

@@ -252,15 +252,27 @@ typedef enum {
     PBIO_PYBRICKS_EVENT_WRITE_STDOUT = 1,
 
     /**
-     * App data sent from the hub to the host. This is similar to stdout, but
-     * typically used for data that should not be shown in the user terminal,
-     * such as sensor telemetry.
+     * User data sent from the hub to the host app. This is similar to stdout,
+     * but typically used for data that should not be shown in the user
+     * terminal.
      *
      * The payload is a variable number of bytes that was written to app data.
      *
      * @since Pybricks Profile v1.4.0
      */
     PBIO_PYBRICKS_EVENT_WRITE_APP_DATA = 2,
+
+    /**
+     * Telemetry data sent from the hub to the host.
+     *
+     * The payload is a variable number of bytes that was written to app data.
+     *
+     * Pybricks profile version defines exact encoding of this data.
+     *
+     * @since Unreleased. Should not be considered final.
+     */
+    PBIO_PYBRICKS_EVENT_WRITE_TELEMETRY = 3,
+
     /**
      * The total number of events that can be queued and sent.
      */

lib/pbio/platform/virtual_hub/animation.py

@@ -59,11 +59,8 @@ def load_and_scale_image(filename, scale=0.25):
 
 
 def update_state():
-    global angles
-
     while not data_queue.empty():
         data = data_queue.get_nowait()
-        print(data)
         if not isinstance(data, bytes) or len(data) < 2:
             continue
         event = data[0]
@@ -75,7 +72,10 @@ def update_state():
             except UnicodeDecodeError:
                 print(payload)
         elif event == PBIO_PYBRICKS_EVENT_WRITE_PORT_VIEW:
-            angles = struct.unpack("<iiiiii", payload)
+            if len(payload) == 6:
+                type_id, index, angle = struct.unpack("<bbi", payload[0:6])
+                if type_id == 96:
+                    angles[index] = angle
 
 
 def main():

lib/pbio/platform/virtual_hub/pbsysconfig.h

@@ -24,5 +24,6 @@
 #define PBSYS_CONFIG_STATUS_LIGHT_BLUETOOTH         (0)
 #define PBSYS_CONFIG_STATUS_LIGHT_STATE_ANIMATIONS  (1)
 #define PBSYS_CONFIG_STATUS_LIGHT_STATE_ANIMATIONS_HUE (240)
+#define PBSYS_CONFIG_TELEMETRY                      (1)
 #define PBSYS_CONFIG_USER_PROGRAM                   (0)
 #define PBSYS_CONFIG_PROGRAM_STOP                   (1)

lib/pbio/sys/core.c

@@ -19,6 +19,7 @@
 #include "light.h"
 #include "storage.h"
 #include "program_stop.h"
+#include "telemetry.h"
 
 static pbio_os_process_t pbsys_system_poll_process;
 
@@ -74,6 +75,7 @@ void pbsys_init(void) {
     pbsys_hmi_init();
     pbsys_host_init();
     pbsys_status_light_init();
+    pbsys_telemetry_init();
 
     pbio_os_process_start(&pbsys_system_poll_process, pbsys_system_poll_process_thread, NULL);
 

lib/pbio/sys/telemetry.c

@@ -0,0 +1,90 @@
+// SPDX-License-Identifier: MIT
+// Copyright (c) 2024 The Pybricks Authors
+
+#include <pbsys/config.h>
+
+#if PBSYS_CONFIG_TELEMETRY
+
+#include <stdio.h>
+
+#include <pbio/os.h>
+#include <pbio/port_interface.h>
+#include <pbio/util.h>
+
+#include <pbsys/host.h>
+
+typedef struct {
+    lego_device_type_id_t type_id;
+    int32_t value;
+} pbsys_telemetry_port_data_t;
+
+static pbsys_telemetry_port_data_t last_data[PBIO_CONFIG_PORT_NUM_DEV];
+
+// Revisit: Come up with a data encoding protocol. Right now it just sends
+// six motor positions to drive the existing motor animation.
+static uint8_t update_port_data(uint8_t index, uint8_t *buf) {
+
+    // Get type and angle.
+    int32_t degrees = 0;
+    pbio_angle_t angle;
+    lego_device_type_id_t type_id = LEGO_DEVICE_TYPE_ID_NONE;
+    pbio_port_t *port = pbio_port_by_index(index);
+    pbio_error_t err = pbio_port_get_angle(port, &angle);
+    if (err == PBIO_SUCCESS) {
+        type_id = LEGO_DEVICE_TYPE_ID_ANY_ENCODED_MOTOR;
+        degrees = pbio_angle_to_low_res(&angle, 1000);
+    }
+
+    pbsys_telemetry_port_data_t *data = &last_data[index];
+
+    if (data->type_id == type_id && data->value == degrees) {
+        return 0;
+    }
+
+    data->type_id = type_id;
+    data->value = degrees;
+
+    buf[0] = type_id;
+    buf[1] = index;
+    pbio_set_uint32_le(&buf[2], degrees);
+    return 6;
+}
+
+/**
+ * Hub, motor, and sensor telemetry to host.
+ */
+static pbio_error_t pbsys_telemetry_process_thread(pbio_os_state_t *state, void *context) {
+
+    static pbio_os_timer_t timer;
+    static pbio_os_state_t sub;
+    static uint8_t buf[20];
+    static uint8_t size;
+    static uint32_t i = 0;
+
+    PBIO_OS_ASYNC_BEGIN(state);
+
+    for (;;) {
+        PBIO_OS_AWAIT_MS(state, &timer, 40);
+
+        // Revisit, could concatenate packages for more efficient BLE sending.
+        for (i = 0; i < PBIO_CONFIG_PORT_NUM_DEV; i++) {
+            size = update_port_data(i, buf);
+            if (size) {
+                PBIO_OS_AWAIT(state, &sub, pbsys_host_send_event(&sub, PBIO_PYBRICKS_EVENT_WRITE_TELEMETRY, buf, size));
+            }
+        }
+    }
+
+    PBIO_OS_ASYNC_END(PBIO_SUCCESS);
+}
+
+/**
+ *
+ * Starts telemetry process.
+ */
+void pbsys_telemetry_init(void) {
+    static pbio_os_process_t pbsys_telemetry_process;
+    pbio_os_process_start(&pbsys_telemetry_process, pbsys_telemetry_process_thread, NULL);
+}
+
+#endif // PBSYS_CONFIG_TELEMETRY

lib/pbio/sys/telemetry.h

@@ -0,0 +1,21 @@
+// SPDX-License-Identifier: MIT
+// Copyright (c) 2024 The Pybricks Authors
+
+#ifndef _PBSYS_SYS_TELEMETRY_H_
+#define _PBSYS_SYS_TELEMETRY_H_
+
+#include <pbsys/config.h>
+
+
+#if PBSYS_CONFIG_TELEMETRY
+
+void pbsys_telemetry_init(void);
+
+#else
+
+static inline void pbsys_telemetry_init(void) {
+}
+
+#endif // PBSYS_CONFIG_TELEMETRY
+
+#endif // _PBSYS_SYS_TELEMETRY_H_