ISM330DLC FIFO AXL+Gyro+Timestamp

[andross092]

Dear Support
I want to add timestamp registration to the FIFO.
I already use FIFO to save gyroscope and accelerometer samples, but I'm struggling to add timestamps.
Since ODR is too high for polling mode the only solution is this one.
I attach some code snippets to make it clear, also consider them as pseudo code taken from a more complex structure.
ism330dlc_fifo_time_set and timer_rd are not present in the library but I wrote them based on the AN5125

int32_t ism330dlc_fifo_time_set(stmdev_ctx_t *ctx, uint8_t val) {
	ism330dlc_fifo_ctrl2_t fifo_ctrl2;
	int32_t ret;
	ret = ism330dlc_read_reg(ctx, ISM330DLC_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2,
							 1);
	if (ret == 0) {
		fifo_ctrl2.fifo_timer_en = val;
		ret = ism330dlc_write_reg(ctx, ISM330DLC_FIFO_CTRL2,
								  (uint8_t *)&fifo_ctrl2, 1);
	}

	return ret;
}​
uint64_t ism330dlc_timer_rd(stmdev_ctx_t *ctx) {
	uint64_t time = 0;
	uint8_t t0;

	ism330dlc_read_reg(ctx, ISM330DLC_TIMESTAMP0_REG, (uint8_t *)&t0, 1);

	time = t0;
	ism330dlc_read_reg(ctx, ISM330DLC_TIMESTAMP1_REG, (uint8_t *)&t0, 1);
	time |= uint64_t(t0 << 8);
	ism330dlc_read_reg(ctx, ISM330DLC_TIMESTAMP2_REG, (uint8_t *)&t0, 1);
	time |= uint64_t(t0 << 16);

	return time * 25;
}
void start_routine() {		
                sens.freq = 104u;
		pattern_len = 18;

		ism330dlc_block_data_update_set(&ctx.Ctx, PROPERTY_ENABLE);

		ism330dlc_int1_route_t int_1_reg{};
		ism330dlc_pin_int1_route_get(&ctx.Ctx, &int_1_reg);
		int_1_reg.int1_fth = PROPERTY_ENABLE;
		ism330dlc_pin_int1_route_set(&ctx.Ctx, int_1_reg);
		ism330dlc_fifo_watermark_set(&ctx.Ctx, sens.freq * pattern_len);

 		ISM330DLC_ACC_Enable(&ctx);
		ISM330DLC_GYRO_Enable(&ctx);
		ISM330DLC_ACC_SetOutputDataRate(&ctx, sens.freq);
		ISM330DLC_GYRO_SetOutputDataRate(&ctx, sens.freq);
		ism330dlc_fifo_dataset_4_batch_set(&ctx.Ctx, ISM330DLC_FIFO_DS4_NO_DEC);
		ism330dlc_fifo_xl_batch_set(&ctx.Ctx, ISM330DLC_FIFO_XL_NO_DEC);
		ism330dlc_fifo_gy_batch_set(&ctx.Ctx, ISM330DLC_FIFO_GY_NO_DEC);

		ism330dlc_timestamp_res_set(&ctx.Ctx, ISM330DLC_LSB_25us);
		// ism330dlc_timer_int_set(&ctx.Ctx, 1);

		ism330dlc_timestamp_set(&ctx.Ctx, 1);

		ism330dlc_fifo_time_set(&ctx.Ctx, 1);

		ISM330DLC_FIFO_Set_ODR_Value(&ctx, sens.freq);
		ism330dlc_fifo_mode_set(&ctx.Ctx, ISM330DLC_STREAM_MODE);​
}

void loop(){
        int16_t num;
	ism330dlc_fifo_data_level_get(&ctx.Ctx, &num);
		
		for (auto i = 0u; i < num; i++) {
			uint16_t pat;
			axis ax[3]{};

			ism330dlc_fifo_pattern_get(&ctx.Ctx, &pat);
			auto &v = ax[pat / 3].ax.i16bit[pat % 3];
			ism330dlc_fifo_raw_data_get(&ctx.Ctx, (uint8_t *)&v, sizeof(v));
			auto m = (pat < 3) ? sens.gy / 1000
					 : pat < 6 ? sens.axl / 1000
							   : 25;
			auto e = su::stringify(float(v) * m, 2);
			auto tm = ism330dlc_timer_rd(&ctx.Ctx);
			slog::println("IMU", [&]() {
				return su::stringify(tm) + " pat " + su::stringify(pat) + " " +
					   e;
			}());
		}
}

Notes:
- trying different ODRs makes the same results
- enabling/disabling BDU won't change anything

- Loop() is called in a task when the IRQ is triggered

These are the results:
Gyro and accelerometer data are well converted, but I always read 0 to pattern 6-7-8, even if I can read timestamp registers correctly.

results: Timestamp in polling + #pattern + converted gy/axl and zero timestamp
Looking forward to hearing from you,
Best regards,
Andrea

[Federica Bossi]

Hi @andross092 ,

To add timestamp registration to your FIFO data, you can follow these steps:

1. Enable the FIFO timestamp feature: This is done by setting the appropriate bits in the FIFO control registers.
2. Read the timestamp values: Each time you read data from the FIFO, also read the corresponding timestamp.

Try these functions with for the timestamp functionality:

%Function to Set FIFO Time

int32_t ism330dlc_fifo_time_set(stmdev_ctx_t *ctx, uint8_t val) {
    ism330dlc_fifo_ctrl2_t fifo_ctrl2;
    int32_t ret;
    ret = ism330dlc_read_reg(ctx, ISM330DLC_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1);
    if (ret == 0) {
        fifo_ctrl2.fifo_timer_en = val;
        ret = ism330dlc_write_reg(ctx, ISM330DLC_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1);
    }
    return ret;
}

%Function to Read Timer
uint64_t ism330dlc_timer_rd(stmdev_ctx_t *ctx) {
    uint64_t time = 0;
    uint8_t t0;

    ism330dlc_read_reg(ctx, ISM330DLC_TIMESTAMP0_REG, (uint8_t *)&t0, 1);
    time = t0;
    ism330dlc_read_reg(ctx, ISM330DLC_TIMESTAMP1_REG, (uint8_t *)&t0, 1);
    time |= uint64_t(t0 << 8);
    ism330dlc_read_reg(ctx, ISM330DLC_TIMESTAMP2_REG, (uint8_t *)&t0, 1);
    time |= uint64_t(t0 << 16);

    return time * 25;  // Assuming 25us resolution
}

[andross092]

Hi Federica
Thanks for your reply.
Since those were my functions and you just copied them I suppose what I'm trying to do is already correct.
But as you can see from my logs those configurations aren't working as expected, because I cannot retrieve any valid timestamp from the FIFO

[andross092]

Dear support,
Is there any update on the topic?

[Andrea VITALI]

Did you set the batch data for batch #4 dedicated to timestamps? 

ism330dlc_fifo_dataset_4_batch_set()

Then looking at the AN5125 it seems that bit #7 (128) must be set in FIFO_CTRL2 to enable the timestamps in batch #4 (you seem to set bit #0)

The corresponding function does not exist in the official driver on GitHub, but one can copy rename and change the similar function ism330dlc_fifo_temp_batch_set() that enables temperature in batch #4

let us know if this solves

[andross092]

HI Andrea,
Thanks for your reply.
I actually set that bit to one in this function

int32_t ism330dlc_fifo_time_set(stmdev_ctx_t *ctx, uint8_t val) {
	ism330dlc_fifo_ctrl2_t fifo_ctrl2;
	int32_t ret;
	ret = ism330dlc_read_reg(ctx, ISM330DLC_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2,
							 1);
	if (ret == 0) {
		fifo_ctrl2.fifo_timer_en = val;
		ret = ism330dlc_write_reg(ctx, ISM330DLC_FIFO_CTRL2,
								  (uint8_t *)&fifo_ctrl2, 1);
	}

	return ret;
}

 when I call 

		ism330dlc_fifo_time_set(&ctx.Ctx, 1);

in the init
but since you highlighted the fact that is the 7th bit I guess I have an answer:

Take a look at ism330dlc_reg.h:

this struct splits register in 9bit instead of 8, so I don't know where I'm actually writing

[Andrea VITALI]

agreed - in the official driver there is a bug: not_used_01:3 is the correct code for the bitfield definition of FIFO_CTRL2

will amend ASAP - let us know if this fixes your issue

also, can you reconfirm you set the batch #4 rate by writing to FIFO_CTRL4?

[andross092]

I opened this pull request
https://github.com/STMicroelectronics/stm32-ism330dlc/pull/2 
The problem was there so this fixed the issue

[Andrea VITALI]

glad we found the issue!