123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553 |
- /*
- * Copyright (C) 2019 Freie Universität Berlin
- *
- * This file is subject to the terms and conditions of the GNU Lesser
- * General Public License v2.1. See the file LICENSE in the top level
- * directory for more details.
- */
- /**
- * @ingroup examples
- * @{
- *
- * @file
- * @brief (Mock-up) BLE heart rate sensor example
- *
- * @author Hauke Petersen <hauke.petersen@fu-berlin.de>
- *
- * @}
- */
- #include <stdio.h>
- #include <stdint.h>
- #include "assert.h"
- #include "event/timeout.h"
- #include "nimble_riot.h"
- #include "net/bluetil/ad.h"
- #include "periph/gpio.h"
- #include "timex.h"
- #include "roomba.h"
- #include "board.h"
- #include "host/ble_hs.h"
- #include "host/ble_gatt.h"
- #include "services/gap/ble_svc_gap.h"
- #include "services/gatt/ble_svc_gatt.h"
- #define BLE_GATT_SVC_IPS 0x1821
- #define BLE_GATT_CHAR_IPS_CONFIG 0x2AAD
- #define BLE_GATT_CHAR_LATITUDE 0x2AAE
- #define BLE_GATT_CHAR_LONGITUDE 0x2AAF
- #define BLE_GATT_CHAR_LOCAL_NORTH 0x2AB0
- #define BLE_GATT_CHAR_LOCAL_EAST 0x2AB1
- #define BLE_GATT_CHAR_FLOOR_N 0x2AB2
- #define BLE_GATT_CHAR_ALTITUDE 0x2AB3
- #define BLE_GATT_CHAR_UNCERTAIN 0x2AB4
- #define BLE_GATT_CHAR_LOC_NAME 0x2AB4
- #define IPS_CONFIG_COORD_PRESENT (1 << 0)
- #define IPS_CONFIG_COORD_IN_ADV (1 << 1)
- #define IPS_CONFIG_TXP_IN_ADV (1 << 2)
- #define IPS_CONFIG_ALT_IN_ADV (1 << 3)
- #define IPS_CONFIG_FLOOR_IN_ADV (1 << 4)
- #define IPS_CONFIG_UNCE_IN_ADV (1 << 5)
- #define IPS_CONFIG_LOC_NAME_AVAIL (1 << 6)
- static const char *_device_name = "ROOMBAHAHAHACKED";
- static const char *_manufacturer_name = "Insane adding machines";
- static const char *_model_number = "2X";
- static const char *_serial_number = "a8b302c7f3-29183-x8";
- static const char *_fw_ver = "1.0.0";
- static const char *_hw_ver = "1.0";
- static event_queue_t _eq;
- static event_t _update_evt;
- static event_timeout_t _update_timeout_evt;
- static uint16_t _conn_handle;
- static uint16_t _ips_val_handle;
- static uint16_t _sensors_val_handle;
- static int _ips_handler(uint16_t conn_handle, uint16_t attr_handle,
- struct ble_gatt_access_ctxt *ctxt, void *arg);
- static int _devinfo_handler(uint16_t conn_handle, uint16_t attr_handle,
- struct ble_gatt_access_ctxt *ctxt, void *arg);
- static int _roomba_ctrl_handler(uint16_t conn_handle, uint16_t attr_handle,
- struct ble_gatt_access_ctxt *ctxt, void *arg);
- static int _roomba_sensors_handler(uint16_t conn_handle, uint16_t attr_handle,
- struct ble_gatt_access_ctxt *ctxt, void *arg);
- static int _bas_handler(uint16_t conn_handle, uint16_t attr_handle,
- struct ble_gatt_access_ctxt *ctxt, void *arg);
- static void _start_advertising(void);
- /* UUID = 35f28386-3070-4f3b-ba38-27507e991760 */
- static const ble_uuid128_t gatt_svr_svc_roomba = BLE_UUID128_INIT(
- 0x60, 0x17, 0x99, 0x7e, 0x50, 0x27, 0x38, 0xba,
- 0x3b, 0x4f, 0x70, 0x30, 0x86, 0x83, 0xf2, 0x35
- );
- /* UUID = 35f28386-3070-4f3b-ba38-27507e991762 */
- static const ble_uuid128_t gatt_svr_chr_roomba_ctl = BLE_UUID128_INIT(
- 0x62, 0x17, 0x99, 0x7e, 0x50, 0x27, 0x38, 0xba,
- 0x3b, 0x4f, 0x70, 0x30, 0x86, 0x83, 0xf2, 0x35
- );
- /* UUID = 35f28386-3070-4f3b-ba38-27507e991764 */
- static const ble_uuid128_t gatt_svr_chr_roomba_sensors = BLE_UUID128_INIT(
- 0x64, 0x17, 0x99, 0x7e, 0x50, 0x27, 0x38, 0xba,
- 0x3b, 0x4f, 0x70, 0x30, 0x86, 0x83, 0xf2, 0x35
- );
- /* UUID = 35f28386-3070-4f3b-ba38-27507e991766 */
- static const ble_uuid128_t gatt_svr_chr_roomba_sensors_notify = BLE_UUID128_INIT(
- 0x66, 0x17, 0x99, 0x7e, 0x50, 0x27, 0x38, 0xba,
- 0x3b, 0x4f, 0x70, 0x30, 0x86, 0x83, 0xf2, 0x35
- );
- /* GATT service definitions */
- static const struct ble_gatt_svc_def gatt_svr_svcs[] = {
- {
- /* Indoor positioning service */
- .type = BLE_GATT_SVC_TYPE_PRIMARY,
- .uuid = BLE_UUID16_DECLARE(BLE_GATT_SVC_IPS),
- .characteristics = (struct ble_gatt_chr_def[]) { {
- .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_IPS_CONFIG),
- .access_cb = _ips_handler,
- .val_handle = &_ips_val_handle,
- .flags = BLE_GATT_CHR_F_NOTIFY,
- }, {
- .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_LATITUDE),
- .access_cb = _ips_handler,
- .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE,
- }, {
- .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_LONGITUDE),
- .access_cb = _ips_handler,
- .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE,
- }, {
- 0, /* no more characteristics in this service */
- }, }
- },
- {
- /* Roomba controls */
- .type = BLE_GATT_SVC_TYPE_PRIMARY,
- .uuid = (ble_uuid_t*) &gatt_svr_svc_roomba.u,
- .characteristics = (struct ble_gatt_chr_def[]) { {
- /* Characteristic: ROOMBA CTRL */
- .uuid = (ble_uuid_t*) &gatt_svr_chr_roomba_ctl.u,
- .access_cb = _roomba_ctrl_handler,
- .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE,
- }, {
- /* Characteristic: ROOMBA sensors read/notify */
- .uuid = (ble_uuid_t*) &gatt_svr_chr_roomba_sensors.u,
- .access_cb = _roomba_sensors_handler,
- .flags = BLE_GATT_CHR_F_READ,
- }, {
- .uuid = (ble_uuid_t*) &gatt_svr_chr_roomba_sensors_notify.u,
- .access_cb = _roomba_sensors_handler,
- .val_handle = &_sensors_val_handle,
- .flags = BLE_GATT_CHR_F_NOTIFY,
- }, {
- 0, /* no more characteristics in this service */
- }, }
- },
- {
- /* Device Information Service */
- .type = BLE_GATT_SVC_TYPE_PRIMARY,
- .uuid = BLE_UUID16_DECLARE(BLE_GATT_SVC_DEVINFO),
- .characteristics = (struct ble_gatt_chr_def[]) { {
- .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_MANUFACTURER_NAME),
- .access_cb = _devinfo_handler,
- .flags = BLE_GATT_CHR_F_READ,
- }, {
- .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_MODEL_NUMBER_STR),
- .access_cb = _devinfo_handler,
- .flags = BLE_GATT_CHR_F_READ,
- }, {
- .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_SERIAL_NUMBER_STR),
- .access_cb = _devinfo_handler,
- .flags = BLE_GATT_CHR_F_READ,
- }, {
- .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_FW_REV_STR),
- .access_cb = _devinfo_handler,
- .flags = BLE_GATT_CHR_F_READ,
- }, {
- .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_HW_REV_STR),
- .access_cb = _devinfo_handler,
- .flags = BLE_GATT_CHR_F_READ,
- }, {
- 0, /* no more characteristics in this service */
- }, }
- },
- {
- /* Battery Level Service */
- .type = BLE_GATT_SVC_TYPE_PRIMARY,
- .uuid = BLE_UUID16_DECLARE(BLE_GATT_SVC_BAS),
- .characteristics = (struct ble_gatt_chr_def[]) { {
- .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_BATTERY_LEVEL),
- .access_cb = _bas_handler,
- .flags = BLE_GATT_CHR_F_READ,
- }, {
- 0, /* no more characteristics in this service */
- }, }
- },
- {
- 0, /* no more services */
- },
- };
- int cmd_mode(int argc, char **argv);
- uint8_t get_mode(void);
- int switch_mode(int newmode);
- static uint8_t cmd_buf[20];
- static int cmd_buf_off = 0;
- static int cmd_buf_exp_len = 0;
- void order66(void);
- static void parse_bt_char(uint8_t c)
- {
- uint8_t mode;
- cmd_buf[cmd_buf_off++] = c;
- if (c == 0xFF) {
- int len = cmd_buf_off - 1;
- cmd_buf_off = 0;
- if (len == 0)
- return;
- switch (cmd_buf[0]) {
- case 'M':
- mode = cmd_buf[1];
- if (len < 1)
- return;
- printf("Mode switch: %d\n", mode);
- if (mode <= ROOMBA_MODE_DOCK) {
- switch_mode(mode);
- }
- break;
- case 'B':
- printf("Order 66\n");
- order66();
- break;
- case 'D':
- {
- int16_t speed, radius;
- if (len < 5)
- return;
- memcpy(&speed, cmd_buf + 1, 2);
- memcpy(&radius, cmd_buf + 3, 2);
- printf("Drive %hd %hd\n", speed, radius);
- motors_drive(speed, radius);
- }
- break;
- case 'C':
- {
- if (len < 4)
- return;
- printf("Clean brushes/vac/sidebrush: %d %d %d\n", cmd_buf[1], cmd_buf[2], cmd_buf[3]);
- motors_clean_set(cmd_buf[1], cmd_buf[2], cmd_buf[3]);
- }
- break;
- case 'S':
- motors_stop();
- break;
- default:
- printf("Unknown command %02x\n", cmd_buf[0]);
- }
- }
- }
- static int _roomba_sensors_handler(
- uint16_t conn_handle, uint16_t attr_handle,
- struct ble_gatt_access_ctxt *ctxt, void *arg)
- {
- int rc = 0;
- uint16_t om_len;
- ble_uuid_t* read_uuid = (ble_uuid_t*) &gatt_svr_chr_roomba_sensors.u;
- static uint8_t ch;
- struct roomba_sensors_data *bt_sensors;
- puts("[BT] Sensor command");
- (void) conn_handle;
- (void) attr_handle;
- (void) arg;
- bt_sensors = roomba_sensors();
- printf("bt_sensors.valid = %s\n", bt_sensors->valid?"yes":"no");
- if (ble_uuid_cmp(ctxt->chr->uuid, read_uuid) == 0) {
- puts("access to characteristic 'roomba sensors'");
- rc = os_mbuf_append(ctxt->om, bt_sensors, sizeof(struct roomba_sensors_data));
- puts("");
- return rc;
- }
- puts("unhandled uuid!");
- return 1;
- }
- static int _roomba_ctrl_handler(
- uint16_t conn_handle, uint16_t attr_handle,
- struct ble_gatt_access_ctxt *ctxt, void *arg)
- {
- int rc = 0;
- uint16_t om_len;
- ble_uuid_t* write_uuid = (ble_uuid_t*) &gatt_svr_chr_roomba_ctl.u;
- static uint8_t ch;
- puts("[BT] Command");
- (void) conn_handle;
- (void) attr_handle;
- (void) arg;
- int mode = get_mode();
- if (ble_uuid_cmp(ctxt->chr->uuid, write_uuid) == 0) {
- puts("access to characteristic 'roomba ctl'");
- switch (ctxt->op) {
- case BLE_GATT_ACCESS_OP_READ_CHR:
- puts("read mode");
- /* send given data to the client */
- rc = os_mbuf_append(ctxt->om, &mode, sizeof(unsigned int));
- break;
- case BLE_GATT_ACCESS_OP_WRITE_CHR:
- rc = ble_hs_mbuf_to_flat(ctxt->om, &ch, 1, &om_len);
- printf("om_len = %d, char: %02x\n", om_len, ch);
- parse_bt_char(ch);
- break;
- case BLE_GATT_ACCESS_OP_READ_DSC:
- puts("read from descriptor");
- break;
- case BLE_GATT_ACCESS_OP_WRITE_DSC:
- puts("write to descriptor");
- break;
- default:
- puts("unhandled operation!");
- rc = 1;
- break;
- }
- puts("");
- return rc;
- }
- puts("unhandled uuid!");
- return 1;
- }
- static int _ips_handler(uint16_t conn_handle, uint16_t attr_handle,
- struct ble_gatt_access_ctxt *ctxt, void *arg)
- {
- (void)conn_handle;
- (void)attr_handle;
- (void)arg;
- int res;
- static uint32_t lat = 0x01020304, lon = 0x05060708;
- puts("[ACCESS] indoor positioning value");
- if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) {
- printf("[READ] ");
- if (ble_uuid_u16(ctxt->chr->uuid) == BLE_GATT_CHAR_LATITUDE) {
- printf("LAT\n");
- res = os_mbuf_append(ctxt->om, &lat, sizeof(lat));
- return (res == 0)? res: BLE_ATT_ERR_INSUFFICIENT_RES;
- }
- if (ble_uuid_u16(ctxt->chr->uuid) == BLE_GATT_CHAR_LONGITUDE) {
- printf("LON\n");
- res = os_mbuf_append(ctxt->om, &lon, sizeof(lon));
- return (res == 0)? res: BLE_ATT_ERR_INSUFFICIENT_RES;
- }
- } else if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
- printf("[WRITE] ");
- if (ble_uuid_u16(ctxt->chr->uuid) == BLE_GATT_CHAR_LATITUDE) {
- printf("LAT\n");
- memcpy(&lat, ctxt->om->om_databuf, sizeof(lat));
- res = os_mbuf_append(ctxt->om, &lat, sizeof(lat));
- return (res == 0)? res: BLE_ATT_ERR_INSUFFICIENT_RES;
- }
- if (ble_uuid_u16(ctxt->chr->uuid) == BLE_GATT_CHAR_LONGITUDE) {
- printf("LON\n");
- printf("%08x\n", *((unsigned int *)(ctxt->om->om_databuf)));
- printf("%08x\n", *((unsigned int *)(ctxt->om->om_databuf + 4)));
- memcpy(&lon, ctxt->om->om_databuf, sizeof(lon));
- res = os_mbuf_append(ctxt->om, &lon, sizeof(lon));
- return (res == 0)? res: BLE_ATT_ERR_INSUFFICIENT_RES;
- }
- }
- return BLE_ATT_ERR_INSUFFICIENT_RES;
- }
- static int _devinfo_handler(uint16_t conn_handle, uint16_t attr_handle,
- struct ble_gatt_access_ctxt *ctxt, void *arg)
- {
- (void)conn_handle;
- (void)attr_handle;
- (void)arg;
- const char *str;
- switch (ble_uuid_u16(ctxt->chr->uuid)) {
- case BLE_GATT_CHAR_MANUFACTURER_NAME:
- puts("[READ] device information service: manufacturer name value");
- str = _manufacturer_name;
- break;
- case BLE_GATT_CHAR_MODEL_NUMBER_STR:
- puts("[READ] device information service: model number value");
- str = _model_number;
- break;
- case BLE_GATT_CHAR_SERIAL_NUMBER_STR:
- puts("[READ] device information service: serial number value");
- str = _serial_number;
- break;
- case BLE_GATT_CHAR_FW_REV_STR:
- puts("[READ] device information service: firmware revision value");
- str = _fw_ver;
- break;
- case BLE_GATT_CHAR_HW_REV_STR:
- puts("[READ] device information service: hardware revision value");
- str = _hw_ver;
- break;
- default:
- return BLE_ATT_ERR_UNLIKELY;
- }
- int res = os_mbuf_append(ctxt->om, str, strlen(str));
- return (res == 0) ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
- }
- static int _bas_handler(uint16_t conn_handle, uint16_t attr_handle,
- struct ble_gatt_access_ctxt *ctxt, void *arg)
- {
- (void)conn_handle;
- (void)attr_handle;
- (void)arg;
- puts("[READ] battery level service: battery level value");
- uint8_t level = 50; /* this battery will never drain :-) */
- int res = os_mbuf_append(ctxt->om, &level, sizeof(level));
- return (res == 0) ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
- }
- static int gap_event_cb(struct ble_gap_event *event, void *arg)
- {
- (void)arg;
- switch (event->type) {
- case BLE_GAP_EVENT_CONNECT:
- printf("Ev: connect, Status: %d\n", event->connect.status);
- if (event->connect.status) {
- _start_advertising();
- LED2_OFF;
- return 0;
- }
- LED2_ON;
- _conn_handle = event->connect.conn_handle;
- break;
- case BLE_GAP_EVENT_DISCONNECT:
- _start_advertising();
- break;
- case BLE_GAP_EVENT_SUBSCRIBE:
- if (event->subscribe.attr_handle == _ips_val_handle) {
- if (event->subscribe.cur_notify == 1) {
- }
- else {
- }
- }
- break;
- }
- return 0;
- }
- static void _start_advertising(void)
- {
- struct ble_gap_adv_params advp;
- int res;
- memset(&advp, 0, sizeof advp);
- advp.conn_mode = BLE_GAP_CONN_MODE_UND;
- advp.disc_mode = BLE_GAP_DISC_MODE_GEN;
- advp.itvl_min = BLE_GAP_ADV_FAST_INTERVAL1_MIN;
- advp.itvl_max = BLE_GAP_ADV_FAST_INTERVAL1_MAX;
- res = ble_gap_adv_start(nimble_riot_own_addr_type, NULL, BLE_HS_FOREVER,
- &advp, gap_event_cb, NULL);
- assert(res == 0);
- (void)res;
- }
-
- static void update_sensors(void)
- {
- struct os_mbuf *om;
- struct roomba_sensors_data *sens;
- printf("[GATT] Sensor values changed\n");
- sens = roomba_sensors();
- /* send data notification to GATT client */
- om = ble_hs_mbuf_from_flat(sens, sizeof(struct roomba_sensors_data));
- if (!om) {
- printf("[GATT] Error incapsulating sensor data in frame \n");
- return;
- }
- ble_gattc_notify_custom(_conn_handle, _sensors_val_handle, om);
- }
- void *gatt_srv(void *arg)
- {
- puts("NimBLE GATT server starting");
- int res = 0;
- msg_t msg;
- (void)res;
- /* setup local event queue (for handling heart rate updates) */
- /* verify and add our custom services */
- res = ble_gatts_count_cfg(gatt_svr_svcs);
- assert(res == 0);
- res = ble_gatts_add_svcs(gatt_svr_svcs);
- assert(res == 0);
- /* set the device name */
- ble_svc_gap_device_name_set(_device_name);
- /* reload the GATT server to link our added services */
- ble_gatts_start();
- /* configure and set the advertising data */
- uint8_t buf[BLE_HS_ADV_MAX_SZ];
- bluetil_ad_t ad;
- bluetil_ad_init_with_flags(&ad, buf, sizeof(buf), BLUETIL_AD_FLAGS_DEFAULT);
- uint16_t ips_uuid = BLE_GATT_SVC_IPS;
- bluetil_ad_add(&ad, BLE_GAP_AD_UUID16_INCOMP, &ips_uuid, sizeof(ips_uuid));
- bluetil_ad_add_name(&ad, _device_name);
- ble_gap_adv_set_data(ad.buf, ad.pos);
- /* start to advertise this node */
- _start_advertising();
- while (1) {
- if (msg_receive(&msg) >= 0) {
- (void)msg;
- update_sensors();
- }
- }
- return 0;
- }
|