ESP32 Forum

ESP32 Official Forum
http://esp32.io/

100 Hz hardware timer-based producer/consumer + LittleFS --> non-uniform sampling?

http://esp32.io/viewtopic.php?f=19&t=27242

Page 1 of 1

100 Hz hardware timer-based producer/consumer + LittleFS --> non-uniform sampling?

Posted: Mon Apr 11, 2022 3:30 pm
by plusk01
I use a hw timer at 100 Hz with xSemaphoreGiveFromISR and I buffer data in a task pinned to core 0 with xSemaphoreTake. The buffer is implemented as a ring buffer. In the Arduino loop function (a task pinned to core 1), I write new elements in the ring buffer to the internal flash using LittleFS.

As a test, I write 200 samples (2 seconds) to flash and afterward read them back and print using serial. Each sample contains an index/sequence number and a timestamp, along with other fields for future use. The difference in timestamps is nominally 10 ms (100 Hz sample rate), except for the occasional 40 - 50 ms spike. These spikes occur after writing 4096 bytes (~102 samples), which corresponds to the CONFIG_LITTLEFS_BLOCK_SIZE of 4096 bytes. An example output is posted, showing a ~50 ms jump from sample 102 to sample 103. It is very odd to me is that there is a spike in time, yet the sequence numbers are still consecutive.

I need to sample the data uniformly and eventually I need to be able to sample IMU sensor data at 4 kHz. I expected writing to file to occasionally take significant time, which is why I used a producer/consumer pattern with a ring buffer. Further, I expected that putting the sampling / writing tasks on separate cores would have made this a non-issue altogether.

Some questions:
  1. The use of LittleFS in my code appears to prevent uniform sampling even though a hardware timer is used. Why?
  2. Why does it appear that the cores are not independent? i.e., why does a slow task on core 1 affect the sampling task on core 0?
  3. Is there a better way to achieve fast (eventually 4 kHz), uniform sampling + writing to file?
11:03:32.628 -> /data.bin: 8000 bytes
11:03:34.682 -> 200 samples in 2.03 seconds
11:03:34.748 ->
11:03:34.748 -> Reading...
11:03:34.748 ->
11:03:34.748 -> /data.bin: 8000 bytes
11:03:34.748 -> 0, 1.04981
11:03:34.748 -> 1, 1.05981
11:03:34.748 -> 2, 1.06981
11:03:34.748 -> 3, 1.07981
11:03:34.748 -> 4, 1.08981
11:03:34.748 -> 5, 1.09981
11:03:34.748 -> 6, 1.10981
11:03:34.748 -> 7, 1.11981
11:03:34.748 -> 8, 1.12981
11:03:34.748 -> 9, 1.13981
11:03:34.748 -> 10, 1.14981
// everything is fine from here to sample 90
11:03:34.935 -> 90, 1.94981
11:03:34.935 -> 91, 1.95981
11:03:34.935 -> 92, 1.96981
11:03:34.935 -> 93, 1.97981
11:03:34.935 -> 94, 1.98981
11:03:34.935 -> 95, 1.99981
11:03:34.935 -> 96, 2.00981
11:03:34.935 -> 97, 2.01981
11:03:34.935 -> 98, 2.02981
11:03:34.935 -> 99, 2.03981
11:03:34.935 -> 100, 2.04981
11:03:34.935 -> 101, 2.05981
11:03:34.935 -> 102, 2.06981
11:03:34.935 -> 103, 2.11381
11:03:34.935 -> 104, 2.12467
11:03:34.935 -> 105, 2.13380
11:03:34.935 -> 106, 2.14380
11:03:34.935 -> 107, 2.15380
11:03:34.935 -> 108, 2.16380
11:03:34.935 -> 109, 2.17380
// everything is fine from here to sample 191
11:03:34.947 -> 191, 2.99380
11:03:34.947 -> 192, 3.00380
11:03:34.947 -> 193, 3.01380
11:03:34.947 -> 194, 3.02380
11:03:34.947 -> 195, 3.03380
11:03:34.980 -> 196, 3.04380
11:03:34.980 -> 197, 3.05380
11:03:34.980 -> 198, 3.06380
11:03:34.980 -> 199, 3.07380

Code: Select all

#include <FS.h>
#include <SPI.h>
#include <FFat.h>
#include <LittleFS.h>
#include <SD.h>
#include <ICM_20948.h>

union imudata_t {
 uint8_t bytes[40];
 struct {
   float t_sec;
   uint32_t idx;
   float a[3];
   float g[3];
   // float m[3];
   uint32_t unused[2];
 } data;
};

SPIClass SPI2(HSPI);
SPIClass SPI3(VSPI);

// IMU ring buffer
static constexpr int IMUBUF_SIZE = 1000;
imudata_t imubuf_[IMUBUF_SIZE];
uint16_t imubuf_head_ = 0;
uint16_t imubuf_tail_ = 0;
uint32_t seq = 0;

// logger state machine
enum State { IDLE, LOG, READ, DONE };
State state;

ICM_20948_SPI imu;
float Gscale = (M_PI / 180.0) / 32.8;
float Ascale = 1.0 / 16384.0;
float Mscale = 1.0;

// sample rate, controlled by hardware timer
hw_timer_t * timer = nullptr;
static constexpr int TICKS_PER_SEC = 1000000;
static constexpr int SAMPLE_PER_SEC = 100;
static constexpr int PERIOD_TICKS = TICKS_PER_SEC / SAMPLE_PER_SEC;

// how many samples to collect?
static constexpr int NUM_SAMPLES = 2 * SAMPLE_PER_SEC;

// hw timer <--> sample task semaphore
SemaphoreHandle_t semDataReady = NULL;

// #define USE_SERIAL
 #define USE_LittleFS
// #define USE_FFat
// #define USE_SD

#if defined(USE_SERIAL)
FS& fs_ = LittleFS; // doesn't matter
#elif defined(USE_LittleFS)
FS& fs_ = LittleFS;
#elif defined(USE_FFat)
FS& fs_ = FFat;
#elif defined(USE_SD)
FS& fs_ = SD;
#endif

char DATABIN[] = "/data.bin";
File file_;

///////////////////////////////////////////////////////////////////////

bool init_fs() {
  static constexpr bool format_if_failed = true;
  
#if defined(USE_SERIAL) || defined(USE_LittleFS)
  if (!LittleFS.begin(format_if_failed)) {
#elif defined(USE_FFat)
 FFat.format();
 if (!FFat.begin()) {
#elif defined(USE_SD)
 SPI2.begin();
 if (!SD.begin(SPI2.pinSS(), SPI2, 25000000)) {
#endif
    return false;
  }

  return true;
}

// --------------------------------------------------------------------

void init_imu() {
  SPI3.begin();

  bool initialized = false;
  while (!initialized) {
    imu.begin(SPI3.pinSS(), SPI3);
    if (imu.status != ICM_20948_Stat_Ok) {
      Serial.println("IMU connection issue ... trying again");
      delay(500);
    } else {
      initialized = true;
    }
  }

  // reset sensor so it is in known state
  imu.swReset();
  delay(500);

  // wake it up
  imu.sleep(false);
  imu.lowPower(false);

  // configure sample mode
  imu.setSampleMode((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Continuous);

  // set full scale ranges
  ICM_20948_fss_t fss;
  fss.a = gpm2; // gpm2, gpm4, gpm8, gpm16
  fss.g = dps1000; // dps250, dps500, dps1000, dps2000
  imu.setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), fss);

  // set digital low-pass filter config
  ICM_20948_dlpcfg_t dlpcfg;
  dlpcfg.a = acc_d473bw_n499bw;
  dlpcfg.g = gyr_d361bw4_n376bw5;
  imu.setDLPFcfg((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), dlpcfg);
  imu.enableDLPF((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), false);

  // set sample rates to maximum
  ICM_20948_smplrt_t smplrt;
  smplrt.a = 1;
  smplrt.g = 1;
  imu.setSampleRate((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), smplrt);

  // startup mag
  imu.startupMagnetometer();
  delay(1000);

  Serial.println("IMU config'd");
}

// -------------------------------------------------------------------------

void IRAM_ATTR timer_isr() {
  xSemaphoreGiveFromISR(semDataReady, NULL);
  // TODO: if higher priority woken, do context switch?

//  imubuf_[imubuf_head_].data.t_sec = micros() * 1e-6;
//  imubuf_[imubuf_head_].data.idx = seq++;
//  
//  // move the head of the buffer, wrapping around if neccessary
//  imubuf_head_ = (imubuf_head_ + 1) % IMUBUF_SIZE; 
}

// -------------------------------------------------------------------------

void vTaskGetData(void * pvParameters) {
  (void) pvParameters;
  for (;;) {
    if (xSemaphoreTake(semDataReady, portMAX_DELAY) == pdTRUE) {

       // complete a SPI transaction with IMU to get data
       // ICM_20948_AGMT_t agmt = imu.getAGMT(); // takes 205 to 206 usec

       imubuf_[imubuf_head_].data.t_sec = micros() * 1e-6;
       imubuf_[imubuf_head_].data.idx = seq++;
       // imubuf_[imubuf_head_].data.a[0] = Ascale * agmt.acc.axes.x;
       // imubuf_[imubuf_head_].data.a[1] = Ascale * agmt.acc.axes.y;
       // imubuf_[imubuf_head_].data.a[2] = Ascale * agmt.acc.axes.z;
       // imubuf_[imubuf_head_].data.g[0] = Gscale * agmt.gyr.axes.x;
       // imubuf_[imubuf_head_].data.g[1] = Gscale * agmt.gyr.axes.y;
       // imubuf_[imubuf_head_].data.g[2] = Gscale * agmt.gyr.axes.z;
       // imubuf_[imubuf_head_].data.m[0] = Mscale * agmt.mag.axes.x;
       // imubuf_[imubuf_head_].data.m[1] = Mscale * agmt.mag.axes.y;
       // imubuf_[imubuf_head_].data.m[2] = Mscale * agmt.mag.axes.z;

        // move the head of the buffer, wrapping around if neccessary
        imubuf_head_ = (imubuf_head_ + 1) % IMUBUF_SIZE;      
    }
  }
}

// -------------------------------------------------------------------------

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

  if (!init_fs()) {
    Serial.println("filesystem mount failed");
    return;
  }
  
//  init_imu();

  semDataReady = xSemaphoreCreateBinary();
  xTaskCreatePinnedToCore(vTaskGetData, "vTaskGetData", 1024, NULL, 3, NULL, 0);

  delay(1000);
}

// -------------------------------------------------------------------------

void loop() {
  static float t0 = 0;
  static float tf = 0;
  static uint32_t seqnow = 0;

  if (state == State::IDLE) {

    file_ = fs_.open(DATABIN, FILE_WRITE);
    Serial.print(DATABIN); Serial.print(": "); Serial.print(file_.size()); Serial.println(" bytes");
    
    t0 = millis() * 1e-3;

    // begin the hardware timer / sampling
    timer = timerBegin(0, 80, true);
    timerAttachInterrupt(timer, timer_isr, true);
    timerAlarmWrite(timer, PERIOD_TICKS, true);
    timerAlarmEnable(timer);
    
    state = State::LOG;
  } else if (state == State::LOG) {

#ifdef USE_SERIAL
    static constexpr int txtlen = 4000;
    char txt[txtlen];
    int j = 0;
#endif
  
    // timer may put additional data in buffer while this loop executes,
    // but we will only read up to what we can see right now
    const uint16_t head = imubuf_head_;
    seqnow = seq;

    // if there is new data to write
    if (head != imubuf_tail_) {


      // if head ptr has looped back around, first get the data from here to the end
      if (head < imubuf_tail_) {
          const uint16_t len = IMUBUF_SIZE - imubuf_tail_;
#ifndef USE_SERIAL
          file_.write(reinterpret_cast<uint8_t*>(&imubuf_[imubuf_tail_]), len * sizeof(imudata_t));
#else
          for (size_t i=0; i<len; ++i) {
            j += snprintf(txt+j, txtlen-j, "%d, %2.5f, %d, %d, %d\n",
                    imubuf_[imubuf_tail_+i].data.idx,
                    imubuf_[imubuf_tail_+i].data.t_sec,
                    imubuf_tail_+i, head, imubuf_head_);
          }
#endif
          imubuf_tail_ = 0;
      }

      
      // getting data is easy, from tail to head
      if (head > imubuf_tail_) {
        const uint16_t len = head - imubuf_tail_;
#ifndef USE_SERIAL
        file_.write(reinterpret_cast<uint8_t*>(&imubuf_[imubuf_tail_]), len * sizeof(imudata_t));
#else
        for (size_t i=0; i<len; ++i) {
          j += snprintf(txt+j, txtlen-j, "%d, %2.5f, %d, %d, %d\n",
                    imubuf_[imubuf_tail_+i].data.idx,
                    imubuf_[imubuf_tail_+i].data.t_sec,
                    imubuf_tail_+i, head, imubuf_head_);
        }
#endif  
        imubuf_tail_ = head;
      }


#ifdef USE_SERIAL
      Serial.println(txt);
#endif
      
    }

    if (seqnow >= NUM_SAMPLES) {
      tf = (millis() * 1e-3) - t0;
      Serial.print(seqnow); Serial.print(" samples in "); Serial.print(tf); Serial.println(" seconds");
      state = State::READ;
    }
  } else if (state == State::READ) {

    file_.close();

    Serial.println("");
    Serial.println("Reading...");
    Serial.println("");

    file_ = fs_.open(DATABIN);
    Serial.print(DATABIN); Serial.print(": "); Serial.print(file_.size()); Serial.println(" bytes");

    if (file_) {
      imudata_t imu;
      while (file_.position() < file_.size()) {
        file_.read(imu.bytes, sizeof(imudata_t));

        static constexpr int txtlen = 200;
        char txt[txtlen];
        snprintf(txt, txtlen, "%d, %3.5f\n",
            imu.data.idx, imu.data.t_sec);
        Serial.print(txt);
      }
      
    } else {
      Serial.println("Failed to open data file for reading");
    }

    state = State::DONE;
  } else if (state == State::DONE) {

  }
  
  vTaskDelay(1);
}

Re: 100 Hz hardware timer-based producer/consumer + LittleFS --> non-uniform sampling?

Posted: Tue Apr 12, 2022 8:03 am
by ESP_Sprite
Your issue is likely that you're writing to the same flash your program is running out of, and flash can't write and read at the same time, so your program effectively is halted when a flash write needs to be done. Solution to that would be to put the interrupt in IRAM and mark it as such. Unfortunately I don't know how to do that on Arduino, perhaps someone else can help you with that. Another solution would be to write the data to non-internal memory, e.g. an SD-card.
All times are UTC
Page 1 of 1
Powered by phpBB® Forum Software © phpBB Limited
https://www.phpbb.com/