diff --git a/2 b/2 new file mode 100644 index 0000000..e69de29 diff --git a/platformio.ini b/platformio.ini index 7e36255..3fb75bc 100644 --- a/platformio.ini +++ b/platformio.ini @@ -18,6 +18,8 @@ lib_deps = bblanchon/ArduinoJson @ ^7.2.0 links2004/WebSockets @ ^2.4.2 adafruit/Adafruit NeoPixel@^1.12.2 + adafruit/Adafruit MPU6050 @ ^2.0.0 + adafruit/Adafruit Unified Sensor @ ^1.1.3 ; build_flags = ; -D ARDUINO_USB_MODE=1 ; -D ARDUINO_USB_CDC_ON_BOOT=1 diff --git a/src/main.cpp b/src/main.cpp index fbf6c57..e99da5a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -29,6 +29,9 @@ #include #include #include +#include +#include +#include const char *ssid = "ORIC-EEG"; const char *password = ""; @@ -39,8 +42,8 @@ const char *password = ""; #define ADS_DATA_SIZE (CHANNELS * 3) #define ADS_STATUS_SIZE 3 #define BLOCK_SIZE 32 // Data + Timestamp + Counter -#define SAMPLES_PER_BUFFER 250 -#define PACKET_SIZE (BLOCK_SIZE * SAMPLES_PER_BUFFER) +#define SAMPLES_PER_BUFFER 10 +#define PACKET_SIZE (BLOCK_SIZE * SAMPLES_PER_BUFFER + 6) #define NUM_BUFFERS 20 #define MAX_PAYLOAD_SIZE 256 @@ -62,6 +65,31 @@ int num_active_channels = 0; boolean active_channels[9]; boolean is_rdatac = false; +// accelerometer data +#define ACCEL_DATA_SIZE_IN_BYTES 6 +union +{ + uint8_t accel_bytes[ACCEL_DATA_SIZE_IN_BYTES]; + struct { + int16_t x; + int16_t y; + int16_t z; + } accel; +} accel_union; + +// MPU6050 setup +Adafruit_MPU6050 mpu; +const float INV_X = 1.0; +const float INV_Y = 1.0; +const float INV_Z = 1.0; +const int CAL_SAMPLES = 200; +const float CAL_DELAY_MS = 5.0; +float offsetX = 0.0; +float offsetY = 0.0; +float offsetZ = 0.0; +bool mpu_initialized = false; + + // microseconds timestamp #define TIMESTAMP_SIZE_IN_BYTES 4 union @@ -112,6 +140,60 @@ void readRegisterCommand(unsigned char unused1, unsigned char unused2); void writeRegisterCommand(unsigned char register_number, unsigned char register_value); void helpCommand(unsigned char unused1, unsigned char unused2); +void mpuSetup() { + Wire.begin(); + Wire.setClock(400000); + + if (!mpu.begin()) { + ESP_LOGE("MPU6050", "MPU not found!"); + mpu_initialized = false; + return; + } + + mpu.setFilterBandwidth(MPU6050_BAND_260_HZ); + delay(100); + + // Quick calibration + float sumX = 0, sumY = 0, sumZ = 0; + sensors_event_t a, g, temp; + + for (int i = 0; i < CAL_SAMPLES; i++) { + mpu.getEvent(&a, &g, &temp); + sumX += a.acceleration.x; + sumY += a.acceleration.y; + sumZ += a.acceleration.z; + delay(CAL_DELAY_MS); + } + + offsetX = sumX / CAL_SAMPLES; + offsetY = sumY / CAL_SAMPLES; + offsetZ = sumZ / CAL_SAMPLES; + + mpu_initialized = true; + ESP_LOGD("MPU6050", "MPU6050 initialized and calibrated"); +} + +void readAccelerometer() { + if (!mpu_initialized) { + accel_union.accel.x = 0; + accel_union.accel.y = 0; + accel_union.accel.z = 0; + return; + } + + sensors_event_t a, g, temp; + mpu.getEvent(&a, &g, &temp); + + float ax = (a.acceleration.x - offsetX) * INV_X; + float ay = (a.acceleration.y - offsetY) * INV_Y; + float az = (a.acceleration.z - offsetZ) * INV_Z; + + // Convert to int16_t (multiply by 1000 for precision) + accel_union.accel.x = (int16_t)(ax * 1000); + accel_union.accel.y = (int16_t)(ay * 1000); + accel_union.accel.z = (int16_t)(az * 1000); +} + void setup() { Serial.begin(BAUD_RATE); @@ -123,6 +205,7 @@ void setup() // Hardware setup espSetup(); adsSetup(); + mpuSetup(); // Setup callbacks for SerialCommand commands wsCommand.addCommand("nop", nopCommand); // No operation (does nothing) @@ -241,10 +324,17 @@ void loop() { if (buffer_completed[buffer_to_send]) { + // Read accelerometer data once per packet + readAccelerometer(); + + // Add accelerometer data to the end of the packet + uint8_t *accel_ptr = &data_buffers[buffer_to_send][BLOCK_SIZE * SAMPLES_PER_BUFFER]; + memcpy(accel_ptr, accel_union.accel_bytes, ACCEL_DATA_SIZE_IN_BYTES); + // Send the current buffer via WebSocket webSocket.sendBIN(0, (uint8_t *)&data_buffers[buffer_to_send], PACKET_SIZE); - vTaskDelay(20 / portTICK_PERIOD_MS); + vTaskDelay(10 / portTICK_PERIOD_MS); // Move to the next buffer in sequence buffer_completed[buffer_to_send] = false; buffer_to_send = (buffer_to_send + 1) % NUM_BUFFERS; @@ -262,6 +352,7 @@ void webSocketEvent(byte num, WStype_t type, uint8_t *payload, size_t length) ESP_LOGD("WEBSOCKET", "Client %d disconnected", num); pixels.setPixelColor(0, pixels.Color(PIXEL_BRIGHTNESS, PIXEL_BRIGHTNESS, 0)); // Yellow pixels.show(); + ESP.restart(); break; case WStype_CONNECTED: // if a client is connected, then type == WStype_CONNECTED ESP_LOGD("WEBSOCKET", "Client %d connected", num); diff --git a/test/main.py b/test/main.py new file mode 100644 index 0000000..bb00aa6 --- /dev/null +++ b/test/main.py @@ -0,0 +1,146 @@ +import websocket +import socket +import json +import time +import datetime +import math +from pylsl import StreamInfo, StreamOutlet + +def calculate_rate(data_size, elapsed_time): + rate = data_size / elapsed_time + return rate + +blockSize = 32 +samples_per_buffer = 10 +accel_data_size = 6 +expected_packet_size = (blockSize * samples_per_buffer) + accel_data_size # 326 bytes + +data_size = 0 +sample_size = 0 +packet_size = 0 +previousSampleNumber = -1 +previousTimeStamp = -1 +previousData = [] + +stream_name = 'ORIC2' +data = StreamInfo(stream_name, 'EEG', 8, 500, 'float32', 'uid007') +outlet = StreamOutlet(data) + +# Add accelerometer stream (50Hz since it's updated once per packet of 10 samples at 500Hz) +accel_stream_name = 'ORIC_Accel' +accel_data = StreamInfo(accel_stream_name, 'Accelerometer', 3, 50, 'float32', 'uid008') +accel_outlet = StreamOutlet(accel_data) + +ws = websocket.WebSocket() +ws.connect("ws://"+socket.gethostbyname("oric.local")+":81") +# ws.connect("ws://192.168.0.23:81") + +ws.send_text(json.dumps({"command":"sdatac", "parameters":[]})) +# ws.send_text(json.dumps({"command":"wreg", "parameters":[0x01, 0b10010100]})) +ws.send_text(json.dumps({"command":"wreg", "parameters":[0x01, 0x95]})) +ws.send_text(json.dumps({"command":"wreg", "parameters":[0x02, 0xC0]})) +ws.send_text(json.dumps({"command":"wreg", "parameters":[0x03, 0xEC]})) +ws.send_text(json.dumps({"command":"wreg", "parameters":[0x15, 0b00100000]})) +ws.send_text(json.dumps({"command":"wreg", "parameters":[0x05, 0x00]})) +ws.send_text(json.dumps({"command":"wreg", "parameters":[0x06, 0x00]})) +ws.send_text(json.dumps({"command":"wreg", "parameters":[0x07, 0x00]})) +ws.send_text(json.dumps({"command":"wreg", "parameters":[0x08, 0x00]})) +ws.send_text(json.dumps({"command":"wreg", "parameters":[0x09, 0x00]})) +ws.send_text(json.dumps({"command":"wreg", "parameters":[0x0A, 0x00]})) +ws.send_text(json.dumps({"command":"wreg", "parameters":[0x0B, 0x00]})) +ws.send_text(json.dumps({"command":"wreg", "parameters":[0x0C, 0x00]})) +ws.send_text(json.dumps({"command":"status", "parameters":[]})) +ws.send_text(json.dumps({"command":"rdatac", "parameters":[]})) + +print("Setup Done!") + +start_time = time.time() + +while 1: + data = ws.recv() + data_size += len(data) + + current_time = time.time() + elapsed_time = current_time - start_time + if elapsed_time >= 1.0: + samples_per_second = calculate_rate(sample_size, elapsed_time) + refresh_rate = calculate_rate(packet_size, elapsed_time) + bytes_per_second = calculate_rate(data_size, elapsed_time) + + # Get the current local time + local_time = datetime.datetime.now() + # Extract hours, minutes, and seconds + hours = local_time.hour + minutes = local_time.minute + seconds = local_time.second + + print(f"{math.ceil(refresh_rate)} Packets : {math.ceil(samples_per_second)} Samples : {math.ceil(bytes_per_second)} Bytes") + packet_size = 0 + sample_size = 0 + data_size = 0 + start_time = current_time + + if data and (type(data) is list or type(data) is bytes): + packet_size += 1 + + # Check if packet size is correct + if len(data) != expected_packet_size: + print(f"Error: Expected packet size {expected_packet_size}, got {len(data)}") + continue + + # Extract accelerometer data from the end of the packet + accel_data = data[-accel_data_size:] # Last 6 bytes + accel_x = int.from_bytes(accel_data[0:2], byteorder='little', signed=True) + accel_y = int.from_bytes(accel_data[2:4], byteorder='little', signed=True) + accel_z = int.from_bytes(accel_data[4:6], byteorder='little', signed=True) + + # Process only the EEG data part (exclude accelerometer data) + eeg_data = data[:-accel_data_size] # All except last 6 bytes + + for blockLocation in range(0, len(eeg_data), blockSize): + sample_size += 1 + block = eeg_data[blockLocation:blockLocation + blockSize] + + # data_hex = ":".join("{:02x}".format(c) for c in data) + timestamp = int.from_bytes(block[0:4], byteorder='little') + sample_number = int.from_bytes(block[4:8], byteorder='little') + channel_data = [] + + for channel in range(0, 8): + channel_offset = 8 + (channel * 3) + sample = int.from_bytes(block[channel_offset:channel_offset + 3], byteorder='big', signed=True) + channel_data.append(sample) + + if previousSampleNumber == -1: + previousSampleNumber = sample_number + previousTimeStamp = timestamp + previousData = channel_data + else: + if sample_number - previousSampleNumber > 1: + print("Error: Sample Lost") + exit() + elif sample_number == previousSampleNumber: + print("Error: Duplicate sample") + exit() + elif sample_number - previousSampleNumber < 1: + print("Error: Sample order missed") + exit() + else: + # print(timestamp - previousTimeStamp) + previousTimeStamp = timestamp + previousSampleNumber = sample_number + previousData = channel_data + + # Check only EEG channels for blank data (first 8 channels) + if(all(v == 0 for v in channel_data[:3]) and all(v > 0 for v in channel_data[4:8])): + print("Blank Data: ",timestamp, sample_number, channel_data[0], channel_data[1], channel_data[2], channel_data[3], channel_data[4], channel_data[5], channel_data[6], channel_data[7]) + # print(f"Accel Data: X={accel_x:.3f}, Y={accel_y:.3f}, Z={accel_z:.3f}") + exit() + else: + # Push all 11 channels: 8 EEG + 3 accelerometer + outlet.push_sample(channel_data) + # Push accelerometer data once per packet + accel_outlet.push_sample([accel_x, accel_y, accel_z]) + + # Optional: Print data periodically for debugging + print(f"EEG: {channel_data[:8]}, Accel: [{accel_x:.3f}, {accel_y:.3f}, {accel_z:.3f}]") diff --git a/test/requirements.txt b/test/requirements.txt new file mode 100644 index 0000000..8d1c279 --- /dev/null +++ b/test/requirements.txt @@ -0,0 +1,2 @@ +websocket-client +pylsl