posix hrt add latency buckets

platforms/posix/src/px4/common/drv_hrt.cpp

@@ -43,6 +43,8 @@
 #include <px4_platform_common/workqueue.h>
 #include <px4_platform_common/tasks.h>
 #include <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+
 #include <semaphore.h>
 #include <time.h>
 #include <string.h>
@@ -57,7 +59,17 @@
 static constexpr unsigned HRT_INTERVAL_MIN = 50;
 static constexpr unsigned HRT_INTERVAL_MAX = 50000000;
 
+/*
+ * Queue of callout entries.
+ */
 static struct sq_queue_s	callout_queue;
+
+/* latency baseline (last compare value applied) */
+static uint64_t			latency_baseline;
+
+/* timer count at interrupt (for latency purposes) */
+static uint64_t			latency_actual;
+
 static px4_sem_t 	_hrt_lock;
 static struct work_s	_hrt_work;
 
@@ -71,6 +83,7 @@ static int32_t dsp_offset = 0;
 static LockstepScheduler *lockstep_scheduler = new LockstepScheduler();
 #endif
 
+static void hrt_latency_update();
 
 static void hrt_call_reschedule();
 static void hrt_call_invoke();
@@ -250,6 +263,23 @@ void	hrt_cancel(struct hrt_call *entry)
 	// endif
 }
 
+static void hrt_latency_update()
+{
+	uint16_t latency = latency_actual - latency_baseline;
+	unsigned	index;
+
+	/* bounded buckets */
+	for (index = 0; index < LATENCY_BUCKET_COUNT; index++) {
+		if (latency <= latency_buckets[index]) {
+			latency_counters[index]++;
+			return;
+		}
+	}
+
+	/* catch-all at the end */
+	latency_counters[index]++;
+}
+
 /*
  * initialise a hrt_call structure
  */
@@ -323,8 +353,12 @@ hrt_call_enter(struct hrt_call *entry)
 static void
 hrt_tim_isr(void *p)
 {
+	/* grab the timer for latency tracking purposes */
+	latency_actual = hrt_absolute_time();
+
+	/* do latency calculations */
+	hrt_latency_update();
 
-	//PX4_INFO("hrt_tim_isr");
 	/* run any callouts that have met their deadline */
 	hrt_call_invoke();
 
@@ -349,8 +383,6 @@ hrt_call_reschedule()
 	struct hrt_call	*next = (struct hrt_call *)sq_peek(&callout_queue);
 	hrt_abstime	deadline = now + HRT_INTERVAL_MAX;
 
-	//PX4_INFO("hrt_call_reschedule");
-
 	/*
 	 * Determine what the next deadline will be.
 	 *
@@ -376,6 +408,9 @@ hrt_call_reschedule()
 		}
 	}
 
+	/* set the new compare value and remember it for latency tracking */
+	latency_baseline = now + delay;
+
 	// There is no timer ISR, so simulate one by putting an event on the
 	// high priority work queue