int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts) { work_s _hpwork = {}; // Get the current time. struct timespec ts_now; px4_clock_gettime(CLOCK_MONOTONIC, &ts_now); // We get an absolute time but want to calculate a timeout in us. hrt_abstime timeout_us = ts_to_abstime((struct timespec *)ts) - ts_to_abstime(&ts_now); // Create a timer to unblock. hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout_us); sem_wait(sem); hrt_work_cancel(&_hpwork); return 0; }
/** * Reschedule the next timer interrupt. * * This routine must be called with interrupts disabled. */ static void hrt_call_reschedule() { hrt_abstime now = hrt_absolute_time(); hrt_abstime delay = HRT_INTERVAL_MAX; struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); hrt_abstime deadline = now + HRT_INTERVAL_MAX; //PX4_INFO("hrt_call_reschedule"); /* * Determine what the next deadline will be. * * Note that we ensure that this will be within the counter * period, so that when we truncate all but the low 16 bits * the next time the compare matches it will be the deadline * we want. * * It is important for accurate timekeeping that the compare * interrupt fires sufficiently often that the base_time update in * hrt_absolute_time runs at least once per timer period. */ if (next != NULL) { //lldbg("entry in queue\n"); if (next->deadline <= (now + HRT_INTERVAL_MIN)) { //lldbg("pre-expired\n"); /* set a minimal deadline so that we call ASAP */ delay = HRT_INTERVAL_MIN; } else if (next->deadline < deadline) { //lldbg("due soon\n"); delay = next->deadline - now; } } // There is no timer ISR, so simulate one by putting an event on the // high priority work queue // Remove the existing expiry and update with the new expiry hrt_work_cancel(&_hrt_work); hrt_work_queue(&_hrt_work, (worker_t)&hrt_tim_isr, NULL, delay); }