From cb6daed95bb4982e04a44356d006be4a7f7dc410 Mon Sep 17 00:00:00 2001 From: yikestone Date: Mon, 2 Dec 2019 14:52:26 +0530 Subject: [PATCH] added ble_connect lib and tests --- .gitignore | 2 + CMakeLists.txt | 3 + gazebo_sim/CMakeLists.txt | 1 - qt_pi/CMakeLists.txt | 12 +- qt_pi/config/70-emotiv.rules | 7 ++ qt_pi/doc/Connecting on debian.md | 80 ++++++++++++ qt_pi/include/ble_connect/ble_connect.h | 33 +++++ qt_pi/src/Ble_connect.cpp | 136 -------------------- qt_pi/src/ble_connect.c | 160 ++++++++++++++++++++++++ qt_pi/tests/ble_test.cpp | 31 +++++ 10 files changed, 324 insertions(+), 141 deletions(-) create mode 100644 CMakeLists.txt create mode 100644 qt_pi/config/70-emotiv.rules create mode 100644 qt_pi/doc/Connecting on debian.md create mode 100644 qt_pi/include/ble_connect/ble_connect.h delete mode 100644 qt_pi/src/Ble_connect.cpp create mode 100644 qt_pi/src/ble_connect.c create mode 100644 qt_pi/tests/ble_test.cpp diff --git a/.gitignore b/.gitignore index 652a2fa..c057cef 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,3 @@ /scripts/qt_pi/*.pyc +.ccls-cache +compile_commands.json diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..191ccf8 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,3 @@ +cmake_minimum_required(VERSION 3.5) +ADD_SUBDIRECTORY( qt_pi ) +ADD_SUBDIRECTORY( gazebo_sim ) diff --git a/gazebo_sim/CMakeLists.txt b/gazebo_sim/CMakeLists.txt index 1591e19..94fdf7d 100644 --- a/gazebo_sim/CMakeLists.txt +++ b/gazebo_sim/CMakeLists.txt @@ -5,5 +5,4 @@ set(CMAKE_CXX_FLAGS "-fpermissive -std=c++0x") find_package(catkin REQUIRED) -catkin_python_setup() catkin_package(DEPENDS) diff --git a/qt_pi/CMakeLists.txt b/qt_pi/CMakeLists.txt index dd7dc4a..520a8dd 100644 --- a/qt_pi/CMakeLists.txt +++ b/qt_pi/CMakeLists.txt @@ -11,10 +11,10 @@ catkin_package(DEPENDS) find_package(PkgConfig REQUIRED) find_path(Mcrypt_INCLUDE_DIR mcrypt.h PATHS - /usr/local/include + /usr/include ) -set(Mcrypt_LIB_PATHS /usr/local/lib) +set(Mcrypt_LIB_PATHS /usr/lib) find_library(Mcrypt_LIBS NAMES mcrypt rtfilter PATHS ${Mcrypt_LIB_PATHS}) include_directories(include) @@ -38,5 +38,9 @@ include_directories(${GLIB_INCLUDE_DIRS}) include_directories(${Mcrypt_INCLUDE_DIRS}) link_directories(${Mcrypt_LIBS}) -add_executable(ble_connect src/Ble_connect.cpp) -target_link_libraries(ble_connect ${GATTLIB_LIBRARIES} ${GATTLIB_LDFLAGS} ${GLIB_LDFLAGS} pthread ${Mcrypt_LIBS}) +add_library(ble_connect_lib include/ble_connect/ble_connect.h src/ble_connect.c) + +target_link_libraries(ble_connect_lib ${GATTLIB_LIBRARIES} ${GATTLIB_LDFLAGS} ${GLIB_LDFLAGS} pthread ${Mcrypt_LIBS}) + +add_executable(ble_connect_test tests/ble_test.cpp) +target_link_libraries(ble_connect_test ble_connect_lib) diff --git a/qt_pi/config/70-emotiv.rules b/qt_pi/config/70-emotiv.rules new file mode 100644 index 0000000..a6c4e05 --- /dev/null +++ b/qt_pi/config/70-emotiv.rules @@ -0,0 +1,7 @@ +SUBSYSTEM=="hidraw", ACTION=="add", SUBSYSTEMS=="hid", DRIVERS=="generic-usb", KERNEL=="hidraw*", GROUP="username", MODE="0666" + +SUBSYSTEM=="hidraw", ACTION=="add", SUBSYSTEMS=="usb", DRIVERS=="usbhid", GROUP="username", MODE="0666" + +SUBSYSTEM=="usb", ACTION=="add", ATTR{idVendor}=="1234", ATTR{idProduct}=="ed02", GROUP="username", MODE="0666" + +SUBSYSTEM=="usb", ACTION=="add", SUBSYSTEMS=="usb", ATTRS{idVendor}=="1d6b", GROUP="username", MODE="0666" diff --git a/qt_pi/doc/Connecting on debian.md b/qt_pi/doc/Connecting on debian.md new file mode 100644 index 0000000..187a3f2 --- /dev/null +++ b/qt_pi/doc/Connecting on debian.md @@ -0,0 +1,80 @@ +## Connecting Emotiv device via USB dongle on Ubuntu 16.04 and Debian 8.3 + +### Install HID configure for Emotiv USB dongle +```shell +sudo cp etc/udev/rules.d/70-emotiv.rules /etc/udev/rules.d/ +sudo service udev restart +``` + +Then plugin the USB dongle, you should see the flashing green light and it is ready to be connected to an Emotiv headset. + +## Connecting Emotiv device via BTLE on Ubuntu 16.04 and Debian 8.3 with BlueZ library +### Install and configure latest bluez +#### Build bluez +```shell +sudo apt-get update +sudo apt-get install libglib2.0-dev libdbus-1-dev libudev-dev libical-dev libreadline-dev +mkdir -p ~/tmp/bluez && cd ~/tmp/bluez +wget -c https://www.kernel.org/pub/linux/bluetooth/bluez-5.37.tar.gz +tar -xvzf bluez-5.37.tar.gz +cd bluez-5.37/ +./configure --disable-systemd --enable-threads --enable-library +make +``` + +#### Configure + * Add `EnableGatt = true` to file `/etc/bluetooth/main.conf` + * Backup then edit file `/etc/dbus-1/system.d/bluetooth.conf`, remove `1` from all the lines, such as: + + `` to `` + + `` to `` + +### Run bluez service and test connection +#### Stop old bluez service +`sudo /etc/init.d/bluetooth stop` + +#### Start new bluez +`sudo ./src/bluetoothd -n -E --plugin=audio` + +#### Test connection +Open other terminal and run `bluetoothctl`. Use following commands to test connection: +* List available Bluetooth adapters + + `[bluetooth]#list` + +* Power on Bluetooth adapter + + `[bluetooth]#power on` + +* Set Bluetooth adapter discoverable + + `[bluetooth]#discoverable on` + +* Set Bluetooth adapter pairable + + `[bluetooth]#pairable on` + +* Tell Bluetooth adapter to scan Bluetooth devices + + `[bluetooth]#scan on` + +#### List and connect to device +* List all available bluetooth devices + + `[bluetooth]#devices` + +* Get device info + + `[bluetooth]#info [MAC_Address]` + + > Each Emotiv device has a specific service UUID `81072f40-9f3d-11e3-a9dc-0002a5d5c51b` + +* Connect to an Emotiv device + + `[bluetooth]#connect [MAC Address]` + +Now you can run the SDK or app on Ubuntu - enjoy! + +#### Using graphical view +To use the graphical view instead of command line, run `d-feet &` then select `/org.bluez/hci0/[MAC_Address]` diff --git a/qt_pi/include/ble_connect/ble_connect.h b/qt_pi/include/ble_connect/ble_connect.h new file mode 100644 index 0000000..511a78f --- /dev/null +++ b/qt_pi/include/ble_connect/ble_connect.h @@ -0,0 +1,33 @@ +#ifndef BLE_CONNECT +#define BLE_CONNECT +#include "gattlib.h" +#include +#include +#include +#include +#include +#include +#include + +enum Channels { AF3 = 0, T7 = 1, Pz = 2, T8 = 3, AF4 = 4 }; + +struct insight_data { + double uV[5]; + struct timespec ts; +} insight_data; +#ifdef __cplusplus +extern "C" { +#endif +extern int insight_init(const char *a, const char *b); +extern int insight_destroy(); +extern int insight_get_status(); +extern int insight_start_notif(); +extern int insight_stop_notif(); +extern void insight_write_to_buffer(int len, struct insight_data *buf); +extern int insight_buffer_status(); +extern int insight_stop_write_to_buffer(); +extern int insight_version(); +#ifdef __cplusplus +} +#endif +#endif diff --git a/qt_pi/src/Ble_connect.cpp b/qt_pi/src/Ble_connect.cpp deleted file mode 100644 index 995195c..0000000 --- a/qt_pi/src/Ble_connect.cpp +++ /dev/null @@ -1,136 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include - -#include "gattlib.h" - -static uuid_t g_uuid; -const uint8_t BT_key[4] = {0x5a, 0x68, 0x5b, 0xb6}; -const uint8_t key[16] = {BT_key[0], 00, BT_key[1], 21, BT_key[2], 00, BT_key[3], 12, BT_key[2], 00, BT_key[1], 68, BT_key[0], 00, BT_key[1], 88 }; -static uint8_t data[16]; -int data_length = 16; - -MCRYPT mcrpt; -double uV[5]; -uint8_t raw_data[72]; -enum Channels{ - AF3 = 0, - T7 = 1, - Pz = 2, - T8 = 3, - AF4 = 4 -}; - -int k = 0; - -double raw(uint8_t h, uint8_t l){ - return (((double)h - 128) * 32.82051289 + (double)l * -0.128205128205129 + 4201.02564096001); -} - -void notification_handler(const uuid_t* uuid, const uint8_t* val, size_t values_length, void* user_data) { - int i, j, k; - //for(i = 0; i < values_length; i++) - // std::cout<<(int)val[i]<<"\t"; - //std::cout<= 0; k--) - raw_data[i * 8 + (7 - k)] = (val[i] >> k) & 1; - } - - for(i = 0; i < 5; i++) - { - uint8_t h = 0, l = 0; - - j=-1; - while((++j) < 8) - h = (h << 1) | ((uint8_t)(raw_data[(14 * i ) + j] )); - j--; - while((++j) < 14) - l = (l << 1) | ((uint8_t)(raw_data[(14 * i ) + j])); - - uV[i] = raw(h,l); - } - - //for (i = 0; i < data_length; i++) { - //if(val[9] == 194 || val[9]==0)continue; - //std::cout<> 3); - for (c = 7; c >= 0; c--) - { - k = n >> c; - if (k & 1) - printf("1"); - else - printf("0"); - }*/ - printf("\n"); - //if(val[0]==0)printf("\n"); -} - -static void usage(char *argv[]) { - printf("%s \n", argv[0]); -} - -GMainLoop *loop; - -void run(){ - loop = g_main_loop_new(NULL, 0); - g_main_loop_run(loop); - pthread_exit(NULL); -} - -int main(int argc, char *argv[]) { - - mcrpt = mcrypt_module_open(MCRYPT_RIJNDAEL_128, NULL, MCRYPT_ECB, NULL); - mcrypt_generic_init(mcrpt, key, data_length, NULL); - int ret; - gatt_connection_t* connection; - - - if (argc != 3) { - usage(argv); - return 1; - } - - if (gattlib_string_to_uuid(argv[2], strlen(argv[2]) + 1, &g_uuid) < 0) { - usage(argv); - return 1; - } - connection = gattlib_connect(NULL, argv[1], BDADDR_LE_PUBLIC, BT_SEC_LOW, 0, 0); - if (connection == NULL) { - fprintf(stderr, "Fail to connect to the bluetooth device.\n"); - return 1; - } - gattlib_register_notification(connection, notification_handler, NULL); - ret = gattlib_notification_start(connection, &g_uuid); - if (ret) { - fprintf(stderr, "Fail to start notification\n."); - return 1; - } - pthread_t thread; - pthread_create(&thread, NULL,run,NULL); - getchar(); - g_main_loop_quit(loop); - g_main_loop_unref(loop); - gattlib_notification_stop(connection, &g_uuid); - gattlib_disconnect(connection); - mcrypt_generic_deinit (mcrpt); - mcrypt_module_close(mcrpt); - return 0; -} diff --git a/qt_pi/src/ble_connect.c b/qt_pi/src/ble_connect.c new file mode 100644 index 0000000..3249ca2 --- /dev/null +++ b/qt_pi/src/ble_connect.c @@ -0,0 +1,160 @@ +#include "ble_connect/ble_connect.h" + +#define B0 0x5a +#define B1 0x68 +#define B2 0x5b +#define B3 0xb6 + +MCRYPT mcrpt; +GMainLoop *loop; +gatt_connection_t *connection; +const int data_length = 16; +uint8_t key[16] = {B0, 00, B1, 21, B2, 00, B3, 12, + B2, 00, B1, 68, B0, 00, B1, 88}; +static uint8_t data[16]; +static uuid_t g_uuid; + +static uint8_t raw_data[72]; + +struct insight_data *data_buffer; +int data_buffer_length; +int data_index; + +int status = 0; +int write_to_buffer = 0; + +pthread_t thread; + +double raw(uint8_t h, uint8_t l) { + return (((double)h - 128) * 32.82051289 + (double)l * -0.128205128205129 + + 4201.02564096001); +} + +void notification_handler(const uuid_t *uuid, const uint8_t *val, + size_t values_length, void *user_data) { + + if (write_to_buffer == 0 || write_to_buffer == 2) + return; + + timespec_get(&data_buffer[data_index].ts, TIME_UTC); + + mdecrypt_generic(mcrpt, (void *)val, data_length); + + for (int i = 0; i < 9; i++) { + for (int k = 7; k >= 0; k--) + raw_data[i * 8 + (7 - k)] = (val[i] >> k) & 1; + } + + for (int i = 0; i < 5; i++) { + uint8_t h = 0, l = 0; + + int j = -1; + while ((++j) < 8) + h = (h << 1) | ((uint8_t)(raw_data[(14 * i) + j])); + j--; + while ((++j) < 14) + l = (l << 1) | ((uint8_t)(raw_data[(14 * i) + j])); + + data_buffer[data_index].uV[i] = raw(h, l); + } + + data_index++; + + if (data_index == data_buffer_length) + write_to_buffer = 2; +} + +void insight_write_to_buffer(int len, struct insight_data *buf) { + data_buffer = buf; + data_buffer_length = len; + data_index = 0; + write_to_buffer = 1; +} + +int insight_stop_write_to_buffer() { + write_to_buffer = 0; + return data_index; +} + +int insight_buffer_status() { return write_to_buffer; } + +void run() { + g_main_loop_run(loop); + pthread_exit(NULL); +} + +int insight_init(const char *a, const char *b) { + + if (status != 0) + return 1; + + mcrpt = mcrypt_module_open(MCRYPT_RIJNDAEL_128, NULL, MCRYPT_ECB, NULL); + mcrypt_generic_init(mcrpt, (void *)key, data_length, NULL); + + if (gattlib_string_to_uuid(b, strlen(b) + 1, &g_uuid) < 0) { + return 2; + } + + connection = gattlib_connect(NULL, a, BDADDR_LE_PUBLIC, BT_SEC_LOW, 0, 0); + + if (connection == NULL) { + return 3; + } + + gattlib_register_notification(connection, notification_handler, NULL); + loop = g_main_loop_new(NULL, 0); + + status = 1; + + return 0; +} + +int insight_start_notif() { + + if (status != 1) + return 2; + + write_to_buffer = 0; + int ret = gattlib_notification_start(connection, &g_uuid); + + if (ret) + return ret; + + pthread_create(&thread, NULL, (void *(*)(void *))run, NULL); + + while (!g_main_loop_is_running(loop)) + sleep(0); + + status = 2; + return 0; +} + +int insight_stop_notif() { + if (status != 2) + return 1; + + write_to_buffer = 0; + + g_main_loop_quit(loop); + gattlib_notification_stop(connection, &g_uuid); + pthread_join(thread, NULL); + + status = 1; + return 0; +} + +int insight_destroy() { + if (status != 1) + return status; + + g_main_loop_unref(loop); + gattlib_disconnect(connection); + mcrypt_generic_deinit(mcrpt); + mcrypt_module_close(mcrpt); + status = 0; + return 0; +} + +int insight_get_status() { return status; } + +int insight_version() { return 0; } diff --git a/qt_pi/tests/ble_test.cpp b/qt_pi/tests/ble_test.cpp new file mode 100644 index 0000000..865af77 --- /dev/null +++ b/qt_pi/tests/ble_test.cpp @@ -0,0 +1,31 @@ +#include "ble_connect/ble_connect.h" +#include +int main() { + const char *a = "EF:BD:39:4E:08:49"; + const char *b = "81072f41-9f3d-11e3-a9dc-0002a5d5c51b"; + + insight_init(a, b); + insight_start_notif(); + + struct insight_data *buf = + (struct insight_data *)malloc(sizeof(struct insight_data) * 10); + + int len = 10; + + insight_write_to_buffer(len, buf); + + while (insight_buffer_status() == 1) + sleep(0); + + // sleep(5); + + printf("Stopping notif %d \n", insight_stop_notif()); + + for (int i = 0; i < len; i++) { + printf("%f\n", buf[i].uV[0]); + } + + printf("Destroy called %d\n", insight_destroy()); + printf("%d\n", insight_version()); + return 0; +}