void hrt_work_cancel(struct work_s *work) { struct wqueue_s *wqueue = &g_hrt_work; //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. */ hrt_work_lock(); if (work->worker != NULL) { /* A little test of the integrity of the work queue */ //DEBUGASSERT(work->dq.flink ||(dq_entry_t *)work == wqueue->q.tail); //DEBUGASSERT(work->dq.blink ||(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). */ dq_rem((dq_entry_t *)work, &wqueue->q); work->worker = NULL; } hrt_work_unlock(); }
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; }