diff --git a/lib/pbio/src/broadcast.c b/lib/pbio/src/broadcast.c index 987a6de8c..012b18cf9 100644 --- a/lib/pbio/src/broadcast.c +++ b/lib/pbio/src/broadcast.c @@ -136,7 +136,7 @@ void pbio_broadcast_transmit(uint32_t hash, const uint8_t *payload, uint8_t size void pbio_broadcast_parse_advertising_data(const uint8_t *data, uint8_t size) { // Return immediately for programs that don't use broadcast. - if (!num_scan_signals || size < 9) { + if (!num_scan_signals || size < PBIO_BROADCAST_META_SIZE) { return; } diff --git a/pybricks/ble/pb_type_broadcast.c b/pybricks/ble/pb_type_broadcast.c index 98364a2f3..13cdb5549 100644 --- a/pybricks/ble/pb_type_broadcast.c +++ b/pybricks/ble/pb_type_broadcast.c @@ -11,6 +11,7 @@ #include #include +#include #include @@ -21,6 +22,185 @@ #include #include +enum { + /** Zero-terminated string. */ + BROADCAST_DATA_TYPE_STRING = 0x00, + + /** little-endian 16-bit signed integer. */ + BROADCAST_DATA_TYPE_INT16 = 0x01, + + /** little-endian 32-bit signed integer. */ + BROADCAST_DATA_TYPE_INT32 = 0x02, + + /** little-endian 32-bit floating point. */ + BROADCAST_DATA_TYPE_FLOAT = 0x03, +}; + +#define BROADCAST_MAX_OBJECTS (8) + +// Broadcast data format: HEADER + ENCODING + DATA +// +// HEADER (8 bit) +// BIT 0--3 (LSB): Tuple size. +// BIT 4--8 (MSB): Reserved. +// +// ENCODING (16 bit little-endian) +// BIT 0--1 (LSB): Data type of first object in tuple, or type of the single object +// BIT 2--3 : Data type of second object in tuple +// ... +// BIT 15--16 (MSB): Data type of eighth object in tuple +// +// DATA (up to 20 bytes, representing tuple with up to 8 values.) + +// Encodes objects into a format for broadcasting +STATIC void broadcast_encode_data(mp_obj_t data_in, uint8_t *dest, uint8_t *len) { + + // Send empty message for None. + if (data_in == mp_const_none) { + *len = 0; + return; + } + + // If it's a single object, remember this for transmission but treat as tuple anyway. + bool is_single_object = false; + if (mp_obj_is_int(data_in) || mp_obj_is_float(data_in) || mp_obj_is_str(data_in)) { + data_in = mp_obj_new_tuple(1, &data_in); + is_single_object = true; + } + + // Iterable buffer. + mp_obj_iter_buf_t iter_buf; + mp_obj_t data_iter = mp_getiter(data_in, &iter_buf); + + // Length is at least the header with the encoding + *len = 3; + + // Reset encoding. + uint16_t *encoding = (uint16_t *)&dest[1]; + *encoding = 0; + + // Go through all values. + uint8_t n; + for (n = 0; n < BROADCAST_MAX_OBJECTS; n++) { + mp_obj_t obj = mp_iternext(data_iter); + + // Break when iterator complete. + if (obj == MP_OBJ_STOP_ITERATION) { + break; + } + + // Encode integer + if (mp_obj_is_int(obj)) { + mp_int_t value = mp_obj_get_int(obj); + + // Check integer size. + if (value < INT16_MIN || value > INT16_MAX) { + // Result must be inside max data length. + if (*len + sizeof(int32_t) > PBIO_BROADCAST_MAX_PAYLOAD_SIZE) { + pb_assert(PBIO_ERROR_INVALID_ARG); + } + // Encode 32 bit integer. + *encoding |= BROADCAST_DATA_TYPE_INT32 << n * 2; + *(int32_t *)&dest[*len] = value; + *len += sizeof(int32_t); + } else { + // Result must be inside max data length. + if (*len + sizeof(int16_t) > PBIO_BROADCAST_MAX_PAYLOAD_SIZE) { + pb_assert(PBIO_ERROR_INVALID_ARG); + } + // Encode 16 bit integer. + *encoding |= BROADCAST_DATA_TYPE_INT16 << n * 2; + *(int16_t *)&dest[*len] = value; + *len += sizeof(int16_t); + } + } else if (mp_obj_is_float(obj)) { + // Result must be inside max data length. + if (*len + sizeof(mp_float_t) > PBIO_BROADCAST_MAX_PAYLOAD_SIZE) { + pb_assert(PBIO_ERROR_INVALID_ARG); + } + // Encode float + *encoding |= BROADCAST_DATA_TYPE_FLOAT << n * 2; + // Revisit: Make proper pack/unpack functions. + mp_float_t f = mp_obj_get_float(obj); + int32_t *encoded = (int32_t *)&f; + *(int32_t *)&dest[*len] = *encoded; + *len += sizeof(mp_float_t); + } else { + // get string info. + GET_STR_DATA_LEN(obj, str_data, str_len); + + // Result must be inside max data length. + if (*len + str_len + 1 > PBIO_BROADCAST_MAX_PAYLOAD_SIZE) { + pb_assert(PBIO_ERROR_INVALID_ARG); + } + + // Copy string including null terminator. + *encoding |= BROADCAST_DATA_TYPE_STRING << n * 2; + memcpy(&dest[*len], str_data, str_len + 1); + *len += str_len + 1; + } + } + + // Set header to number of objects, or 0 if it is not a tuple. + dest[0] = is_single_object ? 0 : n; +} + +STATIC mp_obj_t broadcast_decode_data(uint8_t *data, uint8_t len) { + + // Pybricks data requires at least 4 bytes + if (len < 4) { + return mp_const_none; + } + + // Get encoding data + uint16_t encoding = *(uint16_t *)&data[1]; + + // If tuple size is 0 but there is nonzero data, it's a single value. + // We'll remember this for the return value but but treat it as a tuple. + // while unpacking the data. + bool is_single_object = data[0] == 0 && len > 3; + uint8_t n = is_single_object ? 1 : data[0]; + + // Check that objects are within bounds. + if (n > BROADCAST_MAX_OBJECTS) { + return mp_const_none; + } + + // Populate all objects + uint8_t *value = &data[3]; + mp_obj_t objs[BROADCAST_MAX_OBJECTS]; + for (uint8_t i = 0; i < n; i++) { + // Decode object based on data type. + switch ((encoding >> i * 2) & 0x03) { + case BROADCAST_DATA_TYPE_STRING: { + uint8_t str_size = strlen((char *)value); + objs[i] = mp_obj_new_str((char *)value, str_size); + value += str_size + 1; + break; + } + case BROADCAST_DATA_TYPE_INT16: + objs[i] = mp_obj_new_int(*(int16_t *)value); + value += sizeof(int16_t); + break; + case BROADCAST_DATA_TYPE_INT32: + objs[i] = mp_obj_new_int(*(int32_t *)value); + value += sizeof(int32_t); + break; + case BROADCAST_DATA_TYPE_FLOAT: { + // Revisit: Make proper pack/unpack functions. + uint32_t encoded = *(uint32_t *)value; + mp_float_t *f = (mp_float_t *)&encoded; + objs[i] = mp_obj_new_float(*f); + value += sizeof(mp_float_t); + break; + } + } + } + + // Return the single object or tuple + return is_single_object ? objs[0] : mp_obj_new_tuple(n, objs); +} + // Class structure for Broadcast typedef struct _ble_Broadcast_obj_t { mp_obj_base_t base; @@ -104,30 +284,70 @@ STATIC mp_obj_t ble_Broadcast_receive_bytes(size_t n_args, const mp_obj_t *pos_a uint8_t *data; uint8_t size; pbio_broadcast_receive(broadcast_get_hash(topic_in), &data, &size); + + // Return bytes as-is. return mp_obj_new_bytes(data, size); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(ble_Broadcast_receive_bytes_obj, 1, ble_Broadcast_receive_bytes); +// pybricks.ble.Broadcast.receive +STATIC mp_obj_t ble_Broadcast_receive(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, + ble_Broadcast_obj_t, self, + PB_ARG_REQUIRED(topic)); + + (void)self; + + uint8_t *data; + uint8_t size; + pbio_broadcast_receive(broadcast_get_hash(topic_in), &data, &size); + + // Return decoded data + return broadcast_decode_data(data, size); +} +STATIC MP_DEFINE_CONST_FUN_OBJ_KW(ble_Broadcast_receive_obj, 1, ble_Broadcast_receive); + // pybricks.ble.Broadcast.send_bytes STATIC mp_obj_t ble_Broadcast_send_bytes(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, ble_Broadcast_obj_t, self, PB_ARG_REQUIRED(topic), - PB_ARG_REQUIRED(message)); + PB_ARG_REQUIRED(data)); (void)self; // Assert that data argument are bytes. - if (!(mp_obj_is_type(message_in, &mp_type_bytes) || mp_obj_is_type(message_in, &mp_type_bytearray))) { + if (!(mp_obj_is_type(data_in, &mp_type_bytes) || mp_obj_is_type(data_in, &mp_type_bytearray))) { pb_assert(PBIO_ERROR_INVALID_ARG); } - // Unpack user argument to update signal. - mp_obj_str_t *byte_data = MP_OBJ_TO_PTR(message_in); + // Transmit bytes as-is. + mp_obj_str_t *byte_data = MP_OBJ_TO_PTR(data_in); pbio_broadcast_transmit(broadcast_get_hash(topic_in), byte_data->data, byte_data->len); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(ble_Broadcast_send_bytes_obj, 1, ble_Broadcast_send_bytes); +// pybricks.ble.Broadcast.send +STATIC mp_obj_t ble_Broadcast_send(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, + ble_Broadcast_obj_t, self, + PB_ARG_REQUIRED(topic), + PB_ARG_REQUIRED(data)); + + (void)self; + + // Encode the data + uint8_t buf[PBIO_BROADCAST_MAX_PAYLOAD_SIZE]; + uint8_t size; + broadcast_encode_data(data_in, buf, &size); + + // Transmit it. + pbio_broadcast_transmit(broadcast_get_hash(topic_in), buf, size); + + return mp_const_none; +} +STATIC MP_DEFINE_CONST_FUN_OBJ_KW(ble_Broadcast_send_obj, 1, ble_Broadcast_send); + // pybricks.ble.Broadcast.scan STATIC mp_obj_t ble_Broadcast_scan(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, @@ -141,7 +361,9 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(ble_Broadcast_scan_obj, 1, ble_Broadcast_scan) // dir(pybricks.ble.Broadcast) STATIC const mp_rom_map_elem_t ble_Broadcast_locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR_receive), MP_ROM_PTR(&ble_Broadcast_receive_obj) }, { MP_ROM_QSTR(MP_QSTR_receive_bytes), MP_ROM_PTR(&ble_Broadcast_receive_bytes_obj) }, + { MP_ROM_QSTR(MP_QSTR_send), MP_ROM_PTR(&ble_Broadcast_send_obj) }, { MP_ROM_QSTR(MP_QSTR_send_bytes), MP_ROM_PTR(&ble_Broadcast_send_bytes_obj) }, { MP_ROM_QSTR(MP_QSTR_scan), MP_ROM_PTR(&ble_Broadcast_scan_obj) }, }; diff --git a/tests/pup/ble/ble2_prime.py b/tests/pup/ble/ble2_prime.py new file mode 100644 index 000000000..8337cb72b --- /dev/null +++ b/tests/pup/ble/ble2_prime.py @@ -0,0 +1,41 @@ +# Copyright (c) 2021 The Pybricks Authors + +""" +Hardware Module: Technic Hub (ble1_technic.py) and Prime Hub (ble1_prime.py) + +Description: Tests broadcast receive and transmit. +""" + +from pybricks.hubs import PrimeHub +from pybricks.parameters import Color +from pybricks.tools import wait +from pybricks.ble import Broadcast + +from umath import pi + +hub = PrimeHub() +hub.light.on(Color.WHITE) + +# Initialize broadcast with two topics. +radio = Broadcast(topics=["data"]) + +# This hub only sends, so we can turn off scanning. +radio.scan(False) + +for i in range(20): + + # Send a number + counter = radio.send("data", 123) + wait(1000) + + # Send a large number and a float + counter = radio.send("data", (-456789, pi)) + wait(1000) + + # Send even more. + counter = radio.send("data", ("or", 1.234, "this!")) + wait(1000) + + # Send nothing, to clear their data. + counter = radio.send("data", None) + wait(1000) diff --git a/tests/pup/ble/ble2_technic.py b/tests/pup/ble/ble2_technic.py new file mode 100644 index 000000000..82b0c4574 --- /dev/null +++ b/tests/pup/ble/ble2_technic.py @@ -0,0 +1,26 @@ +# Copyright (c) 2021 The Pybricks Authors + +""" +Hardware Module: Technic Hub (ble2_technic.py) and Prime Hub (ble2_prime.py) + +Description: Tests broadcast receive and transmit. +""" + +from pybricks.hubs import TechnicHub +from pybricks.parameters import Color +from pybricks.tools import wait +from pybricks.ble import Broadcast + +hub = TechnicHub() +hub.light.on(Color.WHITE) + +# Initialize broadcast with one topic. +radio = Broadcast(["data"]) + +for i in range(100): + + # Get the data topic. + data = radio.receive("data") + print(data) + + wait(1000)