IMU rate is too low

[IArchi]

Hello,

I’ve bought some Xiao NRF52840 sense boards and trying to read IMU as fast as possible.
In the following piece of code, I can barely reach 700 Hz while I need 1kHz to perform a FFT on my computer.
How to speed it up ?
If I enable BLE, the rate drops to 70Hz

#include <LSM6DS3.h>
#include <Wire.h>

#define SAMPLING_RATE                 1000   //Hz

// Declare IMU devices
LSM6DS3 myIMU(I2C_MODE, 0x6A);
float ax, ay, az;
float gx, gy, gz;
uint8_t readData = 0;
int numSamples = 0;
int samplesRead = numSamples;

void setup() {
  Serial.begin(9600);
  unsigned long startTime = millis();
  while (!Serial && millis() - startTime < 1000);
  
  // Toggle LEDs
  pinMode(LED_RED, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);
  pinMode(LED_BLUE, OUTPUT);
  digitalWrite(LED_RED, HIGH);
  digitalWrite(LED_BLUE, HIGH);
  digitalWrite(LED_GREEN, LOW);

  // Initialize IMU
  myIMU.settings.gyroEnabled = 0;
  if (myIMU.begin() != 0) {
    Serial.println("[FAILURE] Failed to start IMU.");
    while (1);
  }
  Wire1.setClock(400000UL);
  myIMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL2_G, 0x8C);     //1.66kHz 2000dps
  myIMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, 0x8A);    //1.66kHz 4G
  myIMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL7_G, 0x00);
  myIMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL8_XL, 0x09);

  Serial.println("[INFO] Ready.");
  Serial.println("aX,aY,aZ,gX,gY,gZ");
}

void loop() {
  // Collect samples
  numSamples = SAMPLING_RATE * 5;

  unsigned long startTime = micros();
  unsigned long elapsedTime;

  // Wait until IMU is ready
  unsigned int timestamp = micros();
  do {
    myIMU.readRegister(&readData, LSM6DS3_ACC_GYRO_STATUS_REG); // 0,0,0,0,0,TDA,GDA,XLDA 
  } while ((readData & 0x07) != 0x07);
  Serial.println("Ready to read samples");

  // Read samples
  uint8_t dataBuff[12];
  unsigned long previousMicros = 0;
  unsigned long delayTime = 1000000 / SAMPLING_RATE;
  String data;
  while (samplesRead < numSamples) {
    unsigned long currentMicros = micros();
    if (currentMicros - previousMicros < delayTime) continue;
    previousMicros = currentMicros;

    // Read raw data
    myIMU.readRegisterRegion(dataBuff, LSM6DS3_ACC_GYRO_OUTX_L_G, 12);
    //gx = myIMU.calcGyro((int16_t)dataBuff[0] | int16_t(dataBuff[1] << 8));
    //gy = myIMU.calcGyro((int16_t)dataBuff[2] | int16_t(dataBuff[3] << 8));
    //gz = myIMU.calcGyro((int16_t)dataBuff[4] | int16_t(dataBuff[5] << 8));
    ax = myIMU.calcAccel((int16_t)dataBuff[6] | int16_t(dataBuff[7] << 8));
    ay = myIMU.calcAccel((int16_t)dataBuff[8] | int16_t(dataBuff[9] << 8));
    az = myIMU.calcAccel((int16_t)dataBuff[10] | int16_t(dataBuff[11] << 8));

    // Format data
    data = String(samplesRead)+","+ String(ax, 3)+","+String(ay, 3)+","+String(az, 3);
    //data = data + ","+ String(_calcGyro(gX), 3)+","+String(_calcGyro(gY), 3)+","+String(_calcGyro(gZ), 3);
    //Serial.println(data);
    samplesRead++;
  }

  elapsedTime = micros() - startTime;
  float frequency = (float)samplesRead / (elapsedTime / 1000000.0);
  Serial.print("Frequency : ");
  Serial.print(frequency);
  Serial.println(" Hz");

  // Reset counter
  samplesRead = 0;
}

[PJ_Glasso]

Hi there,

Yes I too ran into this issue some time ago, several have and there are posts about how to handle it. The most Obvious is “USE THE FIFO”

the Assistant breaks it down this way…
To achieve higher sampling rates for reading IMU data with the LSM6DS3 on the Xiao nRF52840 Sense, you can focus on optimizing the communication and processing of data. Here are a few key strategies to improve your sampling rate:

1. Utilize Direct FIFO Reading

The LSM6DS3 has a built-in FIFO (First-In, First-Out) buffer that allows for continuous sampling without waiting for data availability flags. By configuring the FIFO and reading data in batches, you can significantly reduce the overhead of individual read operations.

2. Optimize I2C Speed

You’ve already set the I2C clock to 400 kHz. If the hardware supports it, consider increasing the clock speed to 1 MHz (Fast Mode Plus). This reduces the time spent on I2C transactions.

Wire1.setClock(1000000UL); // Set I2C speed to 1 MHz

3. Configure the IMU for Maximum Output Data Rate

The LSM6DS3 supports an accelerometer output data rate (ODR) of up to 1.66 kHz. To utilize the highest rate:

• Set CTRL1_XL to 0x80 (1.66 kHz).
• Disable filters or unnecessary processing in the IMU.

4. Reduce String Formatting Overhead

String operations in Arduino are relatively slow. Instead of building strings in real-time, store raw values in a buffer and process them later.

5. Minimize Interruptions

Disable unnecessary interrupts and avoid using Serial.print inside the high-frequency sampling loop. Use a large buffer to store samples and print data after sampling is complete.

6. Use Interrupts for Data Availability

Instead of polling the status register (STATUS_REG), use the LSM6DS3’s data-ready interrupt pin. This reduces CPU time spent on checking for new data.

Optimized Code Example:

Below is a modified version of your code that incorporates these optimizations:

#include <Arduino.h>
#include <LSM6DS3.h>
#include <Wire.h>

#define SAMPLING_RATE 1660 // Maximum supported by LSM6DS3
#define BUFFER_SIZE 1024   // Adjust buffer size as needed
//#define IMU_INT_PIN 2      // Connect INT1 or INT2 to this pin

LSM6DS3 myIMU(I2C_MODE, 0x6A);
//#define int2Pin PIN_LSM6DS3TR_C_INT1
#define int2Pin P0_11
volatile bool dataReady = false;
float ax, ay, az;
int16_t accelData[BUFFER_SIZE][3];
int sampleIndex = 0;

// Interrupt handler for IMU
void imuInterruptHandler() {
  dataReady = true;
}

void setup() {
  Serial.begin(9600);
  while (!Serial);

  pinMode(int2Pin, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(int2Pin), imuInterruptHandler, RISING);

  // Initialize IMU
  myIMU.settings.accelEnabled = 1;
  myIMU.settings.accelRange = 16; // Set range to ±16g for better resolution
  myIMU.settings.accelSampleRate = 1660; // 1.66 kHz
  if (myIMU.begin() != 0) {
    Serial.println("Failed to initialize IMU!");
    while (1);
  }

  // Configure IMU interrupts
  myIMU.writeRegister(LSM6DS3_ACC_GYRO_INT1_CTRL, 0x01); // Enable data-ready interrupt
  myIMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, 0x80);  // 1.66 kHz, ±16g

  Serial.println("IMU initialized. Starting sampling...");
}

void loop() {
  if (dataReady) {
    dataReady = false;

    uint8_t dataBuff[6];
    myIMU.readRegisterRegion(dataBuff, LSM6DS3_ACC_GYRO_OUTX_L_XL, 6);

    accelData[sampleIndex][0] = (int16_t)(dataBuff[0] | (dataBuff[1] << 8));
    accelData[sampleIndex][1] = (int16_t)(dataBuff[2] | (dataBuff[3] << 8));
    accelData[sampleIndex][2] = (int16_t)(dataBuff[4] | (dataBuff[5] << 8));

    sampleIndex++;
    if (sampleIndex >= BUFFER_SIZE) {
      printData();
      sampleIndex = 0;
    }
  }
}

void printData() {
  for (int i = 0; i < BUFFER_SIZE; i++) {
    Serial.print(accelData[i][0]);
    Serial.print(",");
    Serial.print(accelData[i][1]);
    Serial.print(",");
    Serial.println(accelData[i][2]);
  }
}

Key Features:

1. Interrupt-Driven Sampling:

• Data-ready interrupts eliminate polling overhead.

2. FIFO Buffering:

• Use the BUFFER_SIZE to temporarily store samples and print them in bulk.

3. Minimal Processing in the Loop:

• Data is read directly and stored in a buffer without additional computation.

4. I2C at Maximum Speed:

• The Wire1.setClock(1000000UL); ensures fast communication.

With these optimizations, you should be able to get closer to the IMU’s maximum output rate of 1.66 kHz. Let US know how it does.

HTH
GL PJ

[msfujino]

I measured the I2C read time with a digital scope.

With the SCL clock set to the maximum value of 400 kHz, it takes 600 uS to read 12 bytes of data from consecutive registers using the readRegisterRegion() function.
Even with interrupts and FIFOs, it would be difficult to achieve a sample rate of 1.666 kHz.

[PJ_Glasso]

Hi there,

Hmm, I didn’t measure it , sorry but my estimate was:

• I2C Speed: At 400 kHz, each clock cycle is 2.5 µs.
• Data Transfer: Reading 12 bytes (let’s assume 8-bit data per byte) will involve 96 bits to transfer, which at 400 kHz would take roughly 240 µs (96 bits / 400 kHz = 240 µs).
  I know there is some overhead but that seems slow to me?

Correct, this assumes I2C the clock is Higher too. and the buffer is HUGE…
Plus I’m not certain the read register region ()function is the optimum method.
I always felt like the I2C wasn’t the fastest way too, SPI would be way better IMO.
can you triple the buffer and measure again?
TIA
GL PJ

SO I thought the Nrf52840 Supported the Fast Mode Plus?

Wire.setClock(1000000);  // Set I2C to 1 MHz (Fast Mode Plus)

It may be the IMU that doesn’t , SMH?

[msfujino]

The maximum clock frequency in IMU fast mode is 400 kHz.
Reading LSM6DS3.cpp:165, readRegisterRegion()
is the same as the method described in “6.3.1 I2C operation” in the datasheet.
I can’t analyze further how much overhead there is because the IMU is under the can, so I can’t access the I2C pins.

When compiled with mbed, it is 600uS, but when compiled with non-mbed, it is 400uS. This may be usable.