Browse Source

Roomba modes + sensors + ble notification service

Daniele Lacamera 5 months ago
parent
commit
897e6f3cc7

+ 32 - 0
Makefile

@@ -0,0 +1,32 @@
+CROSS_COMPILE:=arm-none-eabi-
+OBJCOPY:=$(CROSS_COMPILE)objcopy
+JLINK_OPTS = -Device NRF52 -if swd -speed 1000
+
+
+APP_SRC:=$(PWD)/roomba
+BOOT_ELF:=roomba/bin/arduino-nano-33-iot/roomba.elf
+
+all: $(BOOT_IMG)
+
+clean:
+	make -C $(WOLFBOOT) clean
+	#make -C ota-server clean
+	make -C $(APP_SRC) clean
+	rm -f $(APP_SRC)/*.bin 
+	rm -f *.bin
+	rm -f tags
+	
+flash: $(WOLFBOOT_BIN) $(BOOT_IMG)
+	JLinkExe $(JLINK_OPTS) -CommanderScript flash_all.jlink 
+
+reset: FORCE
+	JLinkExe $(JLINK_OPTS) -CommanderScript reset.jlink
+
+erase: FORCE
+	JLinkExe $(JLINK_OPTS) -CommanderScript flash_erase.jlink 
+
+gdbserver: FORCE
+	JLinkGDBServer -device nrf52 -if swd -port 3333
+
+
+.PHONY: FORCE

+ 49 - 0
bluez/bluezutils.py

@@ -0,0 +1,49 @@
+# SPDX-License-Identifier: LGPL-2.1-or-later
+
+import dbus
+
+SERVICE_NAME = "org.bluez"
+ADAPTER_INTERFACE = SERVICE_NAME + ".Adapter1"
+DEVICE_INTERFACE = SERVICE_NAME + ".Device1"
+
+def get_managed_objects():
+	bus = dbus.SystemBus()
+	manager = dbus.Interface(bus.get_object("org.bluez", "/"),
+				"org.freedesktop.DBus.ObjectManager")
+	return manager.GetManagedObjects()
+
+def find_adapter(pattern=None):
+	return find_adapter_in_objects(get_managed_objects(), pattern)
+
+def find_adapter_in_objects(objects, pattern=None):
+	bus = dbus.SystemBus()
+	for path, ifaces in objects.items():
+		adapter = ifaces.get(ADAPTER_INTERFACE)
+		if adapter is None:
+			continue
+		if not pattern or pattern == adapter["Address"] or \
+							path.endswith(pattern):
+			obj = bus.get_object(SERVICE_NAME, path)
+			return dbus.Interface(obj, ADAPTER_INTERFACE)
+	raise Exception("Bluetooth adapter not found")
+
+def find_device(device_address, adapter_pattern=None):
+	return find_device_in_objects(get_managed_objects(), device_address,
+								adapter_pattern)
+
+def find_device_in_objects(objects, device_address, adapter_pattern=None):
+	bus = dbus.SystemBus()
+	path_prefix = ""
+	if adapter_pattern:
+		adapter = find_adapter_in_objects(objects, adapter_pattern)
+		path_prefix = adapter.object_path
+	for path, ifaces in objects.items():
+		device = ifaces.get(DEVICE_INTERFACE)
+		if device is None:
+			continue
+		if (device["Address"] == device_address and
+						path.startswith(path_prefix)):
+			obj = bus.get_object(SERVICE_NAME, path)
+			return dbus.Interface(obj, DEVICE_INTERFACE)
+
+	raise Exception("Bluetooth device not found")

+ 204 - 0
bluez/roomba.py

@@ -0,0 +1,204 @@
+#!/usr/bin/env python3
+# SPDX-License-Identifier: LGPL-2.1-or-later
+
+import dbus
+try:
+  from gi.repository import GObject
+except ImportError:
+  import gobject as GObject
+import sys
+
+from dbus.mainloop.glib import DBusGMainLoop
+
+import bluezutils
+
+bus = None
+mainloop = None
+
+BLUEZ_SERVICE_NAME = 'org.bluez'
+DBUS_OM_IFACE =      'org.freedesktop.DBus.ObjectManager'
+DBUS_PROP_IFACE =    'org.freedesktop.DBus.Properties'
+
+GATT_SERVICE_IFACE = 'org.bluez.GattService1'
+GATT_CHRC_IFACE =    'org.bluez.GattCharacteristic1'
+
+ROOMBA_SVC_UUID =    '35f28386-3070-4f3b-ba38-27507e991760'
+
+ROOMBA_CHR_CTL_UUID =    '35f28386-3070-4f3b-ba38-27507e991762'
+ROOMBA_CHR_SENSORS_UUID = '35f28386-3070-4f3b-ba38-27507e991764'
+
+ROOMBA="F4:A4:72:BA:27:C1"
+
+
+# The objects that we interact with.
+roomba_service = None
+roomba_ctrl_chrc = None
+roomba_sensors_chrc = None
+
+
+def generic_error_cb(error):
+    print('D-Bus call failed: ' + str(error))
+    mainloop.quit()
+
+
+
+def sensor_contact_val_to_str(val):
+    if val == 0 or val == 1:
+        return 'not supported'
+    if val == 2:
+        return 'no contact detected'
+    if val == 3:
+        return 'contact detected'
+
+    return 'invalid value'
+
+
+def mode_val_cb(value):
+    print('Mode value: ' + hex(value[0]))
+
+def sensors_val_cb(value):
+    roomba_sensors = value[0] | (value[1] << 8) | (value[2] << 16) | (value[3] << 24)
+    print('Sensors Value : ' + str(hex(roomba_sensors)))
+
+def roomba_sensors_start_notify_cb():
+    print('Roomba sensors: notification enabled')
+
+
+def roomba_sensors_changed_cb(iface, changed_props, invalidated_props):
+    if iface != GATT_CHRC_IFACE:
+        return
+
+    if not len(changed_props):
+        return
+
+    value = changed_props.get('Value', None)
+    if not value:
+        return
+
+    print('New Sensor values')
+    roomba_sensors = value[0] | (value[1] << 8) | (value[2] << 16) | (value[3] << 24)
+    print('\tValue : ' + str(hex(roomba_sensors)))
+
+
+def start_client():
+    roomba_ctrl_chrc[0].ReadValue({}, reply_handler=mode_val_cb,
+                                    error_handler=generic_error_cb,
+                                    dbus_interface=GATT_CHRC_IFACE)
+
+    roomba_sensors_chrc[0].ReadValue({}, reply_handler=sensors_val_cb,
+                                    error_handler=generic_error_cb,
+                                    dbus_interface=GATT_CHRC_IFACE)
+
+
+
+def process_chrc(chrc_path):
+    chrc = bus.get_object(BLUEZ_SERVICE_NAME, chrc_path)
+    chrc_props = chrc.GetAll(GATT_CHRC_IFACE,
+                             dbus_interface=DBUS_PROP_IFACE)
+
+    uuid = chrc_props['UUID']
+
+    if uuid == ROOMBA_CHR_CTL_UUID:
+        global roomba_ctrl_chrc
+        roomba_ctrl_chrc = (chrc, chrc_props)
+    elif uuid == ROOMBA_CHR_SENSORS_UUID:
+        global roomba_sensors_chrc
+        roomba_sensors_chrc = (chrc, chrc_props)
+    else:
+        print('Unrecognized characteristic: ' + uuid)
+
+    return True
+
+
+def process_roomba_service(service_path, chrc_paths):
+    service = bus.get_object(BLUEZ_SERVICE_NAME, service_path)
+    service_props = service.GetAll(GATT_SERVICE_IFACE,
+                                   dbus_interface=DBUS_PROP_IFACE)
+
+    uuid = service_props['UUID']
+
+    if uuid != ROOMBA_SVC_UUID:
+        return False
+
+    print('roomba GATT service found: ' + service_path)
+
+
+
+    global roomba_service
+    roomba_service = (service, service_props, service_path)
+    
+    # Process the characteristics.
+    for chrc_path in chrc_paths:
+        process_chrc(chrc_path)
+
+    return True
+
+
+def interfaces_removed_cb(object_path, interfaces):
+    if not roomba_service:
+        return
+
+    if object_path == roomba_service[2]:
+        print('Service was removed')
+        mainloop.quit()
+
+
+def main():
+    # Set up the main loop.
+    DBusGMainLoop(set_as_default=True)
+    global bus
+    bus = dbus.SystemBus()
+    global mainloop
+    mainloop = GObject.MainLoop()
+
+    om = dbus.Interface(bus.get_object(BLUEZ_SERVICE_NAME, '/'), DBUS_OM_IFACE)
+    om.connect_to_signal('InterfacesRemoved', interfaces_removed_cb)
+
+    print('Getting objects...')
+    objects = om.GetManagedObjects()
+    chrcs = []
+
+    # List devices found
+    for path, interfaces in objects.items():
+        device = interfaces.get("org.bluez.Device1")
+        if (device is None):
+            continue
+        if (device["Address"] == ROOMBA):
+            print("Found ROOMBA!")
+            print(device["Address"])
+
+    device = bluezutils.find_device(ROOMBA, "hci1")
+    if (device is None):
+        print("Cannot 'find_device'")
+    else:
+        device.Connect()
+        print("Connected")
+            
+
+    # List characteristics found
+    for path, interfaces in objects.items():
+        if GATT_CHRC_IFACE not in interfaces.keys():
+            continue
+        chrcs.append(path)
+
+    # List sevices found
+    for path, interfaces in objects.items():
+        if GATT_SERVICE_IFACE not in interfaces.keys():
+            continue
+
+        chrc_paths = [d for d in chrcs if d.startswith(path + "/")]
+
+        if process_roomba_service(path, chrc_paths):
+            break
+
+    if not roomba_service:
+        print('No ROOMBA found.')
+        sys.exit(1)
+
+    start_client()
+
+    mainloop.run()
+
+
+if __name__ == '__main__':
+    main()

+ 48 - 0
riot-arduino-33-iot.patch

@@ -0,0 +1,48 @@
+diff --git a/boards/nrf52840dk/include/board.h b/boards/nrf52840dk/include/board.h
+index 0874aff0a..41d4171b1 100644
+--- a/boards/nrf52840dk/include/board.h
++++ b/boards/nrf52840dk/include/board.h
+@@ -31,17 +31,15 @@ extern "C" {
+  * @name    LED pin configuration
+  * @{
+  */
+-#define LED0_PIN            GPIO_PIN(0, 13)
+-#define LED1_PIN            GPIO_PIN(0, 14)
+-#define LED2_PIN            GPIO_PIN(0, 15)
+-#define LED3_PIN            GPIO_PIN(0, 16)
++#define LED0_PIN            GPIO_PIN(0, 16)
++#define LED1_PIN            GPIO_PIN(0, 24)
++#define LED2_PIN            GPIO_PIN(0, 6)
+ 
+ #define LED_PORT            (NRF_P0)
+-#define LED0_MASK           (1 << 13)
+-#define LED1_MASK           (1 << 14)
+-#define LED2_MASK           (1 << 15)
+-#define LED3_MASK           (1 << 16)
+-#define LED_MASK            (LED0_MASK | LED1_MASK | LED2_MASK | LED3_MASK)
++#define LED0_MASK           (1 << 16)
++#define LED1_MASK           (1 << 24)
++#define LED2_MASK           (1 << 6)
++#define LED_MASK            (LED0_MASK | LED1_MASK | LED2_MASK)
+ 
+ #define LED0_ON             (LED_PORT->OUTCLR = LED0_MASK)
+ #define LED0_OFF            (LED_PORT->OUTSET = LED0_MASK)
+@@ -55,9 +53,6 @@ extern "C" {
+ #define LED2_OFF            (LED_PORT->OUTSET = LED2_MASK)
+ #define LED2_TOGGLE         (LED_PORT->OUT   ^= LED2_MASK)
+ 
+-#define LED3_ON             (LED_PORT->OUTCLR = LED3_MASK)
+-#define LED3_OFF            (LED_PORT->OUTSET = LED3_MASK)
+-#define LED3_TOGGLE         (LED_PORT->OUT   ^= LED3_MASK)
+ /** @} */
+ 
+ /**
+@@ -90,7 +85,7 @@ extern mtd_dev_t *mtd0;
+ #define BTN0_MODE           GPIO_IN_PU
+ #define BTN1_PIN            GPIO_PIN(0, 12)
+ #define BTN1_MODE           GPIO_IN_PU
+-#define BTN2_PIN            GPIO_PIN(0, 24)
++#define BTN2_PIN            GPIO_PIN(0, 22)
+ #define BTN2_MODE           GPIO_IN_PU
+ #define BTN3_PIN            GPIO_PIN(0, 25)
+ #define BTN3_MODE           GPIO_IN_PU

+ 47 - 0
roomba/Makefile

@@ -0,0 +1,47 @@
+# name of your application
+APPLICATION = roomba
+
+# If no BOARD is found in the environment, use this default:
+BOARD ?= nrf52840dk
+#BOARD ?= arduino-nano-33-iot
+
+# This has to be the absolute path to the RIOT base directory:
+RIOTBASE ?= $(CURDIR)/../RIOT/
+
+# Require uart module
+FEATURES_REQUIRED += periph_uart
+
+# Some RIOT modules needed for this example
+USEMODULE += event_timeout
+
+# Network
+USEMODULE += nimble
+USEMODULE += nimble_addr
+USEMODULE += nimble_scanner
+USEMODULE += nimble_scanlist
+USEMODULE += nimble_svc_gap
+USEMODULE += nimble_svc_gatt
+USEMODULE += nimble_drivers_nrf5x
+USEMODULE += random
+
+#USEMODULE += periph_flashpage
+
+
+USEMODULE += usbus
+USEMODULE += stdio_cdc_acm
+USEMODULE += auto_init_usbus
+USEMODULE += shell
+USEMODULE += shell_commands
+USEMODULE += ps
+
+# Comment this out to disable code in RIOT that does safety checking
+# which is not needed in a production environment but helps in the
+# development process:
+DEVELHELP ?= 1
+CFLAGS+=-Wno-missing-field-initializers -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -I$(CURDIR)/include
+
+# Change this to 0 show compiler invocation lines by default:
+QUIET ?= 1
+
+include $(RIOTBASE)/Makefile.include
+include nimble.inc.mk

+ 534 - 0
roomba/gatt-srv.c

@@ -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;
+}

+ 76 - 0
roomba/imperial_march.c

@@ -0,0 +1,76 @@
+#include <stdint.h>
+#include "song.h"
+const uint16_t song0[] = { 
+    (9 << 8),
+    Note(NOTE_A,2,Q),
+    Note(NOTE_A,2,Q),
+    Note(NOTE_A,2,Q),
+    Note(NOTE_F,2,THREESIXT),
+    Note(NOTE_C,3,SIXT),
+    Note(NOTE_A,2,Q),
+    Note(NOTE_F,2,THREESIXT),
+    Note(NOTE_C,3,SIXT),
+    Note(NOTE_A,2,HALF)
+};
+
+const uint16_t song1[] = {
+    (9 << 8),
+    Note(NOTE_E,3,Q),
+    Note(NOTE_E,3,Q),
+    Note(NOTE_E,3,Q),
+    Note(NOTE_F,3,THREESIXT),
+    Note(NOTE_C,3,SIXT),
+    Note(NOTE_Gd,2,Q),
+    Note(NOTE_F,2,THREESIXT),
+    Note(NOTE_C,3,SIXT),
+    Note(NOTE_A,2,HALF)
+};
+
+const uint16_t song2[] = {
+    (9 << 8),
+    Note(NOTE_A,3,Q),
+    Note(NOTE_A,2,THREESIXT),
+    Note(NOTE_A,2,SIXT),
+    Note(NOTE_A,3,Q),
+    Note(NOTE_Gd,3,OCT),
+    Note(NOTE_G,3,OCT),
+    Note(NOTE_Fd,3,SIXT),
+    Note(NOTE_F,3,SIXT),
+    Note(NOTE_Fd,2,SIXT)
+};
+
+const uint16_t song3[] = {
+    (7 << 8),
+    Note(NOTE_Ad,2,OCT),
+    Note(NOTE_Dd,3,Q),
+    Note(NOTE_D,3,OCT),
+    Note(NOTE_Cd,3,OCT),
+    Note(NOTE_C,3,SIXT),
+    Note(NOTE_B,2,SIXT),
+    Note(NOTE_C,3,OCT),
+};
+
+const uint16_t song4[] = {
+    (8 << 8),
+    Note(NOTE_F,2,OCT),
+    Note(NOTE_Gd,2,Q),
+    Note(NOTE_F,2,THREESIXT),
+    Note(NOTE_Gd,2,SIXT),
+    Note(NOTE_C,3,Q),
+    Note(NOTE_A,2,THREESIXT),
+    Note(NOTE_C,3,SIXT),
+    Note(NOTE_E,3,HALF),
+};
+
+const uint16_t song5[] =
+{
+    (8 << 8),
+    Note(NOTE_F,2,OCT),
+    Note(NOTE_Gd,2,Q),
+    Note(NOTE_F,2,THREESIXT),
+    Note(NOTE_C,3,SIXT),
+    Note(NOTE_A,2,Q),
+    Note(NOTE_F,2,THREESIXT),
+    Note(NOTE_C,3,SIXT),
+    Note(NOTE_A,2,HALF),
+};

+ 54 - 0
roomba/include/ble_svc_gap.h

@@ -0,0 +1,54 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_SVC_GAP_
+#define H_BLE_SVC_GAP_
+
+#include <inttypes.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BLE_SVC_GAP_UUID16                                  0x1800
+#define BLE_SVC_GAP_CHR_UUID16_DEVICE_NAME                  0x2a00
+#define BLE_SVC_GAP_CHR_UUID16_APPEARANCE                   0x2a01
+#define BLE_SVC_GAP_CHR_UUID16_PERIPH_PREF_CONN_PARAMS      0x2a04
+#define BLE_SVC_GAP_CHR_UUID16_CENTRAL_ADDRESS_RESOLUTION   0x2aa6
+
+#define BLE_SVC_GAP_APPEARANCE_GEN_UNKNOWN                         0
+#define BLE_SVC_GAP_APPEARANCE_GEN_COMPUTER                        128
+#define BLE_SVC_GAP_APPEARANCE_CYC_SPEED_AND_CADENCE_SENSOR        1157
+
+typedef void (ble_svc_gap_chr_changed_fn) (uint16_t uuid);
+
+void ble_svc_gap_set_chr_changed_cb(ble_svc_gap_chr_changed_fn *cb);
+
+const char *ble_svc_gap_device_name(void);
+int ble_svc_gap_device_name_set(const char *name);
+uint16_t ble_svc_gap_device_appearance(void);
+int ble_svc_gap_device_appearance_set(uint16_t appearance);
+
+void ble_svc_gap_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

+ 40 - 0
roomba/include/ble_svc_gatt.h

@@ -0,0 +1,40 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_SVC_GATT_
+#define H_BLE_SVC_GATT_
+
+#include <inttypes.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct ble_hs_cfg;
+
+#define BLE_SVC_GATT_CHR_SERVICE_CHANGED_UUID16     0x2a05
+
+void ble_svc_gatt_changed(uint16_t start_handle, uint16_t end_handle);
+void ble_svc_gatt_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

+ 108 - 0
roomba/include/bleman.h

@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) 2019 Koen Zandberg <koen@bergzand.net>
+ *
+ * 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.
+ */
+
+#ifndef APP_BLEMAN_H
+#define APP_BLEMAN_H
+
+#include "host/ble_gap.h"
+#include <stdint.h>
+#include "event.h"
+#include "thread.h"
+#include "xtimer.h"
+#include "bleman/bas.h"
+#include "bleman/hrs.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef CONFIG_BLEMAN_DEVICE_NAME
+#define CONFIG_BLEMAN_DEVICE_NAME   "ROOMBAHAHACKED"
+#endif
+
+#ifndef CONFIG_BLEMAN_MANUF_NAME
+#define CONFIG_BLEMAN_MANUF_NAME     "danielinux"
+#endif
+
+#ifndef CONFIG_BLEMAN_MODEL_NUM
+#define CONFIG_BLEMAN_MODEL_NUM     "1"
+#endif
+
+#ifndef CONFIG_BLEMAN_FW_VERSION
+#define CONFIG_BLEMAN_FW_VERSION    "1.0.0"
+#endif
+
+#ifndef CONFIG_BLEMAN_HW_VERSION
+#define CONFIG_BLEMAN_HW_VERSION    "1"
+#endif
+
+#ifndef CONFIG_BLEMAN_SERIAL_LEN
+#define CONFIG_BLEMAN_SERIAL_LEN    24
+#endif
+
+#ifndef CONFIG_BLEMAN_ROOMBA_UUID
+#define CONFIG_BLEMAN_ROOMBA_UUID 0xc5, 0xb1, 0x8c, 0x78, 0x38, 0x3b, 0x46, 0x56, 0x99, 0x13, 0x4a, 0xb0, 0x0a, 0xdc, 0x51, 0x98
+#endif
+
+typedef struct _bleman bleman_t;
+typedef struct _bleman_event_handler bleman_event_handler_t;
+
+/**
+ * @brief event notification handler
+ *
+ * @param   event       NimBLE gap event
+ * @param   bleman      bleman context
+ * @param   arg         Extra argument passed to the call
+ */
+typedef void (*bleman_event_cb_t)(bleman_t *bleman, struct ble_gap_event *event,
+                                  void *arg);
+
+typedef enum {
+    BLEMAN_BLE_STATE_INACTIVE,
+    BLEMAN_BLE_STATE_ADVERTISING,
+    BLEMAN_BLE_STATE_DISCONNECTED,
+    BLEMAN_BLE_STATE_CONNECTED,
+} bleman_ble_state_t;
+
+struct _bleman_event_handler {
+    struct _bleman_event_handler *next; /**< linked list iterator */
+    bleman_event_cb_t handler; /**< Handler function pointer */
+    void *arg; /**< Argument passed to the handler function call */
+};
+
+struct _bleman {
+    char serial[CONFIG_BLEMAN_SERIAL_LEN];
+    event_queue_t eq;
+    uint16_t conn_handle;
+    bleman_event_handler_t *handlers;
+    bleman_ble_state_t state;
+    bleman_bas_t bas;
+    bleman_hrs_t hrs;
+};
+
+int bleman_thread_create(void);
+
+bleman_t *bleman_get(void);
+bleman_ble_state_t bleman_get_conn_state(bleman_t *bleman, struct ble_gap_conn_desc *state);
+
+/**
+ * @brief Add a notification handler to the bleman thread
+ *
+ * @param   bleman  bleman context
+ * @param   event   event handler context to add
+ * @param   cb      callback function
+ * @param   arg     argument to add to the handler
+ */
+void bleman_add_event_handler(bleman_t *bleman, bleman_event_handler_t *event,
+                              bleman_event_cb_t cb, void *arg);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* APP_BLEMAN_H */

+ 49 - 0
roomba/include/bleman/bas.h

@@ -0,0 +1,49 @@
+/*
+ * Copyright (C) 2020 Koen Zandberg <koen@bergzand.net>
+ *
+ * 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.
+ */
+
+#ifndef _APP_BLEMAN_BAS_H
+#define _APP_BLEMAN_BAS_H
+
+#include <stdint.h>
+#include "event.h"
+#include "event/timeout.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief Controller battery measurement interval in milliseconds
+ */
+#ifndef BLEMAN_BATTERY_UPDATE_INTERVAL
+#define BLEMAN_BATTERY_UPDATE_INTERVAL     30 * MS_PER_SEC
+#endif
+
+typedef struct {
+    event_timeout_t evt;        /**< Event timeout for battery notifications  */
+    event_t ev;                 /**< Event for battery notifications */
+    uint8_t last_percentage;    /**< Last submitted notification percentage */
+    uint16_t handle;            /**< notification handle */
+} bleman_bas_t;
+
+/**
+ * @brief handler for bleman battery service requests
+ */
+int bleman_bas_handler(uint16_t conn_handle, uint16_t attr_handle,
+                       struct ble_gatt_access_ctxt *ctxt, void *arg);
+
+/**
+ * @brief Enable or disable bleman battery service notifications
+ */
+void bleman_bas_notify(bleman_bas_t *bas, bool enable);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* APP_BLEMAN_BAS_H */

+ 51 - 0
roomba/include/bleman/hrs.h

@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) 2020 Inria
+ *
+ * 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.
+ */
+
+#ifndef _APP_BLEMAN_HRS_H
+#define _APP_BLEMAN_HRS_H
+
+#include <stdint.h>
+#include "event.h"
+#include "event/timeout.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief Controller battery measurement interval in milliseconds
+ */
+#ifndef BLEMAN_HEART_RATE_UPDATE_INTERVAL
+#define BLEMAN_HEART_RATE_UPDATE_INTERVAL     1 * MS_PER_SEC
+#endif
+
+typedef struct {
+    event_timeout_t evt;        /**< Event timeout for HRS notifications  */
+    event_t ev;                 /**< Event for HRS notifications */
+    uint16_t bpm;         /**< Last submitted heart rate */
+    int step;
+    uint16_t handle;            /**< notification handle */
+} bleman_hrs_t;
+
+/**
+ * @brief handler for bleman heart rate service requests
+ */
+int bleman_hrs_handler(uint16_t conn_handle, uint16_t attr_handle,
+                       struct ble_gatt_access_ctxt *ctxt, void *arg);
+
+/**
+ * @brief Enable or disable bleman heart rate service notifications
+ */
+void bleman_hrs_notify(bleman_hrs_t *hrs, bool enable);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* APP_BLEMAN_BAS_H */
+

+ 56 - 0
roomba/include/bleman/timesync.h

@@ -0,0 +1,56 @@
+/*
+ * Copyright (C) 2019 Koen Zandberg <koen@bergzand.net>
+ *
+ * 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.
+ */
+
+#ifndef APP_BLEMAN_TIMESYNC_H
+#define APP_BLEMAN_TIMESYNC_H
+
+#include <stdint.h>
+#include "event.h"
+#include "event/timeout.h"
+#include "ts_event.h"
+#include "bleman.h"
+#include "host/ble_gatt.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct __attribute__((packed)) {
+    uint16_t year;
+    uint8_t month;
+    uint8_t dayofmonth;
+    uint8_t hour;
+    uint8_t minute;
+    uint8_t second;
+    uint8_t millis;
+    uint8_t reason;
+} bleman_timesync_ble_cts_t;
+
+typedef struct {
+    event_t ev;
+    event_t start_chrs_ev;
+    event_t start_read_ev;
+    bleman_event_handler_t handler;
+    event_timeout_t timeout_ev;
+    bleman_t *bleman;
+    struct ble_gatt_svc time_svc;
+    struct ble_gatt_chr time_chr;
+} bleman_timesync_t;
+
+void bleman_timesync_init(bleman_t *bleman, bleman_timesync_t *sync);
+void bleman_timesync_start_events(bleman_timesync_t *sync);
+
+void bleman_timesync_stop_events(bleman_timesync_t *sync);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* APP_BLEMAN_TIMESYNC_H */
+
+

+ 105 - 0
roomba/include/bleman_conf.h

@@ -0,0 +1,105 @@
+/*
+ * Copyright (C) 2019 Koen Zandberg <koen@bergzand.net>
+ *
+ * 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.
+ */
+
+#ifndef APP_BLEMAN_CONF_H
+#define APP_BLEMAN_CONF_H
+
+#include "ble_svc_gap.h"
+#include "ble_svc_gatt.h"
+#include "net/bluetil/ad.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef CONFIG_BLEMAN_CUSTOM_SERVICE
+/* GATT service definitions */
+static const struct ble_gatt_svc_def gatt_svr_svcs[] = {
+    {
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID128_DECLARE(CONFIG_BLEMAN_ROOMBA_UUID),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            0, /* no more characteristics in this service */
+        }, }
+
+    },
+    {
+        /* Heart Rate Service */
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(BLE_GATT_SVC_HRS),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_HEART_RATE_MEASURE),
+            .access_cb = bleman_hrs_handler,
+            .val_handle = &_bleman.hrs.handle,
+            .flags = BLE_GATT_CHR_F_NOTIFY,
+        }, {
+            .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_BODY_SENSE_LOC),
+            .access_cb = bleman_hrs_handler,
+            .flags = BLE_GATT_CHR_F_READ,
+        }, {
+            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,
+            .arg = &_bleman,
+        }, {
+            .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_MODEL_NUMBER_STR),
+            .access_cb = _devinfo_handler,
+            .flags = BLE_GATT_CHR_F_READ,
+            .arg = &_bleman,
+        }, {
+            .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_SERIAL_NUMBER_STR),
+            .access_cb = _devinfo_handler,
+            .flags = BLE_GATT_CHR_F_READ,
+            .arg = &_bleman,
+        }, {
+            .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_FW_REV_STR),
+            .access_cb = _devinfo_handler,
+            .flags = BLE_GATT_CHR_F_READ,
+            .arg = &_bleman,
+        }, {
+            .uuid = BLE_UUID16_DECLARE(BLE_GATT_CHAR_HW_REV_STR),
+            .access_cb = _devinfo_handler,
+            .flags = BLE_GATT_CHR_F_READ,
+            .arg = &_bleman,
+        }, {
+            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 = bleman_bas_handler,
+            .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_READ_AUTHEN | BLE_GATT_CHR_F_NOTIFY,
+            .val_handle = &_bleman.bas.handle,
+            .arg = &_bleman,
+        }, {
+            0, /* no more characteristics in this service */
+        }, }
+    },
+    {
+        0, /* no more services */
+    },
+};
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* APP_BLEMAN_CONF_H */

+ 24 - 0
roomba/include/log/log.h

@@ -0,0 +1,24 @@
+/*
+ * Copyright (C) 2020 Koen Zandberg <koen@bergzand.net>
+ *
+ * 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.
+ */
+
+/**
+ * This file overrides the NimBLE log definitions to prevent conflicts with the
+ * RIOT-provided LOG defines
+ */
+
+#ifndef _BLEMAN_LOG_H
+#define _BLEMAN_LOG_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct log {
+};
+
+#endif

+ 521 - 0
roomba/main.c

@@ -0,0 +1,521 @@
+/* main.c
+ *
+ *
+ * this is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * this is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA
+ */
+
+#include <stdio.h>
+#include "board.h"
+#include "net/ipv6/addr.h"
+#include "net/gnrc.h"
+#include "net/gnrc/netif.h"
+#include "shell.h"
+#include "periph/flashpage.h"
+#include "thread.h"
+#include "periph/uart.h"
+#include "periph/gpio.h"
+#include "xtimer.h"
+#include "ringbuffer.h"
+#include "roomba.h"
+#include "song.h"
+#include "mutex.h"
+#include "msg.h"
+
+#ifndef SHELL_BUFSIZE
+#define SHELL_BUFSIZE       (128U)
+#endif
+
+#define DISPATCHER_PRIO        (THREAD_PRIORITY_MAIN - 1)
+#define BT_PRIO                (THREAD_PRIORITY_MAIN - 1)
+
+#define MSGQ_SIZE (4)
+
+#define GPIO_WAKEUP  GPIO_PIN(1,11)
+
+#define SENSOR_BUFFER_SIZE (sizeof(struct sensors_code1) + sizeof(struct sensors_code2) + \
+                sizeof(struct sensors_code3))
+
+#define MSG_SENSORS_OFF       (0x4000)
+#define MSG_SENSORS_ON        (0x4100)
+#define MSG_SENSORS_AVAILABLE (0x4200)
+#define MSG_SENSORS_TIMEOUT   (0x4300)
+
+const char roomba_modstring[6][6] = {
+    "OFF", "DRIVE", "FULL", "CLEAN", "SPOT", "DOCK"
+};
+
+extern const uint16_t song0[], song1[], song2[], song3[], song4[], song5[];
+// Types
+typedef struct {
+    char rx_mem[SENSOR_BUFFER_SIZE];
+    ringbuffer_t rx_buf;
+} uart_ctx_t;
+
+// Globals
+//
+static mutex_t roomba_mutex = MUTEX_INIT;
+static uart_ctx_t ctx[UART_NUMOF];
+static kernel_pid_t order66_pid, sensors_pid, gatt_srv_pid;
+static char order66_stack[THREAD_STACKSIZE_MAIN];
+static char sensors_stack[THREAD_STACKSIZE_MAIN];
+static char gatt_srv_stack[THREAD_STACKSIZE_MAIN];
+static msg_t sensors_msgq[MSGQ_SIZE];
+static msg_t gatt_srv_msgq[MSGQ_SIZE];
+static xtimer_t sensors_msg_timer;
+const uint16_t *songs[] = { song0, song1, song2, song3, song4, song5 };
+const uint32_t part_len[] = { 5000000, 5000000, 3200000, 2500000, 4200000, 4200000 };
+
+// Prototypes
+void *order66(void *arg);
+void *sensors_loop(void *arg);
+static void roomba_cmd(uint8_t cmd, uint8_t *payload, uint8_t len);
+void roomba_led(uint8_t status, uint8_t spot, uint8_t clean, uint8_t max, uint8_t power_c, uint8_t power_i);
+
+int song_len(const uint16_t *s)
+{
+    int l = (s[0] & 0xFF00) >> 8;
+    return l;
+}
+
+uint32_t load_song(int n)
+{
+    uint8_t cmd = ROOMBA_SONG;
+    int l = song_len(songs[n]);
+    int i;
+    mutex_lock(&roomba_mutex);
+    uart_t dev = UART_DEV(0);
+    uart_write(dev, &cmd, 1);
+    uart_write(dev, (const uint8_t *)songs[n], 2 + (l << 1)); 
+    mutex_unlock(&roomba_mutex);
+    xtimer_usleep(100000);
+    return part_len[n];
+}
+
+
+// Order 66
+void *order66(void *arg)
+{
+    uint8_t x = 0;
+    uint32_t st;
+    st = load_song(0);
+    roomba_cmd(ROOMBA_PLAY, &x, 1);
+    xtimer_usleep(st);
+    st = load_song(1);
+    xtimer_usleep(20000);
+    roomba_cmd(ROOMBA_PLAY, &x, 1);
+    xtimer_usleep(st);
+    st = load_song(2);
+    xtimer_usleep(20000);
+    roomba_cmd(ROOMBA_PLAY, &x, 1);
+    xtimer_usleep(st);
+    st = load_song(3);
+    xtimer_usleep(20000);
+    roomba_cmd(ROOMBA_PLAY, &x, 1);
+    xtimer_usleep(st);
+    st = load_song(4);
+    xtimer_usleep(20000);
+    roomba_cmd(ROOMBA_PLAY, &x, 1);
+    xtimer_usleep(st);
+    st = load_song(2);
+    xtimer_usleep(20000);
+    roomba_cmd(ROOMBA_PLAY, &x, 1);
+    xtimer_usleep(st);
+    st = load_song(3);
+    xtimer_usleep(20000);
+    roomba_cmd(ROOMBA_PLAY, &x, 1);
+    xtimer_usleep(st);
+    st = load_song(5);
+    xtimer_usleep(20000);
+    roomba_cmd(ROOMBA_PLAY, &x, 1);
+    xtimer_usleep(st);
+    return NULL;
+}
+
+static int roomba_mode = ROOMBA_MODE_OFF;
+
+static void roomba_stop(void)
+{
+    uint8_t cmd = ROOMBA_POWER;
+    uart_t dev = UART_DEV(0);
+    mutex_lock(&roomba_mutex);
+    uart_write(dev, &cmd, 1);
+    mutex_unlock(&roomba_mutex);
+    xtimer_usleep(200000);
+}
+
+static void roomba_start(void)
+{
+    gpio_clear(GPIO_WAKEUP);
+    xtimer_usleep(200000);
+    gpio_set(GPIO_WAKEUP);
+    uint8_t cmd = ROOMBA_START;
+    uart_t dev = UART_DEV(0);
+    mutex_lock(&roomba_mutex);
+    uart_write(dev, &cmd, 1);
+    xtimer_usleep(20000);
+    mutex_unlock(&roomba_mutex);
+}
+
+static void roomba_safe_to_full(void)
+{
+    uint8_t cmd = ROOMBA_FULL;
+    uart_t dev = UART_DEV(0);
+    mutex_lock(&roomba_mutex);
+    uart_write(dev, &cmd, 1);
+    xtimer_usleep(20000);
+    mutex_unlock(&roomba_mutex);
+}
+
+static void roomba_full_to_safe(void)
+{
+    uint8_t cmd = ROOMBA_SAFE;
+    uart_t dev = UART_DEV(0);
+    mutex_lock(&roomba_mutex);
+    uart_write(dev, &cmd, 1);
+    xtimer_usleep(20000);
+    mutex_unlock(&roomba_mutex);
+}
+
+static void roomba_cmd(uint8_t cmd, uint8_t *payload, uint8_t len)
+{
+    uart_t dev = UART_DEV(0);
+    mutex_lock(&roomba_mutex);
+    uart_write(dev, &cmd, 1);
+    if(len && payload)
+        uart_write(dev, payload, len);
+    mutex_unlock(&roomba_mutex);
+}
+
+int switch_mode(int newmode)
+{
+    msg_t msg;
+    int oldmode = roomba_mode;
+    if (roomba_mode != newmode) {
+        if (roomba_mode == ROOMBA_MODE_OFF) {
+            roomba_start();
+            roomba_mode = ROOMBA_MODE_DRIVE;
+            roomba_led(0x03, 0, 0, 0, 0x80, 0x80);
+        }
+        switch(newmode) {
+            case ROOMBA_MODE_OFF:
+                roomba_stop();
+                roomba_led(0x00, 0, 0, 0, 0xF0, 0x20);
+                break;
+            case ROOMBA_MODE_DRIVE:
+                if (roomba_mode == newmode)
+                    break;
+                else if (roomba_mode == ROOMBA_MODE_FULL)
+                    roomba_full_to_safe();
+                else {
+                    roomba_stop();
+                    roomba_start();
+                }
+                roomba_led(0x03, 0, 0, 0, 0x80, 0x80);
+                break;
+            case ROOMBA_MODE_FULL:
+                if (roomba_mode == ROOMBA_MODE_DRIVE)
+                    roomba_safe_to_full();
+                else {
+                    roomba_stop();
+                    roomba_start();
+                    roomba_safe_to_full();
+                }
+                roomba_led(0x01, 0, 0, 0, 0xF0, 0xFF);
+                break;
+            case ROOMBA_MODE_CLEAN:
+                roomba_cmd(ROOMBA_CLEAN, NULL, 0);
+                break;
+            case ROOMBA_MODE_SPOT:
+                roomba_cmd(ROOMBA_SPOT, NULL, 0);
+                break;
+            case ROOMBA_MODE_DOCK:
+                roomba_cmd(ROOMBA_DOCK, NULL, 0);
+                break;
+            case 66:
+                order66_pid = thread_create(order66_stack, sizeof(order66_stack),
+                                DISPATCHER_PRIO, 0, order66, NULL, "ORDER66");
+                newmode = roomba_mode;
+                break;
+        }
+        printf("Mode: %s\n", roomba_modstring[newmode]); 
+        roomba_mode = newmode;
+        if (roomba_mode == ROOMBA_MODE_FULL) {
+            msg.type = MSG_SENSORS_ON;
+            msg_send(&msg, sensors_pid);
+        }
+        if (oldmode == ROOMBA_MODE_FULL) {
+            msg.type = MSG_SENSORS_OFF;
+            msg_send(&msg, sensors_pid);
+        }
+    }
+    return 0;
+}
+
+uint8_t get_mode(void)
+{
+    return roomba_mode;
+}
+
+static int cmd_info(int argc, char **argv)
+{
+    (void)argc;
+    (void)argv;
+    puts("iRobot ROOMBA 500 powered by RIOT-OS");
+    printf("You are running RIOT on %s.\n", RIOT_BOARD);
+    printf("This board features a %s MCU.\n", RIOT_MCU);
+    puts("\nUART INFO:");
+    printf("Available devices:               %i\n", UART_NUMOF);
+    if (STDIO_UART_DEV != UART_UNDEF) {
+        printf("UART used for STDIO (the shell): UART_DEV(%i)\n\n", STDIO_UART_DEV);
+    }
+
+    return 0;
+}
+
+
+int cmd_mode(int argc, char **argv)
+{
+    int i;
+    if (argc < 2) {
+        printf("usage: %s <data (string)>\n", argv[0]);
+        return 1;
+    }
+    for (i = 0; i < 6; i++) {
+        if (strcmp(argv[1], roomba_modstring[i]) == 0) {
+            switch_mode(i);
+            return 0;
+        }
+    }
+    if (strcmp(argv[1], "ORDER66") == 0) {
+        order66_pid = thread_create(order66_stack, sizeof(order66_stack),
+                DISPATCHER_PRIO, 0, order66, NULL, "ORDER66");
+        return 0;
+    }
+    printf("Mode '%s' unknown\r\n", argv[1]);
+    return 1;
+}
+
+static int cmd_play(int argc, char **argv)
+{
+    uint8_t x = 0, z = 0;
+    if ((argc != 2) || (strlen(argv[1]) != 1))
+    {
+        printf("usage: %s <song number>\n", argv[0]);
+        return 1;
+    }
+    x = argv[1][0] - '0';
+    load_song(x);
+    roomba_cmd(ROOMBA_PLAY, &z, 1);
+    return 0;
+}
+
+
+#define ALLSENSORS_CODE (0)
+#define ALLSENSORS_SIZE (26)
+#define QUICKSENSORS_CODE (1)
+#define QUICKSENSORS_SIZE (10)
+
+
+static volatile uint8_t sensors_expect_rx = 0;
+static volatile uint8_t sensors_data_rx = 0;
+static uint8_t raw_sensors_data[ALLSENSORS_SIZE];
+
+static void invalidate_sensors(void)
+{
+    msg_t msg;
+    memset(&raw_sensors_data, 0, sizeof(struct roomba_sensors_data));
+    msg.type = MSG_SENSORS_AVAILABLE;
+    msg_send(&msg, gatt_srv_pid);
+}
+
+static int read_sensors(uint8_t mode)
+{
+    uint8_t cmd[2] = {ROOMBA_SENSORS, 0};
+    int len = -1;
+    msg_t msg;
+    uart_t dev = UART_DEV(0);
+    msg_t msg_timeout;
+    msg_timeout.type = MSG_SENSORS_TIMEOUT;
+
+    if (mode == ALLSENSORS_CODE)
+        sensors_expect_rx = ALLSENSORS_SIZE;
+    else
+        sensors_expect_rx = QUICKSENSORS_SIZE;
+
+    cmd[1] = mode;
+    mutex_lock(&roomba_mutex);
+    sensors_data_rx = 0;
+    uart_write(dev, cmd, 1);
+    uart_write(dev, cmd + 1, 1);
+    xtimer_set_msg(&sensors_msg_timer, 500000, &msg_timeout, sensors_pid);
+    if (msg_receive(&msg) >= 0) {
+        if (msg.type == MSG_SENSORS_AVAILABLE) {
+            len = sensors_expect_rx;
+        } else if (msg.type == MSG_SENSORS_TIMEOUT) {
+            printf("Timeout while reading sensors\n");
+            invalidate_sensors();
+        } else if (msg.type == MSG_SENSORS_OFF) {
+            printf("Paused sensor task.\n");
+            invalidate_sensors();
+        } else {
+            printf("Invalid message type %04x\n", msg.type);
+            invalidate_sensors();
+        }
+        sensors_expect_rx = 0;
+    }
+    mutex_unlock(&roomba_mutex);       
+    return len;
+}
+
+
+// Rx callback
+static void rx_cb(void *arg, uint8_t data)
+{
+    uart_t dev = (uart_t)arg;
+    msg_t msg;
+    //printf("RX: %02x\n", data);
+    if (sensors_expect_rx > sensors_data_rx)
+        raw_sensors_data[sensors_data_rx++] = data;
+    if (sensors_expect_rx == sensors_data_rx)
+    {
+        msg.type = MSG_SENSORS_AVAILABLE;
+        msg_send(&msg, sensors_pid);
+    }
+}
+
+static struct roomba_sensors_data roomba_sensors_data = { };
+
+void print_sensors(void)
+{
+    struct roomba_sensors_data *r = &roomba_sensors_data;
+    printf("=== Sensors ===\n");
+    printf("Valid : %s\n", r->valid?"yes":"no");
+    if (r->valid) {
+        printf("Drops : C:%d L:%d R:%d\n", r->drop_caster, r->drop_left, r->drop_right);
+        printf("Bumps : L: %d R: %d\n", r->bump_left, r->bump_right);
+        printf("Cliffs:    FL:%d    FR:%d   \n", r->cliff_frontleft, r->cliff_frontright);
+        printf("       L:%d              R:%d   \n", r->cliff_left, r->cliff_right);
+        printf("OVC: %02x\n", r->ovc);
+    }
+    printf("==============\n\n");
+}
+
+
+void *sensors_loop(void *arg)
+{
+    int ret = 0;
+    msg_t msg;
+    struct roomba_sensors_data tmp;
+    while (1) { 
+        if (get_mode() != ROOMBA_MODE_FULL) {
+            while (1) {
+                if ((msg_receive(&msg) >= 0) && (msg.type == MSG_SENSORS_ON)) {
+                    printf("Sensors thread: resume.\n");
+                    break;
+                }
+            }
+        }
+        xtimer_usleep(250000);
+        ret = read_sensors(ALLSENSORS_CODE);
+        if (ret < 0) {
+            xtimer_usleep(1000000);
+        } else {
+            /* Byte 0: Wheeldrops + Bumps */
+            tmp.drop_caster = !!(raw_sensors_data[0] & (1 << SENSE_DROP_CASTER));
+            tmp.drop_left = !!(raw_sensors_data[0] & (1 << SENSE_DROP_LEFT));
+            tmp.drop_right = !!(raw_sensors_data[0] & (1 << SENSE_DROP_RIGHT));
+            tmp.bump_left = !!(raw_sensors_data[0] & (1 << SENSE_BUMP_LEFT));
+            tmp.bump_right = !!(raw_sensors_data[0] & (1 << SENSE_BUMP_RIGHT));
+            /* Byte 1: Wall */
+            tmp.wall = raw_sensors_data[1];
+            /* Byte 2: Cliff L */
+            tmp.cliff_left = raw_sensors_data[2];
+            /* Byte 3: Cliff FL */
+            tmp.cliff_frontleft = raw_sensors_data[3];
+            /* Byte 4: Cliff FR */
+            tmp.cliff_frontright = raw_sensors_data[4];
+            /* Byte 5: Cliff R */
+            tmp.cliff_right = raw_sensors_data[5];
+            /* Byte 7: Overcurrents */
+            tmp.ovc = raw_sensors_data[7];
+            
+            /* 'valid' flag */
+            tmp.valid = 1;
+        }
+        if (memcmp(&roomba_sensors_data, &tmp, sizeof(struct roomba_sensors_data)) != 0) {
+            msg_t msg;
+            memcpy(&roomba_sensors_data, &tmp, sizeof(struct roomba_sensors_data));
+            print_sensors();
+            msg.type = MSG_SENSORS_AVAILABLE;
+            msg_send(&msg, gatt_srv_pid);
+        }
+    }
+}
+
+struct roomba_sensors_data *roomba_sensors(void)
+{
+   return &roomba_sensors_data; 
+}
+
+void roomba_led(uint8_t status, uint8_t spot, uint8_t clean, uint8_t max, uint8_t power_c, uint8_t power_i)
+{
+    uint8_t led_bytes[3];
+    led_bytes[0] = ((status & 0x03) << 4) | 
+        ((spot & 0x01) << 3) | 
+        ((clean & 0x01) << 2) |
+        ((max &0x01) << 1);
+    led_bytes[1] = power_c;
+    led_bytes[2] = power_i;
+    roomba_cmd(ROOMBA_LEDS, led_bytes, 3);
+}
+
+void *gatt_srv(void*);
+static const shell_command_t shell_commands[] = {
+    { "mode", "Switch mode", cmd_mode },
+    { "play", "play song", cmd_play },
+    { "info", "device info", cmd_info },
+    { NULL, NULL, NULL }
+};
+
+int main(void)
+{
+    /* initialize UART */
+    char line_buf[SHELL_BUFSIZE];
+    uart_init(UART_DEV(0), 115200, rx_cb, (void *)0);
+    /* initialize ringbuffers */
+    for (unsigned i = 0; i < UART_NUMOF; i++) {
+        ringbuffer_init(&(ctx[i].rx_buf), ctx[i].rx_mem, SENSOR_BUFFER_SIZE);
+    }
+    /* initialize GPIO WAKEUP */
+    gpio_init(GPIO_WAKEUP, GPIO_OUT);
+    gpio_set(GPIO_WAKEUP);
+
+    /* Gatt Server */
+    gatt_srv_pid = thread_create(gatt_srv_stack, sizeof(gatt_srv_stack),
+            DISPATCHER_PRIO - 1, 0, gatt_srv, NULL, "BLE_gatt");
+        
+    /* Create Sensors thread */
+    msg_init_queue(sensors_msgq, MSGQ_SIZE);
+    sensors_pid = thread_create(sensors_stack, sizeof(sensors_stack),
+                DISPATCHER_PRIO, 0, sensors_loop, NULL, "sensors");
+
+
+    /* run the shell */
+    shell_run(shell_commands, line_buf, SHELL_BUFSIZE);
+    return 0;
+}
+
+
+    

+ 29 - 0
roomba/nimble.inc.mk

@@ -0,0 +1,29 @@
+# For nRF51-based targets, we need to reduce buffer sizes to make this test fit
+# into RAM
+# Note: as the CPU variable is not set at this point, we manually 'whitelist'
+#       all supported nrf51-boards here
+
+# Set the tests default configuration
+APP_MTU ?= 5000
+APP_BUF_CHUNKSIZE ?= 250    # must be full divider of APP_MTU
+APP_BUF_NUM ?= 3
+APP_NODENAME ?= \"ROOMBAHAHAHACKED\"
+APP_CID ?= 0x0235
+
+# Apply configuration values
+CFLAGS += -DAPP_MTU=$(APP_MTU)
+CFLAGS += -DAPP_BUF_CHUNKSIZE=$(APP_BUF_CHUNKSIZE)
+CFLAGS += -DAPP_BUF_NUM=$(APP_BUF_NUM)
+CFLAGS += -DAPP_NODENAME=$(APP_NODENAME)
+CFLAGS += -DAPP_CID=$(APP_CID)
+
+# configure NimBLE
+#USEPKG += nimble
+MSYS_CNT ?= 40
+#CFLAGS += -DMYNEWT_VAL_BLE_L2CAP_COC_MAX_NUM=1
+#CFLAGS += -DMYNEWT_VAL_BLE_L2CAP_COC_MPS=250
+#CFLAGS += -DMYNEWT_VAL_BLE_MAX_CONNECTIONS=1
+#CFLAGS += -DMYNEWT_VAL_MSYS_1_BLOCK_COUNT=$(MSYS_CNT)
+#CFLAGS += -DMYNEWT_VAL_MSYS_1_BLOCK_SIZE=298
+#CFLAGS += -DMYNEWT_VAL_BLE_LL_CFG_FEAT_DATA_LEN_EXT=1
+

+ 61 - 0
roomba/nimble_l2cap_test_conf.h

@@ -0,0 +1,61 @@
+/*
+ * 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     tests
+ * @{
+ *
+ * @file
+ * @brief       Shared configuration for NimBLE L2CAP tests
+ *
+ * @author      Hauke Petersen <hauke.petersen@fu-berlin.de>
+ */
+
+#ifndef NIMBLE_L2CAP_TEST_CONF_H
+#define NIMBLE_L2CAP_TEST_CONF_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @name    Calculate values for the application specific mbuf pool
+ * @{
+ */
+#define MBUFSIZE_OVHD       (sizeof(struct os_mbuf) + \
+                             sizeof(struct os_mbuf_pkthdr))
+#define MBUFS_PER_MTU       (APP_MTU / APP_BUF_CHUNKSIZE)
+#define MBUFSIZE            (APP_BUF_CHUNKSIZE + MBUFSIZE_OVHD)
+#define MBUFCNT             (APP_BUF_NUM * MBUFS_PER_MTU)
+/** @} */
+
+/**
+ * @name    Default parameters for selected test cases
+ * @{
+ */
+#define PKTSIZE_DEFAULT     (100U)
+#define FLOOD_DEFAULT       (50U)
+/** @} */
+
+/**
+ * @name    Field offsets and type values for test packet payloads
+ * @{
+ */
+#define TYPE_INCTEST        (0x17192123)
+#define TYPE_FLOODING       (0x73829384)
+
+#define POS_TYPE            (0U)
+#define POS_SEQ             (1U)
+/** @} */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* NIMBLE_L2CAP_TEST_CONF_H */
+/** @} */

+ 81 - 0
roomba/roomba.h

@@ -0,0 +1,81 @@
+#ifndef ROOMBA_COMMANDS_H
+#define ROOMBA_COMMANDS_H
+
+/* "Roomba mode" States */
+#define ROOMBA_MODE_OFF      0
+#define ROOMBA_MODE_DRIVE    1
+#define ROOMBA_MODE_FULL     2
+#define ROOMBA_MODE_CLEAN    3
+#define ROOMBA_MODE_SPOT     4
+#define ROOMBA_MODE_DOCK     5
+
+/* Roomba commands */
+#define ROOMBA_START    0x80
+#define ROOMBA_BAUD     0x81 // len = 1
+#define ROOMBA_CTRL     0x82
+#define ROOMBA_SAFE     0x83
+#define ROOMBA_FULL     0x84
+#define ROOMBA_POWER    0x85
+#define ROOMBA_SPOT     0x86
+#define ROOMBA_CLEAN    0x87
+#define ROOMBA_MAX      0x88
+#define ROOMBA_DRIVE    0x89 // len = 4
+#define ROOMBA_MOTORS   0x8a // len = 1
+#define ROOMBA_LEDS     0x8b // len = 3
+#define ROOMBA_SONG     0x8c // len = ...
+#define ROOMBA_PLAY     0x8d // len = 1
+#define ROOMBA_SENSORS  0x8e // len = 1
+#define ROOMBA_DOCK     0x8f
+
+/* Sensor bit-shifting */
+#define SENSE_DROP_CASTER (4)
+#define SENSE_DROP_LEFT   (3)
+#define SENSE_DROP_RIGHT  (2)
+#define SENSE_BUMP_LEFT   (1)
+#define SENSE_BUMP_RIGHT  (0)
+
+struct __attribute__((packed)) sensors_code1 {
+    uint8_t bumps_wheeldrop;
+    uint8_t wall;
+    uint8_t cliff_left;
+    uint8_t cliff_front_left;
+    uint8_t cliff_front_right;
+    uint8_t cliff_right;
+    uint8_t virtual_wall;
+    uint8_t motor_overcurrents;
+    uint8_t dirt_detect_left;
+    uint8_t dirt_detect_right;
+};
+
+struct __attribute__((packed)) sensors_code2 {
+    uint8_t rc_detect;
+    uint8_t buttons;
+    uint16_t distance;
+    uint16_t angle;
+};
+
+struct __attribute__((packed)) sensors_code3 {
+    uint8_t charging_state;
+    uint8_t voltage_lsb;
+    uint8_t voltage_msb;
+    uint8_t current_lsb;
+    uint8_t current_msb;
+    uint8_t temperature;
+    uint16_t charge_mah;
+    uint16_t capacity_mah;
+};
+
+struct __attribute__((packed)) roomba_sensors_data {
+    int valid;
+    uint8_t drop_caster, drop_left, drop_right;
+    uint8_t bump_left, bump_right;
+    uint8_t wall;
+    uint8_t cliff_left, cliff_frontleft, cliff_frontright, cliff_right;
+    uint8_t ovc;
+};
+
+struct roomba_sensors_data *roomba_sensors(void);
+
+
+#endif
+

+ 32 - 0
roomba/song.h

@@ -0,0 +1,32 @@
+#ifndef SONG_H
+#define SONG_H
+
+#define NOTE_C  0
+#define NOTE_Cd 1
+#define NOTE_D  2
+#define NOTE_Dd 3
+#define NOTE_E  4
+#define NOTE_F  5
+#define NOTE_Fd 6
+#define NOTE_G  7
+#define NOTE_Gd 8
+#define NOTE_A  9
+#define NOTE_Ad 10
+#define NOTE_B  11
+#define SILENCE 30
+
+#define MIS 160
+#define HALF (MIS / 2)
+#define Q (HALF / 2)
+#define OCT (Q / 2)
+#define SIXT (OCT / 2)
+#define THREESIXT (SIXT * 3)
+
+#define Note(x,oct,dur) (uint16_t)((((36 + (oct * 12) + (x))) | dur << 8))
+
+
+
+
+
+
+#endif