From 620f77ddb79de683befb458f43c30435759ac0e2 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Mon, 15 Feb 2021 19:18:51 -0500 Subject: msgblock: Add clock estimation helper functions Signed-off-by: Kevin O'Connor --- klippy/chelper/msgblock.c | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) (limited to 'klippy/chelper/msgblock.c') diff --git a/klippy/chelper/msgblock.c b/klippy/chelper/msgblock.c index 3c848b13..e6cb298b 100644 --- a/klippy/chelper/msgblock.c +++ b/klippy/chelper/msgblock.c @@ -181,3 +181,29 @@ message_queue_free(struct list_head *root) message_free(qm); } } + + +/**************************************************************** + * Clock estimation + ****************************************************************/ + +// Extend a 32bit clock value to its full 64bit value +uint64_t +clock_from_clock32(struct clock_estimate *ce, uint32_t clock32) +{ + return ce->last_clock + (int32_t)(clock32 - ce->last_clock); +} + +// Convert a clock to its estimated time +double +clock_to_time(struct clock_estimate *ce, uint64_t clock) +{ + return ce->conv_time + (int64_t)(clock - ce->conv_clock) / ce->est_freq; +} + +// Convert a time to the nearest clock value +uint64_t +clock_from_time(struct clock_estimate *ce, double time) +{ + return (int64_t)((time - ce->conv_time)*ce->est_freq + .5) + ce->conv_clock; +} -- cgit v1.2.3-70-g09d2