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

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

[inv.git-commit]

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>
---
 .../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,
-- 
2.34.1

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

[Jonathan Cameron]

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,

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

[Jean-Baptiste Maneyrol]

Hello Jonathan,

there are indeed a lot of possibilities for handling that. This way is just one among several that keep it simple with the existing code. That's why I was thinking it may be a good idea to backport it.

Instead of synchronizing every time the data timestamp with the IT timestamp, and have system jitter jamming the timestamp, let's just synchronize when the delta is bigger than the acceptable jitter. And keep synchronization at the jitter value.

If you don't feel comfortable backporting it, then I can just issue a standard patch without the Fixes tag.

Thanks,
JB


From: Jonathan Cameron <jic23@kernel.org>
Sent: Saturday, May 6, 2023 19:36
To: INV Git Commit <INV.git-commit@tdk.com>
Cc: linux-iio@vger.kernel.org <linux-iio@vger.kernel.org>; lars@metafoo.de <lars@metafoo.de>; Jean-Baptiste Maneyrol <Jean-Baptiste.Maneyrol@tdk.com>; stable@vger.kernel.org <stable@vger.kernel.org>
Subject: Re: [PATCH] iio: imu: inv_icm42600: fix too big timestamp jitter 
 
[CAUTION] This is an EXTERNAL email. Do not click links or open attachments unless you recognize the sender and know the content is safe.

======================================================================
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,

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

[Jonathan Cameron]

On Tue, 9 May 2023 16:10:55 +0000
Jean-Baptiste Maneyrol <Jean-Baptiste.Maneyrol@tdk.com> wrote:

> Hello Jonathan,
> 
> there are indeed a lot of possibilities for handling that. This way is just one among several that keep it simple with the existing code. That's why I was thinking it may be a good idea to backport it.
> 
> Instead of synchronizing every time the data timestamp with the IT timestamp, and have system jitter jamming the timestamp, let's just synchronize when the delta is bigger than the acceptable jitter. And keep synchronization at the jitter value.
> 
> If you don't feel comfortable backporting it, then I can just issue a standard patch without the Fixes tag.
Let's let it soak for a while without the fixes tag / backport and rename
to 
iio..: Avoid frequent timestamp jitter
(if you leave fix in there it will get picked up anyway).

Then if it looks good after it is in mainline we can request a later backport.

Also add some of your explanation above on the reasoning to the patch description
(simplicity etc). Plus maybe an example of timestamp jitter before and after.

Jonathan

> 
> Thanks,
> JB
> 
> 
> From: Jonathan Cameron <jic23@kernel.org>
> Sent: Saturday, May 6, 2023 19:36
> To: INV Git Commit <INV.git-commit@tdk.com>
> Cc: linux-iio@vger.kernel.org <linux-iio@vger.kernel.org>; lars@metafoo.de <lars@metafoo.de>; Jean-Baptiste Maneyrol <Jean-Baptiste.Maneyrol@tdk.com>; stable@vger.kernel.org <stable@vger.kernel.org>
> Subject: Re: [PATCH] iio: imu: inv_icm42600: fix too big timestamp jitter 
>  
> [CAUTION] This is an EXTERNAL email. Do not click links or open attachments unless you recognize the sender and know the content is safe.
> 
> ======================================================================
> 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,