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

    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)

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

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    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

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10

    2.1 循环触发接口

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

    • 1
    • 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)

    • 1
    • 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)

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

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10

    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

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8

    而实际使用过程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);

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    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;

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10

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

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7

    4. 编译模式

    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)

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 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
}

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24

    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

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91

    4.2.2 用户态代码

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

    5. 参考资料

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

  • 相关阅读:
    算法提升 （三）基础数据结构
    day03-搭建微服务基础环境02
    Java 面试题 —— TCP 粘包、拆包问题
    Android学习笔记 1.2.3 Gradle的属性定义 && 1.2.4 增量式构建
    odoo 视图部分详解（一）
    大模型培训 AUTOWEBGLM：自动网页导航智能体
    [附源码]计算机毕业设计基于springboot校园帮平台管理系统
    大语言模型的三阶段训练
    进度条、git常见指令以及gdb的常用指令
    【华为OD机试真题 JS】找朋友
  • 原文地址：https://blog.csdn.net/lida2003/article/details/126000470