PX4模块设计之十二：High Resolution Timer设计

PX4飞控整体软件设计在嵌入式平台(platform)层面增加了一个公共层(common)来处理work_queue、uORB、event，并统一接口对外提供服务。而这些对外服务的基础又是基于HRT模块。

这里针对Nuttx系统的Flat Build和Protected Build对HRT模块进行研读。

1. HRT模块特性

支持特性：

1. 高精度(32bit毫秒)
2. 支持循环、延迟、及定时触发机制
3. 支持Flat/Protected Build两种编译模式
4. 支持ticket精度latency检测

2. HRT模块基本功能

基本模块功能是通过hrt_call_internal函数的不同入参来区分的：

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

• HRT列表：struct hrt_call *entry
• 开始时间：hrt_abstime delay
• 触发间隔：hrt_abstime interval
• 被调接口：hrt_callout callout
• 被调参数：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 循环触发接口

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

• HRT列表：struct hrt_call *entry
• 开始时间：hrt_abstime delay
• 触发间隔：hrt_abstime interval
• 被调接口：hrt_callout callout
• 被调参数：void *arg

2.2 延迟触发接口

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

• HRT列表：struct hrt_call *entry
• 开始时间：hrt_abstime delay = hrt_absolute_time() + delay
• 触发间隔：hrt_abstime interval = 0
• 被调接口：hrt_callout callout
• 被调参数：void *arg

2.3 定时触发接口

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

• HRT列表：struct hrt_call *entry
• 开始时间：hrt_abstime delay
• 触发间隔：hrt_abstime interval = 0
• 被调接口：hrt_callout callout
• 被调参数：void *arg

2.4 其他功能

bool hrt_called(struct hrt_call *entry) //已经被调用，且从HRT列表中删除
void hrt_cancel(struct hrt_call *entry) //从HRT列表中删除

void hrt_call_init(struct hrt_call *entry) //初始化HRT节点
void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) //将该HRT节点时间从当前时刻再延迟delay毫秒

void hrt_init(void) //初始化高精度模块

hrt_abstime hrt_absolute_time(void) //返回值获取当前绝对时间，单位：ms
void hrt_store_absolute_time(volatile hrt_abstime *t) //传址获取当前绝对时间，单位：ms

3. HRT模块精度

3.1 精度粒度

HRT底层进度通过hrt_tim_isr可以分析出，基本在cpu tick级别；

hrt_tim_isr
 ├──> latency_actual = rCNT; //cpu tick精度
 ├──> 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

而实际使用过程timer要求的精度在毫秒级，主要转换函数见hrt_call_invoke里面的hrt_absolute_time函数。

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 精度误差

实际误差在hrt_latency_update中有latency 统计数据，分别对应1tick，2tick，5tick，，，100tick，1000tick，这个和CPU的性能以及编译使用的模式有关。

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
注：const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };

4. 编译模式

• 【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

略。前面介绍的都是内核态的代码，这里不再细说。

4.2 Protected Build

关于boardctl注册机制，可以参考board_ctrl.c，有兴趣的朋友可以直接阅读。

4.2.1 内核态代码

通过px4_register_boardct_ioctl对hrt_ioctl进行注册。

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第737-760行。

/**
 * 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第1005-1095行。

#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 用户态代码

platforms/nuttx/src/px4/common/usr_hrt.cpp 主要通过boardctl，最终调用hrt_ioctl来执行命令。

5. 参考资料

【1】PX4开源软件框架简明简介