pybricks/pupdevices: Make sensors async-ready.

pybricks/common.h

@@ -20,6 +20,7 @@
 #include <pybricks/util_mp/pb_obj_helper.h>
 
 #include <pybricks/parameters.h>
+#include <pybricks/pupdevices.h>
 #include <pybricks/tools.h>
 #include <pybricks/tools/pb_type_awaitable.h>
 
@@ -52,7 +53,7 @@ mp_obj_t common_ColorLight_internal_obj_new(pbio_color_light_t *light);
 
 #if PYBRICKS_PY_COMMON_LIGHT_ARRAY
 // pybricks._common.LightArray()
-mp_obj_t common_LightArray_obj_make_new(pbio_iodev_t *iodev, uint8_t light_mode, uint8_t number_of_lights);
+mp_obj_t common_LightArray_obj_make_new(pb_pupdevices_obj_base_t *sensor, uint8_t light_mode, uint8_t number_of_lights);
 #endif
 
 #if PYBRICKS_PY_COMMON_LIGHT_MATRIX

pybricks/common/pb_type_lightarray.c

@@ -19,7 +19,7 @@
 // pybricks._common.Light class object
 typedef struct _common_LightArray_obj_t {
     mp_obj_base_t base;
-    pbio_iodev_t *iodev;
+    pb_pupdevices_obj_base_t *sensor;
     uint8_t light_mode;
     uint8_t number_of_lights;
 } common_LightArray_obj_t;
@@ -52,10 +52,8 @@ STATIC mp_obj_t common_LightArray_on(size_t n_args, const mp_obj_t *pos_args, mp
         }
     }
 
-    // Set the brightness values
-    pb_pup_device_set_data(self->iodev, self->light_mode, brightness_values);
-
-    return mp_const_none;
+    // Set the brightness values and wait or await it.
+    return pb_pupdevices_set_data(self->sensor, self->light_mode, brightness_values);
 }
 STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_LightArray_on_obj, 1, common_LightArray_on);
 
@@ -81,9 +79,9 @@ STATIC MP_DEFINE_CONST_OBJ_TYPE(pb_type_LightArray,
     locals_dict, &common_LightArray_locals_dict);
 
 // pybricks._common.LightArray.__init__
-mp_obj_t common_LightArray_obj_make_new(pbio_iodev_t *iodev, uint8_t light_mode, uint8_t number_of_lights) {
+mp_obj_t common_LightArray_obj_make_new(pb_pupdevices_obj_base_t *sensor, uint8_t light_mode, uint8_t number_of_lights) {
     common_LightArray_obj_t *light = mp_obj_malloc(common_LightArray_obj_t, &pb_type_LightArray);
-    light->iodev = iodev;
+    light->sensor = sensor;
     light->light_mode = light_mode;
     light->number_of_lights = number_of_lights;
     return MP_OBJ_FROM_PTR(light);

pybricks/iodevices/pb_type_iodevices_pupdevice.c

@@ -6,6 +6,7 @@
 #if PYBRICKS_PY_IODEVICES && PYBRICKS_PY_PUPDEVICES
 
 #include <pbio/iodev.h>
+#include <pbio/uartdev.h>
 #include <pbio/int_math.h>
 
 #include "py/objstr.h"
@@ -20,8 +21,11 @@
 
 // Class structure for PUPDevice
 typedef struct _iodevices_PUPDevice_obj_t {
-    mp_obj_base_t base;
-    pbio_iodev_t *iodev;
+    pb_pupdevices_obj_base_t pup_base;
+    // Mode used when initiating awaitable read. REVISIT: This should be stored
+    // on the awaitable instead, as extra context. For now, it is safe since
+    // concurrent reads with the same sensor are not permitten.
+    uint8_t last_mode;
 } iodevices_PUPDevice_obj_t;
 
 // pybricks.iodevices.PUPDevice.__init__
@@ -30,43 +34,31 @@ STATIC mp_obj_t iodevices_PUPDevice_make_new(const mp_obj_type_t *type, size_t n
         PB_ARG_REQUIRED(port));
 
     iodevices_PUPDevice_obj_t *self = mp_obj_malloc(iodevices_PUPDevice_obj_t, type);
-
-    pbio_port_id_t port = pb_type_enum_get_value(port_in, &pb_enum_type_Port);
-
-    self->iodev = pb_pup_device_get_device(port, PBIO_IODEV_TYPE_ID_LUMP_UART);
-
+    pb_pupdevices_init_class(&self->pup_base, port_in, PBIO_IODEV_TYPE_ID_LUMP_UART);
     return MP_OBJ_FROM_PTR(self);
 }
 
 // pybricks.iodevices.PUPDevice.info
 STATIC mp_obj_t iodevices_PUPDevice_info(mp_obj_t self_in) {
     iodevices_PUPDevice_obj_t *self = MP_OBJ_TO_PTR(self_in);
-
     mp_obj_t info_dict = mp_obj_new_dict(1);
-    mp_obj_dict_store(info_dict, MP_ROM_QSTR(MP_QSTR_id), MP_OBJ_NEW_SMALL_INT(self->iodev->info->type_id));
-
+    mp_obj_dict_store(info_dict, MP_ROM_QSTR(MP_QSTR_id), MP_OBJ_NEW_SMALL_INT(self->pup_base.iodev->info->type_id));
     return info_dict;
 }
 MP_DEFINE_CONST_FUN_OBJ_1(iodevices_PUPDevice_info_obj, iodevices_PUPDevice_info);
 
-// pybricks.iodevices.PUPDevice.read
-STATIC mp_obj_t iodevices_PUPDevice_read(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
-    PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args,
-        iodevices_PUPDevice_obj_t, self,
-        PB_ARG_REQUIRED(mode));
+STATIC mp_obj_t get_pup_data_tuple(mp_obj_t self_in) {
+    iodevices_PUPDevice_obj_t *self = MP_OBJ_TO_PTR(self_in);
+    void *data = pb_pupdevices_get_data(self_in, self->last_mode);
 
     uint8_t num_values;
     pbio_iodev_data_type_t type;
-
-    uint8_t mode = mp_obj_get_int(mode_in);
-    pb_assert(pbio_iodev_get_data_format(self->iodev, mode, &num_values, &type));
+    pb_assert(pbio_iodev_get_data_format(self->pup_base.iodev, self->last_mode, &num_values, &type));
 
     if (num_values == 0) {
         pb_assert(PBIO_ERROR_INVALID_ARG);
     }
 
-    void *data = pb_pup_device_get_data(self->iodev, mode);
-
     mp_obj_t values[PBIO_IODEV_MAX_DATA_SIZE];
 
     for (uint8_t i = 0; i < num_values; i++) {
@@ -92,6 +84,24 @@ STATIC mp_obj_t iodevices_PUPDevice_read(size_t n_args, const mp_obj_t *pos_args
 
     return mp_obj_new_tuple(num_values, values);
 }
+
+// pybricks.iodevices.PUPDevice.read
+STATIC mp_obj_t iodevices_PUPDevice_read(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
+    PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args,
+        iodevices_PUPDevice_obj_t, self,
+        PB_ARG_REQUIRED(mode));
+
+    self->last_mode = mp_obj_get_int(mode_in);
+
+    // We can re-use the same code as for specific sensor types, only the mode
+    // is not hardcoded per call, so we create that object here.
+    const pb_obj_pupdevices_method_t method = {
+        {&pb_type_pupdevices_method},
+        .mode = self->last_mode,
+        .get_values = get_pup_data_tuple,
+    };
+    return pb_pupdevices_method_call(MP_OBJ_FROM_PTR(&method), 1, 0, pos_args);
+}
 MP_DEFINE_CONST_FUN_OBJ_KW(iodevices_PUPDevice_read_obj, 1, iodevices_PUPDevice_read);
 
 // pybricks.iodevices.PUPDevice.write
@@ -115,7 +125,7 @@ STATIC mp_obj_t iodevices_PUPDevice_write(size_t n_args, const mp_obj_t *pos_arg
     uint8_t data[PBIO_IODEV_MAX_DATA_SIZE];
     uint8_t len;
     pbio_iodev_data_type_t type;
-    pb_assert(pbio_iodev_get_data_format(self->iodev, mode, &len, &type));
+    pb_assert(pbio_iodev_get_data_format(self->pup_base.iodev, mode, &len, &type));
 
     if (len != num_values) {
         pb_assert(PBIO_ERROR_INVALID_ARG);
@@ -142,9 +152,7 @@ STATIC mp_obj_t iodevices_PUPDevice_write(size_t n_args, const mp_obj_t *pos_arg
         }
     }
     // Set the data.
-    pb_pup_device_set_data(self->iodev, mode, data);
-
-    return mp_const_none;
+    return pb_pupdevices_set_data(&self->pup_base, mode, data);
 }
 MP_DEFINE_CONST_FUN_OBJ_KW(iodevices_PUPDevice_write_obj, 1, iodevices_PUPDevice_write);
 

pybricks/pupdevices.h

@@ -10,6 +10,34 @@
 
 #include "py/obj.h"
 
+#include <pbio/iodev.h>
+#include <pybricks/tools/pb_type_awaitable.h>
+
+/**
+ * Used in place of mp_obj_base_t in all pupdevices. This lets us share
+ * code for awaitables for all pupdevices.
+ */
+typedef struct _pb_pupdevices_obj_base_t {
+    mp_obj_base_t base;
+    pbio_iodev_t *iodev;
+    mp_obj_t awaitables;
+} pb_pupdevices_obj_base_t;
+
+/**
+ * Callable sensor method with a particular mode and return map.
+ */
+typedef struct {
+    mp_obj_base_t base;
+    pb_type_awaitable_return_t get_values;
+    uint8_t mode;
+} pb_obj_pupdevices_method_t;
+
+extern const mp_obj_type_t pb_type_pupdevices_method;
+
+#define PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(obj_name, mode_id, get_values_func) \
+    const pb_obj_pupdevices_method_t obj_name = \
+    {{&pb_type_pupdevices_method}, .mode = mode_id, .get_values = get_values_func}
+
 extern const mp_obj_type_t pb_type_pupdevices_ColorDistanceSensor;
 extern const mp_obj_type_t pb_type_pupdevices_ColorLightMatrix;
 extern const mp_obj_type_t pb_type_pupdevices_ColorSensor;
@@ -21,15 +49,21 @@ extern const mp_obj_type_t pb_type_pupdevices_Remote;
 extern const mp_obj_type_t pb_type_pupdevices_TiltSensor;
 extern const mp_obj_type_t pb_type_pupdevices_UltrasonicSensor;
 
-pbio_iodev_t *pupdevices_ColorDistanceSensor__get_device(mp_obj_t obj);
+mp_obj_t pb_pupdevices_method_call(mp_obj_t self_in, size_t n_args, size_t n_kw, const mp_obj_t *args);
+
+pb_pupdevices_obj_base_t *pupdevices_ColorDistanceSensor__get_device(mp_obj_t obj);
 void pb_type_Remote_cleanup(void);
 
-pbio_iodev_t *pb_pup_device_get_device(pbio_port_id_t port, pbio_iodev_type_id_t valid_id);
-void *pb_pup_device_get_data(pbio_iodev_t *iodev, uint8_t mode);
-void pb_pup_device_set_data(pbio_iodev_t *iodev, uint8_t mode, const void *data);
+void pb_pupdevices_init_class(pb_pupdevices_obj_base_t *self, mp_obj_t port_in, pbio_iodev_type_id_t valid_id);
+mp_obj_t pb_pupdevices_set_data(pb_pupdevices_obj_base_t *sensor, uint8_t mode, const void *data);
+void *pb_pupdevices_get_data(mp_obj_t self_in, uint8_t mode);
+void *pb_pupdevices_get_data_blocking(pb_pupdevices_obj_base_t *sensor, uint8_t mode);
 
 void pb_pup_device_setup_motor(pbio_port_id_t port, bool is_servo);
 
+// Delete me: refactor getter only still used for passive external light.
+pbio_iodev_t *pb_pup_device_get_device(pbio_port_id_t port, pbio_iodev_type_id_t valid_id);
+
 #endif // PYBRICKS_PY_PUPDEVICES
 
 #endif // PYBRICKS_INCLUDED_PYBRICKS_PUPDEVICES_H

pybricks/pupdevices/pb_module_pupdevices.c

@@ -14,33 +14,112 @@
 
 #include <pybricks/util_pb/pb_error.h>
 
+#include <py/runtime.h>
 #include <py/mphal.h>
 
-static void pb_pup_device_wait_ready(pbio_iodev_t *iodev) {
+/**
+ * Non-blocking version of powered up data getter. Will raise exception if
+ * sensor is not already in the right mode.
+ *
+ * @param [in]  sensor      The powered up device.
+ * @param [in]  mode        Desired mode.
+ * @return                  Void pointer to data.
+ */
+void *pb_pupdevices_get_data(mp_obj_t self_in, uint8_t mode) {
+    pb_pupdevices_obj_base_t *sensor = MP_OBJ_TO_PTR(self_in);
+    void *data;
+    pb_assert(pbio_uartdev_get_data(sensor->iodev, mode, &data));
+    return data;
+}
+
+/**
+ * Always-blocking version of powered up data getter. Can be used during sensor
+ * or motor initialization.
+ *
+ * @param [in]  sensor      The powered up device.
+ * @param [in]  mode        Desired mode.
+ * @return                  Void pointer to data.
+ */
+void *pb_pupdevices_get_data_blocking(pb_pupdevices_obj_base_t *sensor, uint8_t mode) {
+    pb_assert(pbio_uartdev_set_mode(sensor->iodev, mode));
     pbio_error_t err;
-    while ((err = pbio_uartdev_is_ready(iodev)) == PBIO_ERROR_AGAIN) {
+    while ((err = pbio_uartdev_is_ready(sensor->iodev)) == PBIO_ERROR_AGAIN) {
         MICROPY_EVENT_POLL_HOOK
     }
     pb_assert(err);
+    void *data;
+    pb_assert(pbio_uartdev_get_data(sensor->iodev, mode, &data));
+    return data;
 }
 
-// This will be replaced by an async handler
-void *pb_pup_device_get_data(pbio_iodev_t *iodev, uint8_t mode) {
-    pb_assert(pbio_uartdev_set_mode(iodev, mode));
-    pb_pup_device_wait_ready(iodev);
+STATIC bool pb_pup_device_test_completion(mp_obj_t self_in, uint32_t end_time) {
+    pb_pupdevices_obj_base_t *sensor = MP_OBJ_TO_PTR(self_in);
+    pbio_error_t err = pbio_uartdev_is_ready(sensor->iodev);
+    if (err == PBIO_ERROR_AGAIN) {
+        return false;
+    }
+    pb_assert(err);
+    return true;
+}
 
-    void *data;
-    pb_assert(pbio_uartdev_get_data(iodev, mode, &data));
+mp_obj_t pb_pupdevices_method_call(mp_obj_t self_in, size_t n_args, size_t n_kw, const mp_obj_t *args) {
+    assert(mp_obj_is_type(self_in, &pb_type_pupdevices_method));
+    pb_obj_pupdevices_method_t *method = MP_OBJ_TO_PTR(self_in);
+    mp_arg_check_num(n_args, n_kw, 1, 1, false);
+
+    mp_obj_t sensor_in = args[0];
+    pb_pupdevices_obj_base_t *sensor = MP_OBJ_TO_PTR(sensor_in);
+    pb_assert(pbio_uartdev_set_mode(sensor->iodev, method->mode));
+
+    return pb_type_awaitable_await_or_wait(
+        sensor_in,
+        sensor->awaitables,
+        pb_type_awaitable_end_time_none,
+        pb_pup_device_test_completion,
+        method->get_values,
+        pb_type_awaitable_cancel_none,
+        PB_TYPE_AWAITABLE_OPT_NONE);
+}
 
-    return data;
+MP_DEFINE_CONST_OBJ_TYPE(
+    pb_type_pupdevices_method, MP_QSTR_function, MP_TYPE_FLAG_BINDS_SELF | MP_TYPE_FLAG_BUILTIN_FUN,
+    call, pb_pupdevices_method_call,
+    unary_op, mp_generic_unary_op
+    );
+
+mp_obj_t pb_pupdevices_set_data(pb_pupdevices_obj_base_t *sensor, uint8_t mode, const void *data) {
+    pb_assert(pbio_uartdev_set_mode_with_data(sensor->iodev, mode, data));
+    return pb_type_awaitable_await_or_wait(
+        MP_OBJ_FROM_PTR(sensor),
+        sensor->awaitables,
+        pb_type_awaitable_end_time_none,
+        pb_pup_device_test_completion,
+        pb_type_awaitable_return_none,
+        pb_type_awaitable_cancel_none,
+        PB_TYPE_AWAITABLE_OPT_NONE); // choose opt raise on busy
 }
 
-// This will be replaced by an async handler
-void pb_pup_device_set_data(pbio_iodev_t *iodev, uint8_t mode, const void *data) {
-    pb_assert(pbio_uartdev_set_mode_with_data(iodev, mode, data));
-    pb_pup_device_wait_ready(iodev);
+void pb_pupdevices_init_class(pb_pupdevices_obj_base_t *self, mp_obj_t port_in, pbio_iodev_type_id_t valid_id) {
+
+    pb_module_tools_assert_blocking();
+
+    pbio_port_id_t port = pb_type_enum_get_value(port_in, &pb_enum_type_Port);
+
+    pbio_error_t err;
+    while ((err = pbdrv_ioport_get_iodev(port, &self->iodev)) == PBIO_ERROR_AGAIN) {
+        mp_hal_delay_ms(50);
+    }
+    pb_assert(err);
+
+    // Verify the ID or always allow generic LUMP device
+    if (self->iodev->info->type_id != valid_id && valid_id != PBIO_IODEV_TYPE_ID_LUMP_UART) {
+        pb_assert(PBIO_ERROR_NO_DEV);
+    }
+
+    self->awaitables = mp_obj_new_list(0, NULL);
 }
 
+// Delete me: refactor getter only still used for passive external light.
 pbio_iodev_t *pb_pup_device_get_device(pbio_port_id_t port, pbio_iodev_type_id_t valid_id) {
 
     pbio_iodev_t *iodev;
@@ -58,6 +137,7 @@ pbio_iodev_t *pb_pup_device_get_device(pbio_port_id_t port, pbio_iodev_type_id_t
     return iodev;
 }
 
+// Revisit: get rid of this abstraction
 void pb_pup_device_setup_motor(pbio_port_id_t port, bool is_servo) {
     // HACK: Built-in motors on BOOST Move hub do not have I/O ports associated
     // with them.
@@ -67,6 +147,8 @@ void pb_pup_device_setup_motor(pbio_port_id_t port, bool is_servo) {
     }
     #endif
 
+    pb_module_tools_assert_blocking();
+
     // Get the iodevice
     pbio_iodev_t *iodev;
     pbio_error_t err;
@@ -96,8 +178,10 @@ void pb_pup_device_setup_motor(pbio_port_id_t port, bool is_servo) {
         PBIO_IODEV_MODE_PUP_REL_MOTOR__POS;
 
     // Activate mode.
-    pb_assert(pbio_uartdev_set_mode(iodev, mode_id));
-    pb_pup_device_wait_ready(iodev);
+    pb_pupdevices_obj_base_t device = {
+        .iodev = iodev,
+    };
+    pb_pupdevices_get_data_blocking(&device, mode_id);
 
     #endif // !PYBRICKS_HUB_VIRTUALHUB
 }