Added a new branch for Accelerometer.

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

src/main.cpp

@@ -29,6 +29,9 @@
 #include <WiFiManager.h>
 #include <ESPmDNS.h>
 #include <Adafruit_NeoPixel.h>
+#include <Adafruit_MPU6050.h>
+#include <Adafruit_Sensor.h>
+#include <Wire.h>
 
 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);

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}]")

test/requirements.txt

@@ -0,0 +1,2 @@
+websocket-client
+pylsl