Px4 module design 12: high resolution timer design

PX4 The overall software of flight control is designed on the embedded platform (platform) Layer adds a public layer (common) To deal with it work_queue、uORB、event, And unified interface to provide external services . And the foundation of these external services is based on HRT modular .

Targeted here Nuttx Systematic Flat Build and Protected Build Yes HRT Module for study .

1. HRT Module features

Support characteristics ：

1. High precision (32bit millisecond )
2. Support loops 、 Delay 、 And timing trigger mechanism
3. Support Flat/Protected Build Two compilation modes
4. Support ticket precision latency testing

2. HRT The basic function of the module

The basic module function is through hrt_call_internal Functions are distinguished by different input parameters ：

static void hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)

• HRT list ：struct hrt_call *entry
• Starting time ：hrt_abstime delay
• Trigger interval ：hrt_abstime interval
• Called interface ：hrt_callout callout
• Adjusted parameter ：void *arg

hrt_call_internal
 ├──> px4_enter_critical_section
 ├──> <entry->deadline != 0>
 │   └──> sq_rem(&entry->link, &callout_queue);
 ├──> entry->deadline = deadline;
 ├──> entry->period = interval;
 ├──> entry->callout = callout;
 ├──> entry->arg = arg;
 ├──> hrt_call_enter(entry);
 └──> px4_leave_critical_section(flags);

hrt_call_enter
 ├──> call = (struct hrt_call *)sq_peek(&callout_queue);
 ├──> <(call == NULL) || (entry->deadline < call->deadline)>
 │   ├──> sq_addfirst(&entry->link, &callout_queue);
 │   └──> hrt_call_reschedule();
 └──> <else><do><(call = next) != NULL>
     ├──> next = (struct hrt_call *)sq_next(&call->link);
     ├──> <(next == NULL) || (entry->deadline < next->deadline)>
     ├──> sq_addafter(&call->link, &entry->link, &callout_queue);
     └──> break

2.1 Loop trigger interface

void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)

• HRT list ：struct hrt_call *entry
• Starting time ：hrt_abstime delay
• Trigger interval ：hrt_abstime interval
• Called interface ：hrt_callout callout
• Adjusted parameter ：void *arg

2.2 Delay trigger interface

void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)

• HRT list ：struct hrt_call *entry
• Starting time ：hrt_abstime delay = hrt_absolute_time() + delay
• Trigger interval ：hrt_abstime interval = 0
• Called interface ：hrt_callout callout
• Adjusted parameter ：void *arg

2.3 Timing trigger interface

void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)

• HRT list ：struct hrt_call *entry
• Starting time ：hrt_abstime delay
• Trigger interval ：hrt_abstime interval = 0
• Called interface ：hrt_callout callout
• Adjusted parameter ：void *arg

2.4 Other features

bool hrt_called(struct hrt_call *entry) // Already called , And from HRT Delete... From the list 
void hrt_cancel(struct hrt_call *entry) // from HRT Delete... From the list 

void hrt_call_init(struct hrt_call *entry) // initialization HRT node 
void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) // Will be HRT The node time is delayed from the current time delay millisecond 

void hrt_init(void) // Initialize the high-precision module 

hrt_abstime hrt_absolute_time(void) // Return value to get the current absolute time , Company ：ms
void hrt_store_absolute_time(volatile hrt_abstime *t) // Access the current absolute time , Company ：ms

3. HRT Module accuracy

3.1 Precision granularity

HRT The bottom progress passed hrt_tim_isr It can be analyzed , Basically in cpu tick Level ;

hrt_tim_isr
 ├──> latency_actual = rCNT; //cpu tick precision 
 ├──> status = rSR; //copy interrupt status
 ├──> rSR = ~status; //ack the interrupts we just read
 └──> <status & SR_INT_HRT>
     ├──> hrt_latency_update  //do latency calculations
     ├──> hrt_call_invoke     //run any callouts that have met their deadline
     └──> hrt_call_reschedule //and schedule the next interrupt

And the actual use process timer The accuracy required is in the order of milliseconds , See hrt_call_invoke Inside hrt_absolute_time function .

hrt_call_invoke
 └──> <while (true)>
     ├──> hrt_abstime now = hrt_absolute_time();
     ├──> call = (struct hrt_call *)sq_peek(&callout_queue);
     ├──> <call == NULL>
     │   └──> break
     ├──> <call->deadline > now>
     │   └──> break
     ├──> sq_rem(&call->link, &callout_queue); //remove and execute
     ├──> deadline = call->deadline; //save the intended deadline for periodic calls
     ├──> call->deadline = 0; //zero the deadline, as the call has occurred
     ├──> <call->callout>
     │   └──> call->callout(call->arg); //invoke the callout (if there is one)
     └──> <all->period != 0>
         ├──> <call->deadline <= now>
         │   └──> call->deadline = deadline + call->period;
         └──> hrt_call_enter(call);

hrt_call_reschedule
 ├──> hrt_abstime	now = hrt_absolute_time();
 ├──> struct hrt_call	*next = (struct hrt_call *)sq_peek(&callout_queue);
 ├──> hrt_abstime	deadline = now + HRT_INTERVAL_MAX;
 ├──> <next != NULL>
 │   ├──> <next->deadline <= (now + HRT_INTERVAL_MIN)>
 │   │   └──> deadline = now + HRT_INTERVAL_MIN;
 │   └──> <next->deadline < deadline>
 │       └──> deadline = next->deadline;
 └──> rCCR_HRT = latency_baseline = deadline & 0xffff;

3.2 Accuracy error

The actual error is hrt_latency_update There is latency statistics , They correspond to each other 1tick,2tick,5tick,,,100tick,1000tick, This and CPU The performance of and the mode used in compilation .

hrt_latency_update
 ├──> uint16_t latency = latency_actual - latency_baseline;
 └──> <for (index = 0; index < LATENCY_BUCKET_COUNT; index++)>
     └──> <latency <= latency_buckets[index]>
         ├──> latency_counters[index]++;
         └──> return
 notes ：const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };

4. Compile mode

• 【Flat Build】 /platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c
• 【Protected Build】platforms/nuttx/src/px4/common/usr_hrt.cpp + /platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c

4.1 Flat Build

A little . The previous introduction is all kernel code , I won't go into details here .

4.2 Protected Build

About boardctl Registration mechanism , You can refer to board_ctrl.c, Interested friends can read directly .

4.2.1 Kernel state code

adopt px4_register_boardct_ioctl Yes hrt_ioctl To register .

src/drivers/drv_hrt.h

#define HRT_WAITEVENT		_HRTIOC(1)
#define HRT_ABSOLUTE_TIME	_HRTIOC(2)
#define HRT_CALL_AFTER		_HRTIOC(3)
#define HRT_CALL_AT		_HRTIOC(4)
#define HRT_CALL_EVERY		_HRTIOC(5)
#define HRT_CANCEL		_HRTIOC(6)
#define HRT_GET_LATENCY		_HRTIOC(7)
#define HRT_RESET_LATENCY	_HRTIOC(8)

platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.cp The first 737-760 That's ok .

/**
 * Initialise the high-resolution timing module.
 */
void
hrt_init(void)
{
	sq_init(&callout_queue);
	hrt_tim_init();

#ifdef HRT_PPM_CHANNEL
	/* configure the PPM input pin */
	px4_arch_configgpio(GPIO_PPM_IN);
#endif

#if !defined(CONFIG_BUILD_FLAT)
	/* Create a semaphore for handling hrt driver callbacks */
	px4_sem_init(&g_wait_sem, 0, 0);
	/* this is a signalling semaphore */
	px4_sem_setprotocol(&g_wait_sem, SEM_PRIO_NONE);

	/* register ioctl callbacks */
	px4_register_boardct_ioctl(_HRTIOCBASE, hrt_ioctl);
#endif
}

platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.cp The first 1005-1095 That's ok .

#if !defined(CONFIG_BUILD_FLAT)
/* These functions are inlined in all but NuttX protected/kernel builds */

latency_info_t get_latency(uint16_t bucket_idx, uint16_t counter_idx)
{
	latency_info_t ret = {latency_buckets[bucket_idx], latency_counters[counter_idx]};
	return ret;
}

void reset_latency_counters(void)
{
	for (int i = 0; i <= get_latency_bucket_count(); i++) {
		latency_counters[i] = 0;
	}
}

/* board_ioctl interface for user-space hrt driver */
int
hrt_ioctl(unsigned int cmd, unsigned long arg)
{
	hrt_boardctl_t *h = (hrt_boardctl_t *)arg;

	switch (cmd) {
	case HRT_WAITEVENT: {
			irqstate_t flags;
			px4_sem_wait(&g_wait_sem);
			/* Atomically update the pointer to user side hrt entry */
			flags = px4_enter_critical_section();

			/* This should be always true, but check it anyway */
			if (hrt_entry_queued > 0) {
				*(struct hrt_call **)arg = next_hrt_entry[--hrt_entry_queued];
				next_hrt_entry[hrt_entry_queued] = NULL;

			} else {
				hrt_entry_queue_error = true;
			}

			px4_leave_critical_section(flags);

			/* Warn once for entry queue being full */
			if (hrt_entry_queue_error && !suppress_entry_queue_error) {
				PX4_ERR("HRT entry error, queue size now %d", hrt_entry_queued);
				suppress_entry_queue_error = true;
			}
		}
		break;

	case HRT_ABSOLUTE_TIME:
		*(hrt_abstime *)arg = hrt_absolute_time();
		break;

	case HRT_CALL_AFTER:
		hrt_call_after(h->entry, h->time, (hrt_callout)hrt_usr_call, h->entry);
		break;

	case HRT_CALL_AT:
		hrt_call_at(h->entry, h->time, (hrt_callout)hrt_usr_call, h->entry);
		break;

	case HRT_CALL_EVERY:
		hrt_call_every(h->entry, h->time, h->interval, (hrt_callout)hrt_usr_call, h->entry);
		break;

	case HRT_CANCEL:
		if (h && h->entry) {
			hrt_cancel(h->entry);

		} else {
			PX4_ERR("HRT_CANCEL called with NULL entry");
		}

		break;

	case HRT_GET_LATENCY: {
			latency_boardctl_t *latency = (latency_boardctl_t *)arg;
			latency->latency = get_latency(latency->bucket_idx, latency->counter_idx);
		}
		break;

	case HRT_RESET_LATENCY:
		reset_latency_counters();
		break;

	default:
		return -EINVAL;
	}

	return OK;
}
#endif

4.2.2 User status code

platforms/nuttx/src/px4/common/usr_hrt.cpp Mainly through boardctl, The final call hrt_ioctl To execute an order .

5. Reference material

【1】PX4 A brief introduction to the open source software framework