Add an interrupt-safe way of comparing a timestamp with the current time.

Add an interrupt-safe way of storing the current time into a timestamp.

2 files changed, 46 insertions, 0 deletions

diff --git a/apps/drivers/drv_hrt.h b/apps/drivers/drv_hrt.h
index 3b493a81a..2e30725bd 100644
--- a/apps/drivers/drv_hrt.h
+++ b/apps/drivers/drv_hrt.h
@@ -92,6 +92,22 @@ __EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts);
 __EXPORT extern void	abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
 
 /*
+ * Compute the delta between a timestamp taken in the past
+ * and now.
+ *
+ * This function is safe to use even if the timestamp is updated
+ * by an interrupt during execution.
+ */
+__EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then);
+
+/*
+ * Store the absolute time in an interrupt-safe fashion.
+ *
+ * This function ensures that the timestamp cannot be seen half-written by an interrupt handler.
+ */
+__EXPORT extern hrt_absolute_time hrt_store_absolute_time(volatile hrt_abstime *now);
+
+/*
  * Call callout(arg) after delay has elapsed.
  *
  * If callout is NULL, this can be used to implement a timeout by testing the call
diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c
index cd9cb45b1..0df2a8b74 100644
--- a/apps/drivers/stm32/drv_hrt.c
+++ b/apps/drivers/stm32/drv_hrt.c
@@ -646,6 +646,36 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
 }
 
 /*
+ * Compare a time value with the current time.
+ */
+hrt_abstime
+hrt_elapsed_time(const volatile hrt_abstime *then)
+{
+	irqstate_t flags = irqsave();
+
+	hrt_abstime delta = hrt_absolute_time() - *then;
+
+	irqrestore(flags);
+
+	return delta;
+}
+
+/*
+ * Store the absolute time in an interrupt-safe fashion
+ */
+hrt_absolute_time
+hrt_store_absolute_time(volatile hrt_abstime *now)
+{
+	irqstate_t flags = irqsave();
+
+	hrt_abstime ts = hrt_absolute_time();
+
+	irqrestore(flags);
+
+	return ts;
+}
+
+/*
  * Initalise the high-resolution timing module.
  */
 void