PX4模塊設(shè)計(jì)之十二：High Resolution Timer設(shè)計(jì)

• 1. HRT模塊特性
• 2. HRT模塊基本功能
• • 2.1 循環(huán)觸發(fā)接口
  • 2.2 延遲觸發(fā)接口
  • 2.3 定時(shí)觸發(fā)接口
  • 2.4 其他功能
• 3. HRT模塊精度
• • 3.1 精度粒度
  • 3.2 精度誤差
• 4. 編譯模式
• • 4.1 Flat Build
  • 4.2 Protected Build
  • • 4.2.1 內(nèi)核態(tài)代碼
    • 4.2.2 用戶態(tài)代碼
• 5. 參考資料

PX4飛控整體軟件設(shè)計(jì)在嵌入式平臺(tái)(platform)層面增加了一個(gè)公共層(common)來處理work_queue、uORB、event，并統(tǒng)一接口對(duì)外提供服務(wù)。而這些對(duì)外服務(wù)的基礎(chǔ)又是基于HRT模塊。

這里針對(duì)Nuttx系統(tǒng)的Flat Build和Protected Build對(duì)HRT模塊進(jìn)行研讀。

1. HRT模塊特性

支持特性：

高精度(32bit毫秒)

支持循環(huán)、延遲、及定時(shí)觸發(fā)機(jī)制

支持Flat/Protected Build兩種編譯模式

支持ticket精度latency檢測(cè)

2. HRT模塊基本功能

基本模塊功能是通過hrt_call_internal函數(shù)的不同入?yún)韰^(qū)分的：

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
• 開始時(shí)間：hrt_abstime delay
• 觸發(fā)間隔：hrt_abstime interval
• 被調(diào)接口：hrt_callout callout
• 被調(diào)參數(shù)：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 循環(huán)觸發(fā)接口

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

• HRT列表：struct hrt_call *entry
• 開始時(shí)間：hrt_abstime delay
• 觸發(fā)間隔：hrt_abstime interval
• 被調(diào)接口：hrt_callout callout
• 被調(diào)參數(shù)：void *arg

2.2 延遲觸發(fā)接口

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

• HRT列表：struct hrt_call *entry
• 開始時(shí)間：hrt_abstime delay = hrt_absolute_time() + delay
• 觸發(fā)間隔：hrt_abstime interval = 0
• 被調(diào)接口：hrt_callout callout
• 被調(diào)參數(shù)：void *arg

2.3 定時(shí)觸發(fā)接口

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

• HRT列表：struct hrt_call *entry
• 開始時(shí)間：hrt_abstime delay
• 觸發(fā)間隔：hrt_abstime interval = 0
• 被調(diào)接口：hrt_callout callout
• 被調(diào)參數(shù)：void *arg

2.4 其他功能

bool hrt_called(struct hrt_call *entry) //已經(jīng)被調(diào)用，且從HRT列表中刪除 void hrt_cancel(struct hrt_call *entry) //從HRT列表中刪除void hrt_call_init(struct hrt_call *entry) //初始化HRT節(jié)點(diǎn) void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) //將該HRT節(jié)點(diǎn)時(shí)間從當(dāng)前時(shí)刻再延遲delay毫秒void hrt_init(void) //初始化高精度模塊hrt_abstime hrt_absolute_time(void) //返回值獲取當(dāng)前絕對(duì)時(shí)間，單位：ms void hrt_store_absolute_time(volatile hrt_abstime *t) //傳址獲取當(dāng)前絕對(duì)時(shí)間，單位：ms

3. HRT模塊精度

3.1 精度粒度

HRT底層進(jìn)度通過hrt_tim_isr可以分析出，基本在cpu tick級(jí)別；

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

而實(shí)際使用過程timer要求的精度在毫秒級(jí)，主要轉(zhuǎn)換函數(shù)見hrt_call_invoke里面的hrt_absolute_time函數(shù)。

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

實(shí)際誤差在hrt_latency_update中有l(wèi)atency 統(tǒng)計(jì)數(shù)據(jù)，分別對(duì)應(yīng)1tick，2tick，5tick，，，100tick，1000tick，這個(gè)和CPU的性能以及編譯使用的模式有關(guān)。

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

略。前面介紹的都是內(nèi)核態(tài)的代碼，這里不再細(xì)說。

4.2 Protected Build

關(guān)于boardctl注冊(cè)機(jī)制，可以參考board_ctrl.c，有興趣的朋友可以直接閱讀。

4.2.1 內(nèi)核態(tài)代碼

通過px4_register_boardct_ioctl對(duì)hrt_ioctl進(jìn)行注冊(cè)。

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 用戶態(tài)代碼

platforms/nuttx/src/px4/common/usr_hrt.cpp 主要通過boardctl，最終調(diào)用hrt_ioctl來執(zhí)行命令。

5. 參考資料

【1】PX4開源軟件框架簡(jiǎn)明簡(jiǎn)介

總結(jié)

以上是生活随笔為你收集整理的PX4模块设计之十二：High Resolution Timer设计的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。