Esp32 crashing when trying to open file in FreeRTOS task

[ar3dhel1]

My code is relatively long, and I thought posting a targeted section would improve readability. Apologies if I was incorrect in that, but there's no need to be rude. Here is the entire code. It's not the tidiest I fear, I usually clean up my code properly once I've mostly finished with it.

#include <Arduino.h>

#include "nvs_flash.h"

#include "esp_mac.h"
#include "esp_log.h"
#include "esp_wifi.h"
#include "esp_netif.h"
#include "esp_now.h"
#include "esp_camera.h"
#include "FS.h"
#include "SD.h"
#include "SPI.h"

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <RTClib.h>
#include <Wire.h>

RTC_DS1307 rtc;

Adafruit_MPU6050 imu;

// CAMERA PINS

#define PWDN_GPIO_NUM     -1
#define RESET_GPIO_NUM    -1
#define XCLK_GPIO_NUM     10
#define SIOD_GPIO_NUM     40
#define SIOC_GPIO_NUM     39

#define Y9_GPIO_NUM       48
#define Y8_GPIO_NUM       11
#define Y7_GPIO_NUM       12
#define Y6_GPIO_NUM       14
#define Y5_GPIO_NUM       16
#define Y4_GPIO_NUM       18
#define Y3_GPIO_NUM       17
#define Y2_GPIO_NUM       15
#define VSYNC_GPIO_NUM    38
#define HREF_GPIO_NUM     47
#define PCLK_GPIO_NUM     13

#define LED_GPIO_NUM      21

#define CONFIG_LESS_INTERFERENCE_CHANNEL    11
#define CONFIG_SEND_FREQUENCY               40           // in hz

#define IMU_SIZE                            61
#define TIME_SIZE                           24
#define FILENAME_SIZE                       26

// mac addresses
static const uint8_t MAC_ADDR_SEND[] = {0x1a, 0x00, 0x00, 0x00, 0x00, 0x00};
static const uint8_t MAC_ADDR_RECV[] = {0x1a, 0x01, 0x00, 0x00, 0x00, 0x00};
static const uint8_t MAC_ADDR_BROAD[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};

uint8_t pkt_count = 0;

// define tasks
void task_write_all(void *pvParameters);
void task_save_imu(void *pvParameters);
void task_send_csi(void *pvParameters);

static portMUX_TYPE lock = portMUX_INITIALIZER_UNLOCKED;

// global variables
int imu_idx = 0;
int imu_switch = 0;
int imu_idx_last = 0;

char imu_data0[50][IMU_SIZE+TIME_SIZE];
char imu_data1[50][IMU_SIZE+TIME_SIZE];

unsigned long new_time = 0;
unsigned long old_time = 0;

bool wifi_good;
bool rtc_good;
bool sd_good;
bool cam_good;
bool imu_good;

bool report_esp_error(esp_err_t err)
{
  if(err == ESP_OK)
  {
    return false;
  }
  else
  {
    Serial.printf("Error: %s\n", esp_err_to_name(err));
    return true;
  }
}

bool imu_setup()
{
  if (!imu.begin(0x69)) 
  {
    Serial.println("Failed to find IMU");
    return false;
  }
  Serial.println("IMU Found!");

  imu.setAccelerometerRange(MPU6050_RANGE_2_G);
  Serial.println("Accelerometer range set to: +- 2G");
  imu.setGyroRange(MPU6050_RANGE_250_DEG);
  Serial.println("Gyro range set to: +- 250 deg/s");
  imu.setFilterBandwidth(MPU6050_BAND_44_HZ);
  Serial.println("Filter bandwidth set to: 44 Hz");

  return true;
}

bool rtc_setup()
{
  if (!rtc.begin()) 
  {
    Serial.println("Failed to find RTC");
    return false;
  }
  Serial.println("RTC Found!");

  rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));
  old_time = millis();
  return true;
}

bool cam_setup()
{
  camera_config_t config;
  config.ledc_channel = LEDC_CHANNEL_0;
  config.ledc_timer = LEDC_TIMER_0;
  config.pin_d0 = Y2_GPIO_NUM;
  config.pin_d1 = Y3_GPIO_NUM;
  config.pin_d2 = Y4_GPIO_NUM;
  config.pin_d3 = Y5_GPIO_NUM;
  config.pin_d4 = Y6_GPIO_NUM;
  config.pin_d5 = Y7_GPIO_NUM;
  config.pin_d6 = Y8_GPIO_NUM;
  config.pin_d7 = Y9_GPIO_NUM;
  config.pin_xclk = XCLK_GPIO_NUM;
  config.pin_pclk = PCLK_GPIO_NUM;
  config.pin_vsync = VSYNC_GPIO_NUM;
  config.pin_href = HREF_GPIO_NUM;
  config.pin_sccb_sda = SIOD_GPIO_NUM;
  config.pin_sccb_scl = SIOC_GPIO_NUM;
  config.pin_pwdn = PWDN_GPIO_NUM;
  config.pin_reset = RESET_GPIO_NUM;
  config.xclk_freq_hz = 20000000;
  config.frame_size = FRAMESIZE_UXGA;
  config.pixel_format = PIXFORMAT_JPEG; // for streaming
  config.grab_mode = CAMERA_GRAB_WHEN_EMPTY;
  config.fb_location = CAMERA_FB_IN_PSRAM;
  config.jpeg_quality = 12;
  config.fb_count = 1;
  
  // if PSRAM IC present, init with UXGA resolution and higher JPEG quality
  //                      for larger pre-allocated frame buffer.
  if(config.pixel_format == PIXFORMAT_JPEG)
  {
    if(psramFound())
    {
      config.jpeg_quality = 10;
      config.fb_count = 2;
      config.grab_mode = CAMERA_GRAB_LATEST;
    } else 
    {
      // Limit the frame size when PSRAM is not available
      config.frame_size = FRAMESIZE_SVGA;
      config.fb_location = CAMERA_FB_IN_DRAM;
    }
  } 
  else 
  {
    // Best option for face detection/recognition
    config.frame_size = FRAMESIZE_240X240;
#if CONFIG_IDF_TARGET_ESP32S3
    config.fb_count = 2;
#endif
  }

  // camera init
  if (report_esp_error(esp_camera_init(&config))) return false;
  /*esp_err_t err = esp_camera_init(&config);
  if (err != ESP_OK) 
  {
    Serial.printf("Camera init failed with error 0x%x", err);
    return false;
  }*/
  
  return true;
}

bool sd_setup()
{
  // Initialize SD card
  if(!SD.begin(21))
  {
    Serial.println("Card Mount Failed");
    return false;
  }
  uint8_t cardType = SD.cardType();

  // Determine if the type of SD card is available
  if(cardType == CARD_NONE)
  {
    Serial.println("No SD card attached");
    return false;
  }

  Serial.print("SD Card Type: ");
  if(cardType == CARD_MMC)
  {
    Serial.println("MMC");
  } else if(cardType == CARD_SD){
    Serial.println("SDSC");
  } else if(cardType == CARD_SDHC){
    Serial.println("SDHC");
  } else {
    Serial.println("UNKNOWN");
  }

  return true;
}

bool wifi_setup()
{
  // NVS SETUP //
  // try to initialise the default NVS partition
  esp_err_t errcode = nvs_flash_init();

  // if necessary, clear the flash and re-initialise
  if (errcode == ESP_ERR_NVS_NO_FREE_PAGES || errcode == ESP_ERR_NVS_NEW_VERSION_FOUND) 
  {
    report_esp_error(nvs_flash_erase());
    errcode = nvs_flash_init();
  }

  // if there is still something wrong, report and abort
  if (report_esp_error(errcode)) return false;

  // WIFI SETUP //
  // setup the default event loop, for handling WiFi events
  if (report_esp_error(esp_event_loop_create_default())) return false;

  // initialise the TCP/IP stack
  if (report_esp_error(esp_netif_init())) return false;

  // initialise the WiFi configuration to default config values
  wifi_init_config_t config = WIFI_INIT_CONFIG_DEFAULT();

  // initialise WiFi resources, and start the WiFi task
  if (report_esp_error(esp_wifi_init(&config))) return false;

  // set the WiFi to run in station mode
  if (report_esp_error(esp_wifi_set_mode(WIFI_MODE_STA))) return false;

  // set the WiFi configuration storage as RAM
  if (report_esp_error(esp_wifi_set_storage(WIFI_STORAGE_RAM))) return false;

  // set the bandwidth to HT40
  if (report_esp_error(esp_wifi_set_bandwidth(WIFI_IF_STA, WIFI_BW_HT40))) return false;

  // start WiFi, following current settings
  if (report_esp_error(esp_wifi_start())) return false;

  // set the ESP-NOW rate
  if (report_esp_error(esp_wifi_config_espnow_rate(WIFI_IF_STA, WIFI_PHY_RATE_MCS0_SGI))) return false;

  // set power saving type to none
  if (report_esp_error(esp_wifi_set_ps(WIFI_PS_NONE))) return false;

  // set the primary and secondary channels
  if (report_esp_error(esp_wifi_set_channel(CONFIG_LESS_INTERFERENCE_CHANNEL, WIFI_SECOND_CHAN_BELOW))) return false;

  // set the MAC address
  if (report_esp_error(esp_wifi_set_mac(WIFI_IF_STA, MAC_ADDR_SEND))) return false;

  // initialise ESP-NOW
  if (report_esp_error(esp_now_init())) return false;

  // set the ESP-NOW primary master key
  if (report_esp_error(esp_now_set_pmk((uint8_t *)"pmk1234567890123"))) return false;

  // define the receiver as an ESP-NOW peer
  esp_now_peer_info_t recv = 
  {
    .channel   = CONFIG_LESS_INTERFERENCE_CHANNEL,
    .ifidx     = WIFI_IF_STA,    
    .encrypt   = false   
  };

  memcpy(recv.peer_addr, MAC_ADDR_BROAD, sizeof(uint8_t[6]));

  // add the peer to the ESP-NOW network
  if (report_esp_error(esp_now_add_peer(&recv))) return false;

  // finally, report on the sending parameters
  Serial.printf("Channel: %d\nFrequency: %d Hz\nMAC address: " MACSTR "\n", CONFIG_LESS_INTERFERENCE_CHANNEL, CONFIG_SEND_FREQUENCY, MAC2STR(MAC_ADDR_SEND));

  return true;
}

void generate_timestamp(char* timestamp, DateTime* now, bool img, unsigned long milli = 0)
{
  if(img)
  {
    if(now->month() >= 10 && now->day() >= 10 && now->hour() >= 10 && now->minute() >= 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d", now->year(), now->month(), now->day(), now->hour(), now->minute(), now->second());                           //00000
    }
    else if(now->month() >= 10 && now->day() >= 10 && now->hour() >= 10 && now->minute() >= 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d", now->year(), now->month(), now->day(), now->hour(), now->minute(), 0, now->second());                      //00001
    }
    else if(now->month() >= 10 && now->day() >= 10 && now->hour() >= 10 && now->minute() < 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d", now->year(), now->month(), now->day(), now->hour(), 0, now->minute(), 0, now->second());                 //00011
    }
    else if(now->month() >= 10 && now->day() >= 10 && now->hour() >= 10 && now->minute() < 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d", now->year(), now->month(), now->day(), now->hour(), 0, now->minute(), now->second());                      //00010
    }
    else if(now->month() >= 10 && now->day() >= 10 && now->hour() < 10 && now->minute() < 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d", now->year(), now->month(), now->day(), 0, now->hour(), 0, now->minute(), now->second());                 //00110
    }
    else if(now->month() >= 10 && now->day() >= 10 && now->hour() < 10 && now->minute() < 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d%d", now->year(), now->month(), now->day(), 0, now->hour(), 0, now->minute(), 0, now->second());            //00111
    }
    else if(now->month() >= 10 && now->day() >= 10 && now->hour() < 10 && now->minute() >= 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d", now->year(), now->month(), now->day(), 0, now->hour(), now->minute(), 0, now->second());                 //00101
    }
    else if(now->month() >= 10 && now->day() >= 10 && now->hour() < 10 && now->minute() >= 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d", now->year(), now->month(), now->day(), 0, now->hour(), now->minute(), now->second());                      //00100
    }
    else if(now->month() >= 10 && now->day() < 10 && now->hour() < 10 && now->minute() >= 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d", now->year(), now->month(), 0, now->day(), 0, now->hour(), now->minute(), now->second());                 //01100
    }
    else if(now->month() >= 10 && now->day() < 10 && now->hour() < 10 && now->minute() >= 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d%d", now->year(), now->month(), 0, now->day(), 0, now->hour(), now->minute(), 0, now->second());            //01101
    }
    else if(now->month() >= 10 && now->day() < 10 && now->hour() < 10 && now->minute() < 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d%d%d", now->year(), now->month(), 0, now->day(), 0, now->hour(), 0, now->minute(), 0, now->second());       //01111
    }
    else if(now->month() >= 10 && now->day() < 10 && now->hour() < 10 && now->minute() < 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d%d", now->year(), now->month(), 0, now->day(), 0, now->hour(), 0, now->minute(), now->second());            //01110
    }
    else if(now->month() >= 10 && now->day() < 10 && now->hour() >= 10 && now->minute() < 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d", now->year(), now->month(), 0, now->day(), now->hour(), 0, now->minute(), now->second());                 //01010
    }
    else if(now->month() >= 10 && now->day() < 10 && now->hour() >= 10 && now->minute() < 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d%d", now->year(), now->month(), 0, now->day(), now->hour(), 0, now->minute(), 0, now->second());            //01011
    }
    else if(now->month() >= 10 && now->day() < 10 && now->hour() >= 10 && now->minute() >= 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d", now->year(), now->month(), 0, now->day(), now->hour(), now->minute(), 0, now->second());                 //01001
    }
    else if(now->month() >= 10 && now->day() < 10 && now->hour() >= 10 && now->minute() >= 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d", now->year(), now->month(), 0, now->day(), now->hour(), now->minute(), now->second());                      //01000
    }
    else if(now->month() < 10 && now->day() < 10 && now->hour() >= 10 && now->minute() >= 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d", now->year(), 0, now->month(), 0, now->day(), now->hour(), now->minute(), now->second());                 //11000
    }
    else if(now->month() < 10 && now->day() < 10 && now->hour() >= 10 && now->minute() >= 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d%d", now->year(), 0, now->month(), 0, now->day(), now->hour(), now->minute(), 0, now->second());            //11001
    }
    else if(now->month() < 10 && now->day() < 10 && now->hour() >= 10 && now->minute() < 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d%d%d", now->year(), 0, now->month(), 0, now->day(), now->hour(), 0, now->minute(), 0, now->second());       //11011
    }
    else if(now->month() < 10 && now->day() < 10 && now->hour() >= 10 && now->minute() < 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d%d", now->year(), 0, now->month(), 0, now->day(), now->hour(), 0, now->minute(), now->second());            //11010
    }
    else if(now->month() < 10 && now->day() < 10 && now->hour() < 10 && now->minute() < 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d%d%d", now->year(), 0, now->month(), 0, now->day(), 0, now->hour(), 0, now->minute(), now->second());       //11110
    }
    else if(now->month() < 10 && now->day() < 10 && now->hour() < 10 && now->minute() < 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d%d%d%d", now->year(), 0, now->month(), 0, now->day(), 0, now->hour(), 0, now->minute(), 0, now->second());  //11111
    }
    else if(now->month() < 10 && now->day() < 10 && now->hour() < 10 && now->minute() >= 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d%d%d", now->year(), 0, now->month(), 0, now->day(), 0, now->hour(), now->minute(), 0, now->second());       //11101
    }
    else if(now->month() < 10 && now->day() < 10 && now->hour() < 10 && now->minute() >= 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d%d", now->year(), 0, now->month(), 0, now->day(), 0, now->hour(), now->minute(), now->second());            //11100
    }
    else if(now->month() < 10 && now->day() >= 10 && now->hour() < 10 && now->minute() >= 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d", now->year(), 0, now->month(), now->day(), 0, now->hour(), now->minute(), now->second());                 //10100
    }
    else if(now->month() < 10 && now->day() >= 10 && now->hour() < 10 && now->minute() >= 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d%d", now->year(), 0, now->month(), now->day(), 0, now->hour(), now->minute(), 0, now->second());            //10101
    }
    else if(now->month() < 10 && now->day() >= 10 && now->hour() < 10 && now->minute() < 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d%d%d", now->year(), 0, now->month(), now->day(), 0, now->hour(), 0, now->minute(), 0, now->second());       //10111
    }
    else if(now->month() < 10 && now->day() >= 10 && now->hour() < 10 && now->minute() < 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d%d", now->year(), 0, now->month(), now->day(), 0, now->hour(), 0, now->minute(), now->second());            //10110
    }
    else if(now->month() < 10 && now->day() >= 10 && now->hour() >= 10 && now->minute() < 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d", now->year(), 0, now->month(), now->day(), now->hour(), 0, now->minute(), now->second());                 //10010
    }
    else if(now->month() < 10 && now->day() >= 10 && now->hour() >= 10 && now->minute() < 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d%d", now->year(), 0, now->month(), now->day(), now->hour(), 0, now->minute(), 0, now->second());            //10011
    }
    else if(now->month() < 10 && now->day() >= 10 && now->hour() >= 10 && now->minute() >= 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d%d", now->year(), 0, now->month(), now->day(), now->hour(), now->minute(), 0, now->second());                 //10001
    }
    else
    {
      sprintf(timestamp, "%d%d%d%d%d%d%d", now->year(), 0, now->month(), now->day(), now->hour(), now->minute(), now->second());                      //10000
    }
  }
  else
  {
    if(now->month() >= 10 && now->day() >= 10 && now->hour() >= 10 && now->minute() >= 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d:%d:%d:%d:%d:%d:%lu", now->year(), now->month(), now->day(), now->hour(), now->minute(), now->second(), milli);                           //00000
    }
    else if(now->month() >= 10 && now->day() >= 10 && now->hour() >= 10 && now->minute() >= 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d:%d:%d:%d:%d:%d%d:%lu", now->year(), now->month(), now->day(), now->hour(), now->minute(), 0, now->second(), milli);                      //00001
    }
    else if(now->month() >= 10 && now->day() >= 10 && now->hour() >= 10 && now->minute() < 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d:%d:%d:%d:%d%d:%d%d:%lu", now->year(), now->month(), now->day(), now->hour(), 0, now->minute(), 0, now->second(), milli);                 //00011
    }
    else if(now->month() >= 10 && now->day() >= 10 && now->hour() >= 10 && now->minute() < 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d:%d:%d:%d:%d%d:%d:%lu", now->year(), now->month(), now->day(), now->hour(), 0, now->minute(), now->second(), milli);                      //00010
    }
    else if(now->month() >= 10 && now->day() >= 10 && now->hour() < 10 && now->minute() < 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d:%d:%d:%d%d:%d%d:%d:%lu", now->year(), now->month(), now->day(), 0, now->hour(), 0, now->minute(), now->second(), milli);                 //00110
    }
    else if(now->month() >= 10 && now->day() >= 10 && now->hour() < 10 && now->minute() < 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d:%d:%d:%d%d:%d%d:%d%d:%lu", now->year(), now->month(), now->day(), 0, now->hour(), 0, now->minute(), 0, now->second(), milli);            //00111
    }
    else if(now->month() >= 10 && now->day() >= 10 && now->hour() < 10 && now->minute() >= 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d:%d:%d:%d%d:%d:%d%d:%lu", now->year(), now->month(), now->day(), 0, now->hour(), now->minute(), 0, now->second(), milli);                 //00101
    }
    else if(now->month() >= 10 && now->day() >= 10 && now->hour() < 10 && now->minute() >= 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d:%d:%d:%d%d:%d:%d:%lu", now->year(), now->month(), now->day(), 0, now->hour(), now->minute(), now->second(), milli);                      //00100
    }
    else if(now->month() >= 10 && now->day() < 10 && now->hour() < 10 && now->minute() >= 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d:%d:%d%d:%d%d:%d:%d:%lu", now->year(), now->month(), 0, now->day(), 0, now->hour(), now->minute(), now->second(), milli);                 //01100
    }
    else if(now->month() >= 10 && now->day() < 10 && now->hour() < 10 && now->minute() >= 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d:%d:%d%d:%d%d:%d:%d%d:%lu", now->year(), now->month(), 0, now->day(), 0, now->hour(), now->minute(), 0, now->second(), milli);            //01101
    }
    else if(now->month() >= 10 && now->day() < 10 && now->hour() < 10 && now->minute() < 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d:%d:%d%d:%d%d:%d%d:%d%d:%lu", now->year(), now->month(), 0, now->day(), 0, now->hour(), 0, now->minute(), 0, now->second(), milli);       //01111
    }
    else if(now->month() >= 10 && now->day() < 10 && now->hour() < 10 && now->minute() < 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d:%d:%d%d:%d%d:%d%d:%d:%lu", now->year(), now->month(), 0, now->day(), 0, now->hour(), 0, now->minute(), now->second(), milli);            //01110
    }
    else if(now->month() >= 10 && now->day() < 10 && now->hour() >= 10 && now->minute() < 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d:%d:%d%d:%d:%d%d:%d:%lu", now->year(), now->month(), 0, now->day(), now->hour(), 0, now->minute(), now->second(), milli);                 //01010
    }
    else if(now->month() >= 10 && now->day() < 10 && now->hour() >= 10 && now->minute() < 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d:%d:%d%d:%d:%d%d:%d%d:%lu", now->year(), now->month(), 0, now->day(), now->hour(), 0, now->minute(), 0, now->second(), milli);            //01011
    }
    else if(now->month() >= 10 && now->day() < 10 && now->hour() >= 10 && now->minute() >= 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d:%d:%d%d:%d:%d:%d%d:%lu", now->year(), now->month(), 0, now->day(), now->hour(), now->minute(), 0, now->second(), milli);                 //01001
    }
    else if(now->month() >= 10 && now->day() < 10 && now->hour() >= 10 && now->minute() >= 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d:%d:%d%d:%d:%d:%d:%lu", now->year(), now->month(), 0, now->day(), now->hour(), now->minute(), now->second(), milli);                      //01000
    }
    else if(now->month() < 10 && now->day() < 10 && now->hour() >= 10 && now->minute() >= 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d:%d%d:%d%d:%d:%d:%d:%lu", now->year(), 0, now->month(), 0, now->day(), now->hour(), now->minute(), now->second(), milli);                 //11000
    }
    else if(now->month() < 10 && now->day() < 10 && now->hour() >= 10 && now->minute() >= 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d:%d%d:%d%d:%d:%d:%d%d:%lu", now->year(), 0, now->month(), 0, now->day(), now->hour(), now->minute(), 0, now->second(), milli);            //11001
    }
    else if(now->month() < 10 && now->day() < 10 && now->hour() >= 10 && now->minute() < 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d:%d%d:%d%d:%d:%d%d:%d%d:%lu", now->year(), 0, now->month(), 0, now->day(), now->hour(), 0, now->minute(), 0, now->second(), milli);       //11011
    }
    else if(now->month() < 10 && now->day() < 10 && now->hour() >= 10 && now->minute() < 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d:%d%d:%d%d:%d:%d%d:%d:%lu", now->year(), 0, now->month(), 0, now->day(), now->hour(), 0, now->minute(), now->second(), milli);            //11010
    }
    else if(now->month() < 10 && now->day() < 10 && now->hour() < 10 && now->minute() < 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d:%d%d:%d%d:%d%d:%d%d:%d:%lu", now->year(), 0, now->month(), 0, now->day(), 0, now->hour(), 0, now->minute(), now->second(), milli);       //11110
    }
    else if(now->month() < 10 && now->day() < 10 && now->hour() < 10 && now->minute() < 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d:%d%d:%d%d:%d%d:%d%d:%d%d:%lu", now->year(), 0, now->month(), 0, now->day(), 0, now->hour(), 0, now->minute(), 0, now->second(), milli);  //11111
    }
    else if(now->month() < 10 && now->day() < 10 && now->hour() < 10 && now->minute() >= 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d:%d%d:%d%d:%d%d:%d:%d%d:%lu", now->year(), 0, now->month(), 0, now->day(), 0, now->hour(), now->minute(), 0, now->second(), milli);       //11101
    }
    else if(now->month() < 10 && now->day() < 10 && now->hour() < 10 && now->minute() >= 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d:%d%d:%d%d:%d%d:%d:%d:%lu", now->year(), 0, now->month(), 0, now->day(), 0, now->hour(), now->minute(), now->second(), milli);            //11100
    }
    else if(now->month() < 10 && now->day() >= 10 && now->hour() < 10 && now->minute() >= 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d:%d%d:%d:%d%d:%d:%d:%lu", now->year(), 0, now->month(), now->day(), 0, now->hour(), now->minute(), now->second(), milli);                 //10100
    }
    else if(now->month() < 10 && now->day() >= 10 && now->hour() < 10 && now->minute() >= 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d:%d%d:%d:%d%d:%d:%d%d:%lu", now->year(), 0, now->month(), now->day(), 0, now->hour(), now->minute(), 0, now->second(), milli);            //10101
    }
    else if(now->month() < 10 && now->day() >= 10 && now->hour() < 10 && now->minute() < 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d:%d%d:%d:%d%d:%d%d:%d%d:%lu", now->year(), 0, now->month(), now->day(), 0, now->hour(), 0, now->minute(), 0, now->second(), milli);       //10111
    }
    else if(now->month() < 10 && now->day() >= 10 && now->hour() < 10 && now->minute() < 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d:%d%d:%d:%d%d:%d%d:%d:%lu", now->year(), 0, now->month(), now->day(), 0, now->hour(), 0, now->minute(), now->second(), milli);            //10110
    }
    else if(now->month() < 10 && now->day() >= 10 && now->hour() >= 10 && now->minute() < 10 && now->second() >= 10)
    {
      sprintf(timestamp, "%d:%d%d:%d:%d:%d%d:%d:%lu", now->year(), 0, now->month(), now->day(), now->hour(), 0, now->minute(), now->second(), milli);                 //10010
    }
    else if(now->month() < 10 && now->day() >= 10 && now->hour() >= 10 && now->minute() < 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d:%d%d:%d:%d:%d%d:%d%d:%lu", now->year(), 0, now->month(), now->day(), now->hour(), 0, now->minute(), 0, now->second(), milli);            //10011
    }
    else if(now->month() < 10 && now->day() >= 10 && now->hour() >= 10 && now->minute() >= 10 && now->second() < 10)
    {
      sprintf(timestamp, "%d:%d%d:%d:%d:%d:%d%d:%lu", now->year(), 0, now->month(), now->day(), now->hour(), now->minute(), 0, now->second(), milli);                 //10001
    }
    else
    {
      sprintf(timestamp, "%d:%d%d:%d:%d:%d:%d:%lu", now->year(), 0, now->month(), now->day(), now->hour(), now->minute(), now->second(), milli);                      //10000
    }
  }
}

void setup() 
{
  // put your setup code here, to run once:
  Serial.begin(115200);

  imu_good = imu_setup();
  rtc_good = rtc_setup();
  cam_good = cam_setup();
  sd_good = sd_setup();
  wifi_good = wifi_setup();

  xTaskCreatePinnedToCore(
    task_send_csi, "Task Send CSI"  // A name just for humans
    ,
    3072  // The stack size can be checked by calling `uxHighWaterMark = uxTaskGetStackHighWaterMark(NULL);`
    ,
    NULL  // Task parameter which can modify the task behavior. This must be passed as pointer to void.
    ,
    3  // Priority
    ,
    NULL  // Task handle is not used here - simply pass NULL
    ,
    0
  );
  xTaskCreatePinnedToCore(
    task_save_imu, "Task Save IMU"  // A name just for humans
    ,
    4096  // The stack size can be checked by calling `uxHighWaterMark = uxTaskGetStackHighWaterMark(NULL);`
    ,
    NULL  // Task parameter which can modify the task behavior. This must be passed as pointer to void.
    ,
    3  // Priority
    ,
    NULL  // Task handle is not used here - simply pass NULL
    ,
    0
  );
  xTaskCreatePinnedToCore(
    task_write_all, "Task Write All"  // A name just for humans
    ,
    8192  // The stack size can be checked by calling `uxHighWaterMark = uxTaskGetStackHighWaterMark(NULL);`
    ,
    NULL  // Task parameter which can modify the task behavior. This must be passed as pointer to void.
    ,
    2  // Priority
    ,
    NULL  // Task handle is not used here - simply pass NULL                       
    ,
    1
  );
}

void loop() 
{
  // put your main code here, to run repeatedly:

}

void task_send_csi(void *pvParameters)
{
  esp_now_peer_info_t recv;
  esp_now_get_peer(MAC_ADDR_BROAD, &recv);
  const TickType_t xDelay = (1000 / CONFIG_SEND_FREQUENCY) / portTICK_PERIOD_MS;
  for(;;)
  {
    // send a packet
    //esp_err_t errcode = ;

    report_esp_error(esp_now_send(recv.peer_addr, &pkt_count, sizeof(uint8_t)));

    /*if(errcode != ESP_OK) 
    {
      Serial.print("ESP-NOW send error: ");
      Serial.println(esp_err_to_name(errcode));                                                                           
    }*/

    // wait the specified time before sending again
    vTaskDelay( xDelay ); 
  }
  vTaskDelete( NULL );
}

void task_save_imu(void *pvParameters)
{
  const TickType_t xDelay = (1000 / CONFIG_SEND_FREQUENCY) / portTICK_PERIOD_MS;
  char line[IMU_SIZE + TIME_SIZE]{};
  //char data[60];
  char timestamp[TIME_SIZE]{};
  unsigned long time;
  sensors_event_t a, g, temp;
  DateTime now;

  for(;;)
  {
    imu.getEvent(&a, &g, &temp);
    now = rtc.now();
    new_time = millis();
    time = new_time - old_time;
    memset(timestamp, '\0', TIME_SIZE);
    generate_timestamp(timestamp, &now, false, time);
    sprintf(line, "%s,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f", timestamp, a.acceleration.x, a.acceleration.y, a.acceleration.z, g.gyro.x, g.gyro.y, g.gyro.z);
    //strcpy(line, timestamp);
    //sprintf(data, ",%.6f,%.6f,%.6f,%.6f,%.6f,%.6f", a.acceleration.x, a.acceleration.y, a.acceleration.z, g.gyro.x, g.gyro.y, g.gyro.z);
    //strcat(line, data);

    if(imu_switch == 0)
    {
      taskENTER_CRITICAL(&lock);
      strcpy(imu_data0[imu_idx], line);
      imu_idx++;
      taskEXIT_CRITICAL(&lock);
    }
    else
    {
      taskENTER_CRITICAL(&lock);
      strcpy(imu_data1[imu_idx], line);
      imu_idx++;
      taskEXIT_CRITICAL(&lock);
    }

    vTaskDelay( xDelay ); 
  }
  vTaskDelete( NULL );
}

void task_write_all(void *pvParameters)
{
  const TickType_t xDelay = 1000 / portTICK_PERIOD_MS;
  char filename[FILENAME_SIZE]{};
  char timestamp[TIME_SIZE]{};
  DateTime now;
  File img_file;
  File imu_file;
  
  for(;;)
  {
    Serial.println("started write all");
    taskENTER_CRITICAL(&lock);
    imu_idx_last = imu_idx;
    imu_idx = 0;
    imu_switch = 1 - imu_switch;
    old_time = millis();
    taskEXIT_CRITICAL(&lock);

    if(sd_good)
    {
      now = rtc.now();
      memset(timestamp, '\0', TIME_SIZE);
      memset(filename, '\0', FILENAME_SIZE);
      generate_timestamp(timestamp, &now, true);
      sprintf(filename, "/image%s.jpg", timestamp);

      camera_fb_t *fb = esp_camera_fb_get();
      if (!fb) 
      {
        Serial.println("Failed to get camera frame buffer");
      }

      Serial.printf("Writing files: %s\n", filename);

      img_file = SD.open(filename, FILE_WRITE);
      //imu_file = SD.open("/imu_data.csv", FILE_APPEND);

      if(!img_file)
      {
        Serial.println("Failed to open image file for writing");
      }
      //if(!imu_file)
      //{
      //  Serial.println("Failed to open imu file for writing");
      //}
      Serial.println("Files loaded");

      if(img_file.write(fb->buf, fb->len) == fb->len)
      {
        Serial.println("Image write succeeded");
      } 
      else 
      {
        Serial.println("Image write failed");
      }
      img_file.close();

      // Release image buffer
      esp_camera_fb_return(fb);
      Serial.println("Image saved");
  
      //Serial.println("Writing csv");
      //if(imu_switch == 0)
      //{
      //  for(int i=0;i<imu_idx_last;i++)
      //  {
      //    imu_file.println(imu_data1[i]);
      //  }
      //}
      //else
      //{
      //  for(int i=0;i<imu_idx_last;i++)
      //  {
      //    imu_file.println(imu_data0[i]);
      //  }
    }
      
      
    //  imu_file.close();
    //  Serial.println("imu csv saved");
    //}
    else
    {
      Serial.println("Unknown SD card failure");
    }

    vTaskDelay( xDelay ); 
  }
  vTaskDelete( NULL );
}