Hi there, I’ll try to explain the problem as clear as possible. I’ve this Seeduino XIAO ble with embedded accelerometer LSM6DS3. I’m trying to sample the data at high frequency (at least 1000Hz) and send them through ble with a circular buffer. The problem is that the buffer is 4096 samples, and I have to send all 6 the IMU values Ax,Ay,Az,Wx,Wy,Wz, which means i have to send 4096x12 values (6 channels of 2 bytes each). The problem is that the ble can send only 512 bytes maximum. So i’m trying to break the pieces of the buffer to send in pieces of 32x12 (384 bytes), but i’m stuck with the logic through which I can send the buffer in pieces of 384 bytes, can you help me ? Here’s the code
#define XIAO_MOUTHGUARD
//==============================================================================
// Includes
//==============================================================================
#include <ArduinoBLE.h> // to provide standard Bluetooth services
#include <Wire.h> // for I2C
#include "LSM6DS3.h"
//==============================================================================
// Global Stuff
//==============================================================================
const int SAMPLE_INTERVAL_MS = 1000; // 1 sample per second
bool was_connected = false;
// Sensor stuff
LSM6DS3 myIMU(I2C_MODE, 0x6A); // I2C device address 0x6A
int16_t ax, ay, az, wx, wy, wz;
bool getBufferFlag = false;
// Circular buffer to store 4096 samples of sensor data
const int SAMPLE_BUFFER = 1;
//==============================================================================
// Buffer Class, functions get_all, write, read, count
//==============================================================================
template <typename T>
class CircularBuffer {
public:
CircularBuffer(int size) : size(size), buffer(new T[size]), head(0), tail(0), is_full(false) {}
~CircularBuffer() {
delete[] buffer;
}
void get_all(T* data_array) {
int elementCount = count(); // Renamed the variable to 'elementCount'
if (elementCount == 0) {
// Handle empty buffer as needed
// For example, you can return early, throw an exception, or do nothing
Serial.println("Buffer is Empty");
return;
}
int current_head = head;
for (int i = 0; i < elementCount; i++) {
data_array[i] = buffer[current_head];
current_head = (current_head + 1) % size;
}
}
void write(T data) {
buffer[tail] = data;
tail = (tail + 1) % size;
if (tail == head) {
head = (head + 1) % size; // Overwrite from the last sample
is_full = true;
}
}
T read() {
if (is_empty()) {
// Handle empty buffer as needed
// For example, return a default value or throw an exception
Serial.println("Buffer is Empty");
}
T data = buffer[head];
head = (head + 1) % size;
is_full = false;
return data;
}
bool is_empty() const {
return !is_full && head == tail;
}
int count() const {
if (is_full) {
return size;
} else if (tail >= head) {
return tail - head;
} else {
return size - head + tail;
}
}
bool is_full;
private:
int size;
T* buffer;
int head;
int tail;
};
// Define the buffers for each acceleration
const int BUFFER_SIZE = 4096;
CircularBuffer<int16_t> axBuffer(BUFFER_SIZE);
CircularBuffer<int16_t> ayBuffer(BUFFER_SIZE);
CircularBuffer<int16_t> azBuffer(BUFFER_SIZE);
CircularBuffer<int16_t> wxBuffer(BUFFER_SIZE);
CircularBuffer<int16_t> wyBuffer(BUFFER_SIZE);
CircularBuffer<int16_t> wzBuffer(BUFFER_SIZE);
// Bluetooth low energy stuff
int messageSize = 32*12;
BLEService HRigService("39f17f86-d750-4b1b-9cb6-50f2c2647c5f");
BLECharacteristic sendStream("39f17f86-0001-4b1b-9cb6-50f2c2647c5f", BLERead | BLENotify,12);
BLECharacteristic getBuffer("39f17f86-0002-4b1b-9cb6-50f2c2647c5f", BLERead | BLEWrite, true);
BLECharacteristic sendBuffer("39f17f86-0003-4b1b-9cb6-50f2c2647c5f", BLERead | BLENotify, messageSize);
void rgbLedRed() {
digitalWrite(LEDG, HIGH); // common anode, so high = off
digitalWrite(LEDB, HIGH); // common anode, so high = off
digitalWrite(LEDR, LOW); // common anode, so high = off
}
void rgbLedBlue() {
digitalWrite(LEDG, HIGH); // common anode, so high = off
digitalWrite(LEDB, LOW); // common anode, so high = off
digitalWrite(LEDR, HIGH); // common anode, so high = off
}
void setup() {
rgbLedRed();
Serial.begin(9600);
// Initialize the IMU sensor
if (myIMU.begin() != 0) {
Serial.println("IMU initialized");
}
else {
Serial.println("Failed initializing IMU");
}
// Initialize the BLE module
if (!BLE.begin()) {
Serial.println("BLE failed to initialize");
while (1)
; // stop here!
}
BLE.setDeviceName("XIAO-BLE-Sense");
BLE.setLocalName("XIAO-BLE-Sense");
BLE.setAdvertisedService(HRigService);
HRigService.addCharacteristic(sendStream);
HRigService.addCharacteristic(getBuffer);
HRigService.addCharacteristic(sendBuffer);
BLE.addService(HRigService);
BLE.setConnectionInterval(6, 12); // 7.5ms to 15 ms
BLE.advertise();
}
void loop() {
static uint32_t last_sample = 0;
static uint32_t last_buffer_sample = 0; // New time variable for buffer stacking
if (BLE.connected()) {
if (!was_connected) {
was_connected = true;
rgbLedBlue();
}
// Send 1 sample per second on the GUI in Matlab
if (millis() - last_sample >= SAMPLE_INTERVAL_MS) {
last_sample = millis();
sendSensorData();
}
// Stack the buffer data one by another
if (millis() - last_buffer_sample >= SAMPLE_BUFFER) {
last_buffer_sample = millis();
acquireBufferData();
}
}
if (getBuffer.written()) {
getBufferFlag = true;
Serial.println("I'm sending the buffer");
}
if (!BLE.connected() && was_connected) {
was_connected = false;
rgbLedRed();
}
}
void sendSensorData() {
// Your code to read sensor data
int16_t data[6]; // Create an array to hold the 1x6 data
// Populate the data array with your sensor data
data[0] = myIMU.readFloatAccelX();
data[1] = myIMU.readFloatAccelY();
data[2] = myIMU.readFloatAccelZ();
data[3] = myIMU.readFloatGyroX();
data[4] = myIMU.readFloatGyroY();
data[5] = myIMU.readFloatGyroZ();
// Send the data
sendStream.writeValue(data, sizeof(data));
}
void acquireBufferData() {
int16_t axValue = myIMU.readFloatAccelX();
int16_t ayValue = myIMU.readFloatAccelY();
int16_t azValue = myIMU.readFloatAccelZ();
int16_t wxValue = myIMU.readFloatGyroX();
int16_t wyValue = myIMU.readFloatGyroY();
int16_t wzValue = myIMU.readFloatGyroZ();
axBuffer.write(axValue);
ayBuffer.write(ayValue);
azBuffer.write(azValue);
wxBuffer.write(wxValue);
wyBuffer.write(wyValue);
wzBuffer.write(wzValue);
// Debug statement to see the size of the buffer in that moment
Serial.println(axBuffer.count());
if (getBufferFlag) {
int16_t dataBuffer[6 * BUFFER_SIZE]; // Create a larger array to hold all the data
for (int i = 0; i < BUFFER_SIZE; i++) {
// Use the get_all function to retrieve values from axBuffer
axBuffer.get_all(dataBuffer + i * 6);
// Append the values from ayBuffer, azBuffer, wxBuffer, wyBuffer, and wzBuffer
ayBuffer.get_all(dataBuffer + i * 6 + 1);
azBuffer.get_all(dataBuffer + i * 6 + 2);
wxBuffer.get_all(dataBuffer + i * 6 + 3);
wyBuffer.get_all(dataBuffer + i * 6 + 4);
wzBuffer.get_all(dataBuffer + i * 6 + 5);
}
int dataSize = sizeof(dataBuffer);
Serial.print("Data size sent over BLE: ");
Serial.println(dataSize);
sendBuffer.writeValue(dataBuffer, messageSize);
getBufferFlag = false; // Reset the flag since data has been sent
Serial.println("Data sent over BLE.");
}
}