mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-20 17:53:34 +08:00
Add a generic hrt driver userspace interface
This adds a nuttx userspace interface for hrt driver, communicating with the actual px4 hrt driver via BOARDCTL IOCTLs This is be used when running PX4 in NuttX protected or kernel builds Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
This commit is contained in:
Jukka Laitinen
committed by
Beat Küng
Beat Küng
parent
9f049b4dca
commit
77f71e61d2
@@ -8,6 +8,7 @@ add_library(px4_layer
|
||||
cdc_acm_check.cpp
|
||||
${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/print_load.cpp
|
||||
${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/cpuload.cpp
|
||||
usr_hrt.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(px4_layer
|
||||
|
||||
@@ -0,0 +1,221 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file usr_hrt.c
|
||||
*
|
||||
* Userspace High-resolution timer callouts and timekeeping.
|
||||
*
|
||||
* This can be used with nuttx userspace
|
||||
*
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/shutdown.h>
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/types.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <fcntl.h>
|
||||
#include <sched.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include <assert.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <board_config.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <sys/boardctl.h>
|
||||
|
||||
static px4_task_t g_usr_hrt_task = -1;
|
||||
|
||||
/**
|
||||
* Fetch a never-wrapping absolute time value in microseconds from
|
||||
* some arbitrary epoch shortly after system start.
|
||||
*/
|
||||
hrt_abstime
|
||||
hrt_absolute_time(void)
|
||||
{
|
||||
hrt_abstime abstime = 0;
|
||||
boardctl(HRT_ABSOLUTE_TIME, (uintptr_t)&abstime);
|
||||
return abstime;
|
||||
}
|
||||
|
||||
/**
|
||||
* Store the absolute time in an interrupt-safe fashion
|
||||
*/
|
||||
void
|
||||
hrt_store_absolute_time(volatile hrt_abstime *t)
|
||||
{
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
*t = hrt_absolute_time();
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
|
||||
/**
|
||||
* Event dispatcher thread
|
||||
*/
|
||||
int
|
||||
event_thread(int argc, char *argv[])
|
||||
{
|
||||
struct hrt_call *entry = NULL;
|
||||
|
||||
while (1) {
|
||||
/* Wait for hrt tick */
|
||||
boardctl(HRT_WAITEVENT, (uintptr_t)&entry);
|
||||
|
||||
/* HRT event received, dispatch */
|
||||
if (entry) {
|
||||
entry->usr_callout(entry->usr_arg);
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Request stop.
|
||||
*/
|
||||
bool hrt_request_stop()
|
||||
{
|
||||
px4_task_delete(g_usr_hrt_task);
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialise the high-resolution timing module.
|
||||
*/
|
||||
void
|
||||
hrt_init(void)
|
||||
{
|
||||
px4_register_shutdown_hook(hrt_request_stop);
|
||||
g_usr_hrt_task = px4_task_spawn_cmd("usr_hrt", SCHED_DEFAULT, SCHED_PRIORITY_MAX, 1000, event_thread, NULL);
|
||||
}
|
||||
|
||||
/**
|
||||
* Call callout(arg) after interval has elapsed.
|
||||
*/
|
||||
void
|
||||
hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
|
||||
{
|
||||
hrt_boardctl_t ioc_parm;
|
||||
ioc_parm.entry = entry;
|
||||
ioc_parm.time = delay;
|
||||
ioc_parm.callout = callout;
|
||||
ioc_parm.arg = arg;
|
||||
entry->usr_callout = callout;
|
||||
entry->usr_arg = arg;
|
||||
|
||||
boardctl(HRT_CALL_AFTER, (uintptr_t)&ioc_parm);
|
||||
}
|
||||
|
||||
/**
|
||||
* Call callout(arg) at calltime.
|
||||
*/
|
||||
void
|
||||
hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
|
||||
{
|
||||
hrt_boardctl_t ioc_parm;
|
||||
ioc_parm.entry = entry;
|
||||
ioc_parm.time = calltime;
|
||||
ioc_parm.interval = 0;
|
||||
ioc_parm.callout = callout;
|
||||
ioc_parm.arg = arg;
|
||||
entry->usr_callout = callout;
|
||||
entry->usr_arg = arg;
|
||||
|
||||
boardctl(HRT_CALL_AT, (uintptr_t)&ioc_parm);
|
||||
}
|
||||
|
||||
/**
|
||||
* Call callout(arg) every period.
|
||||
*/
|
||||
void
|
||||
hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
|
||||
{
|
||||
hrt_boardctl_t ioc_parm;
|
||||
ioc_parm.entry = entry;
|
||||
ioc_parm.time = delay;
|
||||
ioc_parm.interval = interval;
|
||||
ioc_parm.callout = callout;
|
||||
ioc_parm.arg = arg;
|
||||
entry->usr_callout = callout;
|
||||
entry->usr_arg = arg;
|
||||
|
||||
boardctl(HRT_CALL_EVERY, (uintptr_t)&ioc_parm);
|
||||
}
|
||||
|
||||
/**
|
||||
* Remove the entry from the callout list.
|
||||
*/
|
||||
void
|
||||
hrt_cancel(struct hrt_call *entry)
|
||||
{
|
||||
boardctl(HRT_CANCEL, (uintptr_t)entry);
|
||||
}
|
||||
|
||||
void
|
||||
hrt_call_init(struct hrt_call *entry)
|
||||
{
|
||||
memset(entry, 0, sizeof(*entry));
|
||||
}
|
||||
|
||||
/**
|
||||
* If this returns true, the call has been invoked and removed from the callout list.
|
||||
*
|
||||
* Always returns false for repeating callouts.
|
||||
*/
|
||||
bool
|
||||
hrt_called(struct hrt_call *entry)
|
||||
{
|
||||
return (entry->deadline == 0);
|
||||
}
|
||||
|
||||
latency_info_t
|
||||
get_latency(uint16_t bucket_idx, uint16_t counter_idx)
|
||||
{
|
||||
latency_boardctl_t latency_ioc;
|
||||
latency_ioc.bucket_idx = bucket_idx;
|
||||
latency_ioc.counter_idx = counter_idx;
|
||||
latency_ioc.latency = {0, 0};
|
||||
boardctl(HRT_GET_LATENCY, (uintptr_t)&latency_ioc);
|
||||
return latency_ioc.latency;
|
||||
}
|
||||
|
||||
void reset_latency_counters()
|
||||
{
|
||||
boardctl(HRT_RESET_LATENCY, NULL);
|
||||
}
|
||||
@@ -72,6 +72,33 @@
|
||||
# define hrtinfo(x...)
|
||||
#endif
|
||||
|
||||
#if !defined(CONFIG_BUILD_FLAT)
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform/board_ctrl.h>
|
||||
#include <px4_platform_common/sem.h>
|
||||
|
||||
#define HRT_ENTRY_QUEUE_MAX_SIZE 3
|
||||
static px4_sem_t g_wait_sem;
|
||||
static struct hrt_call *next_hrt_entry[HRT_ENTRY_QUEUE_MAX_SIZE];
|
||||
static int hrt_entry_queued = 0;
|
||||
static bool suppress_entry_queue_error = false;
|
||||
static bool hrt_entry_queue_error = false;
|
||||
|
||||
void hrt_usr_call(void *arg)
|
||||
{
|
||||
// This is called from hrt interrupt
|
||||
if (hrt_entry_queued < HRT_ENTRY_QUEUE_MAX_SIZE) {
|
||||
next_hrt_entry[hrt_entry_queued++] = (struct hrt_call *)arg;
|
||||
|
||||
} else {
|
||||
hrt_entry_queue_error = true;
|
||||
}
|
||||
|
||||
px4_sem_post(&g_wait_sem);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef HRT_TIMER
|
||||
|
||||
/* HRT configuration */
|
||||
@@ -275,6 +302,8 @@ static void hrt_call_enter(struct hrt_call *entry);
|
||||
static void hrt_call_reschedule(void);
|
||||
static void hrt_call_invoke(void);
|
||||
|
||||
|
||||
int hrt_ioctl(unsigned int cmd, unsigned long arg);
|
||||
/*
|
||||
* Specific registers and bits used by PPM sub-functions
|
||||
*/
|
||||
@@ -718,6 +747,16 @@ hrt_init(void)
|
||||
/* 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
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -978,6 +1017,81 @@ void reset_latency_counters(void)
|
||||
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
|
||||
|
||||
#endif /* HRT_TIMER */
|
||||
|
||||
Reference in New Issue
Block a user