Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[WIP]: posix hrt add latency buckets #12852

Closed
wants to merge 1 commit into from
Closed

[WIP]: posix hrt add latency buckets #12852

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
43 changes: 40 additions & 3 deletions platforms/posix/src/px4/common/drv_hrt.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,10 +43,13 @@
#include <px4_workqueue.h>
#include <px4_tasks.h>
#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>

#include <semaphore.h>
#include <time.h>
#include <string.h>
#include <errno.h>

#include "hrt_work.h"

#if defined(ENABLE_LOCKSTEP_SCHEDULER)
Expand All @@ -57,7 +60,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 hrt_abstime latency_baseline;

/* timer count at interrupt (for latency purposes) */
static hrt_abstime latency_actual;

static px4_sem_t _hrt_lock;
static struct work_s _hrt_work;

Expand All @@ -76,6 +89,7 @@ hrt_abstime hrt_absolute_time_offset();
static void hrt_call_reschedule();
static void hrt_call_invoke();
__EXPORT hrt_abstime hrt_reset();
static void hrt_latency_update(void);

hrt_abstime hrt_absolute_time_offset()
{
Expand Down Expand Up @@ -252,6 +266,24 @@ void hrt_cancel(struct hrt_call *entry)
// endif
}

static void
hrt_latency_update(void)
{
uint64_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
*/
Expand Down Expand Up @@ -325,8 +357,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();

Expand All @@ -351,8 +387,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.
*
Expand All @@ -378,6 +412,9 @@ hrt_call_reschedule()
}
}

/* remember it for latency tracking */
latency_baseline = deadline;

// There is no timer ISR, so simulate one by putting an event on the
// high priority work queue

Expand Down