fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 10:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

POSIX: fixed hrt call and workqueue implementation

Browse Source 
The HRT call processing normally happens via HW timer interrupt
handler. Since the POSIX port has no ISR handling, the HP work
queue is used.

Instead of irq_save() and irq_restore() calls to disable/enable
interrupts, a mutex is used to protect each queue.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
2015-05-05 00:27:14 -07:00
parent 12a25e4b63
commit e24405d374
9 changed files with 144 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/posix/px4_layer/work_thread.c
+38 -9
View File
@@ -45,6 +45,7 @@
#include <queue.h>
#include <px4_workqueue.h>
#include <drivers/drv_hrt.h>
#include "work_lock.h"
#ifdef CONFIG_SCHED_WORKQUEUE
@@ -66,6 +67,7 @@ struct wqueue_s g_work[NWORKERS];
/****************************************************************************
* Private Variables
****************************************************************************/
sem_t _work_lock[NWORKERS];
/****************************************************************************
* Private Functions
@@ -85,11 +87,10 @@ struct wqueue_s g_work[NWORKERS];
*
****************************************************************************/
static void work_process(FAR struct wqueue_s *wqueue)
static void work_process(FAR struct wqueue_s *wqueue, int lock_id)
{
volatile FAR struct work_s *work;
worker_t worker;
//irqstate_t flags;
FAR void *arg;
uint64_t elapsed;
uint32_t remaining;
@@ -100,7 +101,9 @@ static void work_process(FAR struct wqueue_s *wqueue)
*/
next = CONFIG_SCHED_WORKPERIOD;
//flags = irqsave();
work_lock(lock_id);
work = (FAR struct work_s *)wqueue->q.head;
while (work)
{
@@ -133,7 +136,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
* performed... we don't have any idea how long that will take!
*/
//irqrestore(flags);
work_unlock(lock_id);
if (!worker) {
printf("MESSED UP: worker = 0\n");
}
@@ -145,7 +148,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
* back at the head of the list.
*/
//flags = irqsave();
work_lock(lock_id);
work = (FAR struct work_s *)wqueue->q.head;
}
else
@@ -172,14 +175,40 @@ static void work_process(FAR struct wqueue_s *wqueue)
/* Wait awhile to check the work list. We will wait here until either
* the time elapses or until we are awakened by a signal.
*/
work_unlock(lock_id);
usleep(next);
//irqrestore(flags);
}
/****************************************************************************
* Public Functions
****************************************************************************/
void work_queues_init(void)
{
sem_init(&_work_lock[HPWORK], 0, 1);
sem_init(&_work_lock[LPWORK], 0, 1);
#ifdef CONFIG_SCHED_USRWORK
sem_init(&_work_lock[USRWORK], 0, 1);
#endif
// Create high priority worker thread
g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX-1,
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);
}
/****************************************************************************
* Name: work_hpthread, work_lpthread, and work_usrthread
*
@@ -234,7 +263,7 @@ int work_hpthread(int argc, char *argv[])
* we process items in the work list.
*/
work_process(&g_work[HPWORK]);
work_process(&g_work[HPWORK], HPWORK);
}
return PX4_OK; /* To keep some compilers happy */
@@ -261,7 +290,7 @@ int work_lpthread(int argc, char *argv[])
* we process items in the work list.
*/
work_process(&g_work[LPWORK]);
work_process(&g_work[LPWORK], LPWORK);
}
return PX4_OK; /* To keep some compilers happy */
@@ -282,7 +311,7 @@ int work_usrthread(int argc, char *argv[])
* we process items in the work list.
*/
work_process(&g_work[USRWORK]);
work_process(&g_work[USRWORK], USRWORK);
}
return PX4_OK; /* To keep some compilers happy */