I2C register reading with long wait time

[MatheusMarkies]

I'm developing a vibration and temperature analysis system.
To do this, I have developed a sensor based on an STM32L053C8 chip and a KX122-1037 accelerometer.
Looking at the datasheet, this accelerometer can reach a maximum data rate of 25600Hz, which is great for my analysis since I need to generate an FFT of the received data.
My operation is as follows: I have a number of vibration samples that I need to collect, currently I'm using 2048 samples. These samples need to be collected over 1 second, so that they represent one of the operating frequencies of the machine being analyzed. In other words, I need to take 2048 accelerometer vibration readings in 1 second equally spaced by a microsecond delay.

My problem lies in this part of the reading. The time between readings is too long, so I've connected a led that stays on after the reading is taken and goes off when a new reading starts. This way I could measure the waiting time, and I arrived at 4 seconds for the 2048 data to be read.
This shouldn't be happening, right? The speed of my I2C is Fast Plus at 2000KHz. According to the data sheet, reading data from an accelerometer register is also very fast.
So why is it taking so long?

How can I fix it?

main.c file setup:

#define ACC_DATA_RATE 25800

#define KX122_DEVICE_ADDRESS_1E   (0x1E)    // 7bit Address
#define KX122_DEVICE_ADDRESS_1F   (0x1F)    // 7bit Address
#define KX122_WAI_VAL             (0x1B)

#define KX122_XOUT_L              (0x06)
#define KX122_WHO_AM_I            (0x0F)
#define KX122_CNTL1               (0x18)
#define KX122_ODCNTL              (0x1B)

#define KX122_CNTL1_TPE           (1 << 0)
#define KX122_CNTL1_WUFE          (1 << 1)
#define KX122_CNTL1_TDTE          (1 << 2)
#define KX122_CNTL1_GSELMASK      (0x18)
#define KX122_CNTL1_GSEL_2G       (0x00)
#define KX122_CNTL1_GSEL_4G       (0x08)
#define KX122_CNTL1_GSEL_8G       (0x10)
#define KX122_CNTL1_DRDYE         (1 << 5)
#define KX122_CNTL1_RES           (1 << 6)
#define KX122_CNTL1_PC1           (1 << 7)

#define KX122_ODCNTL_OSA_50HZ     (2)
#define KX122_ODCNTL_LPRO         (1 << 6)
#define KX122_IIR_BYPASS          (1 << 7)

#define KX122_CNTL1_VAL           (KX122_CNTL1_RES | KX122_CNTL1_GSEL_2G)
#define KX122_ODCNTL_VAL          (0x0F)

#define BUF_CNTL1 0x3A
#define BUF_CNTL2 0x3B
#define BUF_STATUS_1 0x3C
#define BUF_STATUS_2 0x3D
#define BUF_CLEAR 0x3E
#define BUF_READ 0x3F

unsigned short _g_sens;

typedef struct {
  float x;
  float y;
  float z;
} Vibration;

static void MX_I2C1_Init(void)
{

  /* USER CODE BEGIN I2C1_Init 0 */

  /* USER CODE END I2C1_Init 0 */

  /* USER CODE BEGIN I2C1_Init 1 */

  /* USER CODE END I2C1_Init 1 */
  hi2c1.Instance = I2C1;
  hi2c1.Init.Timing = 0x00000000;
  hi2c1.Init.OwnAddress1 = 0;
  hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
  hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
  hi2c1.Init.OwnAddress2 = 0;
  hi2c1.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
  hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
  hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
  if (HAL_I2C_Init(&hi2c1) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Analogue filter
  */
  if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Digital filter
  */
  if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK)
  {
    Error_Handler();
  }

  /** I2C Fast mode Plus enable
  */
  HAL_I2CEx_EnableFastModePlus(I2C_FASTMODEPLUS_I2C1);
  /* USER CODE BEGIN I2C1_Init 2 */

  /* USER CODE END I2C1_Init 2 */

}

Write and Read Registers: Writing to a Register:

static HAL_StatusTypeDef KX122_WriteRegister(I2C_HandleTypeDef *hi2c, uint8_t reg, uint8_t value) {
    uint8_t data[2] = { reg, value };
    return HAL_I2C_Master_Transmit(hi2c, KX122_DEVICE_ADDRESS_1F << 1, data, 2, HAL_MAX_DELAY);
}

Reading from a Register:

HAL_StatusTypeDef KX122_ReadRegisters(I2C_HandleTypeDef *hi2c, uint8_t reg, uint8_t *data, uint16_t length) {
    return HAL_I2C_Mem_Read(hi2c, KX122_DEVICE_ADDRESS_1F << 1, reg, I2C_MEMADD_SIZE_8BIT, data, length, HAL_MAX_DELAY);
}
static HAL_StatusTypeDef KX122_WriteRegister(I2C_HandleTypeDef *hi2c, uint8_t reg, uint8_t value) {
    uint8_t data[2] = { reg, value };
    return HAL_I2C_Master_Transmit(hi2c, KX122_DEVICE_ADDRESS_1F << 1, data, 2, HAL_MAX_DELAY);
}

static HAL_StatusTypeDef KX122_ReadRegister(I2C_HandleTypeDef *hi2c, uint8_t reg, uint8_t *data) {
    HAL_StatusTypeDef status;
    for (int i = 0; i < 3; i++) {
        status = HAL_I2C_Master_Transmit(hi2c, KX122_DEVICE_ADDRESS_1F << 1, &reg, 1, HAL_MAX_DELAY);
        if (status == HAL_OK) {
            status = HAL_I2C_Master_Receive(hi2c, KX122_DEVICE_ADDRESS_1F << 1, data, 1, HAL_MAX_DELAY);
            if (status == HAL_OK) {
                return HAL_OK;
            }
        }
    }
    return status;
}

Initialization of KX122: KX122 Initialization Function:

HAL_StatusTypeDef KX122_Init(I2C_HandleTypeDef *hi2c) {
    HAL_StatusTypeDef rc;
    unsigned char reg;
    rc = KX122_ReadRegister(hi2c, KX122_WHO_AM_I, &reg);
    if (rc != HAL_OK) {
        DEBUG_PRINT("Can't access KX122 \r\n");
        return (rc);
    }
    if (reg != KX122_WAI_VAL) {
        DEBUG_PRINT("Can't find KX122 \r\n");
        return HAL_ERROR;
    }
    rc = KX122_WriteRegister(hi2c, KX122_CNTL1, KX122_CNTL1_VAL);
    if (rc != HAL_OK) {
        DEBUG_PRINT("Can't write KX122 CNTL1 register at first \r\n");
        return (rc);
    }
    rc = KX122_WriteRegister(hi2c, KX122_ODCNTL, KX122_ODCNTL_VAL);
    if (rc != HAL_OK) {
        DEBUG_PRINT("Can't write KX122 ODCNTL register \r\n");
        return (rc);
    }
    return HAL_OK;
}

Reading Accelerometer Data: Read Buffer Data:

HAL_StatusTypeDef KX122_ReadBufferData(I2C_HandleTypeDef *hi2c, float *data, int index) {
    HAL_StatusTypeDef rc;
    int8_t val[8];
    signed short acc[3];
    rc =  KX122_ReadRegisters(hi2c, BUF_READ, &val, sizeof(val));
    if (rc != HAL_OK) {
        DEBUG_PRINT("Can't get KX122 accel value \r\n");
    }
    acc[0] = ((int16_t)val[index + 1] << 8) | (val[index]);
    acc[1] = ((int16_t)val[index + 3] << 8) | (val[index + 2]);
    acc[2] = ((int16_t)val[index + 5] << 8) | (val[index + 4]);
    data[0] = (float)acc[0] / _g_sens;
    data[1] = (float)acc[1] / _g_sens;
    data[2] = (float)acc[2] / _g_sens;
    return (rc);
}

Reading Accelerometer Data Function:

HAL_StatusTypeDef KX122_ReadAccelData(I2C_HandleTypeDef *hi2c, float *data, Vibration *vib) {
    HAL_StatusTypeDef rc;
    int8_t val[6];
    signed short acc[3];
    rc =  KX122_ReadRegisters(hi2c, KX122_XOUT_L, &val, sizeof(val));
    if (rc != HAL_OK) {
        DEBUG_PRINT("Can't get KX122 accel value \r\n");
    }
    acc[0] = ((int16_t)val[1] << 8) | (val[0]);
    acc[1] = ((int16_t)val[3] << 8) | (val[2]);
    acc[2] = ((int16_t)val[5] << 8) | (val[4]);
    data[0] = (float)acc[0] / _g_sens;
    data[1] = (float)acc[1] / _g_sens;
    data[2] = (float)acc[2] / _g_sens;
    vib->x = data[0];
    vib->y = data[1];
    vib->z = data[2];
    return (rc);
}

Main Function: System Setup:

int main(void) {
    HAL_Init();
    SystemClock_Config();
    MX_GPIO_Init();
    MX_USART1_UART_Init();
    MX_I2C1_Init();
    MX_SPI1_Init();
    TIM2_Init();
    setup();
    while (1) {
        loop();
    }
}

Setup and Loop Functions: Setup Function:

void setup() {
    DEBUG_PRINT("Setup...\r\n");
    I2C_Scan();
    if (KX122_Init(&hi2c1) != HAL_ERROR)
        DEBUG_PRINT("KX122 Ready\r\n");
    if (FRAM_Init(&hi2c1) != HAL_ERROR)
        DEBUG_PRINT("FRAM Ready\r\n");
    writeVibrationInformation();
}

Loop Function:

void loop() {
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_SET);
    DEBUG_PRINT("read...\r\n");
    HAL_Delay(1000);
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_RESET);
    writeVibrationInformation();
}

void writeVibrationInformation() {
	float accel[3];
	data.rms_accel[0] = 0.0;
	data.rms_accel[1] = 0.0;
	data.rms_accel[2] = 0.0;

	data.rms_vel[0] = 0.0;
	data.rms_vel[1] = 0.0;
	data.rms_vel[2] = 0.0;

	float velocity_x = 0.0;
	float velocity_y = 0.0;
	float velocity_z = 0.0;
	
	Vibration vibration;
	for (int i = 0; i < 2048 ; i++) {
		KX122_ReadAccelData(&hi2c1, accel, &vibration);

		data.rms_accel[0] += (vibration.x * vibration.x);
		data.rms_accel[1] += (vibration.y * vibration.y);
		data.rms_accel[2] += (vibration.z * vibration.z);

		float deltaTime = sampling_period_us / 1e6;

		data.rms_vel[0] += (vibration.x * vibration.x) * deltaTime; //Ax * dT
		data.rms_accel[1] += (vibration.y * vibration.y) * deltaTime; //Ay * dT
		data.rms_vel[2] += (vibration.z * vibration.z) * deltaTime; //Az * dT

		address += sizeof(float);

		//delayMicroseconds(sampling_period_us);
    }

	data.rms_accel[0] = sqrt(data.rms_accel[0] / SAMPLES);
	data.rms_accel[1] = sqrt(data.rms_accel[1] / SAMPLES);
	data.rms_accel[2] = sqrt(data.rms_accel[2] / SAMPLES);

	char buffer[256];
	snprintf(buffer, sizeof(buffer),
			"RMS Accel: %.2f, %.2f, %.2f\r\nRMS Vel: %.4f, %.4f, %.4f\r\n",
			data.rms_accel[0], data.rms_accel[1], data.rms_accel[2],
			data.rms_vel[0], data.rms_vel[1], data.rms_vel[2]);
	DEBUG_PRINT(buffer);
}