ISM330DLC : How to properly set FIFO

AmrithHN · ‎2025-03-26

Hi,

I am really new to this sensor and I was able to get the polling mode working.

I am planning for full 6.6k odr rate and FIFO samples can be 256 when INT1 should be triggered or whenever possible so I can use FIFO .

I am really confused with the order / things to keep in mind to properly get this working. I am using steval Idp0051v which has ISM330DLC sensor connected via SPI1 .

This is my code that hass all the functions to use the sensor :

/*
 * memsSensor.c
 *
 *  Created on: Mar 26, 2025
 *      Author: amrith
 */


#include "memsSensor.h"
#include "ism330dlc.h"
#include "main.h"

extern SPI_HandleTypeDef hspi1;

ism330dlc_ctx_t ism330dlc_ctx = {
    .write_reg = ism330dlc_spi_write,
    .read_reg = ism330dlc_spi_read,
    .handle = NULL // Not used in this case, but can point to &hspi1 if needed
};


// IO structure for ism330dlc.h
ISM330DLC_IO_t ism330dlc_io = {
    .Init = ISM330DLC_SPI_Init,
    .DeInit = ISM330DLC_SPI_DeInit,
    .BusType = ISM330DLC_SPI_4WIRES_BUS, // Using SPI 4-wire mode
    .Address = 0x00,                     // Not used for SPI
    .WriteReg = ISM330DLC_SPI_WriteReg,
    .ReadReg = ISM330DLC_SPI_ReadReg,
    .GetTick = ISM330DLC_GetTick
};


// Object structure
ISM330DLC_Object_t ism330dlc_obj;


// Previous data for change detection
static ISM330DLC_Axes_t prev_accel_data = {0, 0, 0};
static ISM330DLC_Axes_t prev_gyro_data = {0, 0, 0};

// FIFO buffer and variables
#define FIFO_WATERMARK 100 // Number of samples to trigger interrupt (max 512)
#define FIFO_SAMPLE_SIZE 12 // 6 bytes accel + 6 bytes gyro per sample
static uint8_t fifo_buffer[FIFO_WATERMARK * FIFO_SAMPLE_SIZE]; // Buffer for FIFO data
static volatile uint8_t fifo_data_ready = 0; // Flag for ISR


// SPI read implementation
int32_t ism330dlc_spi_read(void *handle, uint8_t reg, uint8_t *data, uint16_t len) {
    HAL_GPIO_WritePin(ISM330DLC_CS_PORT, ISM330DLC_CS_PIN, GPIO_PIN_RESET); // CS low
    reg |= 0x80; // Set read bit (MSB = 1 for read)
    if (HAL_SPI_Transmit(&hspi1, &reg, 1, HAL_MAX_DELAY) != HAL_OK) {
        HAL_GPIO_WritePin(ISM330DLC_CS_PORT, ISM330DLC_CS_PIN, GPIO_PIN_SET); // CS high
        return -1; // Error
    }
    if (HAL_SPI_Receive(&hspi1, data, len, HAL_MAX_DELAY) != HAL_OK) {
        HAL_GPIO_WritePin(ISM330DLC_CS_PORT, ISM330DLC_CS_PIN, GPIO_PIN_SET); // CS high
        return -1; // Error
    }
    HAL_GPIO_WritePin(ISM330DLC_CS_PORT, ISM330DLC_CS_PIN, GPIO_PIN_SET); // CS high
    return 0; // Success
}

// SPI write implementation
int32_t ism330dlc_spi_write(void *handle, uint8_t reg, uint8_t *data, uint16_t len) {
    HAL_GPIO_WritePin(ISM330DLC_CS_PORT, ISM330DLC_CS_PIN, GPIO_PIN_RESET); // CS low
    reg &= 0x7F; // Clear read bit (MSB = 0 for write)
    if (HAL_SPI_Transmit(&hspi1, &reg, 1, HAL_MAX_DELAY) != HAL_OK) {
        HAL_GPIO_WritePin(ISM330DLC_CS_PORT, ISM330DLC_CS_PIN, GPIO_PIN_SET); // CS high
        return -1; // Error
    }
    if (HAL_SPI_Transmit(&hspi1, data, len, HAL_MAX_DELAY) != HAL_OK) {
        HAL_GPIO_WritePin(ISM330DLC_CS_PORT, ISM330DLC_CS_PIN, GPIO_PIN_SET); // CS high
        return -1; // Error
    }
    HAL_GPIO_WritePin(ISM330DLC_CS_PORT, ISM330DLC_CS_PIN, GPIO_PIN_SET); // CS high
    return 0; // Success
}




// Higher-level IO implementations
int32_t ISM330DLC_SPI_Init(void) {
    return ISM330DLC_OK; // SPI1 already initialized by CubeMX
}

int32_t ISM330DLC_SPI_DeInit(void) {
    return ISM330DLC_OK; // No de-init needed
}

int32_t ISM330DLC_SPI_WriteReg(uint16_t Addr, uint16_t Reg, uint8_t *pData, uint16_t Length) {
    return ism330dlc_write_reg(&ism330dlc_ctx, (uint8_t)Reg, pData, Length);
}

int32_t ISM330DLC_SPI_ReadReg(uint16_t Addr, uint16_t Reg, uint8_t *pData, uint16_t Length) {
    return ism330dlc_read_reg(&ism330dlc_ctx, (uint8_t)Reg, pData, Length);
}

int32_t ISM330DLC_GetTick(void) {
    return HAL_GetTick();
}


void ISM330DLC_Setup(void) {
    // Register the IO functions
    if (ISM330DLC_RegisterBusIO(&ism330dlc_obj, &ism330dlc_io) != ISM330DLC_OK) {
        // Handle error
        while (1);
    }

    // Link the low-level context
    ism330dlc_obj.Ctx = ism330dlc_ctx;

    // Initialize the sensor
    if (ISM330DLC_Init(&ism330dlc_obj) != ISM330DLC_OK) {
        // Handle error
        while (1);
    }

    // Verify device ID
    uint8_t device_id;
    if (ISM330DLC_ReadID(&ism330dlc_obj, &device_id) != ISM330DLC_OK || device_id != ISM330DLC_ID) {
        // Handle error (ISM330DLC_ID = 0x6A)
        while (1);
    }
}



void ISM330DLC_ConfigureFIFO(void) {

	  if (ISM330DLC_FIFO_Set_Mode(&ism330dlc_obj, ISM330DLC_STREAM_MODE) != BSP_ERROR_NONE)
	      {
	        ret = -1;
	      }

      if (ISM330DLC_FIFO_Set_INT2_FIFO_Full(&ism330dlc_obj, ENABLE) != BSP_ERROR_NONE)
      {
        ret = -1;
      }
    // Verify
    uint8_t reg_val;
    ism330dlc_read_reg(&ism330dlc_ctx, ISM330DLC_FIFO_CTRL5, &reg_val, 1);
    printf("FIFO_CTRL5: 0x%02X (Expected 0x66)\n", reg_val);
    ism330dlc_read_reg(&ism330dlc_ctx, ISM330DLC_INT1_CTRL, &reg_val, 1);
    printf("INT1_CTRL: 0x%02X (Expected 0x08)\n", reg_val);
}

void ISM330DLC_Configure(void) {
    // Enable accelerometer
    ISM330DLC_ACC_Enable(&ism330dlc_obj);
    ISM330DLC_ACC_SetOutputDataRate(&ism330dlc_obj, ISM330DLC_XL_ODR_6k66Hz); // 52 Hz
    ISM330DLC_ACC_SetFullScale(&ism330dlc_obj, ISM330DLC_16g);          // ±2g

    // Enable gyroscope
    ISM330DLC_GYRO_Enable(&ism330dlc_obj);
    ISM330DLC_GYRO_SetOutputDataRate(&ism330dlc_obj, ISM330DLC_GY_ODR_6k66Hz); // 52 Hz
    ISM330DLC_GYRO_SetFullScale(&ism330dlc_obj, ISM330DLC_2000dps);        // ±500 dps

    // Enable Block Data Update
     ism330dlc_block_data_update_set(&ism330dlc_ctx, PROPERTY_ENABLE);

     // Configure FIFO
       ISM330DLC_ConfigureFIFO();


       // Verify
       uint8_t reg_val;
       ism330dlc_read_reg(&ism330dlc_ctx, ISM330DLC_CTRL1_XL, &reg_val, 1);
       printf("CTRL1_XL: 0x%02X (Expected 0x90)\n", reg_val);
       ism330dlc_read_reg(&ism330dlc_ctx, ISM330DLC_CTRL2_G, &reg_val, 1);
       printf("CTRL2_G: 0x%02X (Expected 0x94)\n", reg_val);
}


//void ISM330DLC_ReadData(void) {
//    ISM330DLC_Axes_t accel_data;
//    ISM330DLC_Axes_t gyro_data;
//
//    if (ISM330DLC_ACC_GetAxes(&ism330dlc_obj, &accel_data) == ISM330DLC_OK) {
//       // printf("Accel: X=%ld, Y=%ld, Z=%ld mg\n", accel_data.x, accel_data.y, accel_data.z);
//        printf("$%d %d %d;",accel_data.x, accel_data.y, accel_data.z);
//    }
//
////    if (ISM330DLC_GYRO_GetAxes(&ism330dlc_obj, &gyro_data) == ISM330DLC_OK) {
////        printf("Gyro: X=%ld, Y=%ld, Z=%ld mdps\n", gyro_data.x, gyro_data.y, gyro_data.z);
////    }
//}

void ISM330DLC_ReadData(void) {
    uint16_t fifo_level;
    if (ism330dlc_fifo_data_level_get(&ism330dlc_ctx, &fifo_level) != 0) {
        return; // Error
    }

    if (fifo_level > 0) {
        // Each sample is 12 bytes (6 for accel, 6 for gyro)
        uint8_t buffer[12 * 100]; // Buffer for up to 100 samples
        uint16_t bytes_to_read = fifo_level * 12;

        if (ism330dlc_fifo_raw_data_get(&ism330dlc_ctx, buffer, bytes_to_read) == 0) {
            for (uint16_t i = 0; i < fifo_level; i++) {
                // Extract accel and gyro from buffer (16-bit per axis)
                int16_t accel_x = (int16_t)(buffer[i * 12 + 0] | (buffer[i * 12 + 1] << 8));
                int16_t accel_y = (int16_t)(buffer[i * 12 + 2] | (buffer[i * 12 + 3] << 8));
                int16_t accel_z = (int16_t)(buffer[i * 12 + 4] | (buffer[i * 12 + 5] << 8));
                int16_t gyro_x = (int16_t)(buffer[i * 12 + 6] | (buffer[i * 12 + 7] << 8));
                int16_t gyro_y = (int16_t)(buffer[i * 12 + 8] | (buffer[i * 12 + 9] << 8));
                int16_t gyro_z = (int16_t)(buffer[i * 12 + 10] | (buffer[i * 12 + 11] << 8));

                // Convert raw to physical units (example for ±2g and ±500 dps)
                float accel_x_mg = ISM330DLC_FROM_FS_2g_TO_mg(accel_x);
                float accel_y_mg = ISM330DLC_FROM_FS_2g_TO_mg(accel_y);
                float accel_z_mg = ISM330DLC_FROM_FS_2g_TO_mg(accel_z);
                float gyro_x_mdps = ISM330DLC_FROM_FS_500dps_TO_mdps(gyro_x);
                float gyro_y_mdps = ISM330DLC_FROM_FS_500dps_TO_mdps(gyro_y);
                float gyro_z_mdps = ISM330DLC_FROM_FS_500dps_TO_mdps(gyro_z);

                // Check for changes (optional threshold)
                #define CHANGE_THRESHOLD 10
                if (abs(accel_x_mg - prev_accel_data.x) > CHANGE_THRESHOLD ||
                    abs(accel_y_mg - prev_accel_data.y) > CHANGE_THRESHOLD ||
                    abs(accel_z_mg - prev_accel_data.z) > CHANGE_THRESHOLD) {
                    printf("Accel: X=%.2f, Y=%.2f, Z=%.2f mg\n", accel_x_mg, accel_y_mg, accel_z_mg);
                    prev_accel_data.x = accel_x_mg;
                    prev_accel_data.y = accel_y_mg;
                    prev_accel_data.z = accel_z_mg;
                }

                if (abs(gyro_x_mdps - prev_gyro_data.x) > CHANGE_THRESHOLD ||
                    abs(gyro_y_mdps - prev_gyro_data.y) > CHANGE_THRESHOLD ||
                    abs(gyro_z_mdps - prev_gyro_data.z) > CHANGE_THRESHOLD) {
                    printf("Gyro: X=%.2f, Y=%.2f, Z=%.2f mdps\n", gyro_x_mdps, gyro_y_mdps, gyro_z_mdps);
                    prev_gyro_data.x = gyro_x_mdps;
                    prev_gyro_data.y = gyro_y_mdps;
                    prev_gyro_data.z = gyro_z_mdps;
                }
            }
        }
    }
}