drv_hrt: added hrt_call_init() and hrt_call_delay() APIs

hrt_call_init() can be used to initialise (zero) a hrt_call structure
to ensure safe usage. The hrt_call_every() interface calls this
automatically.

hrt_call_delay() can be used to delay a current callout by the given
number of microseconds

2 files changed, 32 insertions, 1 deletions

diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h
index 8a99eeca7..d130d68b3 100644
--- a/src/drivers/drv_hrt.h
+++ b/src/drivers/drv_hrt.h
@@ -142,6 +142,20 @@ __EXPORT extern bool	hrt_called(struct hrt_call *entry);
 __EXPORT extern void	hrt_cancel(struct hrt_call *entry);
 
 /*
+ * initialise a hrt_call structure
+ */
+__EXPORT extern void	hrt_call_init(struct hrt_call *entry);
+
+/*
+ * delay a hrt_call_every() periodic call by the given number of
+ * microseconds. This should be called from within the callout to
+ * cause the callout to be re-scheduled for a later time. The periodic
+ * callouts will then continue from that new base time at the
+ * previously specified period.
+ */
+__EXPORT extern void	hrt_call_delay(struct hrt_call *entry, hrt_abstime delay);
+
+/*
  * Initialise the HRT.
  */
 __EXPORT extern void	hrt_init(void);
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index e79d7e10a..dc28f446b 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -720,6 +720,7 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v
 void
 hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
 {
+	hrt_call_init(entry);
 	hrt_call_internal(entry,
 			  hrt_absolute_time() + delay,
 			  interval,
@@ -839,7 +840,12 @@ hrt_call_invoke(void)
 
 		/* if the callout has a non-zero period, it has to be re-entered */
 		if (call->period != 0) {
-			call->deadline = deadline + call->period;
+			// re-check call->deadline to allow for
+			// callouts to re-schedule themselves 
+			// using hrt_call_delay()
+			if (call->deadline <= now) {
+				call->deadline = deadline + call->period;
+			}
 			hrt_call_enter(call);
 		}
 	}
@@ -906,5 +912,16 @@ hrt_latency_update(void)
 	latency_counters[index]++;
 }
 
+void
+hrt_call_init(struct hrt_call *entry)
+{
+	memset(entry, 0, sizeof(*entry));
+}
+
+void
+hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
+{
+	entry->deadline = hrt_absolute_time() + delay;
+}
 
 #endif /* HRT_TIMER */