APM:Libraries
Semaphores.cpp
Go to the documentation of this file.
1 #include <AP_HAL/AP_HAL.h>
2 
3 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
4 
5 #include "Semaphores.h"
6 
7 extern const AP_HAL::HAL& hal;
8 
9 using namespace HALSITL;
10 
11 bool Semaphore::give()
12 {
13  return pthread_mutex_unlock(&_lock) == 0;
14 }
15 
16 bool Semaphore::take(uint32_t timeout_ms)
17 {
18  if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) {
19  return pthread_mutex_lock(&_lock) == 0;
20  }
21  if (take_nonblocking()) {
22  return true;
23  }
24  uint64_t start = AP_HAL::micros64();
25  do {
26  hal.scheduler->delay_microseconds(200);
27  if (take_nonblocking()) {
28  return true;
29  }
30  } while ((AP_HAL::micros64() - start) < timeout_ms * 1000);
31  return false;
32 }
33 
35 {
36  return pthread_mutex_trylock(&_lock) == 0;
37 }
38 
39 #endif // CONFIG_HAL_BOARD
const AP_HAL::HAL & hal
Definition: AC_PID_test.cpp:14
#define HAL_SEMAPHORE_BLOCK_FOREVER
Definition: Semaphores.h:5
uint64_t micros64()
Definition: system.cpp:162
virtual void delay_microseconds(uint16_t us)=0
pthread_mutex_t _lock
Definition: Semaphores.h:18
bool take(uint32_t timeout_ms)
AP_HAL::Scheduler * scheduler
Definition: HAL.h:114