FIFO mode LSM6DS3 on seeeduino XIAO

[giuseppe_sacca]

Hi everyone, I will explain in the most detailed way my problem. I’m trying to use the LSM6DS3 IMU in FIFO mode, with a sampling frequency of 800 Hz in order to acquire and then send a buffer of data through BLE. I’m using so the I2C protocol, and setting the fifo mode to 6, i should be able to collect the data in continuous mode, working as in a circular buffer. Now, when i press a button through BLE and the get buffer characteristics is written i trigger the begin of the buffer, and the consequent reading of the first 4000 values before disabling the buffer writing, and then I read the remaining values to empty the buffer.

The problem that i noticed is that when i try to read the values of the buffer while is still writing the values are completely wrong/messed up, and I don’t understand how, since the writing should be a lot faster than reading, so there shouldn’t be any problem with the reading of data. If is useful for you I leave the code used here :

#define XIAO_MOUTHGUARD
////////////////////////////////////// Includes /////////////////////////////////////////

#include <Wire.h>
#include <LSM6DS3.h>
#include <ArduinoBLE.h>  // to provide standard Bluetooth services

////////////////////////////////////// Initialize global stuff /////////////////////////////////////////

bool getBufferFlag = false;
bool was_connected = false;
bool fifoFull = false;
bool fifoStarted = false;
////////////////////////////////////// Initialize sensor stuff /////////////////////////////////////////
LSM6DS3 myIMU(I2C_MODE, 0x6A);  // I2C device address 0x6A
int16_t ax, ay, az;
int16_t gx, gy, gz;

////////////////////////////////////// Initialize the BLE stuff /////////////////////////////////////////
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, 54 * 2);

////////////////////////////////////// Initialize useful functions /////////////////////////////////////////
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 rgbLedYellow() {
  digitalWrite(LEDG, LOW);   // Turn on green (common anode, so low = on)
  digitalWrite(LEDB, HIGH);  // Turn off blue (common anode, so high = off)
  digitalWrite(LEDR, HIGH);  // Turn off red (common anode, so high = off)
}

void turnOFFfifo() {
  myIMU.writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL5, 0b000);  // This code shut off the FIFO ...WORKING...
}

void turnOFFaccel() {
  myIMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, 0);
}

void turnOFFgyro() {
  myIMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL2_G, 0);
}

void disableFifo() {
  uint8_t temp;
  myIMU.readRegister(&temp, LSM6DS3_ACC_GYRO_FIFO_CTRL5);
  myIMU.writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL5, temp & 0x7);
}


void setup() {
  rgbLedRed();  // Power off led set to red

  Serial.begin(115200);
  while (!Serial)
    ;
  ////////////////////////////////////// IMU settings FIFO /////////////////////////////////////////
  myIMU.settings.accelFifoEnabled = 1;
  myIMU.settings.gyroFifoEnabled = 1;
  myIMU.settings.gyroSampleRate = 1660;   // 13, 26, 52, 104, 208, 416, 833, 1660
  myIMU.settings.accelSampleRate = 1660;  // 13, 26, 52, 104, 208, 416, 833, 1660, 3330, 6660, 13330
  myIMU.settings.fifoSampleRate = 800;   // 10, 25, 50, 100, 200, 400, 800, 1600, 3300, 6600
  myIMU.settings.fifoModeWord = 6;       // FIFO mode continuous
  myIMU.settings.fifoThreshold = 2046;   //Can be 0 to 4096 (16 bit bytes), this sets the threshold for the watermark

  if (myIMU.begin() != 0) {
    Serial.println("Device error");
    while (1)
      ;
  }

  ////////////////////////////////////// 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();

  ////////////////////////////////////// Set the clock and initialize the FIFO /////////////////////////////////////////
  Wire1.setClock(400000UL);  // I2C1 SCL=400kHz
}



void loop() {
  ////////////////////////////////////// CONNECTED /////////////////////////////////////////
  if (BLE.connected()) {
    if (!was_connected) {
      was_connected = true;
      rgbLedBlue();
    }
  }
  // If is requested to get the buffer
  if (getBuffer.written()) {
    getBufferFlag = true;
    Serial.println("I'm sending the buffer");
    readFifoAndSend();
  }

  ////////////////////////////////////// DISCONNECTED /////////////////////////////////////////
  if (!BLE.connected() && was_connected) {
    was_connected = false;
    rgbLedRed();
  }
}


////////////////////////////////////// FUNCTIONS /////////////////////////////////////////

void readFifoAndSend() {
  const int maxSamples = 8000;  
  int16_t dataBuffer[maxSamples];
  int counter = 0;

  myIMU.fifoBegin();

  for (counter = 0; counter <4000; counter++){  
    gx = myIMU.fifoRead();
    gy = myIMU.fifoRead();
    gz = myIMU.fifoRead();
    ax = myIMU.fifoRead();
    ay = myIMU.fifoRead();
    az = myIMU.fifoRead();

    // Process and store data
    dataBuffer[counter++] = gx;
    dataBuffer[counter++] = gy;
    dataBuffer[counter++] = gz;
    dataBuffer[counter++] = ax;
    dataBuffer[counter++] = ay;
    dataBuffer[counter++] = az;

    Serial.print("Gyro (deg/s): ");
    Serial.print(gx);
    Serial.print(", ");
    Serial.print(gy);
    Serial.print(", ");
    Serial.print(gz);
    Serial.print(" | ");

    Serial.print("Accel (g): ");
    Serial.print(ax);
    Serial.print(", ");
    Serial.print(ay);
    Serial.print(", ");
    Serial.println(az);
  }
  disableFifo();


  while ((myIMU.fifoGetStatus() & 0x1000) == 0) {  // Means if still contains data
    gx = myIMU.fifoRead();
    gy = myIMU.fifoRead();
    gz = myIMU.fifoRead();
    ax = myIMU.fifoRead();
    ay = myIMU.fifoRead();
    az = myIMU.fifoRead();

    // Process and store data
    dataBuffer[counter++] = gx;
    dataBuffer[counter++] = gy;
    dataBuffer[counter++] = gz;
    dataBuffer[counter++] = ax;
    dataBuffer[counter++] = ay;
    dataBuffer[counter++] = az;

    // Print the gyroscope and accelerometer data
    Serial.print("Gyro (deg/s): ");
    Serial.print(gx);
    Serial.print(", ");
    Serial.print(gy);
    Serial.print(", ");
    Serial.print(gz);
    Serial.print(" | ");

    Serial.print("Accel (g): ");
    Serial.print(ax);
    Serial.print(", ");
    Serial.print(ay);
    Serial.print(", ");
    Serial.println(az);
  }
  Serial.print("Number of samples: ");
  Serial.println(counter);

  // int bufferSize = 50;                               // Set the size of each chunk to 50
  // int chunks = sizeof(dataBuffer) / 2 / bufferSize;  // Calculate the number of chunks

  // for (int chunk = 0; chunk < chunks; chunk++) {
  //   int startIndex = chunk * bufferSize;     // Calculate the start index for the current chunk
  //   int endIndex = startIndex + bufferSize;  // Calculate the end index for the current chunk
  //   // Send the current chunk of data using the sendChunk function
  //   sendChunk(dataBuffer, startIndex, bufferSize);
  //   delay(70);  // pause of 5ms for matlab to have time to receive data
  // }
  // getBufferFlag = false;  // Reset the flag since all data has been sent
  // Serial.println("Data sent over BLE.");
}

////////////////////////////////////// Initialize other functions /////////////////////////////////////////
void sendChunk(int16_t* dataArray, int startIndex, int chunkSize) {
  int16_t chunk[chunkSize];
  for (int i = 0; i < chunkSize; i++) {
    chunk[i] = dataArray[startIndex + i];
  }
  sendBuffer.writeValue(chunk, sizeof(chunk));
}```

[PJ_Glasso]

Hi there, giuseppe_sacca,
Are you getting the interrupt from fifo? How often?
Make sure you have some decimation selected. It’s cannot be ZERO
also take a look here , at the fifo section. I think the threshold is to high and is overwriting data,
set it smaller and try again.

sparkfun/SparkFun_LSM6DS3_Arduino_Library/blob/master/src/SparkFunLSM6DS3.cpp

Also which BSP are you using 2.9.2 ?, also try to roll that back to the previous 2.9.0 or .1 and test again. You getting RAW data no filters ?
HTH
GL PJ
I’m getting this in Nrf connect:

error in the offset?
I would review your buffer logic and reduce the fifo size threshold.
HTH