mirror of
https://gitlab.com/yikestone/qt_pi.git
synced 2025-08-03 05:34:13 +05:30
added BLE EEG data connection
This commit is contained in:
yikestone
parent
a59d052ef4
commit
8cc0486ed6
@ -5,9 +5,17 @@ find_package(catkin REQUIRED)
|
||||
catkin_python_setup()
|
||||
catkin_package(DEPENDS)
|
||||
|
||||
include_directories(
|
||||
find_package(PkgConfig REQUIRED)
|
||||
|
||||
find_path(Mcrypt_INCLUDE_DIR mcrypt.h PATHS
|
||||
/usr/local/include
|
||||
)
|
||||
|
||||
set(Mcrypt_LIB_PATHS /usr/local/lib)
|
||||
find_library(Mcrypt_LIBS NAMES mcrypt rtfilter PATHS ${Mcrypt_LIB_PATHS})
|
||||
|
||||
include_directories(include)
|
||||
|
||||
install(DIRECTORY config
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
@ -19,3 +27,13 @@ install(DIRECTORY launch
|
||||
install(DIRECTORY nodes
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
|
||||
pkg_search_module(GATTLIB REQUIRED gattlib)
|
||||
pkg_search_module(GLIB REQUIRED glib-2.0)
|
||||
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})
|
||||
|
||||
4
msg/EEG_data.msg
Normal file
4
msg/EEG_data.msg
Normal file
@ -0,0 +1,4 @@
|
||||
int channel[5]
|
||||
float32 data[5]
|
||||
int cq[5]
|
||||
int batt_lvl
|
||||
136
src/Ble_connect.cpp
Normal file
136
src/Ble_connect.cpp
Normal file
@ -0,0 +1,136 @@
|
||||
#include <glib.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <iostream>
|
||||
#include <mcrypt.h>
|
||||
#include <pthread.h>
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#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<<std::endl;
|
||||
mdecrypt_generic (mcrpt, val, data_length);
|
||||
for(i = 0; i < 9; i++)
|
||||
{
|
||||
for(k = 7; k >= 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<<std::dec<<(int)val[9]<<" ";
|
||||
//std::cout<<std::dec<<(int)val[i]<<"\t";
|
||||
//}
|
||||
|
||||
std::cout.precision(10);
|
||||
std::cout<<k<<"\t";
|
||||
for (i = 0; i < 5; i++) {
|
||||
std::cout<<uV[i]<<"\t";
|
||||
}
|
||||
|
||||
/*int n=((val[1] & 0b00000011) << 5 ) + (val[2] >> 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 <device_address> <UUID>\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;
|
||||
}
|
||||
161
src/evp-encrypt.cxx
Normal file
161
src/evp-encrypt.cxx
Normal file
@ -0,0 +1,161 @@
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <limits>
|
||||
#include <stdexcept>
|
||||
|
||||
#include <openssl/evp.h>
|
||||
#include <openssl/rand.h>
|
||||
|
||||
static const unsigned int KEY_SIZE = 32;
|
||||
static const unsigned int BLOCK_SIZE = 16;
|
||||
|
||||
template <typename T>
|
||||
struct zallocator
|
||||
{
|
||||
public:
|
||||
typedef T value_type;
|
||||
typedef value_type* pointer;
|
||||
typedef const value_type* const_pointer;
|
||||
typedef value_type& reference;
|
||||
typedef const value_type& const_reference;
|
||||
typedef std::size_t size_type;
|
||||
typedef std::ptrdiff_t difference_type;
|
||||
|
||||
pointer address (reference v) const {return &v;}
|
||||
const_pointer address (const_reference v) const {return &v;}
|
||||
|
||||
pointer allocate (size_type n, const void* hint = 0) {
|
||||
if (n > std::numeric_limits<size_type>::max() / sizeof(T))
|
||||
throw std::bad_alloc();
|
||||
return static_cast<pointer> (::operator new (n * sizeof (value_type)));
|
||||
}
|
||||
|
||||
void deallocate(pointer p, size_type n) {
|
||||
OPENSSL_cleanse(p, n*sizeof(T));
|
||||
::operator delete(p);
|
||||
}
|
||||
|
||||
size_type max_size() const {
|
||||
return std::numeric_limits<size_type>::max() / sizeof (T);
|
||||
}
|
||||
|
||||
template<typename U>
|
||||
struct rebind
|
||||
{
|
||||
typedef zallocator<U> other;
|
||||
};
|
||||
|
||||
void construct (pointer ptr, const T& val) {
|
||||
new (static_cast<T*>(ptr) ) T (val);
|
||||
}
|
||||
|
||||
void destroy(pointer ptr) {
|
||||
static_cast<T*>(ptr)->~T();
|
||||
}
|
||||
|
||||
#if __cpluplus >= 201103L
|
||||
template<typename U, typename... Args>
|
||||
void construct (U* ptr, Args&& ... args) {
|
||||
::new (static_cast<void*> (ptr) ) U (std::forward<Args> (args)...);
|
||||
}
|
||||
|
||||
template<typename U>
|
||||
void destroy(U* ptr) {
|
||||
ptr->~U();
|
||||
}
|
||||
#endif
|
||||
};
|
||||
|
||||
typedef unsigned char byte;
|
||||
typedef std::basic_string<char, std::char_traits<char>, zallocator<char> > secure_string;
|
||||
using EVP_CIPHER_CTX_free_ptr = std::unique_ptr<EVP_CIPHER_CTX, decltype(&::EVP_CIPHER_CTX_free)>;
|
||||
|
||||
void gen_params(byte key[KEY_SIZE], byte iv[BLOCK_SIZE]);
|
||||
void aes_encrypt(const byte key[KEY_SIZE], const byte iv[BLOCK_SIZE], const secure_string& ptext, secure_string& ctext);
|
||||
void aes_decrypt(const byte key[KEY_SIZE], const byte iv[BLOCK_SIZE], const secure_string& ctext, secure_string& rtext);
|
||||
|
||||
// g++ -Wall -std=c++11 evp-encrypt.cxx -o evp-encrypt.exe -lcrypto
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
// Load the necessary cipher
|
||||
EVP_add_cipher(EVP_aes_256_cbc());
|
||||
|
||||
// plaintext, ciphertext, recovered text
|
||||
secure_string ptext = "Now is the time for all good men to come to the aide of their country";
|
||||
secure_string ctext, rtext;
|
||||
|
||||
byte key[KEY_SIZE], iv[BLOCK_SIZE];
|
||||
gen_params(key, iv);
|
||||
|
||||
aes_encrypt(key, iv, ptext, ctext);
|
||||
aes_decrypt(key, iv, ctext, rtext);
|
||||
|
||||
OPENSSL_cleanse(key, KEY_SIZE);
|
||||
OPENSSL_cleanse(iv, BLOCK_SIZE);
|
||||
|
||||
std::cout << "Original message:\n" << ptext << std::endl;
|
||||
std::cout << "Recovered message:\n" << rtext << std::endl;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void gen_params(byte key[KEY_SIZE], byte iv[BLOCK_SIZE])
|
||||
{
|
||||
int rc = RAND_bytes(key, KEY_SIZE);
|
||||
if (rc != 1)
|
||||
throw std::runtime_error("RAND_bytes key failed");
|
||||
|
||||
rc = RAND_bytes(iv, BLOCK_SIZE);
|
||||
if (rc != 1)
|
||||
throw std::runtime_error("RAND_bytes for iv failed");
|
||||
}
|
||||
|
||||
void aes_encrypt(const byte key[KEY_SIZE], const byte iv[BLOCK_SIZE], const secure_string& ptext, secure_string& ctext)
|
||||
{
|
||||
EVP_CIPHER_CTX_free_ptr ctx(EVP_CIPHER_CTX_new(), ::EVP_CIPHER_CTX_free);
|
||||
int rc = EVP_EncryptInit_ex(ctx.get(), EVP_aes_256_cbc(), NULL, key, iv);
|
||||
if (rc != 1)
|
||||
throw std::runtime_error("EVP_EncryptInit_ex failed");
|
||||
|
||||
// Recovered text expands upto BLOCK_SIZE
|
||||
ctext.resize(ptext.size()+BLOCK_SIZE);
|
||||
int out_len1 = (int)ctext.size();
|
||||
|
||||
rc = EVP_EncryptUpdate(ctx.get(), (byte*)&ctext[0], &out_len1, (const byte*)&ptext[0], (int)ptext.size());
|
||||
if (rc != 1)
|
||||
throw std::runtime_error("EVP_EncryptUpdate failed");
|
||||
|
||||
int out_len2 = (int)ctext.size() - out_len1;
|
||||
rc = EVP_EncryptFinal_ex(ctx.get(), (byte*)&ctext[0]+out_len1, &out_len2);
|
||||
if (rc != 1)
|
||||
throw std::runtime_error("EVP_EncryptFinal_ex failed");
|
||||
|
||||
// Set cipher text size now that we know it
|
||||
ctext.resize(out_len1 + out_len2);
|
||||
}
|
||||
|
||||
void aes_decrypt(const byte key[KEY_SIZE], const byte iv[BLOCK_SIZE], const secure_string& ctext, secure_string& rtext)
|
||||
{
|
||||
EVP_CIPHER_CTX_free_ptr ctx(EVP_CIPHER_CTX_new(), ::EVP_CIPHER_CTX_free);
|
||||
int rc = EVP_DecryptInit_ex(ctx.get(), EVP_aes_256_cbc(), NULL, key, iv);
|
||||
if (rc != 1)
|
||||
throw std::runtime_error("EVP_DecryptInit_ex failed");
|
||||
|
||||
// Recovered text contracts upto BLOCK_SIZE
|
||||
rtext.resize(ctext.size());
|
||||
int out_len1 = (int)rtext.size();
|
||||
|
||||
rc = EVP_DecryptUpdate(ctx.get(), (byte*)&rtext[0], &out_len1, (const byte*)&ctext[0], (int)ctext.size());
|
||||
if (rc != 1)
|
||||
throw std::runtime_error("EVP_DecryptUpdate failed");
|
||||
|
||||
int out_len2 = (int)rtext.size() - out_len1;
|
||||
rc = EVP_DecryptFinal_ex(ctx.get(), (byte*)&rtext[0]+out_len1, &out_len2);
|
||||
if (rc != 1)
|
||||
throw std::runtime_error("EVP_DecryptFinal_ex failed");
|
||||
|
||||
// Set recovered text size now that we know it
|
||||
rtext.resize(out_len1 + out_len2);
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user