SD Logger has Occasional High Latency

Power_Broker · Apr 3, 2023

I have a T4.1 and I'm trying to do ~100Hz data logging to an SD card via the built-in slot. Even when running the T4.1 overclocked, I have inconsistent logging rates. Most of the logging is at 100Hz, but sometimes I get lags of up to 0.125 seconds! These lags are not evenly spaced through time. Even more odd, I also sometimes have a log rate of over 100Hz (not sure how that's even possible due to how I have my code setup).

The big question is: what is causing these timing issues (especially the lags) and what can I do to mitigate them? I understand that writing to a larger buffer (i.e. 512 bytes or so) might help, but I'm not writing much data per loop and 100Hz isn't that high of a sample rate. I don't think the SD interface should be struggling so much at 100Hz (IMO).

Below is my code:

Code:

#include "Adafruit_LSM6DS33.h"
#include "Adafruit_LIS3MDL.h"
#include "SdFat.h"
#include "navduino.h"
#include "attitude.h"
#include "FireTimer.h"




#ifdef SDCARD_SS_PIN
const uint8_t SD_CS_PIN = SDCARD_SS_PIN;
#endif // SDCARD_SS_PIN
#define SPI_CLOCK SD_SCK_MHZ(50)
#define SD_CONFIG SdioConfig(FIFO_SDIO)




Adafruit_LSM6DS33 lsm6ds33;
sensors_event_t accel;
sensors_event_t gyro;
sensors_event_t temp;
Adafruit_LIS3MDL lis3mdl;
sensors_event_t mag;
compFilt ahrs;

FireTimer msTimer;

SdFs   sd;
FsFile logFile;




char field[20] = { '\0' };
char buff[200] = { '\0' };

float gyro_x_bias = -0.045967611;
float gyro_y_bias = 0.092546083;
float gyro_z_bias = 0.0813977905;




void setup()
{
    Serial.begin(115200);

    if (!lsm6ds33.begin_I2C(0b1101011))
    {
        Serial.println("Failed to find LSM6DS33 chip");
        while (1);
    }

    lsm6ds33.setAccelRange(LSM6DS_ACCEL_RANGE_4_G);
    lsm6ds33.setGyroRange(LSM6DS_GYRO_RANGE_500_DPS);
    lsm6ds33.setAccelDataRate(LSM6DS_RATE_104_HZ);
    lsm6ds33.setGyroDataRate(LSM6DS_RATE_104_HZ);

    if (! lis3mdl.begin_I2C(0b0011110))
    {
        Serial.println("Failed to find LIS3MDL chip");
        while (1);
    }

    lis3mdl.setPerformanceMode(LIS3MDL_MEDIUMMODE);
    lis3mdl.setOperationMode(LIS3MDL_CONTINUOUSMODE);
    lis3mdl.setDataRate(LIS3MDL_DATARATE_155_HZ);
    lis3mdl.setRange(LIS3MDL_RANGE_4_GAUSS);

    msTimer.begin(10);

    if (!sd.begin(SD_CONFIG))
    {
        Serial.println("SD initialization failed");
        while (1);
    }

    logFile.open("cal_data.csv", FILE_WRITE);

    sprintf(buff,
        "%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s\n",
        "epoch_us",
        "ax_g",
        "ay_g",
        "az_g",
        "gx_rps",
        "gy_rps",
        "gz_rps",
        "mx_nT",
        "my_nT",
        "mz_nT",
        "roll_deg",
        "pitch_deg",
        "yaw_deg");

    logFile.write(buff, strlen(buff));
    logFile.close();
}




void loop()
{
    if(msTimer.fire())
    {
        long ts = micros();
        lsm6ds33.getEvent(&accel, &gyro, &temp);
        lis3mdl.getEvent(&mag);

        float ax = accel.acceleration.x;
        float ay = accel.acceleration.y;
        float az = accel.acceleration.z;

        float gx = (gyro.gyro.x + gyro_x_bias) * 180 / M_PI;
        float gy = (gyro.gyro.y + gyro_y_bias) * 180 / M_PI;
        float gz = (gyro.gyro.z + gyro_z_bias) * 180 / M_PI;

        float mx = mag.magnetic.x;
        float my = mag.magnetic.y;
        float mz = mag.magnetic.z;

        ahrs.updateIMU(ax, ay, az, gx, gy, gz, -1, 0, 0, 0);

        gx *= (M_PI / 180);
        gy *= (M_PI / 180);
        gz *= (M_PI / 180);

        char ts_str[20];
        char ax_str[20];
        char ay_str[20];
        char az_str[20];
        char gx_str[20];
        char gy_str[20];
        char gz_str[20];
        char mx_str[20];
        char my_str[20];
        char mz_str[20];
        char roll_str[20];
        char pitch_str[20];
        char yaw_str[20];

        dtostrf(ts, 1, 9, ts_str);
        dtostrf(ax, 1, 9, ax_str);
        dtostrf(ay, 1, 9, ay_str);
        dtostrf(az, 1, 9, az_str);
        dtostrf(gx, 1, 9, gx_str);
        dtostrf(gy, 1, 9, gy_str);
        dtostrf(gz, 1, 9, gz_str);
        dtostrf(mx, 1, 9, mx_str);
        dtostrf(my, 1, 9, my_str);
        dtostrf(mz, 1, 9, mz_str);
        dtostrf(ahrs.getRoll(), 1, 9, roll_str);
        dtostrf(ahrs.getPitch(), 1, 9, pitch_str);
        dtostrf(ahrs.getYaw(), 1, 9, yaw_str);

        sprintf(buff,
                "%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s\n",
                ts_str,
                ax_str,
                ay_str,
                az_str,
                gx_str,
                gy_str,
                gz_str,
                mx_str,
                my_str,
                mz_str,
                roll_str,
                pitch_str,
                yaw_str);

        logFile.open("cal_data.csv", FILE_WRITE);
        logFile.write(buff, strlen(buff));
        logFile.close();
    }
}

Also, here is the plot showing loop latency:

WMXZ · Apr 3, 2023

these high latencies are quite normal and within the specs and a couple of times discussed on this forum.
They are due to microSD card internal activities (bad block handling, wear levelling). In case you do not know, every microSD card has an internal CPU that is responsible for such activities, otherwise it would be be the user making sure that bad blocks and wear levelling is handled properly.

These latencies have nothing to do with the inefficient disk handling (100 times per second opening, closing files) which is only waste of energy, but a teensy could handle. To understand this you should know that microSD cards are ALWAYS accessed n*512 bytes at a time.

Way to handle this is to store data into a FIFO and writing from FIFO to disk.

AndyA · Apr 4, 2023

To expand on WMXZs reply, the key is to break the link between logging samples and writing them to the SD card.
If you maintain that link then you need the write process to always finish in under 10ms. If you break that link then it will work as long as the SD card writes take less than 10ms on average.
As you've discovered the write times can be very variable, worst case write times can often be over 100 times longer than average write times.

The easiest way to break this link is to use a timer to collect the data and store it in a buffer. The main loop can then write from that buffer to the card as fast as possible.
Something along the lines of this should do the trick...

Code:

// sample rate in Hz
const int sampleRate = 100;

// Max samples to buffer - should be at least 200ms of data. Only upper limit is available memory
const int bufferLength = 200;

// structure to hold a single sample
struct sampleData_s {
  float ax;
  float ay;
  float az;
  ...
}

// array to create buffer
struct sampleData_s DataBuffer[bufferLength];

// read/write counters
volatile int DataButterWriteIndex = 0;
int DataButterReadIndex = 0;

// timer to when to sample
IntervalTimer DataLogTimer;

// sample function. Called in an interrupt context so keep it as minimal as possible.
static void OnNewSample();


startup() {

  .... other setup code

  // calculate update time in us
  int SampleInterval = 1000 *1000 / sampleRate;
  // start timer
  DataLogTimer.begin(OnNewSample, SampleInterval);
}

void OnNewSample() {
  // add new sample to buffer.
  DataBuffer[DataButterWriteIndex] = read new sample;
  // update index
  DataButterWriteIndex++;
  if (DataButterWriteIndex == bufferLength)
    DataButterWriteIndex  = 0;
}

loop () {
  // if data waiting
  if (DataButterReadIndex != DataButterWriteIndex) {
     // write it
     write DataBuffer[DataButterReadIndex] to SD card
     // increase buffer read pointer
     DataButterReadIndex++;
     if (DataButterReadIndex== bufferLength)
        DataButterReadIndex= 0;
  }
}

As indicated ideally the write function should write in blocks of 512 bytes. This would involve adding a second level of buffering at the write stage.
Alternatively if you keep the file open rather than closing and re-opening it every write then the file system will do this buffering for you.
The only reason to constantly close and re-open the file is if you expect power or the card to be removed unexpectedly. If that is the case then implement a counter and close and re-open the file every few hundred samples. That will limit your data loss to the last second or two while allowing the file system to do it's buffering most of the time. Ideally check that you have plenty of space in your buffer first before doing this.

sbfreddie · Apr 4, 2023

Power_Broker:
Perhaps an alternative way to accomplish your goal would be to use the technique I used, noted in these two threads:

https://forum.pjrc.com/threads/72300-Teensy-LittleFS-and-SD-file-names

and:

https://forum.pjrc.com/threads/6813...ation-including-MTP-and-MSC/page56#post323598

I used the onboard 8 Megabyte PSRAM to store data for 24 Hrs and at Midnight each night it copies the entire file to the SD Card.
The write to the PSRAM is very fast, I believe its writes about 8 Megabytes per second without all the issues of the SD card internal data manipulation.

Regards,
Ed

Power_Broker · Apr 4, 2023

WMXZ said:
these high latencies are quite normal and within the specs and a couple of times discussed on this forum.
They are due to microSD card internal activities (bad block handling, wear levelling). In case you do not know, every microSD card has an internal CPU that is responsible for such activities, otherwise it would be be the user making sure that bad blocks and wear levelling is handled properly.

Dang, that's annoying/disappointing. Thanks for the info - I didn't know!

Power_Broker · Apr 4, 2023

AndyA said:
The easiest way to break this link is to use a timer to collect the data and store it in a buffer. The main loop can then write from that buffer to the card as fast as possible.

Thanks!

Dumb question: If I use an interrupt (which is what I assume `DataLogTimer` uses) to sample the sensors, I'm guessing I can still get the sensor data at a predictible rate during these occasions of SD latency?

WMXZ · Apr 4, 2023

Power_Broker said:
Thanks!

Dumb question: If I use an interrupt (which is what I assume `DataLogTimer` uses) to sample the sensors, I'm guessing I can still get the sensor data at a predictible rate during these occasions of SD latency?

The standard procedure is to access microSD card within the loop() function and to sample the sensors on interrupt level (say running a timer)
on a T4.1 a sample easily 6 sensors at 96 kHz and write to disk.

Power_Broker · Apr 4, 2023

WMXZ said:
The standard procedure is to access microSD card within the loop() function and to sample the sensors on interrupt level (say running a timer)
on a T4.1 a sample easily 6 sensors at 96 kHz and write to disk.

Ok, that makes sense. Tangentially: is there an easy way to set interrupt priority levels? I'll need to do this for more complex projects

Power_Broker · Apr 4, 2023

Ok, I gave the interrupt thing a shot. Here's the code:

Code:

#include "Adafruit_LSM6DS33.h"
#include "Adafruit_LIS3MDL.h"
#include "SdFat.h"
#include "navduino.h"
#include "attitude.h"
#include "FireTimer.h"




#ifdef SDCARD_SS_PIN
const uint8_t SD_CS_PIN = SDCARD_SS_PIN;
#endif // SDCARD_SS_PIN
#define SPI_CLOCK SD_SCK_MHZ(50)
#define SD_CONFIG SdioConfig(FIFO_SDIO)




Adafruit_LSM6DS33 lsm6ds33;
sensors_event_t accel;
sensors_event_t gyro;
sensors_event_t temp;
Adafruit_LIS3MDL lis3mdl;
sensors_event_t mag;
compFilt ahrs;

IntervalTimer DataLogTimer;

SdFs   sd;
FsFile logFile;




char field[20] = { '\0' };
char buff[200] = { '\0' };

float gyro_x_bias = -0.045967611;
float gyro_y_bias = 0.092546083;
float gyro_z_bias = 0.0813977905;




const int sampleRate     = 100; // Hz
const int SampleInterval = 1000 * 1000 / sampleRate; // us
const int bufferLength   = 200;

struct sampleData_s
{
    int ts;

    float ax;
    float ay;
    float az;

    float gx;
    float gy;
    float gz;

    float mx;
    float my;
    float mz;

    float roll;
    float pitch;
    float yaw;
};

// array to create buffer
struct sampleData_s DataBuffer[bufferLength];

// read/write counters
volatile int DataButterWriteIndex = 0;
int DataButterReadIndex = 0;




void OnNewSample()
{
    sampleData_s currentSample;

    currentSample.ts = micros();
    lsm6ds33.getEvent(&accel, &gyro, &temp);
    lis3mdl.getEvent(&mag);

    currentSample.ax = accel.acceleration.x;
    currentSample.ay = accel.acceleration.y;
    currentSample.az = accel.acceleration.z;

    currentSample.gx = (gyro.gyro.x + gyro_x_bias) * 180 / M_PI;
    currentSample.gy = (gyro.gyro.y + gyro_y_bias) * 180 / M_PI;
    currentSample.gz = (gyro.gyro.z + gyro_z_bias) * 180 / M_PI;

    currentSample.mx = mag.magnetic.x;
    currentSample.my = mag.magnetic.y;
    currentSample.mz = mag.magnetic.z;

    ahrs.updateIMU(currentSample.ax,
                    currentSample.ay,
                    currentSample.az,
                    currentSample.gx,
                    currentSample.gy,
                    currentSample.gz,
                    -1,
                    0,
                    0,
                    0);
    
    currentSample.roll  = ahrs.getRoll();
    currentSample.pitch = ahrs.getPitch();
    currentSample.yaw   = ahrs.getYaw();

    DataBuffer[DataButterWriteIndex] = currentSample;

    DataButterWriteIndex++;

    if (DataButterWriteIndex == bufferLength)
        DataButterWriteIndex = 0;
    
    // Serial.print("Raw:");
    // Serial.print(currentSample.ts, 9);
    // Serial.print(",");
    // Serial.print(currentSample.ax, 9);
    // Serial.print(",");
    // Serial.print(currentSample.ay, 9);
    // Serial.print(",");
    // Serial.print(currentSample.az, 9);
    // Serial.print(",");
    // Serial.print(currentSample.gx, 9);
    // Serial.print(",");
    // Serial.print(currentSample.gy, 9);
    // Serial.print(",");
    // Serial.print(currentSample.gz, 9);
    // Serial.print(",");
    // Serial.print(currentSample.mx, 9);
    // Serial.print(",");
    // Serial.print(currentSample.my, 9);
    // Serial.print(",");
    // Serial.print(currentSample.mz, 9);
    // Serial.print(",");
    Serial.print(currentSample.roll, 9);
    Serial.print(",");
    Serial.print(currentSample.pitch, 9);
    Serial.print(",");
    Serial.print(currentSample.yaw, 9);
    Serial.println();
}




void setup()
{
    Serial.begin(115200);

    if (!lsm6ds33.begin_I2C(0b1101011))
    {
        Serial.println("Failed to find LSM6DS33 chip");
        while (1);
    }

    lsm6ds33.setAccelRange(LSM6DS_ACCEL_RANGE_4_G);
    lsm6ds33.setGyroRange(LSM6DS_GYRO_RANGE_500_DPS);
    lsm6ds33.setAccelDataRate(LSM6DS_RATE_104_HZ);
    lsm6ds33.setGyroDataRate(LSM6DS_RATE_104_HZ);

    if (! lis3mdl.begin_I2C(0b0011110))
    {
        Serial.println("Failed to find LIS3MDL chip");
        while (1);
    }

    lis3mdl.setPerformanceMode(LIS3MDL_MEDIUMMODE);
    lis3mdl.setOperationMode(LIS3MDL_CONTINUOUSMODE);
    lis3mdl.setDataRate(LIS3MDL_DATARATE_155_HZ);
    lis3mdl.setRange(LIS3MDL_RANGE_4_GAUSS);

    if (!sd.begin(SD_CONFIG))
    {
        Serial.println("SD initialization failed");
        while (1);
    }

    logFile.open("cal_data.csv", FILE_WRITE);

    sprintf(buff,
        "%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s\n",
        "epoch_us",
        "ax_g",
        "ay_g",
        "az_g",
        "gx_rps",
        "gy_rps",
        "gz_rps",
        "mx_nT",
        "my_nT",
        "mz_nT",
        "roll_deg",
        "pitch_deg",
        "yaw_deg");

    logFile.write(buff, strlen(buff));
    logFile.close();
    
    DataLogTimer.begin(OnNewSample, SampleInterval);
}




void loop()
{
    // if data waiting
    if (DataButterReadIndex != DataButterWriteIndex)
    {
        char ts_str[20];
        char ax_str[20];
        char ay_str[20];
        char az_str[20];
        char gx_str[20];
        char gy_str[20];
        char gz_str[20];
        char mx_str[20];
        char my_str[20];
        char mz_str[20];
        char roll_str[20];
        char pitch_str[20];
        char yaw_str[20];

        dtostrf(DataBuffer[DataButterReadIndex].ts,    1, 9, ts_str);
        dtostrf(DataBuffer[DataButterReadIndex].ax,    1, 9, ax_str);
        dtostrf(DataBuffer[DataButterReadIndex].ay,    1, 9, ay_str);
        dtostrf(DataBuffer[DataButterReadIndex].az,    1, 9, az_str);
        dtostrf(DataBuffer[DataButterReadIndex].gx,    1, 9, gx_str);
        dtostrf(DataBuffer[DataButterReadIndex].gy,    1, 9, gy_str);
        dtostrf(DataBuffer[DataButterReadIndex].gz,    1, 9, gz_str);
        dtostrf(DataBuffer[DataButterReadIndex].mx,    1, 9, mx_str);
        dtostrf(DataBuffer[DataButterReadIndex].my,    1, 9, my_str);
        dtostrf(DataBuffer[DataButterReadIndex].mz,    1, 9, mz_str);
        dtostrf(DataBuffer[DataButterReadIndex].roll,  1, 9, roll_str);
        dtostrf(DataBuffer[DataButterReadIndex].pitch, 1, 9, pitch_str);
        dtostrf(DataBuffer[DataButterReadIndex].yaw,   1, 9, yaw_str);

        sprintf(buff,
                "%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s\n",
                ts_str,
                ax_str,
                ay_str,
                az_str,
                gx_str,
                gy_str,
                gz_str,
                mx_str,
                my_str,
                mz_str,
                roll_str,
                pitch_str,
                yaw_str);

        logFile.open("cal_data.csv", FILE_WRITE);
        logFile.write(buff, strlen(buff));
        logFile.close();

        // increase buffer read pointer
        DataButterReadIndex++;

        if (DataButterReadIndex== bufferLength)
            DataButterReadIndex= 0;
    }
}

It seems the log rate is much more stable, however, I'm still getting some 0.13s lag spikes. Am I doing something wrong?

defragster · Apr 4, 2023

Those larger lag spikes are normal as noted in p#2 and #3.

The SD drive can work at typical speeds for some time then the onboard processor 'does some housekeeping stuff' that keeps it busy for some longer time before it completes.

Doing writes in multiples of 512 bytes perhaps to 4KB or larger allows SD to consume and work with the data most efficiently. But it needs to be buffered expecting long delays to happen to loop[() code and data collection as done used with interrupts.

As for priority see: myTimer.priority(number);

Power_Broker · Apr 4, 2023

defragster said:
Those larger lag spikes are normal as noted in p#2 and #3.

How is that possible if I'm doing the sensor collection using interrupts? The timestamps represent exactly when the sensors were polled, not when the main loop is processed (I forgot to update the plot labels)

Power_Broker · Apr 4, 2023

defragster said:
As for priority see: myTimer.priority(number);

What about for normal interrupts? I'll likely be using timer and signal interrupts simultaneously and I need to make sure they're all prioritized correctly

defragster · Apr 4, 2023

Power_Broker said:
How is that possible if I'm doing the sensor collection using interrupts? The timestamps represent exactly when the sensors were polled, not when the main loop is processed (I forgot to update the plot labels)

Seemed the timing should be across the file write operation to see how long it takes to complete and that wasn't apparent in the sketch in p#9.
Seen now as "currentSample.ts = micros();" in the _ISR(). That will measure the time between samples when compared to prior sample time, but would not reflect on the time spent in loop()'s "logFile.write(buff, strlen(buff));" and related open/close completion.
Using "int ts;" for storage is lossy/corrupting as it is signed, and should use an unsigned int: uint ts; for micros() unsigned int math to work.
Doing 'print' from _ISR() code is generally not good.

Power_Broker said:
What about for normal interrupts? I'll likely be using timer and signal interrupts simultaneously and I need to make sure they're all prioritized correctly

Normally interrupts are given default pri of 128. Where there are really only 16 pri levels 256/16 where using 0 (highest) to 255 (lowest) goes in groups of 16. The millis timer and USB are higher pri than 128. If adding only a single _ISR() then perhaps no need to change.

Power_Broker · Apr 4, 2023

defragster said:
Seemed the timing should be across the file write operation to see how long it takes to complete and that wasn't apparent in the sketch in p#9.
Seen now as "currentSample.ts = micros();" in the _ISR(). That will measure the time between samples when compared to prior sample time, but would not reflect on the time spent in loop()'s "logFile.write(buff, strlen(buff));" and related open/close completion.
Using "int ts;" for storage is lossy/corrupting as it is signed, and should use an unsigned int: uint ts; for micros() unsigned int math to work.
Doing 'print' from _ISR() code is generally not good.

I'm still confused - why are my sample timestamps showing a periodic lag if the samples are taken in an ISR callback? (I don't actually care how long it takes for the SD writeout to occur, I only care that both my sensors are sampled consistently and that all the data eventually ends up on the SD card)

defragster said:
Normally interrupts are given default pri of 128. Where there are really only 16 pri levels 256/16 where using 0 (highest) to 255 (lowest) goes in groups of 16. The millis timer and USB are higher pri than 128. If adding only a single _ISR() then perhaps no need to change.

As I alluded to earlier, I plan on using pieces of this sketch in a more complex one that has multiple interrupts. How do I change the interrupt priority of a normal interrupt?

defragster · Apr 4, 2023

Power_Broker said:
I'm still confused - why are my sample timestamps showing a periodic lag if the samples are taken in an ISR callback? (I don't actually care how long it takes for the SD writeout to occur, I only care that both my sensors are sampled consistently and that all the data eventually ends up on the SD card)

As I alluded to earlier, I plan on using pieces of this sketch in a more complex one that has multiple interrupts. How do I change the interrupt priority of a normal interrupt?

The time measurement value stored in .ts needs to be uint - a 32 bit unsigned integer. And even then it will rollover each 2^32 microseconds or 71 minutes and the math will workout. That needs to be fixed to have trustworthy results.

As other interrupts are added they will also generally be at 128. So making this one 16 or 32 lower will have it interrupt them - but the data collection will take some measurable time and that _ISR() should have the prints moved to loop().

Power_Broker · Apr 4, 2023

defragster said:
The time measurement value stored in .ts needs to be uint - a 32 bit unsigned integer. And even then it will rollover each 2^32 microseconds or 71 minutes and the math will workout. That needs to be fixed to have trustworthy results.

I changed it to an unsigned long and I still have the lag spikes in the sample timestamps, so I don't think it's rollover. Here's the code:

Code:

#include "Adafruit_LSM6DS33.h"
#include "Adafruit_LIS3MDL.h"
#include "SdFat.h"
#include "navduino.h"
#include "attitude.h"
#include "FireTimer.h"




#ifdef SDCARD_SS_PIN
const uint8_t SD_CS_PIN = SDCARD_SS_PIN;
#endif // SDCARD_SS_PIN
#define SPI_CLOCK SD_SCK_MHZ(50)
#define SD_CONFIG SdioConfig(FIFO_SDIO)




Adafruit_LSM6DS33 lsm6ds33;
sensors_event_t accel;
sensors_event_t gyro;
sensors_event_t temp;
Adafruit_LIS3MDL lis3mdl;
sensors_event_t mag;
compFilt ahrs;

IntervalTimer DataLogTimer;

SdFs   sd;
FsFile logFile;




char field[20] = { '\0' };
char buff[200] = { '\0' };

float gyro_x_bias = -0.045967611;
float gyro_y_bias = 0.092546083;
float gyro_z_bias = 0.0813977905;




const int sampleRate     = 100; // Hz
const int SampleInterval = 1000 * 1000 / sampleRate; // us
const int bufferLength   = 200;

struct sampleData_s
{
    unsigned long ts;

    float ax;
    float ay;
    float az;

    float gx;
    float gy;
    float gz;

    float mx;
    float my;
    float mz;

    float roll;
    float pitch;
    float yaw;
};

// array to create buffer
struct sampleData_s DataBuffer[bufferLength];

// read/write counters
volatile int DataButterWriteIndex = 0;
int DataButterReadIndex = 0;




void OnNewSample()
{
    sampleData_s currentSample;

    currentSample.ts = micros();
    lsm6ds33.getEvent(&accel, &gyro, &temp);
    lis3mdl.getEvent(&mag);

    currentSample.ax = accel.acceleration.x;
    currentSample.ay = accel.acceleration.y;
    currentSample.az = accel.acceleration.z;

    currentSample.gx = (gyro.gyro.x + gyro_x_bias) * 180 / M_PI;
    currentSample.gy = (gyro.gyro.y + gyro_y_bias) * 180 / M_PI;
    currentSample.gz = (gyro.gyro.z + gyro_z_bias) * 180 / M_PI;

    currentSample.mx = mag.magnetic.x;
    currentSample.my = mag.magnetic.y;
    currentSample.mz = mag.magnetic.z;

    ahrs.updateIMU(currentSample.ax,
                    currentSample.ay,
                    currentSample.az,
                    currentSample.gx,
                    currentSample.gy,
                    currentSample.gz,
                    -1,
                    0,
                    0,
                    0);
    
    currentSample.roll  = ahrs.getRoll();
    currentSample.pitch = ahrs.getPitch();
    currentSample.yaw   = ahrs.getYaw();

    DataBuffer[DataButterWriteIndex] = currentSample;

    DataButterWriteIndex++;

    if (DataButterWriteIndex == bufferLength)
        DataButterWriteIndex = 0;
    
    // Serial.print("Raw:");
    // Serial.print(currentSample.ts, 9);
    // Serial.print(",");
    // Serial.print(currentSample.ax, 9);
    // Serial.print(",");
    // Serial.print(currentSample.ay, 9);
    // Serial.print(",");
    // Serial.print(currentSample.az, 9);
    // Serial.print(",");
    // Serial.print(currentSample.gx, 9);
    // Serial.print(",");
    // Serial.print(currentSample.gy, 9);
    // Serial.print(",");
    // Serial.print(currentSample.gz, 9);
    // Serial.print(",");
    // Serial.print(currentSample.mx, 9);
    // Serial.print(",");
    // Serial.print(currentSample.my, 9);
    // Serial.print(",");
    // Serial.print(currentSample.mz, 9);
    // Serial.print(",");
    Serial.print(currentSample.roll, 9);
    Serial.print(",");
    Serial.print(currentSample.pitch, 9);
    Serial.print(",");
    Serial.print(currentSample.yaw, 9);
    Serial.println();
}




void setup()
{
    Serial.begin(115200);

    if (!lsm6ds33.begin_I2C(0b1101011))
    {
        Serial.println("Failed to find LSM6DS33 chip");
        while (1);
    }

    lsm6ds33.setAccelRange(LSM6DS_ACCEL_RANGE_4_G);
    lsm6ds33.setGyroRange(LSM6DS_GYRO_RANGE_500_DPS);
    lsm6ds33.setAccelDataRate(LSM6DS_RATE_104_HZ);
    lsm6ds33.setGyroDataRate(LSM6DS_RATE_104_HZ);

    if (! lis3mdl.begin_I2C(0b0011110))
    {
        Serial.println("Failed to find LIS3MDL chip");
        while (1);
    }

    lis3mdl.setPerformanceMode(LIS3MDL_MEDIUMMODE);
    lis3mdl.setOperationMode(LIS3MDL_CONTINUOUSMODE);
    lis3mdl.setDataRate(LIS3MDL_DATARATE_155_HZ);
    lis3mdl.setRange(LIS3MDL_RANGE_4_GAUSS);

    if (!sd.begin(SD_CONFIG))
    {
        Serial.println("SD initialization failed");
        while (1);
    }

    logFile.open("cal_data.csv", FILE_WRITE);

    sprintf(buff,
        "%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s\n",
        "epoch_us",
        "ax_g",
        "ay_g",
        "az_g",
        "gx_dps",
        "gy_dps",
        "gz_dps",
        "mx_nT",
        "my_nT",
        "mz_nT",
        "roll_deg",
        "pitch_deg",
        "yaw_deg");

    logFile.write(buff, strlen(buff));
    logFile.close();
    
    DataLogTimer.begin(OnNewSample, SampleInterval);
}




void loop()
{
    // if data waiting
    if (DataButterReadIndex != DataButterWriteIndex)
    {
        char ax_str[20];
        char ay_str[20];
        char az_str[20];
        char gx_str[20];
        char gy_str[20];
        char gz_str[20];
        char mx_str[20];
        char my_str[20];
        char mz_str[20];
        char roll_str[20];
        char pitch_str[20];
        char yaw_str[20];

        dtostrf(DataBuffer[DataButterReadIndex].ax,    1, 9, ax_str);
        dtostrf(DataBuffer[DataButterReadIndex].ay,    1, 9, ay_str);
        dtostrf(DataBuffer[DataButterReadIndex].az,    1, 9, az_str);
        dtostrf(DataBuffer[DataButterReadIndex].gx,    1, 9, gx_str);
        dtostrf(DataBuffer[DataButterReadIndex].gy,    1, 9, gy_str);
        dtostrf(DataBuffer[DataButterReadIndex].gz,    1, 9, gz_str);
        dtostrf(DataBuffer[DataButterReadIndex].mx,    1, 9, mx_str);
        dtostrf(DataBuffer[DataButterReadIndex].my,    1, 9, my_str);
        dtostrf(DataBuffer[DataButterReadIndex].mz,    1, 9, mz_str);
        dtostrf(DataBuffer[DataButterReadIndex].roll,  1, 9, roll_str);
        dtostrf(DataBuffer[DataButterReadIndex].pitch, 1, 9, pitch_str);
        dtostrf(DataBuffer[DataButterReadIndex].yaw,   1, 9, yaw_str);

        sprintf(buff,
                "%lu,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s\n",
                DataBuffer[DataButterReadIndex].ts,
                ax_str,
                ay_str,
                az_str,
                gx_str,
                gy_str,
                gz_str,
                mx_str,
                my_str,
                mz_str,
                roll_str,
                pitch_str,
                yaw_str);

        logFile.open("cal_data.csv", FILE_WRITE);
        logFile.write(buff, strlen(buff));
        logFile.close();

        // increase buffer read pointer
        DataButterReadIndex++;

        if (DataButterReadIndex== bufferLength)
            DataButterReadIndex= 0;
    }
}

Here are the results:

Power_Broker · Apr 4, 2023

defragster said:
As other interrupts are added they will also generally be at 128. So making this one 16 or 32 lower will have it interrupt them - but the data collection will take some measurable time and that _ISR() should have the prints moved to loop().

I'm still looking for how to change the priority of a normal interrupt - is it even possible???

mborgerson · Apr 4, 2023

I think I addressed a lot of these issues in some sample code at:
http://https://forum.pjrc.com/threads/60754-Teensy-3-6-SD-Card-missing-time-in-milliseconds?p=238264&viewfull=1#post238264

As others have stated, the key is to collect the samples and fill the buffers at interrupt time, then collect the buffers and write them to SD in the loop(function)

Power_Broker · Apr 4, 2023

mborgerson said:
As others have stated, the key is to collect the samples and fill the buffers at interrupt time, then collect the buffers and write them to SD in the loop(function)

Thanks, however that's exactly what I'm doing (as far as I can tell), and yet still have the lag

mborgerson · Apr 4, 2023

Before you update and post, you should take all the Serial.print().. lines out of your OnNewsample interrupt handler. Serial.print() can take variable amounts of time depending on the response of the attached Serial device. That's not something you want to have changing the time in your interrupt handler. If you want timing stats, add a field to your output structure to record the micros() value at the time of response and examine and display it in the loop() function as you write the filled buffers to Sd.

defragster · Apr 4, 2023

Indeed, noted the removal of ill placed .print()'s twice - given the time value edit - that is possibly the cause as it would prevent timely exit and re-entry to the _ISR()

As far a interrupts - there are basically none but the two mentioned USB/millis tick. Any adjustment details would depend on what is added as the code develops.

AndyA · Apr 5, 2023

Going back a bit:

Power_Broker said:
Thanks!
Dumb question: If I use an interrupt (which is what I assume `DataLogTimer` uses) to sample the sensors, I'm guessing I can still get the sensor data at a predictable rate during these occasions of SD latency?

Yes. The interrupt will interrupt the background loop and any lower priority interrupt.

I think others have already pointed out the big issue. As I indicated in my initial reply

Code:

// sample function. Called in an interrupt context so keep it as minimal as possible.

printf is itself a relatively slow bit of code and depending on the library used isn't always thread safe (meaning if you call it from an interrupt that interrupted something that was in the middle of printf then nasty things can happen).
Serial ports move at a glacial pace.
Doing both is not "as minimal as possible"

Store the data in the interrupt. Don't output it. Don't do any complex processing on it. Just dump it into a structure and leave everything else to the background task.
If for debugging reasons you need some indication that the interrupt is running then toggle an LED or output a single byte on serial using putc(). Assuming the port is idle or has space in the hardware fifo this will be a couple of register accesses and so not take any significant amount of time.

SD Logger has Occasional High Latency

Well-known member

Well-known member

Well-known member

Well-known member

Well-known member

Well-known member

Well-known member

Well-known member

Well-known member

Senior Member+

Well-known member

Well-known member

Senior Member+

Well-known member

Senior Member+

Well-known member

Well-known member

Well-known member

Well-known member

Well-known member

Senior Member+

Well-known member