FreeRTOS as data logger, ESP32, LiS3DH Accel

Hello, currently working on a ESP32 Adafruit feather running FreeRTOS in an effort to log accel. data @ 1000 Hz. LIS3DH accel hooked up using I2C, SD card hooked up using SPI (Feather hat RTC+SD card). I’ve used the vtaskdelay successfully to get data at 100 to about 400 Hz when things start going bad. Ideally Task “GetData” gets data from the LIS3DH via I2C @ 5000Hz. Then writes to queue and TAsk “SDWrite” takes data via pointers to SD Card. I have a 3rd Task “SDflush” to run a file.flush() on the system to make sure its written. I can always get data but as it gets faster it writes the same values up to 16 times in a row and the fastest it achieves is 2000 usec (about 500 Hz).
Things I’ve tried:

  • remove vtaskdelay(): data timing jumps around and average Hz actually gets lower
  • keep an eye on Queue: Queue never seems to even get close to filling up, SDwrite task works pretty fast
  • log.flush(): adjust time, doesn’t seem to have an effect
  • removed size of data structure: didn’t seem to have any effect on lag

I think I may not be getting data fast enough, but data rate is set to 5000Hz and I2C should definitely be fast enough?

Here is a data file read out with vTaskDelay(1):
439540,0.00000,0.57475,9.31093
441540,0.00000,0.45980,9.31093
449541,-0.11495,0.57475,8.96608
449541,-0.11495,0.57475,8.96608
449541,-0.11495,0.57475,8.96608
449541,-0.11495,0.57475,8.96608
451536,-0.11495,0.57475,9.19598
453541,-0.11495,0.34485,9.31093
455541,-0.11495,0.22990,9.42588
457541,-0.22990,0.34485,9.31093
459541,-0.22990,0.34485,9.19598
461541,-0.22990,0.45980,9.42588
463541,-0.45980,0.34485,9.31093
465541,-0.11495,0.45980,9.54083
467541,-0.11495,0.57475,9.31093
469541,-0.11495,0.80465,9.42588
471541,0.11495,0.45980,9.42588
473541,-0.11495,0.34485,9.42588
481540,0.00000,0.34485,9.42588
481540,0.00000,0.34485,9.42588
481540,0.00000,0.34485,9.42588
481540,0.00000,0.34485,9.42588

You can see “hiccups” of same data logged.

Code Part 1:

/ Data logger:  For measuring acceleration from LIS3DH
// Hardware:  Adafruit ESP32, Adalogger feather+RTC, 1x LIS3DH accels (Currently, 2x in future) 
// Created:  May 12 2020
// Updated:
// Uses SPI for SD, I2C for Accels, hoping for 1000 Hz. sampling 
// files are saves text files, csv = NNNNNNNN.TXT
// See ReadME and photos for additional hook up info

//Use ESP32 duo core
const int TaskCore1  = 1;
const int TaskCore0 = 0;

//Libraries
#include <Wire.h>
#include <SPI.h>
#include <SdFat.h>
#include <Adafruit_LIS3DH.h>
#include <Adafruit_Sensor.h>
#include <stdio.h>
#include "esp_system.h" //This inclusion configures the peripherals in the ESP system.
//#include "freertos/Arduino_FreeRTOS.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/timers.h"
#include "freertos/event_groups.h"
#include "freertos/queue.h"
//#include "freertos/stream_buffer.h"
//------------------------------------------------------------------------------
// SD file definitions
const uint8_t sdChipSelect = 33;
SdFat sd;
SdFile file;
File logfile;
//------------------------------------------------------------------------------
// Queue definitions

// data type for Queue item
struct Data_t {
  uint32_t usec; 
  float valueX;
  float valueY;
  float valueZ;
} xData_t;

//Declare Queue data type for FreeRTOS
QueueHandle_t DataQueue = NULL;

//TickType_t pdMS_TO_TICKS(uint32_t millis);
//TickType_t intervalTicks = pdMS_TO_TICKS(1);
// interval between points in units of 1000 usec
const uint16_t intervalTicks = 1;

//------------------------------------------------------------------------------
// Accel Lis3dh definitions, I2C
// Sensor I2C 
Adafruit_LIS3DH lis = Adafruit_LIS3DH();

// Used for hardware & software SPI
// hardware SPI 1 LIS3DH->Feather:  Power to Vin, Gnd to Gnd, SCL->SCK, SDA->MOSI, SDO->MOSO, CS->CS 14/15
//#define LIS3DH_CS 14  //ESP32: 14/A6 , Cortex m0: 5, Use for upper accel, hbar, seatpost, etc.
//#define LIS3DH_CS2 15  //ESP32: 15/A8, Cortex m0: 9, Use for lower accel, axles, etc. 
// Sensor 1 
//Adafruit_LIS3DH lis = Adafruit_LIS3DH(LIS3DH_CS);
// Sensor 2 
//Adafruit_LIS3DH lis2 = Adafruit_LIS3DH(LIS3DH_CS2);

//------------------------------------------------------------------------------
// define two tasks for Sensor Data and SD Write
void TaskGetData( void *pvParameters );
void TaskSDWrite( void *pvParameters );
void TaskSDFlush( void *pvParameters );
//------------------------------------------------------------------------------

// Start the scheduler so the created tasks start executing. Need this for ESP32???
void vTaskStartScheduler();
   
// the setup function runs once when you press reset or power the board
void setup() {

  // initialize serial communication at 115200 bits per second:
  //Serial.begin(115200);

  //Outputs, Pins, Buttons, Etc. 
  pinMode(13, OUTPUT);  //set Built in LED to show writing on SD Card

  //ACCEL Setup and RUN
  if (! lis.begin(0x18)) {   // change this to 0x19 for alternative i2c address
  Serial.println("Couldnt start");
  while (1) yield();
  }
  Serial.println("LIS3DH found!");
  // Set accel range  
  lis.setRange(LIS3DH_RANGE_16_G);   // 2, 4, 8 or 16 G!
  //lis2.setRange(LIS3DH_RANGE_16_G);
  // Set DataRate
  lis.setDataRate(LIS3DH_DATARATE_LOWPOWER_5KHZ); //OPTIONS:  LIS3DH_DATARATE_400_HZ, LIS3DH_DATARATE_LOWPOWER_1K6HZ, LIS3DH_DATARATE_LOWPOWER_5KHZ
  //lis2.setDataRate(LIS3DH_DATARATE_LOWPOWER_5KHZ); 

  //Setup I2C speed
  //Wire.setClock(400000);

// SD CARD SETUP ====================================================================
// see if the card is present and can be initialized:  (Use highest SD clock possible, but lower if has error, 15 Mhz works, possible to go to to 25 Mhz if sample rate is low enough
if (!sd.begin(sdChipSelect, SD_SCK_MHZ(25))) {
  Serial.println("Card init. failed!");
  //error(2);
}

// Create filename scheme ====================================================================
  char filename[15];
  //  Setup filename to be appropriate for what you are testing
  strcpy(filename, "/DATA00.TXT");
  for (uint8_t i = 0; i < 100; i++) {
    filename[5] = '0' + i/10;
    filename[6] = '0' + i%10;
    // create if does not exist, do not open existing, write, sync after write
    if (! sd.exists(filename)) {
      break;
    }
  }

// Create file and prepare it ============================================================
  logfile = sd.open(filename, O_CREAT | O_WRITE ); // | O_TRUNC);
  if( ! logfile ) {
    Serial.print("Couldnt create "); 
    Serial.println(filename);
    //error(3);
  }
  Serial.print("Writing to "); 
  Serial.println(filename);

  pinMode(13, OUTPUT);
  Serial.println("Ready!");


  //Queue Setup
  DataQueue = xQueueCreate(5000, sizeof( &xData_t ));
  if(DataQueue == NULL){
     Serial.println("Error Creating the Queue");
   }
  
// Setup up Tasks and where to run ============================================================  
// Now set up tasks to run independently.
  xTaskCreatePinnedToCore(
    TaskGetData
    ,  "Get Data from Accel to Queue"   // A name just for humans
    ,  2048  // This stack size can be checked & adjusted by reading the Stack Highwater
    ,  NULL
    ,  3  // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
    ,  NULL 
    ,  TaskCore0);

  xTaskCreatePinnedToCore(
    TaskSDWrite
    ,  "Get Data from Queue"
    ,  2048 // Stack size
    ,  NULL
    ,  2  // Priority
    ,  NULL 
    ,  TaskCore1);

  xTaskCreatePinnedToCore(
    TaskSDFlush
    ,  "Write Data to Card"
    ,  1024 // Stack size
    ,  NULL
    ,  1  // Priority
    ,  NULL 
    ,  TaskCore1);
}

void loop()
{
  // Empty. Things are done in Tasks.
/*--------------------------------------------------*/
/*---------------------- Tasks ---------------------*/
/*--------------------------------------------------*/
}

Part 2:

void TaskGetData(void *pvParameters)  // This is a task.
{
  (void) pvParameters;
  
  struct Data_t *pxPointerToxData_t;

  for (;;) // A Task shall never return or exit.
  {
    pxPointerToxData_t = &xData_t; 

    if(xQueueSend( DataQueue, (void *) &pxPointerToxData_t, 10 ) != pdPASS )  //portMAX_DELAY
      {
        Serial.println("xQueueSend is not working"); 
      }
      
    sensors_event_t event;
    lis.getEvent(&event);
    pxPointerToxData_t->usec = micros();
    pxPointerToxData_t->valueX = event.acceleration.x;
    pxPointerToxData_t->valueY = event.acceleration.y;
    pxPointerToxData_t->valueZ = event.acceleration.z;
    Serial.print(pxPointerToxData_t->usec); 
    Serial.print(',');
    Serial.print(pxPointerToxData_t->valueX,5);
    Serial.print(',');
    Serial.print(pxPointerToxData_t->valueY,5);
    Serial.print(',');
    Serial.print(pxPointerToxData_t->valueZ,5);
    Serial.println();
    
    //digitalWrite(LED_BUILTIN, HIGH);   // turn the LED on (HIGH is the voltage level)
    //vTaskDelay(100);  // one tick delay (15ms) in between reads for stability
    //digitalWrite(LED_BUILTIN, LOW);    // turn the LED off by making the voltage LOW
    //vTaskDelay(100);  // one tick delay (15ms) in between reads for stability
 
    vTaskDelay(intervalTicks);  // one tick delay (1000 uSec/1 mSec) in between reads for 1000 Hz reading 
    
  
  }
}
//------------------------------------------------------------------------------
void TaskSDWrite(void *pvParameters)  // This is a task.
{
  (void) pvParameters;
  struct Data_t xData_RCV, *pxData_RCV;
  
  for (;;)
  {

    if(DataQueue != NULL ) 
    {
      if( xQueueReceive( DataQueue, &( pxData_RCV ), 10 ) != pdPASS )   //portMAX_DELAY
      {
        Serial.println("xQueueRecieve is not working");
      }
        
      // print interval between points
      /*if (last) {
        logfile.print(p->usec - last);
      } else {
        logfile.write("NA");
      }
      last = p->usec;*/
      //for (uint8_t i = 0; i < 10; i++) { 
        logfile.print(pxData_RCV->usec);
        logfile.print(',');
        logfile.print(pxData_RCV->valueX,5);
        logfile.print(',');
        logfile.print(pxData_RCV->valueY,5);
        logfile.print(',');
        logfile.print(pxData_RCV->valueZ,5);
        logfile.println(); 
        /*Serial.print(pxData_RCV->usec); 
        Serial.print(',');
        Serial.print(pxData_RCV->valueX,5);
        Serial.print(',');
        Serial.print(pxData_RCV->valueY,5);
        Serial.print(',');
        Serial.print(pxData_RCV->valueZ,5);
        Serial.println();*/ 
      //}

        uint16_t FreeSpace = uxQueueSpacesAvailable( DataQueue ); 
        Serial.println(FreeSpace); 
      //logfile.flush();
      //uint8_t i = 0; 
    }
   }
}


//------------------------------------------------------------------------------
void TaskSDFlush(void *pvParameters)  // This is a task.
{
  (void) pvParameters;

  for (;;)
  {
    logfile.flush();
    vTaskDelay( 1000 );
    Serial.println("Flushed file"); 
    
  }
}

I can always get data but as it gets faster it writes the same values up to 16 times in a row and the fastest it achieves is 2000 usec (about 500 Hz).

"it gets faster, it writes", What is it?

If the data is being written to the sd card 16 time before new data comes in, you are not receiving new data fast enough, and taht's the issue you want to solve, right?

1st, get a spi device and, 2nd, use the ESP32 SPI API to connect to the SPI module instead of going through SPI.h.

You can try using the ESP32 I2C API but going beyond 400kHz, most likely not going to happen. I did not realize a data performance improvement using the ESP32 I2C API.

I recently posted, on this site, code using the ESP32 SPI API. The ESP32 SPI API will let you run full duplex, background SPI comes that stores data in freeRTOS queues. I found that setting the queue size over 7 did not result in performance.

ESP32 API: API Reference - ESP32 - — ESP-IDF Programming Guide latest documentation.

I tend to give the minimum task size at 10K, in spite of the highwatermark report.

   } <<< end of for loop
vTaskDelete( NULL ); <<<<<<< protection
} end of task

If a task is ever able to get beyond the for loop, a grave error has happened to the task and thing will go bad real fast. To stop the bad things use vTaskDelete( NULL ); after the for loop.

 xTaskCreatePinnedToCore(
    TaskSDFlush
    ,  "Write Data to Card"
    ,  1024 // Stack size
    ,  NULL
    ,  1  // Priority
    ,  NULL
    ,  TaskCore1);

Looking at your code, I'd run all the tasks at priority 3 or 4, which will force equal time allocated to each task.

loop() is ran at task priority 1, don't put code in loop() under freeRTOS. When loop() is run house keeping chorse are done. loop() runs when it can. By setting the above task to a priority of 1, the task runs like loop(), when it can. I'd run the sd card tasks as 4, just one higher then the rest of the tasks. In this way the sd card is a bit more insured completion before the task is switched away.

I'd also put the sd card functions on core 0, with no WiFi. You might want to set the partition scheme to maximize available memory and use log_x(faster) over serial print(slow); see ESP32 API; see ESP32 Logging API Logging library - ESP32 - — ESP-IDF Programming Guide latest documentation

No need to run the convert to ticks macro, freeRTOS. This vTaskDelay(1) states to delay for 1millisecond. Automatic conversion. BTW, it is about 3 clock ticks per mS (not the 240mHz clock but the 80Mhz peripheral clock).

You, also, have a bit of freeRTOS legacy code that is no longer used, such as starting the task scheduler.

And you might look into event triggers for at least one of the tasks you are running.

Idahowalker, thanks so much for the feedback!!! I'll go through items here 1x1 and then give results:

Looking at your code, I'd run all the tasks at priority 3 or 4, which will force equal time allocated to each task.

Done, looks to have helped keep items working together.

I'd also put the sd card functions on core 0,

Put both SDwrite and SDFlush on Core 0 now.

No need to run the convert to ticks macro, freeRTOS. This vTaskDelay(1) states to delay for 1millisecond. Automatic conversion. BTW, it is about 3 clock ticks per mS (not the 240mHz clock but the 80Mhz peripheral clock).

Sounds good, I knew I wasn't getting something correct here because I had experimented and it was following my initial thoughts of how it worked. I figured (1) was equal to 1000 mS but wasn't completely sure.

Task Scheduler

Removed, thank you

Event Triggers

I'll have to keep reading and learning about this

To stop the bad things use vTaskDelete( NULL ); after the for loop.

Added these to be safe, thanks for the tip

"it gets faster, it writes", What is it?

Sorry, if I continue to lower the delay, so trying to increase my sample rate it will continue to write to SD, but it will start writing the same values and time, up to 16x, then start recording samples at 2000 uS difference, then write same values over again. So i figured either queue was overrun, or more likely isn't getting new values quickly enough

If the data is being written to the sd card 16 time before new data comes in, you are not receiving new data fast enough, and that's the issue you want to solve, right?

Yes, if i run serial print to just try and get a glimpse of what is happening it looks like I may not be getting data fast enough, not that i'm not writing to SD properly. So is I2C not fast enough? It seems like it should be fast enough for this, but I can try SPI with this same LIS3DH.

I recently posted, on this site, code using the ESP32 SPI API. The ESP32 SPI API will let you run full duplex, background SPI comes that stores data in freeRTOS queues. I found that setting the queue size over 7 did not result in performance.

I'll look for that.

So with those current updates you've helped me with, as mentioned above, i'm now getting an average of 665 Hz with 4500 uSec jump every ~20 mS. See below for example. Is this having to do with logfile.flush(), but was hoping to use queue and FreeRTOS to avoid this so doubtful this is what it is. Possibly I2C involved?

uSec X Axis Y Axis Z axis Diff Cumulative
86422 -0.34485 0.6897 9.19598
87928 -0.2299 0.80465 9.31093 1506 4541
89442 -0.11495 0.57475 9.54083 1514 6055
90947 -0.34485 0.34485 9.54083 1505 7560
92452 0 0.57475 9.31093 1505 9065
93956 -0.2299 0.6897 9.19598 1504 10569
95464 -0.2299 0.6897 9.31093 1508 12077
96969 -0.2299 0.34485 9.42588 1505 13582
98474 -0.11495 0.2299 9.19598 1505 15087
99979 -0.11495 0.34485 8.96608 1505 16592
101485 -0.11495 0.34485 9.19598 1506 18098
102990 -0.4598 0.4598 9.42588 1505 19603
104495 -0.2299 0.6897 9.65578 1505 21108
109087 -0.2299 1.03455 9.42588 4592 25700
110594 -0.34485 0.4598 9.54083 1507 27207
112098 0 0.4598 9.19598 1504 28711
113604 0 0.80465 9.42588 1506 30217
115107 0 0.9196 9.31093 1503 31720
116615 -0.2299 0.57475 9.08103 1508 33228
118120 0 0.34485 9.42588 1505 34733
119634 -0.2299 0.57475 9.42588 1514 36247
121142 -0.57475 0.6897 9.08103 1508 37755
122647 -0.2299 0.57475 9.19598 1505 39260
124162 -0.11495 0.34485 9.54083 1515 40775
125667 -0.2299 0.57475 9.31093 1505 42280
127173 -0.34485 0.9196 9.42588 1506 43786
128678 -0.2299 0.9196 9.31093 1505 45291
130183 -0.34485 0.9196 9.31093 1505 46796
131688 -0.2299 0.80465 9.31093 1505 48301
136275 -0.11495 0.4598 9.54083 4587 52888

Just wanted to put an update on here. Am able to record well up to 500 Hz now. Still having a watchdog timer issue above that trying to solve. Possibly not getting data fast enough?

read a lot on the queue and decided to go with Queue by copy instead of reference. It sounds like from documentation FreeRTOS does this quite and well and by reference is only necessary with “large” structures but what large is, is not defined. Working well enough for me right now.
Removed flush call and pulled logfile.close() into SD write since having seperate tasks that accidentally meet at wrong time is too much for SD write i believe, a flush called in the middle of logging data is not ideal, so having one Task to handle all interactions with SD I think is ideal, correct?
eventually want to add button functionality to have system armed but not started until button press, I imagine this involves vTAskSuspend until interrupt from semaphore to all tasks to say go?
put data lines (accels) and SD line (SD card) on seperate SPI lines. Ideally i would eventually use ESP32 SPI API but current SPI library seems to be working unless it is causing issue with problem below
The only problem i have now is I can record up to 500Hz very well, consistently and accurately, with very few accel log errors, but going above this forces a watchdog error, not every time but most of the time. I wonder if task is waiting on data but data isn’t coming? It looks like it has to do with TaskGetData.
Error:
Writing to /DATA05.TXT
E (11722) task_wdt: Task watchdog got triggered. The following tasks did not reset the watchdog in time:
E (11722) task_wdt: - IDLE0 (CPU 0)
E (11722) task_wdt: Tasks currently running:
E (11722) task_wdt: CPU 0: Get Data from Q
E (11722) task_wdt: CPU 1: loopTask
E (11722) task_wdt: Aborting.
abort() was called at PC 0x400d7f53 on core 0

Backtrace: 0x4008c2e0:0x3ffbe170 0x4008c50d:0x3ffbe190 0x400d7f53:0x3ffbe1b0 0x40084e69:0x3ffbe1d0 0x400d4ef6:0x3ffc4400 0x400d1546:0x3ffc4420 0x400d297a:0x3ffc4440 0x400d2deb:0x3ffc4460 0x400d2509:0x3ffc4480 0x400d1cd1:0x3ffc44a0 0x400d0fe0:0x3ffc44d0 0x400d24ba:0x3ffc44f0 0x400d5715:0x3ffc4510 0x400d58c4:0x3ffc4560 0x400d5953:0x3ffc4580 0x400d0ebc:0x3ffc45a0 0x40088a91:0x3ffc45c0

// Data logger:  For measuring acceleration from LIS3DH/s
// Hardware:  Adafruit ESP32, Adalogger feather+RTC, 2x LIS3DH accels
// Created:  May 12 2020
// Updated:
// Uses SPI for SD & Accels, hoping for 1000 Hz. sampling 
// files are saves text files = DATANN.TXT
// See ReadME and photos for additional hook up info

const int SampleRate = 1000; //Hz, Set sample rate here
const int SampleLength = 10; //Seconds, Sample Length in Seconds

//Use ESP32 duo core
const int TaskCore1  = 1;
const int TaskCore0 = 0;
int SampleInt = 1000000 / SampleRate; 
int TotalCount = SampleLength * SampleRate;

//Libraries
#include <SPI.h>
#include "SdFat.h"
#include <Adafruit_LIS3DH.h>
#include <Adafruit_Sensor.h>
#include "stdio.h"
#include "esp_system.h" //This inclusion configures the peripherals in the ESP system.
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/timers.h"
#include "freertos/event_groups.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"

#define LED_BUILTIN LED_BUILTIN //LED light for notification
//------------------------------------------------------------------------------
// SD file definitions
const uint8_t sdChipSelect = 33;
SdFat sd;
SdFile file;
File logfile;
//------------------------------------------------------------------------------
// data type for Queue item
struct Data_t {
  uint32_t usec; 
  float value1X;
  float value1Y;
  float value1Z;
  float value2X;
  float value2Y;
  float value2Z;
} TX_Data_t, RX_Data_t;

//------------------------------------------------------------------------------
// Accel Lis3dh definitions, SPI or I2C
// hardware SPI 1 LIS3DH->Feather:  Power to Vin, Gnd to Gnd, SCL->SCK, SDA->MOSO, SDO->MOSI, CS->CS 14/15
// Sensor 1 Hardware SPI
//Adafruit_LIS3DH lis = Adafruit_LIS3DH(LIS3DH_CS);
// Sensor 2 Hardware SPI
//Adafruit_LIS3DH lis2 = Adafruit_LIS3DH(LIS3DH_CS2);

// Used for software SPI
#define LIS3DH_CLK 21
#define LIS3DH_MISO 33
#define LIS3DH_MOSI 32
#define LIS3DH2_CLK 25
#define LIS3DH2_MISO 26
#define LIS3DH2_MOSI 4
// Used for hardware & software SPI
#define LIS3DH_CS 14   //ESP32: 14/A6 , Cortex m0: 5, Use for upper accel (Sensor 1!!!) = hbar, seatpost, etc.
#define LIS3DH2_CS 15  //ESP32: 15/A8, Cortex m0: 9, Use for lower accel (Sensor 2!!!) = axles, etc. 

// software SPI
Adafruit_LIS3DH lis = Adafruit_LIS3DH(LIS3DH_CS, LIS3DH_MOSI, LIS3DH_MISO, LIS3DH_CLK);
// software SPI 2 
Adafruit_LIS3DH lis2 = Adafruit_LIS3DH(LIS3DH2_CS, LIS3DH2_MOSI, LIS3DH2_MISO, LIS3DH2_CLK);

//------------------------------------------------------------------------------
// define tasks for Sensor Data and SD Write
void TaskLed( void *pvParamaters );
void TaskGetData( void *pvParameters );
void TaskSDWrite( void *pvParameters );
//------------------------------------------------------------------------------

//Hardware Timer
hw_timer_t * timer = NULL;  //create timer handler

//Declare Queue data type for FreeRTOS
QueueHandle_t DataQueue; // 

//ISR tools
//Create Interrupt Semaphores
SemaphoreHandle_t timerSemaphore; 
SemaphoreHandle_t ButtonSemaphore;
SemaphoreHandle_t CountSemaphore; 
int Count = 0; 
int G = 0;
int H = 0; 
int F = 0;

/////////////////////////////////////////////////////////////////////////////////////
void IRAM_ATTR vTimerISR()  //Timer ISR 
  {
  xSemaphoreGiveFromISR(timerSemaphore, NULL);
  }
//------------------------------------------------------------------------------
void IRAM_ATTR ButtonISR() 
  {
  xSemaphoreGiveFromISR(ButtonSemaphore, NULL); //Gives permission from button interrupt
  }
//------------------------------------------------------------------------------
void TaskGetData(void *pvParameters)  // This is a task.
{
    (void) pvParameters;

  for (;;) // A Task shall never return or exit.
  {
    if (xSemaphoreTake(timerSemaphore, portMAX_DELAY ) == pdTRUE )
    {
    sensors_event_t event;
    lis.getEvent(&event);
    sensors_event_t event2;
    lis2.getEvent(&event2);
    TX_Data_t.usec = micros();
    TX_Data_t.value1X = event.acceleration.x;
    TX_Data_t.value1Y = event.acceleration.y;
    TX_Data_t.value1Z = event.acceleration.z;
    TX_Data_t.value2X = event2.acceleration.x;
    TX_Data_t.value2Y = event2.acceleration.y;
    TX_Data_t.value2Z = event2.acceleration.z;
    if(xQueueSend( DataQueue, ( void * ) &TX_Data_t, portMAX_DELAY ) != pdPASS )  //portMAX_DELAY
      {
        Serial.println("xQueueSend is not working"); 
      }
    }
  }
  vTaskDelete( NULL );
}
//------------------------------------------------------------------------------
void TaskSDWrite(void *pvParameters)  // This is a task.
{
  (void) pvParameters;
  
  //struct Data_t *RCV_Data; 
  
  for (;;)
  {

      if( xQueueReceive( DataQueue, &( RX_Data_t ), portMAX_DELAY ) != pdPASS )   //portMAX_DELAY
      {
        Serial.println("xQueueRecieve is not working");
      }
      logfile.print(RX_Data_t.usec);
      logfile.print(',');
      logfile.print(RX_Data_t.value1X,4);
      logfile.print(',');
      logfile.print(RX_Data_t.value1Y,4);
      logfile.print(',');
      logfile.print(RX_Data_t.value1Z,4);
      logfile.print(',');
      logfile.print(RX_Data_t.value2X,4);
      logfile.print(',');
      logfile.print(RX_Data_t.value2Y,4);
      logfile.print(',');
      logfile.print(RX_Data_t.value2Z,4);
      logfile.println(); 
      Count++;
      //Serial.println(Count); 
      if ( Count == TotalCount )
        {
          logfile.close(); 
          vTaskDelay ( 8000 / portTICK_PERIOD_MS ); 
          Serial.println("All done here"); 
          vTaskDelay( 20000000 / portTICK_PERIOD_MS );
          //vTaskSuspendAll(); 
        }

      //uint16_t FreeSpace = uxQueueSpacesAvailable( DataQueue ); 
      //Serial.println(FreeSpace);
      }
   vTaskDelete( NULL ); 
}

//------------------------------------------------------------------------------
void TaskLed(void *pvParameters)
{
  (void) pvParameters;

  for (;;) 
    {
    // Take the semaphore.
    if( xSemaphoreTake(CountSemaphore, portMAX_DELAY) == pdPASS )
        {
        //vTaskSuspend( (void *) &TaskGetData );
        //vTaskSuspend( (void *) &TaskSDWrite );
        //Serial.println("Recieved count semaphore"); 
        //Serial.println("All done here");
        //vTaskDelay( 20000000 / portTICK_PERIOD_MS );
        //vTaskSuspendAll(); 
        }  
       
    if (xSemaphoreTake(ButtonSemaphore, portMAX_DELAY) == pdPASS) {
    digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
    }
   }
}

Before adding more code, time to hunt down the WDT error source.

In stall, into the Arduino IDE a ESP exception decoder GitHub - me-no-dev/EspExceptionDecoder: Exception Stack Trace Decoder for ESP8266 and ESP32

The fine people at ESPRESSIF wrote a add on to freeRTOS called RingBuffer.

See FreeRTOS Additions FreeRTOS Additions - ESP32 - — ESP-IDF Programming Guide latest documentation, if I was sending data to a SD card, I'd use a ring buffer. I use a ring buffer as the go between of several MyoWear sensors and the display. Wonderful stream API.

Please post your revised code in a new message. That way its easier for me to keep up with the changes.


Here is what I get from the backtrace you posted

0x4008c2e0: xEventGroupSetBits at /home/runner/work/esp32-arduino-lib-builder/esp32-arduino-lib-builder/esp-idf/components/freertos/event_groups.c line 560
0x4008c50d: prvInitialiseNewTask at /home/runner/work/esp32-arduino-lib-builder/esp32-arduino-lib-builder/esp-idf/components/freertos/tasks.c line 1009
0x400d7f53: psramInit at C:\Users\KESOURLA\Documents\ArduinoData\packages\esp32\hardware\esp32\1.0.4\cores\esp32\esp32-hal-psram.c line 20
0x40084e69: psram_cmd_recv_start at /home/runner/work/esp32-arduino-lib-builder/esp32-arduino-lib-builder/esp-idf/components/esp32/spiram_psram.c line 274
0x400d4ef6: std::function ::operator=(std::function const&) at c:\users\kesourla\documents\arduinodata\packages\esp32\tools\xtensa-esp32-elf-gcc\1.22.0-80-g6c4433a-5.2.0\xtensa-esp32-elf\include\c++\5.2.0/functional line 2075
0x400d1546: fDoTheHumidityThing(void*) at C:\Users\KESOURLA\Documents\Arduino\ESP32_Chappie_MQTT_Home_Controller/ESP32_Chappie_MQTT_Home_Controller.ino line 276
0x400d297a: MQTTkeepalive(void*) at C:\Users\KESOURLA\Documents\Arduino\ESP32_Chappie_MQTT_Home_Controller/ESP32_Chappie_MQTT_Home_Controller.ino line 459
0x400d2deb: WiFiClient::write(unsigned char const*, unsigned int) at C:\Users\KESOURLA\Documents\ArduinoData\packages\esp32\hardware\esp32\1.0.4\libraries\WiFi\src\WiFiClient.cpp line 374
0x400d2509: fUpdateDisplay(void*) at C:\Users\KESOURLA\Documents\Arduino\ESP32_Chappie_MQTT_Home_Controller/ESP32_Chappie_MQTT_Home_Controller.ino line 356
0x400d1cd1: fDoTheSunLampThing(void*) at C:\Users\KESOURLA\Documents\Arduino\ESP32_Chappie_MQTT_Home_Controller/ESP32_Chappie_MQTT_Home_Controller.ino line 225
0x400d24ba: fUpdateDisplay(void*) at C:\Users\KESOURLA\Documents\Arduino\libraries\Adafruit_GFX_Library/Adafruit_GFX.h line 129
0x400d5715: Adafruit_BME280::init() at C:\Users\KESOURLA\Documents\Arduino\libraries\Adafruit_BME280_Library\Adafruit_BME280.cpp line 105
0x400d58c4: Adafruit_BME280::readPressure() at C:\Users\KESOURLA\Documents\Arduino\libraries\Adafruit_BME280_Library\Adafruit_BME280.cpp line 444
0x400d5953: Adafruit_BME280::readPressure() at C:\Users\KESOURLA\Documents\Arduino\libraries\Adafruit_BME280_Library\Adafruit_BME280.cpp line 459

You'll want to put your backtrace into the exception decoder on your computer.

Here is a task

void DoTheBME280Thing( void *pvParameters )
{
  for (;;)
  {
    xEventGroupWaitBits (eg, evtDoTheBME280Thing, pdTRUE, pdTRUE, portMAX_DELAY ); //
    // log_i( "Signal strength %d", WiFi.RSSI() );
    vTaskDelay( 2 );
    if ( !isnan(bme.readTemperature()) )
    {
      ePtr[0] = bme.readTemperature();
      ePtr[1] = bme.readPressure() / 133.3223684f; // mmHg
      ePtr[2] = bme.readHumidity();
      if ( MQTTclient.connected() ) // broker online, then send stuff
      {
        xSemaphoreTake( sema_MQTT_KeepAlive, portMAX_DELAY ); // do not send whiles keep alive is in progress and pause keep-alive during send
        MQTTclient.publish( topicInsideTemp, String(ePtr[0]).c_str() );
        vTaskDelay( 2 ); // gives the Raspberry Pi 4 time to receive the message and process
        MQTTclient.publish( topicInsideHumidity, String(ePtr[1]).c_str() );
        vTaskDelay( 2 ); // no delay and RPi is still processing previous message
        MQTTclient.publish( topicInsidePressure, String(ePtr[2]).c_str() );
        xSemaphoreGive( sema_MQTT_KeepAlive );
      }
    }
    //log_i( "DoTheBME280Thing high watermark %d",  uxTaskGetStackHighWaterMark( NULL ) );
  }
  vTaskDelete ( NULL );
}

Note the line //log_i( "DoTheBME280Thing high watermark %d", uxTaskGetStackHighWaterMark( NULL ) );, A good troubleshooting method to hunt down memory issues is to cause the last task initialized to print out its high water mark for memory, Let the task run for an hour or so and note the high water mark memory used. Does it exceed your assigned task space?

If the last task looks good, then move up one task and monitor its water mark, keep moving up the ladder. A task is in a good stack space with about 2K unused.

Watch those Adafruit libraries, Adafruit got some good programmers who do not write for the ESP32's memory map scheme. With the core dump info, back trace info, and decode info, Adafruit will assign someone to fix the issue. You may want to look at other libraries not made by Adafruit.

Frankly, I2C is not fast and SPI is ok when using the Arduino Wire or SPI libraries. I found that writing to the ESP32 SPI API library provides a significantly improved data rate spee. At this point, for me, it is easy to take a library and convert it to using the ESP32 SPI API.

here is a cpp of code using the ESP32 SPI API:

#include "ESP32_SPI_API.h"
/////////////////////////////
///////////////////////////
uint8_t txData[2] = { };
uint8_t rxData[25] = { };
uint8_t low;
int8_t high;
//////
//////////////////////////////////
uint8_t GetLowBits()
{
  return low;
}
int8_t GetHighBits()
{
  return high;
}
////////////////////////////////////////
int fInitializeSPI_Channel( int spiCLK, int spiMOSI, int spiMISO, spi_host_device_t SPI_Host, bool EnableDMA)
{
  esp_err_t intError;
  spi_bus_config_t bus_config = { };
  bus_config.sclk_io_num = spiCLK; // CLK
  bus_config.mosi_io_num = spiMOSI; // MOSI
  bus_config.miso_io_num = spiMISO; // MISO
  bus_config.quadwp_io_num = -1; // Not used
  bus_config.quadhd_io_num = -1; // Not used
  intError = spi_bus_initialize( HSPI_HOST, &bus_config, EnableDMA) ;
  return intError;
}
//////
int fInitializeSPI_Devices( spi_device_handle_t &h, int csPin)
{
  esp_err_t intError;
  spi_device_interface_config_t dev_config = { };  // initializes all field to 0
  dev_config.address_bits     = 0;
  dev_config.command_bits     = 0;
  dev_config.dummy_bits       = 0;
  dev_config.mode             = 3 ;
  dev_config.duty_cycle_pos   = 0;
  dev_config.cs_ena_posttrans = 0;
  dev_config.cs_ena_pretrans  = 0;
  dev_config.clock_speed_hz   = 5000000;
  dev_config.spics_io_num     = csPin;
  dev_config.flags            = 0;
  dev_config.queue_size       = 1;
  dev_config.pre_cb           = NULL;
  dev_config.post_cb          = NULL;
  spi_bus_add_device(HSPI_HOST, &dev_config, &h);
  // return intError;
  // return h;
} // void fInitializeSPI_Devices()
///////////////////////////////////////////////////////////////
int fReadSPIdata16bits( spi_device_handle_t &h, int _address )
{
  uint8_t address = _address;
    esp_err_t intError = 0;
    low=0; high=0;
    spi_transaction_t trans_desc;
    trans_desc = { };
    trans_desc.addr =  0;
    trans_desc.cmd = 0;
    trans_desc.flags = 0;
    trans_desc.length = (8 * 3); // total data bits
    trans_desc.tx_buffer = txData;
    trans_desc.rxlength = 8 * 2 ; // Number of bits NOT number of bytes
    trans_desc.rx_buffer = rxData;
    txData[0] = address | 0x80;
    intError = spi_device_transmit( h, &trans_desc);
    low = rxData[0]; high = rxData[1];
  //  if ( intError != 0 )
  //  {
  //    Serial.print( " WHO I am LSM9DS1. Transmitting error = ");
  //    Serial.println ( esp_err_to_name(intError) );
  //  }
  return intError;
} // void fSendSPI( uint8_t count, uint8_t address, uint8_t DataToSend)
////
int fWriteSPIdata8bits( spi_device_handle_t &h, int _address, int _sendData )
{
  uint8_t address =  _address;
  uint8_t sendData = _sendData;
  esp_err_t intError;
  spi_transaction_t trans_desc;
  trans_desc = { };
  trans_desc.addr =  0;
  trans_desc.cmd = 0;
  trans_desc.flags = 0;
  trans_desc.length = (8 * 2); // total data bits
  trans_desc.tx_buffer = txData;
  trans_desc.rxlength = 0 ; // Number of bits NOT number of bytes
  trans_desc.rx_buffer = NULL;
  txData[0] = address  & 0x7F;
  txData[1] = sendData;
  intError = spi_device_transmit( h, &trans_desc);
  return intError;
//  //  if ( intError != 0 )
//  //  {
//  //    Serial.print( " LSM9DS1_REGISTER_CTRL_REG6_XL. Transmitting error = ");
//  //    Serial.println ( esp_err_to_name(intError) );
//  //  }
} // void fWriteSPIdata8bits(  spi_device_handle_t &h, uint8_t address, uint8_t sendData )
//

here is the h file

#include <driver/spi_master.h>
#include "sdkconfig.h"
#include "esp_system.h" //This inclusion configures the peripherals in the ESP system.
////////////////////////////////////
//
//#define MAGTYPE  true
//#define XGTYPE   false
//////////////////////////
///////////////////////////
//
////////////////////////////
 uint8_t GetLowBits();
 int8_t GetHighBits();
 int fReadSPIdata16bits( spi_device_handle_t &h, int address );
 int fWriteSPIdata8bits( spi_device_handle_t &h, int address, int sendData );
 int fInitializeSPI_Devices( spi_device_handle_t &h, int csPin);
// spi_device_handle_t fInitializeSPI_Devices( int csPin);
int fInitializeSPI_Channel( int spiCLK, int spiMOSI, int spiMISO, spi_host_device_t SPI_Host, bool EnableDMA);

I wanted to give an update since I've finally achieved my goal. I'm now able to record up to and slightly past 1000 Hz. reliably and consistently. As suspected my biggest issue (weakest link) was still SD writing. I switched to an ESP32 dev board with Pin 2 pinned out for use. This allowed me to use the ESP32 SDMMC drivers which seem to be much more reliable and take advantage of the board (I'm guessing HSPI). Thank you Idahowalker for pushing me down this road. My current program is using One bit mode, it would be interesting to go to 4 bit mode but my board/program has issues with this, not sure why, but reading online it has plagued others as well. Either way, One bit works so "if it ain't broke, don't fix it." Once this was figured out the queue runs very smoothly, so sticking with it for now as well. I still assume a ring buffer would be more efficient, but not sure it would really get me anything if queue is working correct? Referring to Idahowalkers SPI commands, i'm sure that would help the program too. I'm going to stick with this version and i'll attach the code just for reference in case others find this post. Just for future reference, i wonder what would make this program capable of 2000 Hz. (probably won't ever need to go there)

  • Ring buffer?
  • Write binary data?
  • Using ESP API for SPI?
  • Using ESP idf timer vs Arduino hardware timer? Any difference of accuracy?
  • 4 bit mode SDMMC mode?
    Once again thank you to everyone who contributed!!!!!!
// Data logger:  For measuring acceleration from LIS3DH/s
// Hardware:  ESP32 dev Board Kit C, Micro SD reader, 2x LIS3DH accels, 1x LED, 1x push button
// Created:  May 12 2020
// Updated:  July 6 2020
// Uses SPI for Accels, SDMMC for SD
// Record reliably up to 1000 Hz.  
// files are saved in .CSV or .TXT format, file name scheme is = DATANN.CSV or .TXT
// See ReadME and photos for additional hook up info
// You cannot compile and upload with SD card in slot!

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
const int SampleRate = 1000; //Hz, Set sample rate here                    +
const int SampleLength = 10; //Seconds, How long to record                +
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

//Use ESP32 duo core
const int TaskCore1  = 1;
const int TaskCore0 = 0;
int SampleInt = 1000000 / SampleRate; 
int TotalCount = SampleLength * SampleRate;

//Libraries
#include "FS.h"
#include "SD_MMC.h"
#include <SPI.h>
#include <Adafruit_LIS3DH.h>
#include <Adafruit_Sensor.h>
#include "stdio.h"
#include "esp_system.h" //This inclusion configures the peripherals in the ESP system.
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/timers.h"
#include "freertos/event_groups.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"

#define ONE_BIT_MODE true //true = 1 bit mode, 4 bit mode is faster (false) but never got to work

//------------------------------------------------------------------------------
//File callout for SDMMC
File logfile;
//------------------------------------------------------------------------------
// data type for Queue item
struct Data_t {
  uint32_t usec; 
  float value1X;
  float value1Y;
  float value1Z;
  float value2X;
  float value2Y;
  float value2Z;
} TX_Data_t, RX_Data_t;

UBaseType_t uxHighWaterMark;  // Use for debug and reviewing stack height limits

//------------------------------------------------------------------------------
// Accel Lis3dh definitions, SPI 
// Connect the SD card to the following pins:
// SD Card | ESP32, DS->12 D3->13 CMD->15 VSS->GND VDD->3.3V CLK->14 VSS->GND D0->2 D1->4
// Power to Vin, Gnd to Gnd, SCL->SCK, SDA->MISO, SDO->MOSI, CS->CS 
//SPI3 / VSPI / CS = 5 / SCK = 18 / MISO/SDA = 19 / MOSI/SDO = 23
// Used for SPI3/VSPI
#define LIS3DH_CLK 18
#define LIS3DH_MISO 19
#define LIS3DH_MOSI 23
#define LIS3DH2_CLK 18
#define LIS3DH2_MISO 19
#define LIS3DH2_MOSI 23
// Used for hardware & software SPI
#define LIS3DH_CS 5    //ESP32: Use for upper accel (Sensor 1!!!) = hbar, seatpost, etc.
#define LIS3DH2_CS 17  //ESP32: Use for lower accel (Sensor 2!!!) = axles, etc. 

// Selct Pins
Adafruit_LIS3DH lis = Adafruit_LIS3DH(LIS3DH_CS, LIS3DH_MOSI, LIS3DH_MISO, LIS3DH_CLK);
Adafruit_LIS3DH lis2 = Adafruit_LIS3DH(LIS3DH2_CS, LIS3DH2_MOSI, LIS3DH2_MISO, LIS3DH2_CLK);

//------------------------------------------------------------------------------
// define tasks for Sensor Data and SD Write
void TaskLed( void *pvParamaters );
void TaskGetData( void *pvParameters );
void TaskSDWrite( void *pvParameters );
TaskHandle_t xGetData;
TaskHandle_t xSDWrite;
TaskHandle_t xLed; 
//------------------------------------------------------------------------------

//Hardware Timer
hw_timer_t * timer = NULL;  //create timer handler

//Declare Queue data type for FreeRTOS
QueueHandle_t DataQueue; // 
//RingbufHandle_t buf_handle;

//ISR tools
//Create Interrupt Semaphores
SemaphoreHandle_t timerSemaphore; 
SemaphoreHandle_t ButtonSemaphore;
int Count = 0; 
int LED = 32; 
int BUTTON = 34; 

//------------------------------------------------------------------------------
void IRAM_ATTR vTimerISR()  //Timer ISR 
  {
  xSemaphoreGiveFromISR(timerSemaphore, NULL);
  }
//------------------------------------------------------------------------------
void IRAM_ATTR ButtonISR() //Gives permission from button interrupt
  {
  xSemaphoreGiveFromISR(ButtonSemaphore, NULL); 
  }
//------------------------------------------------------------------------------
void TaskGetData(void *pvParameters)  // Get Data from Sensors Task
{
    (void) pvParameters;

  for (;;) // A Task shall never return or exit.
  {
    if (xSemaphoreTake(timerSemaphore, portMAX_DELAY ) == pdTRUE )
    {
    sensors_event_t event;
    lis.getEvent(&event);
    sensors_event_t event2;
    lis2.getEvent(&event2);
    TX_Data_t.usec = micros();
    TX_Data_t.value1X = event.acceleration.x;
    TX_Data_t.value1Y = event.acceleration.y;
    TX_Data_t.value1Z = event.acceleration.z;
    TX_Data_t.value2X = event2.acceleration.x;
    TX_Data_t.value2Y = event2.acceleration.y;
    TX_Data_t.value2Z = event2.acceleration.z;
    //uxHighWaterMark = uxTaskGetStackHighWaterMark( NULL );
    //Serial.println(uxHighWaterMark);
    if(xQueueSend( DataQueue, ( void * ) &TX_Data_t, portMAX_DELAY ) != pdPASS )  //portMAX_DELAY
      {
        Serial.println("xQueueSend is not working"); 
      }
    }
  }
  vTaskDelete( NULL );
}
//------------------------------------------------------------------------------
void TaskSDWrite(void *pvParameters)  // Write Data to SD card
{
  (void) pvParameters;
  
  for (;;)
  {

      if( xQueueReceive( DataQueue, &( RX_Data_t ), portMAX_DELAY ) != pdPASS )   //portMAX_DELAY
      {
        Serial.println("xQueueRecieve is not working");
      }
      logfile.print(RX_Data_t.usec);
      logfile.print(',');
      logfile.print(RX_Data_t.value1X,5);
      logfile.print(',');
      logfile.print(RX_Data_t.value1Y,5);
      logfile.print(',');
      logfile.print(RX_Data_t.value1Z,5);
      logfile.print(',');
      logfile.print(RX_Data_t.value2X,5);
      logfile.print(',');
      logfile.print(RX_Data_t.value2Y,5);
      logfile.print(',');
      logfile.print(RX_Data_t.value2Z,5);
      logfile.println(); 
      //uxHighWaterMark = uxTaskGetStackHighWaterMark( NULL );
      //Serial.println(uxHighWaterMark);
      Count++;
      //Serial.println(Count); 
      if ( Count == TotalCount )
        {
          logfile.close(); 
          
          vTaskDelay ( 8000 / portTICK_PERIOD_MS ); // Adding delay just to give SD card time close but may not be needed
          Serial.println("Data Saved to SD Card and Closed"); 
          digitalWrite(LED, LOW); 
          vTaskSuspend( xGetData );
          vTaskSuspend( NULL );
        }

      //uint16_t FreeSpace = uxQueueSpacesAvailable( DataQueue ); //Use if need to evaluate SD write and queue availability
      //Serial.println(FreeSpace);
      }
   vTaskDelete( NULL ); 
}

//------------------------------------------------------------------------------
void TaskLed(void *pvParameters)  //Always on Task, start task and Led notifications
{
  (void) pvParameters;

  // Start off Suspended, no recording, only runs once here
  vTaskSuspend( xGetData );
  vTaskSuspend( xSDWrite );

  for (;;) 
    {
     if (xSemaphoreTake(ButtonSemaphore, portMAX_DELAY) == pdPASS) 
      {
      digitalWrite(LED, !digitalRead(LED));
      Serial.println("Button Pressed, Writing"); 
      vTaskResume( xGetData );
      vTaskResume( xSDWrite ); 
      vTaskDelay( 10000 / portTICK_PERIOD_MS );
      } 
    }
}
//===================================================================================================================
//===================================================================================================================
// the setup function runs once when you press reset or power the board
void setup() {

  // initialize serial communication at 115200 bits per second:
  Serial.begin(115200);

  // For SDMMC hookup need to pull certain channels high during startup
  pinMode(2, INPUT_PULLUP);
  pinMode(4, INPUT_PULLUP);
  pinMode(12, INPUT_PULLUP);
  pinMode(13, INPUT_PULLUP);
  pinMode(15, INPUT_PULLUP);

  //Queue Setup
  DataQueue = xQueueCreate(10, sizeof( Data_t ));
  if(DataQueue == NULL){
     Serial.println("Error Creating the Queue");
   }

  /*buf_handle = xRingbufferCreate(1028, RINGBUF_TYPE_NOSPLIT);
  if (buf_handle == NULL){
        printf("Failed to create ring buffer\n");
    }*/

  //============================================================================================================
  //Outputs, Pins, Buttons, Etc. 
  pinMode(LED, OUTPUT);  // Turn on LED for notification
  pinMode(BUTTON, INPUT); //button to turn recording on/off, Pulled HIGH to begin
  digitalWrite(LED, LOW); // Turn LED on

  ButtonSemaphore = xSemaphoreCreateBinary();  //Create button Interrupt Semaphore
  if (ButtonSemaphore != NULL) {
    // Attach interrupt for Arduino digital pin
    attachInterrupt(digitalPinToInterrupt(BUTTON), ButtonISR, FALLING);
  }

  timerSemaphore = xSemaphoreCreateBinary();  // Create semaphore to inform us when the timer has fired

  //ACCEL Setup and RUN
  if (! lis.begin(0x18)) {   // Sensor 1, change this to 0x19 for alternative i2c address
  Serial.println("Couldnt start");
  while (1) yield();
  }
  Serial.println("LIS3DH Sensor 1 found!");

  if (! lis2.begin(0x19)) {   // Sensor 2
  Serial.println("Couldnt start Sensor 2");
  while (1) yield();
  }
  Serial.println("LIS3DH Sensor 2 found!");
  Serial.print("Sample Interval time (uS):"); Serial.println(SampleInt); 
  Serial.print("Total # of samples:"); Serial.println(TotalCount); 
  
  // Set accel range  2, 4, 8 or 16 G!
  lis.setRange(LIS3DH_RANGE_16_G);   
  lis2.setRange(LIS3DH_RANGE_16_G);
  // Set DataRate  OPTIONS:  LIS3DH_DATARATE_400_HZ, LIS3DH_DATARATE_LOWPOWER_1K6HZ, LIS3DH_DATARATE_LOWPOWER_5KHZ
  lis.setDataRate(LIS3DH_DATARATE_LOWPOWER_5KHZ); 
  lis2.setDataRate(LIS3DH_DATARATE_LOWPOWER_5KHZ); 

  // SD CARD SETUP ====================================================================
  // see if the card is present and can be initialized:  
  if (!SD_MMC.begin("/sdcard", ONE_BIT_MODE)) {
    Serial.println("Card init. failed!");
    while (1) yield(); 
  }

  // Create filename scheme ====================================================================
  char filename[15];  //  Setup filename 
  strcpy(filename, "/DATA00.TXT");
  for (uint8_t i = 0; i < 100; i++) {
    filename[5] = '0' + i/10;
    filename[6] = '0' + i%10;
    // create if does not exist, do not open existing, write, sync after write
    if (! SD_MMC.exists(filename)) {
      break;
    }
  }

  // Create file and prepare it ============================================================
  logfile = SD_MMC.open(filename, FILE_WRITE); 
  if( ! logfile ) {
    Serial.print("Couldnt create "); 
    Serial.println(filename);
    while (1) yield(); 
  }
  Serial.print("Ready to write to:"); Serial.println(filename);

  //Column labels, change if not doing full 6 channel output
  logfile.print("Time uS"); 
  logfile.print(",");
  logfile.print("Sensor 1 X");
  logfile.print(",");
  logfile.print("Sensor 1 Y");
  logfile.print(",");
  logfile.print("Sensor 1 Z");
  logfile.print(",");
  logfile.print("Sensor 2 X");
  logfile.print(",");
  logfile.print("Sensor 2 Y");
  logfile.print(",");
  logfile.print("Sensor 2 Z");
  logfile.println();

  // Setup up Tasks and where to run ============================================================  
  // Now set up tasks to run independently.
  xTaskCreatePinnedToCore(
    TaskGetData
    ,  "Grab Accel Data"   // A name just for humans
    ,  15000 // This stack size can be checked & adjusted by reading the Stack Highwater
    ,  NULL
    ,  3  // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
    ,  &xGetData
    ,  TaskCore1);

  xTaskCreatePinnedToCore(
    TaskSDWrite
    ,  "SD Write"
    ,  15000 // Stack size
    ,  NULL
    ,  3 // Priority
    ,  &xSDWrite
    ,  TaskCore0);

    xTaskCreatePinnedToCore(
    TaskLed
    ,  "LED" // Use for LED and button interrupts
    ,  10000 // Stack size
    ,  NULL
    ,  1  // Priority
    ,  &xLed
    ,  TaskCore1);  
  
  // Create Timer ===============================================================================
  timer = timerBegin(1, 80, true);    // Set 80 divider for prescaler (see ESP32 Technical Reference Manual for more info).
  timerAttachInterrupt(timer, &vTimerISR, true);  // Attach &vTimerISR to our timer.
  timerAlarmWrite(timer, SampleInt, true);  // Repeat the alarm (third parameter)
  timerAlarmEnable(timer);  // Start an alarm

  // LED notification system is ready
  int i = 0; 
  for (i=0; i <=7; i++){
    digitalWrite(LED, HIGH);
    delay(100);
    digitalWrite(LED, LOW);
    delay(100); 
    }

}

//================================================================================================================================
void loop()
{
  // Empty. Things are done in Tasks.
/*--------------------------------------------------*/
/*---------------------- Tasks ---------------------*/
/*--------------------------------------------------*/
}

//================================================================================================================================
//================================================================================================================================