|
@@ -0,0 +1,534 @@
|
|
|
|
+/*
|
|
|
|
+ * 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;
|
|
|
|
+
|
|
|
|
+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;
|
|
|
|
+ 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();
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ /* run an event loop for handling the heart rate update events */
|
|
|
|
+ //event_loop(&_eq);
|
|
|
|
+
|
|
|
|
+ return 0;
|
|
|
|
+}
|