Skip to content

Commit

Permalink
pybricks.ble.broadcast: Add transmit and receive
Browse files Browse the repository at this point in the history 
Adds support for broadcast.transmit and broadcast.received for hubs: PrimeHub, InventorHub, EssentialHub, TechnicHub, and CityHub
  • Loading branch information
@NStrijbosch
NStrijbosch committed Nov 30, 2021
1 parent bc11fd4 commit 662a229
Show file tree
Hide file tree
Showing 4 changed files with 213 additions and 7 deletions.
58 changes: 57 additions & 1 deletion lib/pbio/drv/bluetooth/bluetooth_btstack.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,7 @@ static pbdrv_bluetooth_on_event_t bluetooth_on_event;
static pbdrv_bluetooth_receive_handler_t receive_handler;
static pbdrv_bluetooth_receive_handler_t notification_handler;
static pup_handset_t handset;
static pbdrv_bluetooth_advertisement_data_handler_t advertisement_data_handler;
static uint8_t *event_packet;
static const pbdrv_bluetooth_btstack_platform_data_t *pdata = &pbdrv_bluetooth_btstack_platform_data;

Expand Down Expand Up @@ -351,6 +352,8 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe
handset.con_state = CON_STATE_CONNECT_FAILED;
}
}
} else if (advertisement_data_handler) {
advertisement_data_handler(data,data_length);
}

break;
Expand Down Expand Up @@ -431,6 +434,13 @@ void pbdrv_bluetooth_init(void) {

pybricks_service_server_init(pybricks_data_received, pybricks_configured);
nordic_spp_service_server_init(nordic_spp_packet_handler);

// Set the max number of device the hub can be a peripheral to
// This value gets checked when starting to advertise
// max 2 peripheral connections is is necessary for:
// - peripheral connection to connect to code.pybricks.com
// - advertising as a peripheral for pybricks.ble.broadcast.transmit
gap_set_max_number_peripheral_connections(2);
}

void pbdrv_bluetooth_power_on(bool on) {
Expand All @@ -447,7 +457,7 @@ const char *pbdrv_bluetooth_get_hub_name(void) {

static void init_advertising_data(void) {
bd_addr_t null_addr = { };
gap_advertisements_set_params(0x0030, 0x0030, 0x00, 0x00, null_addr, 0x07, 0x00);
gap_advertisements_set_params(0x0030, 0x0030, ADV_IND, 0x00, null_addr, 0x07, 0x00);

static const uint8_t adv_data[] = {
// Flags general discoverable, BR/EDR not supported
Expand Down Expand Up @@ -494,6 +504,52 @@ void pbdrv_bluetooth_stop_advertising(void) {
gap_advertisements_enable(false);
}

static PT_THREAD(data_advertising_task(struct pt *pt, pbio_task_t *task)) {
pbdrv_bluetooth_value_t *value = task->context;
static struct etimer timer;

PT_BEGIN(pt);

bd_addr_t null_addr = { };
gap_advertisements_set_params(0x0030, 0x0030, ADV_SCAN_IND, 0x00, null_addr, 0x07, 0x00);

// TODO: remove need for adv_data variable
// use value->data instead (I tried but didn't find the correct syntax...)
static uint8_t adv_data[31];
memcpy(&adv_data[0],&value->data,value->size);

gap_advertisements_set_data(value->size, (uint8_t *)adv_data);

// start advertising
gap_advertisements_enable(true);

// wait for 1000ms
etimer_set(&timer, 1000);
PT_WAIT_UNTIL(pt, etimer_expired(&timer));

// stop advertising
gap_advertisements_enable(false);

task->status = PBIO_SUCCESS;

PT_END(pt);
}

void pbdrv_bluetooth_start_data_advertising(pbdrv_bluetooth_value_t *value) {
static pbio_task_t task;
pbio_task_init(&task, data_advertising_task, value);
pbio_task_queue_add(task_queue, &task);
}

void pbdrv_bluetooth_start_scan(void) {
gap_set_scan_params(1, 0x30, 0x30, 0);
gap_start_scan();
}

void pbdrv_bluetooth_set_advertising_data_handler(pbdrv_bluetooth_advertisement_data_handler_t handler) {
advertisement_data_handler = handler;
}

bool pbdrv_bluetooth_is_connected(pbdrv_bluetooth_connection_t connection) {
if (connection == PBDRV_BLUETOOTH_CONNECTION_LE && le_con_handle != HCI_CON_HANDLE_INVALID) {
return true;
Expand Down
83 changes: 83 additions & 0 deletions lib/pbio/drv/bluetooth/bluetooth_stm32_cc2640.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -140,6 +140,7 @@ static bool bluetooth_ready;
static pbdrv_bluetooth_on_event_t bluetooth_on_event;
static pbdrv_bluetooth_receive_handler_t receive_handler;
static pbdrv_bluetooth_receive_handler_t notification_handler;
static pbdrv_bluetooth_advertisement_data_handler_t advertisement_data_handler;
static const pbdrv_bluetooth_stm32_cc2640_platform_data_t *pdata = &pbdrv_bluetooth_stm32_cc2640_platform_data;

/**
Expand Down Expand Up @@ -331,6 +332,88 @@ void pbdrv_bluetooth_stop_advertising(void) {
pbio_task_queue_add(task_queue, &task);
}

/**
* Sets advertising data and enables advertisements.
*/
static PT_THREAD(set_data_advertising(struct pt *pt, pbio_task_t *task)) {
pbdrv_bluetooth_value_t *value = task->context;
static struct etimer timer;

PT_BEGIN(pt);

// Set advertising data

PT_WAIT_WHILE(pt, write_xfer_size);
GAP_updateAdvertistigData(GAP_AD_TYPE_ADVERTISEMNT_DATA, value->size, value->data);
PT_WAIT_UNTIL(pt, hci_command_complete);

// start advertising
PT_WAIT_WHILE(pt, write_xfer_size);
// HACK: for compatability with official RI software adv_type should be ADV_SCAN_IND
// - ADV_SCAN_IND does not seem to advertise data at all,
// - ADV_NONCONN_IND only advertises if program is run over bluetooth (not when program in main.py)
// - ADV_IND works both when program is run over bluetooth and in main.py
// current hack: use ADV_IND. This means transmit on technic/city hub is not compatible with official RI hubs.
GAP_makeDiscoverable(ADV_IND, GAP_INITIATOR_ADDR_TYPE_PUBLIC, NULL,
GAP_CHANNEL_MAP_ALL, GAP_FILTER_POLICY_SCAN_ANY_CONNECT_ANY);
PT_WAIT_UNTIL(pt, hci_command_complete);
// ignoring response data

// wait for 1000ms
etimer_set(&timer, 1000);
PT_WAIT_UNTIL(pt, etimer_expired(&timer));

// stop advertising
PT_WAIT_WHILE(pt, write_xfer_size);
GAP_endDiscoverable();
PT_WAIT_UNTIL(pt, hci_command_complete);
// ignoring response data

task->status = PBIO_SUCCESS;

PT_END(pt);
}

void pbdrv_bluetooth_start_data_advertising(pbdrv_bluetooth_value_t *value) {
static pbio_task_t task;
pbio_task_init(&task, set_data_advertising, value);
pbio_task_queue_add(task_queue, &task);
}

static PT_THREAD(set_start_scan(struct pt *pt, pbio_task_t *task)) {
PT_BEGIN(pt);

// start scanning
PT_WAIT_WHILE(pt, write_xfer_size);
GAP_DeviceDiscoveryRequest(GAP_DEVICE_DISCOVERY_MODE_ALL, 1, GAP_FILTER_POLICY_SCAN_ANY_CONNECT_ANY);
PT_WAIT_UNTIL(pt, hci_command_status);

for (;;) {
advertising_data_received = false;
PT_WAIT_UNTIL(pt, {
advertising_data_received;
});

// TODO: Properly parse advertising data. For now, we are assuming that
// advertisement data always starts at read_buf[19] and its sizes is given at read_buf[18]
advertisement_data_handler(&read_buf[19],read_buf[18]);
}

task->status = PBIO_SUCCESS;

PT_END(pt);
}

void pbdrv_bluetooth_start_scan(void) {
static pbio_task_t task;
pbio_task_init(&task, set_start_scan, NULL);
pbio_task_queue_add(task_queue, &task);
}

void pbdrv_bluetooth_set_advertising_data_handler(pbdrv_bluetooth_advertisement_data_handler_t handler) {
advertisement_data_handler = handler;
}

bool pbdrv_bluetooth_is_connected(pbdrv_bluetooth_connection_t connection) {
if (connection == PBDRV_BLUETOOTH_CONNECTION_LE && conn_handle != NO_CONNECTION) {
return true;
Expand Down
30 changes: 30 additions & 0 deletions lib/pbio/include/pbdrv/bluetooth.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,15 @@ typedef void (*pbdrv_bluetooth_send_done_t)(void);
*/
typedef void (*pbdrv_bluetooth_receive_handler_t)(pbdrv_bluetooth_connection_t connection, const uint8_t *data, uint8_t size);

/**
* Callback that is called when BLE broadcast data is received
*
* @param [in] data The data that was received.
* @param [in] size The size of @p data in bytes.
*/
typedef void (*pbdrv_bluetooth_advertisement_data_handler_t)(const uint8_t *data, uint8_t size);


struct _pbdrv_bluetooth_send_context_t {
/** Callback that is called when the data has been sent. */
pbdrv_bluetooth_send_done_t done;
Expand Down Expand Up @@ -121,6 +130,27 @@ void pbdrv_bluetooth_start_advertising(void);
*/
void pbdrv_bluetooth_stop_advertising(void);

/**
* Starts data advertising process, including configuring advertisement data
* and telling the Bluetooth chip to start advertising.
*
* @param [in] value The advertising data
*/
void pbdrv_bluetooth_start_data_advertising(pbdrv_bluetooth_value_t *value);

/**
* Starts scanning for a advertisement data
*/
void pbdrv_bluetooth_start_scan();

/**
* Registers a callback that will be called when broadcast data is received in
* the advertisement data
*
* @param [in] handler The function that will be called.
*/
void pbdrv_bluetooth_set_advertising_data_handler(pbdrv_bluetooth_advertisement_data_handler_t handler);

/**
* Tests if a central is connected to the Bluetooth chip.
* @param [in] connection The type of connection of interest.
Expand Down
49 changes: 43 additions & 6 deletions pybricks/ble/pb_type_broadcast.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,8 @@

#include "py/objstr.h"

#include <pbdrv/bluetooth.h>

#include <pybricks/ble.h>

#include <pybricks/util_pb/pb_error.h>
Expand Down Expand Up @@ -36,9 +38,25 @@ typedef struct _ble_Broadcast_obj_t {
// There is at most one Broadcast object
ble_Broadcast_obj_t *broadcast_obj;

// NARD: Process scan results and populate any broadcast_signal_t whose
// crc32 hash matches the scanned broadcast. This process is started from
// function below.
// Broadcast header to comply with official LEGO MINDSTORMS App hub to hub word blocks
// 0xFF for manufacturer data, followed by 0x0397 LEGO company ID
// Note: advertising data does not start with length as commonly used
static uint8_t BROADCAST_HEADER[3] = {255,3,151};

// Handles received advertising data
STATIC void handle_receive(const uint8_t *value, uint8_t size) {
if (memcmp(&value[0],&BROADCAST_HEADER,3) == 0) {
for (size_t i = 0; i < broadcast_obj->n_signals; i++) {
if (memcmp(&broadcast_obj->signals[i].hash,&value[4],4) == 0
&& (broadcast_obj->signals[i].counter < value[3] ||
(broadcast_obj->signals[i].counter > 192 && value[3] <= 64))) {
memcpy(&broadcast_obj->signals[i].message,&value[8],size - 8);
broadcast_obj->signals[i].size = size - 8;
broadcast_obj->signals[i].counter = value[3];
}
}
}
}

// pybricks.ble.Broadcast.__init__
STATIC mp_obj_t ble_Broadcast_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) {
Expand Down Expand Up @@ -72,7 +90,9 @@ STATIC mp_obj_t ble_Broadcast_make_new(const mp_obj_type_t *type, size_t n_args,

}

// NARD: Call function to start scanning.
pbdrv_bluetooth_set_advertising_data_handler(handle_receive);

pbdrv_bluetooth_start_scan();

return MP_OBJ_FROM_PTR(broadcast_obj);
}
Expand Down Expand Up @@ -128,11 +148,28 @@ STATIC mp_obj_t ble_Broadcast_transmit(size_t n_args, const mp_obj_t *pos_args,
signal->size = byte_data->len;
signal->counter++;

// NARD: Broadcast the updated signal.
struct {
pbdrv_bluetooth_value_t value;
uint8_t header[3];
uint8_t index;
uint32_t hash;
char payload[23];
} __attribute__((packed)) msg;

msg.value.size = signal->size + 8;
memcpy(msg.header, BROADCAST_HEADER, 3);
msg.index = signal->counter;
msg.hash = signal->hash;
memcpy(msg.payload, signal->message, signal->size);

pbdrv_bluetooth_start_data_advertising(&msg.value);

return mp_const_none;
}
STATIC MP_DEFINE_CONST_FUN_OBJ_KW(ble_Broadcast_transmit_obj, 1, ble_Broadcast_transmit);
STATIC MP_DEFINE_CONST_FUN_OBJ_KW(// NARD: Process scan results and populate any broadcast_signal_t whose
// crc32 hash matches the scanned broadcast. This process is started from
// function below.
ble_Broadcast_transmit_obj, 1, ble_Broadcast_transmit);

// dir(pybricks.ble.Broadcast)
STATIC const mp_rom_map_elem_t ble_Broadcast_locals_dict_table[] = {
Expand Down

0 comments on commit 662a229

Please sign in to comment.