int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay)
{
struct wqueue_s *wqueue = &g_work[qid];
//DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);
/* First, initialize the work structure */
work->worker = worker; /* Work callback */
work->arg = arg; /* Callback argument */
work->delay = delay; /* Delay until work performed */
/* Now, time-tag that entry and put it in the work queue. This must be
* done with interrupts disabled. This permits this function to be called
* from with task logic or interrupt handlers.
*/
work_lock(qid);
work->qtime = clock_systimer(); /* Time work queued */
dq_addlast((dq_entry_t *)work, &wqueue->q);
#ifdef __PX4_QURT
px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */
#else
px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */
#endif
work_unlock(qid);
return PX4_OK;
}
int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay)
{
struct wqueue_s *wqueue = &g_hrt_work;
/* First, initialize the work structure */
work->worker = worker; /* Work callback */
work->arg = arg; /* Callback argument */
work->delay = delay; /* Delay until work performed */
/* Now, time-tag that entry and put it in the work queue. This must be
* done with interrupts disabled. This permits this function to be called
* from with task logic or interrupt handlers.
*/
hrt_work_lock();
work->qtime = hrt_absolute_time(); /* Time work queued */
//PX4_INFO("hrt work_queue adding work delay=%u time=%lu", delay, work->qtime);
dq_addlast((dq_entry_t *)work, &wqueue->q);
px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */
hrt_work_unlock();
return PX4_OK;
}
