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