dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Linux: Added preliminary work queue support 

Based on NuttX work queue code.

Not yet functional.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois 2015-03-25 10:53:08 -07:00
parent 056d5f9075
commit aedf6fe628
9 changed files with 95 additions and 129 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/platforms/linux/px4_layer/module.mk
View File

@ -38,6 +38,9 @@
SRCS = \
px4_linux_impl.cpp \
px4_linux_tasks.c \
work_thread.c \
work_queue.c \
work_cancel.c \
lib_crc32.c \
drv_hrt.c \
queue.c \
@ -45,6 +48,7 @@ SRCS = \
dq_remfirst.c \
sq_addlast.c \
sq_remfirst.c \
sq_addafter.c
sq_addafter.c \
dq_rem.c
MAXOPTIMIZATION = -Os

66
src/platforms/linux/px4_layer/px4_linux_impl.cpp
View File

@ -56,60 +56,32 @@ long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK);
__END_DECLS
extern struct wqueue_s gwork[NWORKERS];
namespace px4
{
void init(int argc, char *argv[], const char *app_name)
{
struct param_info_s test_1 = {
"TEST_1",
PARAM_TYPE_INT32
};
test_1.val.i = 2;
struct param_info_s test_2 = {
"TEST_2",
PARAM_TYPE_INT32
};
test_2.val.i = 4;
struct param_info_s rc_x = {
"RC_X",
PARAM_TYPE_INT32
};
rc_x.val.i = 8;
struct param_info_s rc2_x = {
"RC2_X",
PARAM_TYPE_INT32
};
rc2_x.val.i = 16;
param_array[0] = test_1;
param_array[1] = test_2;
param_array[2] = rc_x;
param_array[3] = rc2_x;
param_info_base = (struct param_info_s *) &param_array[0];
param_info_limit = (struct param_info_s *) &param_array[4]; // needs to point at the end of the data,
// therefore number of params + 1
printf("App name: %s\n", app_name);
// Create high priority worker thread
g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX,
2000,
work_hpthread,
(char* const*)NULL);
// Create low priority worker thread
g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low",
SCHED_DEFAULT,
SCHED_PRIORITY_MIN,
2000,
work_lpthread,
(char* const*)NULL);
}
}
int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay)
{
printf("work_queue: UNIMPLEMENTED\n");
return PX4_OK;
}
int work_cancel(int qid, struct work_s *work)
{
printf("work_cancel: UNIMPLEMENTED\n");
return PX4_OK;
}

29
src/platforms/linux/px4_layer/work_cancel.c
View File

@ -37,15 +37,10 @@
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <queue.h>
#include <assert.h>
#include <errno.h>
#include <debug.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <px4_workqueue.h>
#ifdef CONFIG_SCHED_WORKQUEUE
@ -90,25 +85,25 @@
*
****************************************************************************/
int work_cancel(int qid, FAR struct work_s *work)
int work_cancel(int qid, struct work_s *work)
{
FAR struct wqueue_s *wqueue = &g_work[qid];
irqstate_t flags;
struct wqueue_s *wqueue = &g_work[qid];
//irqstate_t flags;
DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);
//DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);
/* Cancelling the work is simply a matter of removing the work structure
* from the work queue. This must be done with interrupts disabled because
* new work is typically added to the work queue from interrupt handlers.
*/
flags = irqsave();
//flags = irqsave();
if (work->worker != NULL)
{
/* A little test of the integrity of the work queue */
DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail);
DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head);
//DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail);
//DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head);
/* Remove the entry from the work queue and make sure that it is
* mark as availalbe (i.e., the worker field is nullified).
@ -118,8 +113,8 @@ int work_cancel(int qid, FAR struct work_s *work)
work->worker = NULL;
}
irqrestore(flags);
return OK;
//irqrestore(flags);
return PX4_OK;
}
#endif /* CONFIG_SCHED_WORKQUEUE */

30
src/platforms/linux/px4_layer/work_queue.c
View File

@ -37,17 +37,13 @@
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <signal.h>
#include <stdint.h>
#include <queue.h>
#include <assert.h>
#include <errno.h>
#include <debug.h>
#include <nuttx/arch.h>
#include <nuttx/clock.h>
#include <nuttx/wqueue.h>
#include <px4_workqueue.h>
#ifdef CONFIG_SCHED_WORKQUEUE
@ -104,13 +100,11 @@
*
****************************************************************************/
int work_queue(int qid, FAR struct work_s *work, worker_t worker,
FAR void *arg, uint32_t delay)
int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay)
{
FAR struct wqueue_s *wqueue = &g_work[qid];
irqstate_t flags;
struct wqueue_s *wqueue = &g_work[qid];
DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);
//DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);
/* First, initialize the work structure */
@ -123,14 +117,14 @@ int work_queue(int qid, FAR struct work_s *work, worker_t worker,
* from with task logic or interrupt handlers.
*/
flags = irqsave();
//flags = irqsave();
work->qtime = clock_systimer(); /* Time work queued */
dq_addlast((FAR dq_entry_t *)work, &wqueue->q);
kill(wqueue->pid, SIGWORK); /* Wake up the worker thread */
dq_addlast((dq_entry_t *)work, &wqueue->q);
pthread_kill(wqueue->pid, SIGUSR1); /* Wake up the worker thread */
irqrestore(flags);
return OK;
//irqrestore(flags);
return PX4_OK;
}
#endif /* CONFIG_SCHED_WORKQUEUE */

59
src/platforms/linux/px4_layer/work_thread.c
View File

@ -37,19 +37,12 @@
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <stdint.h>
#include <unistd.h>
#include <queue.h>
#include <assert.h>
#include <errno.h>
#include <debug.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <nuttx/kmalloc.h>
#include <px4_workqueue.h>
#ifdef CONFIG_SCHED_WORKQUEUE
@ -66,29 +59,8 @@
****************************************************************************/
/* The state of each work queue. */
#ifdef CONFIG_NUTTX_KERNEL
/* Play some games in the kernel mode build to assure that different
* naming is used for the global work queue data structures. This may
* not be necessary but it safer.
*
* In this case g_work is #define'd to be either g_kernelwork or
* g_usrwork in include/nuttx/wqueue.h
*/
# ifdef __KERNEL__
struct wqueue_s g_kernelwork[NWORKERS];
# else
struct wqueue_s g_usrwork[NWORKERS];
# endif
#else /* CONFIG_NUTTX_KERNEL */
struct wqueue_s g_work[NWORKERS];
#endif /* CONFIG_NUTTX_KERNEL */
/****************************************************************************
* Private Variables
****************************************************************************/
@ -115,7 +87,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
{
volatile FAR struct work_s *work;
worker_t worker;
irqstate_t flags;
//irqstate_t flags;
FAR void *arg;
uint32_t elapsed;
uint32_t remaining;
@ -125,8 +97,9 @@ static void work_process(FAR struct wqueue_s *wqueue)
* we process items in the work list.
*/
next = CONFIG_SCHED_WORKPERIOD / USEC_PER_TICK;
flags = irqsave();
//next = CONFIG_SCHED_WORKPERIOD / USEC_PER_TICK;
next = 100;
//flags = irqsave();
work = (FAR struct work_s *)wqueue->q.head;
while (work)
{
@ -158,7 +131,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
* performed... we don't have any idea how long that will take!
*/
irqrestore(flags);
//irqrestore(flags);
worker(arg);
/* Now, unfortunately, since we re-enabled interrupts we don't
@ -166,7 +139,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
* back at the head of the list.
*/
flags = irqsave();
//flags = irqsave();
work = (FAR struct work_s *)wqueue->q.head;
}
else
@ -195,7 +168,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
*/
usleep(next * USEC_PER_TICK);
irqrestore(flags);
//irqrestore(flags);
}
/****************************************************************************
@ -258,7 +231,7 @@ int work_hpthread(int argc, char *argv[])
work_process(&g_work[HPWORK]);
}
return OK; /* To keep some compilers happy */
return PX4_OK; /* To keep some compilers happy */
}
#ifdef CONFIG_SCHED_LPWORK
@ -276,7 +249,7 @@ int work_lpthread(int argc, char *argv[])
* the IDLE thread (at a very, very low priority).
*/
sched_garbagecollection();
//sched_garbagecollection();
/* Then process queued work. We need to keep interrupts disabled while
* we process items in the work list.
@ -285,7 +258,7 @@ int work_lpthread(int argc, char *argv[])
work_process(&g_work[LPWORK]);
}
return OK; /* To keep some compilers happy */
return PX4_OK; /* To keep some compilers happy */
}
#endif /* CONFIG_SCHED_LPWORK */
@ -306,9 +279,13 @@ int work_usrthread(int argc, char *argv[])
work_process(&g_work[USRWORK]);
}
return OK; /* To keep some compilers happy */
return PX4_OK; /* To keep some compilers happy */
}
#endif /* CONFIG_SCHED_USRWORK */
int clock_systimer()
{
return 1;
}
#endif /* CONFIG_SCHED_WORKQUEUE */

3
src/platforms/px4_config.h
View File

@ -44,6 +44,9 @@
#include <px4_config.h>
#elif defined (__PX4_LINUX)
#define CONFIG_NFILE_STREAMS 1
#define CONFIG_SCHED_WORKQUEUE 1
#define CONFIG_SCHED_HPWORK 1
#define CONFIG_SCHED_LPWORK 1
#define CONFIG_ARCH_BOARD_LINUXTEST 1
#define px4_errx(x, ...) errx(x, __VA_ARGS__)

3
src/platforms/px4_defines.h
View File

@ -123,7 +123,8 @@ __BEGIN_DECLS
extern long PX4_TICKS_PER_SEC;
__END_DECLS
#define USEC2TICK(x) (PX4_TICKS_PER_SEC*(long)x/1000000L)
#define USEC2TICK(x) (PX4_TICKS_PER_SEC*(long)(x)/1000000L)
#define USEC_PER_TICK (1000000L/PX4_TICKS_PER_SEC)
#define px4_statfs_buf_f_bavail_t unsigned long

1
src/platforms/px4_tasks.h
View File

@ -55,6 +55,7 @@ typedef int px4_task_t;
#define SCHED_DEFAULT SCHED_FIFO
#define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO)
#define SCHED_PRIORITY_MIN sched_get_priority_min(SCHED_FIFO)
#define SCHED_PRIORITY_DEFAULT sched_get_priority_max(SCHED_FIFO)
typedef pthread_t px4_task_t;

27
src/platforms/px4_workqueue.h
View File

@ -43,10 +43,22 @@
#include <nuttx/clock.h>
#elif defined(__PX4_LINUX)
#include <stdint.h>
#include <queue.h>
__BEGIN_DECLS
#define HPWORK 0
#define LPWORK 1
#define HPWORK 2
#define NWORKERS 2
struct wqueue_s
{
pid_t pid; /* The task ID of the worker thread */
struct dq_queue_s q; /* The queue of pending work */
};
extern struct wqueue_s g_work[NWORKERS];
/* Defines the work callback */
@ -90,8 +102,7 @@ struct work_s
*
****************************************************************************/
int work_queue(int qid, FAR struct work_s *work, worker_t worker,
FAR void *arg, uint32_t delay);
int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay);
/****************************************************************************
* Name: work_cancel
@ -110,7 +121,15 @@ int work_queue(int qid, FAR struct work_s *work, worker_t worker,
*
****************************************************************************/
int work_cancel(int qid, FAR struct work_s *work);
int work_cancel(int qid, struct work_s *work);
int clock_systimer(void);
int work_hpthread(int argc, char *argv[]);
int work_lpthread(int argc, char *argv[]);
__END_DECLS
#else
#error "Unknown target OS"
#endif