Re: [PATCH] iio: imu: inv_icm42600: fix too big timestamp jitter

[Jonathan Cameron <jic23@kernel.org>]

On Thu,  4 May 2023 09:52:04 +0000
inv.git-commit@tdk.com wrote:

> From: Jean-Baptiste Maneyrol <jean-baptiste.maneyrol@tdk.com>
> 
> 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@tdk.com>
> Signed-off-by: <inv.git-commit@tdk.com>
> Cc: <stable@vger.kernel.org>

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,