common/inv_sensors/inv_sensors_timestamp.c

41 void inv_sensors_timestamp_init(struct inv_sensors_timestamp *ts,
44 	memset(ts, 0, sizeof(*ts));
47 	ts->chip = *chip;
48 	ts->min_period = INV_SENSORS_TIMESTAMP_MIN(chip->clock_period, chip->jitter);
49 	ts->max_period = INV_SENSORS_TIMESTAMP_MAX(chip->clock_period, chip->jitter);
52 	ts->mult = chip->init_period / chip->clock_period;
53 	ts->period = chip->init_period;
56 	inv_update_acc(&ts->chip_period, chip->clock_period);
60 int inv_sensors_timestamp_update_odr(struct inv_sensors_timestamp *ts,
64 	if (fifo && ts->new_mult != 0)
67 	ts->new_mult = period / ts->chip.clock_period;
73 static bool inv_validate_period(struct inv_sensors_timestamp *ts, uint32_t period, uint32_t mult)
78 	period_min = ts->min_period * mult;
79 	period_max = ts->max_period * mult;
86 static bool inv_update_chip_period(struct inv_sensors_timestamp *ts,
91 	if (!inv_validate_period(ts, period, mult))
96 	inv_update_acc(&ts->chip_period, new_chip_period);
97 	ts->period = ts->mult * ts->chip_period.val;
102 static void inv_align_timestamp_it(struct inv_sensors_timestamp *ts)
108 	delta = ts->it.lo - ts->timestamp;
111 	jitter = INV_SENSORS_TIMESTAMP_JITTER((int64_t)ts->period, ts->chip.jitter);
119 	ts->timestamp += adjust;
122 void inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp *ts,
128 	const uint32_t fifo_mult = fifo_period / ts->chip.clock_period;
136 	it = &ts->it;
143 		valid = inv_update_chip_period(ts, fifo_mult, period);
147 	if (ts->timestamp == 0) {
149 		interval = (int64_t)ts->period * (int64_t)sensor_nb;
150 		ts->timestamp = it->up - interval;
156 		inv_align_timestamp_it(ts);
160 void inv_sensors_timestamp_apply_odr(struct inv_sensors_timestamp *ts,
167 	if (ts->new_mult == 0)
171 	ts->mult = ts->new_mult;
172 	ts->new_mult = 0;
173 	ts->period = ts->mult * ts->chip_period.val;
181 	if (ts->timestamp != 0) {
183 		fifo_mult = fifo_period / ts->chip.clock_period;
184 		fifo_period = fifo_mult * ts->chip_period.val;
187 		ts->timestamp = ts->it.up - interval;