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

1. HRT模块特性
2. HRT模块基本功能
- 2.1 循环触发接口
- 2.2 延迟触发接口
- 2.3 定时触发接口
- 2.4 其他功能
3. HRT模块精度
- 3.1 精度粒度
- 3.2 精度误差
4. 编译模式
- 4.1 Flat Build
- 4.2 Protected Build
- - 4.2.1 内核态代码
  - 4.2.2 用户态代码
5. 参考资料

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

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

1. HRT模块特性

支持特性：

高精度(32bit毫秒)
支持循环、延迟、及定时触发机制
支持Flat/Protected Build两种编译模式
支持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
 ├──> 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();
 └──> <(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
 └──> 
     ├──> 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
 └──> 
     ├──> hrt_abstime now = hrt_absolute_time();
     ├──> call = (struct hrt_call *)sq_peek(&callout_queue);
     ├──> 
     │   └──> break
     ├──> 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
     ├──> callout>
     │   └──> call->callout(call->arg); //invoke the callout (if there is one)
     └──> period != 0>
         ├──> 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;
 ├──> 
 │   ├──> deadline <= (now + HRT_INTERVAL_MIN)>
 │   │   └──> deadline = now + HRT_INTERVAL_MIN;
 │   └──> 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;
 └──> 
     └──> 
         ├──> 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行。

void
hrt_init(void)
{
	sq_init(&callout_queue);
	hrt_tim_init();

#ifdef HRT_PPM_CHANNEL
	
	px4_arch_configgpio(GPIO_PPM_IN);
#endif

#if !defined(CONFIG_BUILD_FLAT)
	
	px4_sem_init(&g_wait_sem, 0, 0);
	
	px4_sem_setprotocol(&g_wait_sem, SEM_PRIO_NONE);

	
	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)


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;
	}
}


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);
			
			flags = px4_enter_critical_section();

			
			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);

			
			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开源软件框架简明简介

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

Java相关栏目本月热门文章