ESP32 sends packets with random data past the first 4 bytes

Question

I have some code that I am using to transmit sensor data from an ESP32 through Bluetooth Low Energy to a raspberry pi. I have noticed that the data I get on the receiver past the first 4 bytes is incorrect and contains random values, but this does not affect every packet. I am using FreeRTOS to work with multiple sensors and BLE simultaneously.

I verified that I am receiving the packets with the incorrect data past the first 4 bytes on Wireshark. In the code below, I also print out the data I am going to transmit, and what I see on that Arduino Serial Monitor is inconsistent with what I see on Wireshark when I look for the corrupted packets, i.e the Serial Monitor displays the data I expect to see

I've tried updating the BLE libraries, increasing the stack size for my FreeRTOS tasks in case I was overflowing the stack. I have also tried comparing the packets I see on Wireshark to the data I transmit before and after my ble notify calls, and my data on the buffer looks fine, but the packets I get on Wireshark are incorrect. I've also tried increasing the MTU size in case my packets were too big. My best guess at this point is that it has something to do with the RTOS because I've followed the examples from https://github.com/nkolban/esp32-snippets, and on forums around the ESP32's BLE functionality.

Any help or pointers in the right direction would be greatly appreciated.

This is how I set up the BLE Service and Characteristics

void init_BLE() {
  Serial.println("Starting setup");
  btStart();

  BLEDevice::init("esp32");
  BLEDevice::setMTU(64); // debug with larger MTU size
  bleServer = BLEDevice::createServer();
  bleServer->setCallbacks(new MyServerCallbacks());

  /*
     BLE SERVICE SETUP
  */
  BLEService *bleService = bleServer->createService(BLE_SERVICE_UUID);

  bleCharacteristic = bleService->createCharacteristic(
                        BLE_CHARACTERISTIC_UUID,
                        BLECharacteristic::PROPERTY_NOTIFY | 
                        BLECharacteristic::PROPERTY_READ
                      );
  bleCharacteristic->addDescriptor(new BLE2902());

  bleService->start();


  //Advertising
  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID(BLE_SERVICE_UUID);
  pAdvertising->setScanResponse(true);
  pAdvertising->setMinPreferred(0x0);
  BLEDevice::startAdvertising();
}

This is how I transmit over bluetooth

void valueToByteArray(void* val, size_t val_size, uint8_t *byte_arr) {
  /*
     Takes an arbitrary value and its size, then copies it into the byte array
  */
  for (int c_pos = 0; c_pos < val_size; ++c_pos) {
    byte_arr[c_pos] = *((uint8_t*)val + c_pos);
  }
}

/*
   BLE Publishing Task
*/
void bleTaskHandler(void* pvParameters) {
  //Delay Var
  TickType_t lastUnblock; // used to prevent bluetooth stack congestion
  EventBits_t uxBits; //used to keep track of sensor events

  //Packet Counter
  static uint16_t counter = 0;

  // Send
  bool send_buffer = false;

  //Packet Storage
  uint8_t ble_buffer[20]; //

  //imu variables
  struct imuReading xyztReading;

  //rht variables
  struct rhtReading rht;

  //adc variables
  float adcVal;


  init_BLE();
  while (true) {
    send_buffer = false;
    uxBits = xEventGroupWaitBits(eg, ADC_TASK_BIT | IMU_TASK_BIT | RHT_TASK_BIT, pdTRUE, pdFALSE, (TickType_t) pdMS_TO_TICKS(5));


/*
Everything below here is just for getting data and putting it on the buffer
I've removed the extra members in the packet to simplify things. I still see the problem with this reduced version.
*/
    if ( (IMU_TASK_BIT & uxBits) != 0 ) {
      if (uxQueueMessagesWaiting(imuStack) != 0) {
        xQueueReceive(imuStack, &xyztReading, portMAX_DELAY);
        if (_bleConnected) {
          //fitting the data in one packet requires that the numbers be sent in a contiguous
          //format
          // x(16 bits)->y(16 bits)->z(16 bits) in memory
          valueToByteArray(&xyztReading.xVal, sizeof(uint16_t), &ble_buffer[X_BUFFER_POS]);// x values first in memory
          valueToByteArray(&xyztReading.yVal, sizeof(uint16_t), &ble_buffer[Y_BUFFER_POS]);
          valueToByteArray(&xyztReading.zVal, sizeof(uint16_t), &ble_buffer[Z_BUFFER_POS]);

          send_buffer = true;
        }
      }
    }
/*
Send the data if the buffer was updated
*/
    if (send_buffer) {

      valueToByteArray(&counter, sizeof(counter), &ble_buffer[COUNTER_BUFFER_POS]);
      ++counter;

      char buff[2];
      for (int i = 0; i  < 20; ++i) {
        sprintf(buff, "%02X", ble_buffer[i]);
        Serial.print(buff);
      }
      Serial.println("  end packet");

      bleCharacteristic->setValue(ble_buffer, 20); //send full 20 byte buffer
      bleCharacteristic->notify();
    }

    //disconnecting
    if (!_bleConnected && _bleConnectedOld) {
      Serial.println("disconnected");
      vTaskDelayUntil(&lastUnblock, pdMS_TO_TICKS(500));
      bleServer->startAdvertising();
      _bleConnectedOld = _bleConnected;
    }
    //connecting
    if (_bleConnected && !_bleConnectedOld) {
      Serial.println("connected");
      _bleConnectedOld = _bleConnected;
      vTaskDelayUntil(&lastUnblock, pdMS_TO_TICKS(500)); // wait for ble stack to catch up
    }
  }
}

This is what I get when I graph my data. Imgur

I expect for the z line to be smooth without discontinuities since nothing is moving.

Additionally, whenever I get a packet with corrupted data, the following packet is dropped entirely. I checked this with a packet counter in each packet. I would get a sequence like 5,6,garbage,9,10...

Any help or advice is appreciated!

Edits: Added in missing information

#define X_BUFFER_POS (0)
#define Y_BUFFER_POS (2)
#define Z_BUFFER_POS (4)