On Thu, 4 May 2023 09:52:04 +0000
inv.git-commit@xxxxxxx wrote:
> From: Jean-Baptiste Maneyrol <jean-baptiste.maneyrol@xxxxxxx>
>
> We are adjusting timestamp with interrupt every time, leading to
> a lot of jitter in timestamp values. Now the adjustment is done
> only when the delta is bigger than the jitter.
>
> Refactorize code and delete the unnecessary handling of multiple
> FIFO data.
>
> Fixes: ec74ae9fd37c ("iio: imu: inv_icm42600: add accurate timestamping")
> Signed-off-by: Jean-Baptiste Maneyrol <jean-baptiste.maneyrol@xxxxxxx>
> Signed-off-by: <inv.git-commit@xxxxxxx>
> Cc: <stable@xxxxxxxxxxxxxxx>
Hmm. Whilst this may be an improvement, I'm not totally convinced it's
something we should backport.
Also, there are a lot of possible solutions to this and I'm not sure why
or if this is the best option.
Perhaps a simple filter on the jitter adjustment to smooth it out?
Something as simple as adjusting by only 10% of the measured difference
if it is small might work for example. Or carry a moving window of
recently measured jitter and apply some sort of filtering to that.
Perhaps that would incorporate a 'reset' approach if the measurement is
way off to allow faster correction if something has gone wrong.
Hence, I'd like more discussion of why this solution in the patch description.
> ---
> .../imu/inv_icm42600/inv_icm42600_timestamp.c | 49 ++++++++++---------
> 1 file changed, 26 insertions(+), 23 deletions(-)
>
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
> index 7f2dc41f807b..af2e59fb7258 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
> @@ -93,8 +93,8 @@ static bool inv_validate_period(uint32_t period, uint32_t mult)
> return false;
> }
>
> -static bool inv_compute_chip_period(struct inv_icm42600_timestamp *ts,
> - uint32_t mult, uint32_t period)
> +static bool inv_update_chip_period(struct inv_icm42600_timestamp *ts,
> + uint32_t mult, uint32_t period)
> {
> uint32_t new_chip_period;
>
> @@ -104,10 +104,31 @@ static bool inv_compute_chip_period(struct inv_icm42600_timestamp *ts,
> /* update chip internal period estimation */
> new_chip_period = period / mult;
> inv_update_acc(&ts->chip_period, new_chip_period);
> + ts->period = ts->mult * ts->chip_period.val;
>
> return true;
> }
>
> +static void inv_align_timestamp_it(struct inv_icm42600_timestamp *ts)
> +{
> + int64_t delta, jitter;
> + int64_t adjust;
> +
> + /* delta time between last sample and last interrupt */
> + delta = ts->it.lo - ts->timestamp;
> +
> + /* adjust timestamp while respecting jitter */
> + jitter = ((int64_t)ts->period * INV_ICM42600_TIMESTAMP_JITTER) / 100;
> + if (delta > jitter)
> + adjust = jitter;
> + else if (delta < -jitter)
> + adjust = -jitter;
> + else
> + adjust = 0;
> +
> + ts->timestamp += adjust;
> +}
> +
> void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts,
> uint32_t fifo_period, size_t fifo_nb,
> size_t sensor_nb, int64_t timestamp)
> @@ -116,7 +137,6 @@ void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts,
> int64_t delta, interval;
> const uint32_t fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD;
> uint32_t period = ts->period;
> - int32_t m;
> bool valid = false;
>
> if (fifo_nb == 0)
> @@ -130,10 +150,7 @@ void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts,
> if (it->lo != 0) {
> /* compute period: delta time divided by number of samples */
> period = div_s64(delta, fifo_nb);
> - valid = inv_compute_chip_period(ts, fifo_mult, period);
> - /* update sensor period if chip internal period is updated */
> - if (valid)
> - ts->period = ts->mult * ts->chip_period.val;
> + valid = inv_update_chip_period(ts, fifo_mult, period);
> }
>
> /* no previous data, compute theoritical value from interrupt */
> @@ -145,22 +162,8 @@ void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts,
> }
>
> /* if interrupt interval is valid, sync with interrupt timestamp */
> - if (valid) {
> - /* compute measured fifo_period */
> - fifo_period = fifo_mult * ts->chip_period.val;
> - /* delta time between last sample and last interrupt */
> - delta = it->lo - ts->timestamp;
> - /* if there are multiple samples, go back to first one */
> - while (delta >= (fifo_period * 3 / 2))
> - delta -= fifo_period;
> - /* compute maximal adjustment value */
> - m = INV_ICM42600_TIMESTAMP_MAX_PERIOD(ts->period) - ts->period;
> - if (delta > m)
> - delta = m;
> - else if (delta < -m)
> - delta = -m;
> - ts->timestamp += delta;
> - }
> + if (valid)
> + inv_align_timestamp_it(ts);
> }
>
> void inv_icm42600_timestamp_apply_odr(struct inv_icm42600_timestamp *ts,
